diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index 398dca71745..cdd4c5adbdb 100644 --- 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;