[amcl] check the recieved initialpose in the size of map (ros-navigat…

…ion#4416) * reorder pointers-reset & remove rebundant code Signed-off-by: GoesM <[email protected]> * check the initialpose in map Signed-off-by: GoesM <[email protected]> * back Signed-off-by: GoesM <[email protected]> * remove the redunbant code Signed-off-by: GoesM <[email protected]> * code style Signed-off-by: GoesM <[email protected]> * code style Signed-off-by: GoesM <[email protected]> * code style Signed-off-by: GoesM <[email protected]> * codestyle Signed-off-by: GoesM <[email protected]> * Update nav2_amcl/src/amcl_node.cpp Signed-off-by: Steve Macenski <[email protected]> * Update nav2_amcl/src/amcl_node.cpp Signed-off-by: Steve Macenski <[email protected]> --------- Signed-off-by: GoesM <[email protected]> Signed-off-by: Steve Macenski <[email protected]> Co-authored-by: GoesM <[email protected]> Co-authored-by: Steve Macenski <[email protected]>
stevedanomodolor · Jun 11, 2024 · e8adc71 · e8adc71
1 parent 3e9e0db
commit e8adc71
Showing 1 changed file with 8 additions and 1 deletion.
diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp
@@ -328,7 +328,6 @@ AmclNode::on_cleanup(const rclcpp_lifecycle::State & /*state*/)
   global_loc_srv_.reset();
   initial_guess_srv_.reset();
   nomotion_update_srv_.reset();
-  executor_thread_.reset();  //  to make sure initial_pose_sub_ completely exit
   initial_pose_sub_.reset();
   laser_scan_connection_.disconnect();
   tf_listener_.reset();  //  listener may access lase_scan_filter_, so it should be reset earlier
@@ -536,6 +535,14 @@ 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)
+  {
+    RCLCPP_ERROR(
+      get_logger(), "Received initialpose from message is out of the size of map. Rejecting.");
+    return;
+  }
+
   // Overriding last published pose to initial pose
   last_published_pose_ = *msg;