diff --git a/nav2_amcl/include/nav2_amcl/amcl_node.hpp b/nav2_amcl/include/nav2_amcl/amcl_node.hpp index 2360d09c25..030a85f38c 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 an initial pose guess request + */ + void initialPoseReceivedSrv( + 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_; /* diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index 6f2415cf50..b311b7ac5e 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(); executor_thread_.reset(); // to make sure initial_pose_sub_ completely exit initial_pose_sub_.reset(); @@ -495,6 +496,15 @@ AmclNode::globalLocalizationCallback( pf_init_ = false; } +void +AmclNode::initialPoseReceivedSrv( + const std::shared_ptr/*request_header*/, + const std::shared_ptr req, + std::shared_ptr/*res*/) +{ + initialPoseReceived(std::make_shared(req->pose)); +} + // force nomotion updates (amcl updating without requiring motion) void AmclNode::nomotionUpdateCallback( @@ -1544,6 +1554,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::initialPoseReceivedSrv, 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..ec1e50b65f --- /dev/null +++ b/nav2_msgs/srv/SetInitialPose.srv @@ -0,0 +1,3 @@ +geometry_msgs/PoseWithCovarianceStamped pose +--- +