Fix race condition in AMCL for #4537 (#4605)

ros-navigation · Aug 23, 2024 · a750f8c · a750f8c
1 parent 27ed8e9
commit a750f8c
Showing 1 changed file with 2 additions and 2 deletions.
diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp
@@ -541,8 +541,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.");