From 873572aefa11d2d2e4c09f89b38a3d53887ebe60 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) Signed-off-by: Joseph Duchesne --- 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 5c6b165d594..595ffb7626c 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.");