From 19fdf5cd5ccb8b5de6f02efb7901142325a2924a Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Wed, 7 Aug 2024 16:08:35 -0700 Subject: [PATCH] Fix race condition in AMCL for #4537 --- 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 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.");