Adding a getRobotPose service to world model.

nav2_msgs/srv/GetCostmap.srv

@@ -4,3 +4,5 @@
 nav2_msgs/CostmapMetaData specs
 ---
 nav2_msgs/Costmap map
+geometry_msgs/PoseStamped pose
+bool is_pose_valid

nav2_world_model/include/nav2_world_model/world_model.hpp

@@ -22,6 +22,7 @@
 #include "nav2_util/lifecycle_node.hpp"
 #include "nav2_msgs/msg/costmap.hpp"
 #include "nav2_msgs/srv/get_costmap.hpp"
+#include "nav2_msgs/srv/get_robot_pose.hpp"
 
 namespace nav2_world_model
 {
@@ -41,14 +42,18 @@ protected:
   nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override;
   nav2_util::CallbackReturn on_error(const rclcpp_lifecycle::State & state) override;
 
-  // The WorldModel provides the GetCostmap service
+  // The WorldModel provides these services
   rclcpp::Service<nav2_msgs::srv::GetCostmap>::SharedPtr costmap_service_;
+  rclcpp::Service<nav2_msgs::srv::GetRobotPose>::SharedPtr get_robot_pose_service_;
 
-  // The callback for the GetCostmap service
   void costmap_service_callback(
     const std::shared_ptr<rmw_request_id_t> request_header,
     const std::shared_ptr<nav2_msgs::srv::GetCostmap::Request> request,
     const std::shared_ptr<nav2_msgs::srv::GetCostmap::Response> response);
+  void get_robot_pose_callback(
+    const std::shared_ptr<rmw_request_id_t> request_header,
+    const std::shared_ptr<nav2_msgs::srv::GetRobotPose::Request> request,
+    const std::shared_ptr<nav2_msgs::srv::GetRobotPose::Response> response);
 
   // The implementation of the WorldModel uses a Costmap2DROS node
   std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_;

nav2_world_model/src/world_model.cpp

@@ -16,6 +16,8 @@
 
 #include <memory>
 
+using namespace std::placeholders;
+
 namespace nav2_world_model
 {
 
@@ -56,10 +58,11 @@ WorldModel::on_configure(const rclcpp_lifecycle::State & state)
 
   costmap_ros_->on_configure(state);
 
-  // Create a service that will use the callback function to handle requests
+  // Create a service that will use the callback function to handle requests.
   costmap_service_ = create_service<nav2_msgs::srv::GetCostmap>("GetCostmap",
-      std::bind(&WorldModel::costmap_service_callback, this,
-      std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
+      std::bind(&WorldModel::costmap_service_callback, this, _1, _2, _3));
+  get_robot_pose_service_ = create_service<nav2_msgs::srv::GetRobotPose>("GetRobotPose",
+      std::bind(&WorldModel::get_robot_pose_callback, this, _1, _2, _3));
 
   return nav2_util::CallbackReturn::SUCCESS;
 }
@@ -145,4 +148,12 @@ WorldModel::costmap_service_callback(
   response->map.data.assign(data, data + data_length);
 }
 
+void WorldModel::get_robot_pose_callback(
+  const std::shared_ptr<rmw_request_id_t>/*request_header*/,
+  const std::shared_ptr<nav2_msgs::srv::GetRobotPose::Request>/*request*/,
+  const std::shared_ptr<nav2_msgs::srv::GetRobotPose::Response> response)
+{
+  response->is_pose_valid = costmap_ros_->getRobotPose(response->pose);
+}
+
 }  // namespace nav2_world_model