From bfa9184ac8fa7ce7d549bf0e29907b9bbdaad2d1 Mon Sep 17 00:00:00 2001 From: Alexander Mock Date: Fri, 15 Mar 2024 01:20:12 +0100 Subject: [PATCH 1/4] Added initial guess service. Signed-off-by: Alexander Mock Signed-off-by: Alexander Mock --- nav2_amcl/include/nav2_amcl/amcl_node.hpp | 12 ++++++++++++ nav2_amcl/src/amcl_node.cpp | 14 ++++++++++++++ nav2_msgs/CMakeLists.txt | 1 + nav2_msgs/srv/SetInitialPose.srv | 2 ++ 4 files changed, 29 insertions(+) create mode 100644 nav2_msgs/srv/SetInitialPose.srv diff --git a/nav2_amcl/include/nav2_amcl/amcl_node.hpp b/nav2_amcl/include/nav2_amcl/amcl_node.hpp index 2360d09c25..1542ab4d48 100644 --- a/nav2_amcl/include/nav2_amcl/amcl_node.hpp +++ b/nav2_amcl/include/nav2_amcl/amcl_node.hpp @@ -35,6 +35,7 @@ #include "nav2_amcl/sensors/laser/laser.hpp" #include "nav2_msgs/msg/particle.hpp" #include "nav2_msgs/msg/particle_cloud.hpp" +#include "nav2_msgs/srv/set_initial_pose.hpp" #include "nav_msgs/srv/set_map.hpp" #include "sensor_msgs/msg/laser_scan.hpp" #include "std_srvs/srv/empty.hpp" @@ -210,6 +211,16 @@ class AmclNode : public nav2_util::LifecycleNode const std::shared_ptr request, std::shared_ptr response); + // service server for providing an initial pose guess + rclcpp::Service::SharedPtr initial_guess_srv_; + /* + * @brief Service callback for a global relocalization request + */ + void initialGuessCallback( + const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response); + // Let amcl update samples without requiring motion rclcpp::Service::SharedPtr nomotion_update_srv_; /* @@ -223,6 +234,7 @@ class AmclNode : public nav2_util::LifecycleNode // Nomotion update control. Used to temporarily let amcl update samples even when no motion occurs std::atomic force_update_{false}; + // Odometry /* * @brief Initialize odometry diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index 192111439f..bf7f2352f5 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -494,6 +494,16 @@ AmclNode::globalLocalizationCallback( pf_init_ = false; } +void +AmclNode::initialGuessCallback( + const std::shared_ptr/*request_header*/, + const std::shared_ptr req, + std::shared_ptr/*res*/) +{ + std::lock_guard cfl(mutex_); + initialPoseReceived(std::make_shared(req->pose)); +} + // force nomotion updates (amcl updating without requiring motion) void AmclNode::nomotionUpdateCallback( @@ -1543,6 +1553,10 @@ AmclNode::initServices() "reinitialize_global_localization", std::bind(&AmclNode::globalLocalizationCallback, this, _1, _2, _3)); + initial_guess_srv_ = create_service( + "set_initial_pose", + std::bind(&AmclNode::initialGuessCallback, this, _1, _2, _3)); + nomotion_update_srv_ = create_service( "request_nomotion_update", std::bind(&AmclNode::nomotionUpdateCallback, this, _1, _2, _3)); diff --git a/nav2_msgs/CMakeLists.txt b/nav2_msgs/CMakeLists.txt index 206ae86322..12637abca6 100644 --- a/nav2_msgs/CMakeLists.txt +++ b/nav2_msgs/CMakeLists.txt @@ -38,6 +38,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "srv/ManageLifecycleNodes.srv" "srv/LoadMap.srv" "srv/SaveMap.srv" + "srv/SetInitialPose.srv" "action/AssistedTeleop.action" "action/BackUp.action" "action/ComputePathToPose.action" diff --git a/nav2_msgs/srv/SetInitialPose.srv b/nav2_msgs/srv/SetInitialPose.srv new file mode 100644 index 0000000000..0fcd8e0a7a --- /dev/null +++ b/nav2_msgs/srv/SetInitialPose.srv @@ -0,0 +1,2 @@ +geometry_msgs/PoseWithCovarianceStamped pose +--- \ No newline at end of file From 1bfb42ef79dfc02be83307e85b4ea42632b6f0f7 Mon Sep 17 00:00:00 2001 From: Alexander Mock Date: Sat, 16 Mar 2024 01:30:54 +0100 Subject: [PATCH 2/4] - Removed added empty line - Renamed initialGuessCallback to initialPoseReceivedSrv - Added new line to SetInitialPose service definition - Removed mutex from initialPoseReceived - Cleanup service server Signed-off-by: Alexander Mock --- nav2_amcl/include/nav2_amcl/amcl_node.hpp | 5 ++--- nav2_amcl/src/amcl_node.cpp | 4 ++-- nav2_msgs/srv/SetInitialPose.srv | 2 +- 3 files changed, 5 insertions(+), 6 deletions(-) diff --git a/nav2_amcl/include/nav2_amcl/amcl_node.hpp b/nav2_amcl/include/nav2_amcl/amcl_node.hpp index 1542ab4d48..030a85f38c 100644 --- a/nav2_amcl/include/nav2_amcl/amcl_node.hpp +++ b/nav2_amcl/include/nav2_amcl/amcl_node.hpp @@ -214,9 +214,9 @@ class AmclNode : public nav2_util::LifecycleNode // service server for providing an initial pose guess rclcpp::Service::SharedPtr initial_guess_srv_; /* - * @brief Service callback for a global relocalization request + * @brief Service callback for an initial pose guess request */ - void initialGuessCallback( + void initialPoseReceivedSrv( const std::shared_ptr request_header, const std::shared_ptr request, std::shared_ptr response); @@ -234,7 +234,6 @@ class AmclNode : public nav2_util::LifecycleNode // Nomotion update control. Used to temporarily let amcl update samples even when no motion occurs std::atomic force_update_{false}; - // Odometry /* * @brief Initialize odometry diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index bf7f2352f5..ae27b8e869 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -325,6 +325,7 @@ AmclNode::on_cleanup(const rclcpp_lifecycle::State & /*state*/) // 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(); initial_pose_sub_.reset(); laser_scan_connection_.disconnect(); @@ -495,12 +496,11 @@ AmclNode::globalLocalizationCallback( } void -AmclNode::initialGuessCallback( +AmclNode::initialPoseReceivedSrv( const std::shared_ptr/*request_header*/, const std::shared_ptr req, std::shared_ptr/*res*/) { - std::lock_guard cfl(mutex_); initialPoseReceived(std::make_shared(req->pose)); } diff --git a/nav2_msgs/srv/SetInitialPose.srv b/nav2_msgs/srv/SetInitialPose.srv index 0fcd8e0a7a..7089d1a587 100644 --- a/nav2_msgs/srv/SetInitialPose.srv +++ b/nav2_msgs/srv/SetInitialPose.srv @@ -1,2 +1,2 @@ geometry_msgs/PoseWithCovarianceStamped pose ---- \ No newline at end of file +--- From 739453e0685941d0fa66e5663a8c382fe30da6fd Mon Sep 17 00:00:00 2001 From: Alexander Mock Date: Sat, 16 Mar 2024 01:56:49 +0100 Subject: [PATCH 3/4] added whitespace Signed-off-by: Alexander Mock --- nav2_msgs/srv/SetInitialPose.srv | 1 + 1 file changed, 1 insertion(+) diff --git a/nav2_msgs/srv/SetInitialPose.srv b/nav2_msgs/srv/SetInitialPose.srv index 7089d1a587..ec1e50b65f 100644 --- a/nav2_msgs/srv/SetInitialPose.srv +++ b/nav2_msgs/srv/SetInitialPose.srv @@ -1,2 +1,3 @@ geometry_msgs/PoseWithCovarianceStamped pose --- + From 7c435dcf2ad3c759532a1530e5c1b1cb01818803 Mon Sep 17 00:00:00 2001 From: Alexander Mock Date: Mon, 18 Mar 2024 10:52:57 +0100 Subject: [PATCH 4/4] renamed initial pose service in callback bind Signed-off-by: Alexander Mock --- 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 ae27b8e869..e3635ceb45 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -1555,7 +1555,7 @@ AmclNode::initServices() initial_guess_srv_ = create_service( "set_initial_pose", - std::bind(&AmclNode::initialGuessCallback, this, _1, _2, _3)); + std::bind(&AmclNode::initialPoseReceivedSrv, this, _1, _2, _3)); nomotion_update_srv_ = create_service( "request_nomotion_update",