Fix race condition in AMCL for ros-navigation#4537 (ros-navigation#4605)

Signed-off-by: Joseph Duchesne <[email protected]>
josephduchesne · Dec 10, 2024 · 873572a · 873572a
1 parent c01ec92
commit 873572a
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
@@ -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.");