Added initial guess service. Signed-off-by: Alexander Mock

ros-navigation · Mar 15, 2024 · 3a36f2e · 3a36f2e
1 parent 2636371
commit 3a36f2e
Show file tree

Hide file tree

Showing 4 changed files with 29 additions and 0 deletions.
diff --git 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<std_srvs::srv::Empty::Request> request,
     std::shared_ptr<std_srvs::srv::Empty::Response> response);
 
+  // service server for providing an initial pose guess
+  rclcpp::Service<nav2_msgs::srv::SetInitialPose>::SharedPtr initial_guess_srv_;
+  /*
+   * @brief Service callback for a global relocalization request
+   */
+  void initialGuessCallback(
+    const std::shared_ptr<rmw_request_id_t> request_header,
+    const std::shared_ptr<nav2_msgs::srv::SetInitialPose::Request> request,
+    std::shared_ptr<nav2_msgs::srv::SetInitialPose::Response> response);
+
   // Let amcl update samples without requiring motion
   rclcpp::Service<std_srvs::srv::Empty>::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<bool> force_update_{false};
 
+
   // Odometry
   /*
    * @brief Initialize odometry

diff --git 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<rmw_request_id_t>/*request_header*/,
+  const std::shared_ptr<nav2_msgs::srv::SetInitialPose::Request> req,
+  std::shared_ptr<nav2_msgs::srv::SetInitialPose::Response>/*res*/)
+{
+  std::lock_guard<std::recursive_mutex> cfl(mutex_);
+  initialPoseReceived(std::make_shared<geometry_msgs::msg::PoseWithCovarianceStamped>(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<nav2_msgs::srv::SetInitialPose>(
+    "set_initial_pose",
+    std::bind(&AmclNode::initialGuessCallback, this, _1, _2, _3));
+
   nomotion_update_srv_ = create_service<std_srvs::srv::Empty>(
     "request_nomotion_update",
     std::bind(&AmclNode::nomotionUpdateCallback, this, _1, _2, _3));

diff --git 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
@@ -0,0 +1,2 @@
+geometry_msgs/PoseWithCovarianceStamped pose
+---