From 87303bf11f8a51db1e3ef2e408cd23cf832c4b73 Mon Sep 17 00:00:00 2001 From: GoesM Date: Tue, 11 Jun 2024 16:50:34 +0800 Subject: [PATCH 01/10] reorder pointers-reset & remove rebundant code Signed-off-by: GoesM --- nav2_amcl/src/amcl_node.cpp | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index 398dca71745..6f5b1c201fc 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -321,14 +321,13 @@ AmclNode::on_cleanup(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Cleaning up"); - executor_thread_.reset(); + executor_thread_.reset(); // at the very beginning, shutdown threads // Get rid of the inputs first (services and message filter input), so we // don't continue to process incoming messages global_loc_srv_.reset(); initial_guess_srv_.reset(); - nomotion_update_srv_.reset(); - executor_thread_.reset(); // to make sure initial_pose_sub_ completely exit + nomotion_update_srv_.reset(); initial_pose_sub_.reset(); laser_scan_connection_.disconnect(); tf_listener_.reset(); // listener may access lase_scan_filter_, so it should be reset earlier @@ -1363,13 +1362,15 @@ AmclNode::dynamicParametersCallback( // Re-initialize the lasers and it's filters if (reinit_laser) { - lasers_.clear(); - lasers_update_.clear(); - frame_to_laser_.clear(); + // firstly, shutdown threads of plugins laser_scan_connection_.disconnect(); laser_scan_filter_.reset(); laser_scan_sub_.reset(); - + // secondly, clear memory used in these threads. + lasers_.clear(); + lasers_update_.clear(); + frame_to_laser_.clear(); + // thirdly, launch plugins within new configuration initMessageFilters(); } From fc3654eb1642d82f6d46c73aafc8187488d58093 Mon Sep 17 00:00:00 2001 From: GoesM Date: Tue, 11 Jun 2024 23:11:49 +0800 Subject: [PATCH 02/10] check the initialpose in map Signed-off-by: GoesM --- nav2_amcl/src/amcl_node.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index 6f5b1c201fc..2e28b4130ab 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -535,6 +535,12 @@ 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; From 0a59133e011da38f8a8af73d922b87b9b4920d55 Mon Sep 17 00:00:00 2001 From: GoesM Date: Tue, 11 Jun 2024 23:20:31 +0800 Subject: [PATCH 03/10] back Signed-off-by: GoesM --- nav2_amcl/src/amcl_node.cpp | 23 ++++++++--------------- 1 file changed, 8 insertions(+), 15 deletions(-) diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index 2e28b4130ab..1701eea5a7d 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -321,13 +321,14 @@ AmclNode::on_cleanup(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Cleaning up"); - executor_thread_.reset(); // at the very beginning, shutdown threads + executor_thread_.reset(); // Get rid of the inputs first (services and message filter input), so we // don't continue to process incoming messages global_loc_srv_.reset(); initial_guess_srv_.reset(); - nomotion_update_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 @@ -535,12 +536,6 @@ 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; @@ -1368,15 +1363,13 @@ AmclNode::dynamicParametersCallback( // Re-initialize the lasers and it's filters if (reinit_laser) { - // firstly, shutdown threads of plugins - laser_scan_connection_.disconnect(); - laser_scan_filter_.reset(); - laser_scan_sub_.reset(); - // secondly, clear memory used in these threads. lasers_.clear(); lasers_update_.clear(); frame_to_laser_.clear(); - // thirdly, launch plugins within new configuration + laser_scan_connection_.disconnect(); + laser_scan_filter_.reset(); + laser_scan_sub_.reset(); + initMessageFilters(); } @@ -1642,4 +1635,4 @@ AmclNode::initLaserScan() // Register the component with class_loader. // This acts as a sort of entry point, allowing the component to be discoverable when its library // is being loaded into a running process. -RCLCPP_COMPONENTS_REGISTER_NODE(nav2_amcl::AmclNode) +RCLCPP_COMPONENTS_REGISTER_NODE(nav2_amcl::AmclNode) \ No newline at end of file From 5d656d395b0e1cf3ec2f7c4a888ebb7748c01b04 Mon Sep 17 00:00:00 2001 From: GoesM Date: Tue, 11 Jun 2024 23:23:00 +0800 Subject: [PATCH 04/10] remove the redunbant code Signed-off-by: GoesM --- nav2_amcl/src/amcl_node.cpp | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index 1701eea5a7d..64af726d5ee 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -321,14 +321,13 @@ AmclNode::on_cleanup(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Cleaning up"); - executor_thread_.reset(); + executor_thread_.reset(); // shutdown excutor_thread at the very begeinng, to make sure initial_pose_sub_ completely exit // Get rid of the inputs first (services and message filter input), so we // don't continue to process incoming messages 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,12 @@ 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; From 15991cd4033d461774286e50b07da12546e5f766 Mon Sep 17 00:00:00 2001 From: GoesM Date: Tue, 11 Jun 2024 23:24:26 +0800 Subject: [PATCH 05/10] code style Signed-off-by: GoesM --- nav2_amcl/src/amcl_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index 64af726d5ee..c5d70824359 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -1640,4 +1640,4 @@ AmclNode::initLaserScan() // Register the component with class_loader. // This acts as a sort of entry point, allowing the component to be discoverable when its library // is being loaded into a running process. -RCLCPP_COMPONENTS_REGISTER_NODE(nav2_amcl::AmclNode) \ No newline at end of file +RCLCPP_COMPONENTS_REGISTER_NODE(nav2_amcl::AmclNode) From 83689fb98af95efa13b55a0f7601a344e415f601 Mon Sep 17 00:00:00 2001 From: GoesM Date: Tue, 11 Jun 2024 23:25:02 +0800 Subject: [PATCH 06/10] code style Signed-off-by: GoesM --- nav2_amcl/src/amcl_node.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index c5d70824359..2341d4bae11 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -535,12 +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) + 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; + 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; From 6b808405e09b15267a0d6b38e647f5b1ff0b948b Mon Sep 17 00:00:00 2001 From: GoesM Date: Tue, 11 Jun 2024 23:29:10 +0800 Subject: [PATCH 07/10] code style Signed-off-by: GoesM --- nav2_amcl/src/amcl_node.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index 2341d4bae11..9044ed251f2 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -535,8 +535,7 @@ 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 (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."); From 6ec2fd5844ed8b1d9530aa8cb10a07673e8b36cf Mon Sep 17 00:00:00 2001 From: GoesM Date: Tue, 11 Jun 2024 23:33:22 +0800 Subject: [PATCH 08/10] codestyle Signed-off-by: GoesM --- nav2_amcl/src/amcl_node.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index 9044ed251f2..2341d4bae11 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -535,7 +535,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 (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."); From 6fd8bd6226f5f216ba197f1104d826d60df60f51 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Tue, 11 Jun 2024 08:39:11 -0700 Subject: [PATCH 09/10] Update nav2_amcl/src/amcl_node.cpp Signed-off-by: Steve Macenski --- nav2_amcl/src/amcl_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index 2341d4bae11..8e3a22cd0d9 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -539,7 +539,7 @@ AmclNode::initialPoseReceived(geometry_msgs::msg::PoseWithCovarianceStamped::Sha 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."); + get_logger(), "Received initialpose from message is out of the size of map. Rejecting."); return; } From 4d5ddc4351cd56095e2030f647db236926e3c0f3 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Tue, 11 Jun 2024 08:39:15 -0700 Subject: [PATCH 10/10] Update nav2_amcl/src/amcl_node.cpp Signed-off-by: Steve Macenski --- nav2_amcl/src/amcl_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index 8e3a22cd0d9..cdd4c5adbdb 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -321,7 +321,7 @@ AmclNode::on_cleanup(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Cleaning up"); - executor_thread_.reset(); // shutdown excutor_thread at the very begeinng, to make sure initial_pose_sub_ completely exit + executor_thread_.reset(); // Get rid of the inputs first (services and message filter input), so we // don't continue to process incoming messages