diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index 5c6b165d59..595ffb7626 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -537,8 +537,8 @@ AmclNode::initialPoseReceived(geometry_msgs::msg::PoseWithCovarianceStamped::Sha global_frame_id_.c_str()); return; } - if (abs(msg->pose.pose.position.x) > map_->size_x || - abs(msg->pose.pose.position.y) > map_->size_y) + if (first_map_received_ && (abs(msg->pose.pose.position.x) > map_->size_x || + abs(msg->pose.pose.position.y) > map_->size_y)) { RCLCPP_ERROR( get_logger(), "Received initialpose from message is out of the size of map. Rejecting.");