From a750f8c49a5d117913d8ff04e0cd25933b05ee6b Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Wed, 7 Aug 2024 16:32:32 -0700 Subject: [PATCH] Fix race condition in AMCL for #4537 (#4605) --- nav2_amcl/src/amcl_node.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index b5e512ba94..88e28e2903 100644 --- 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.");