Transform robot pose lifecycle

nav2_msgs/CMakeLists.txt

@@ -17,6 +17,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "msg/Path.msg"
   "msg/VoxelGrid.msg"
   "srv/GetCostmap.srv"
+  "srv/GetRobotPose.srv"
   "srv/ClearCostmapExceptRegion.srv"
   "srv/ClearCostmapAroundRobot.srv"
   "srv/ClearEntireCostmap.srv"

nav2_msgs/srv/GetRobotPose.srv

@@ -0,0 +1,6 @@
+# Get the latest robot pose in the frame of the costmap
+
+std_msgs/Empty request
+---
+geometry_msgs/PoseStamped pose
+bool is_pose_valid

nav2_navfn_planner/CMakeLists.txt

@@ -10,7 +10,6 @@ find_package(std_msgs REQUIRED)
 find_package(visualization_msgs REQUIRED)
 find_package(nav2_util REQUIRED)
 find_package(nav2_msgs REQUIRED)
-find_package(nav2_robot REQUIRED)
 find_package(nav_msgs REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(builtin_interfaces REQUIRED)
@@ -32,7 +31,6 @@ set(dependencies
   visualization_msgs
   nav2_util
   nav2_msgs
-  nav2_robot
   nav_msgs
   geometry_msgs
   builtin_interfaces

nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp

@@ -28,8 +28,8 @@
 #include "nav2_msgs/msg/costmap.hpp"
 #include "nav2_msgs/msg/path.hpp"
 #include "nav2_navfn_planner/navfn.hpp"
-#include "nav2_robot/robot.hpp"
 #include "nav2_util/costmap_service_client.hpp"
+#include "nav2_util/get_robot_pose_client.hpp"
 #include "nav2_util/simple_action_server.hpp"
 #include "nav_msgs/msg/path.hpp"
 #include "visualization_msgs/msg/marker.hpp"
@@ -111,8 +111,11 @@ class NavfnPlanner : public nav2_util::LifecycleNode
 
   // Request costmap from world model
   void getCostmap(
-    nav2_msgs::msg::Costmap & costmap, const std::string layer = "master",
-    const std::chrono::milliseconds waitTime = std::chrono::milliseconds(100));
+    nav2_msgs::msg::Costmap & costmap,
+    const std::string layer = "master");
+
+  // get latest robot pose from the world model
+  geometry_msgs::msg::Pose getRobotPose();
 
   // Print costmap to terminal
   void printCostmap(const nav2_msgs::msg::Costmap & costmap);
@@ -126,13 +129,12 @@ class NavfnPlanner : public nav2_util::LifecycleNode
   // Determine if a new planner object should be made
   bool isPlannerOutOfDate();
 
-  std::unique_ptr<nav2_robot::Robot> robot_;
-
   // Planner based on ROS1 NavFn algorithm
   std::unique_ptr<NavFn> planner_;
 
   // Service client for getting the costmap
   nav2_util::CostmapServiceClient costmap_client_{"navfn_planner"};
+  nav2_util::GetRobotPoseClient get_robot_pose_client_{"navfn_planner"};
 
   // Publishers for the path and endpoints
   rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::Path>::SharedPtr plan_publisher_;

nav2_navfn_planner/package.xml

@@ -16,7 +16,6 @@
   <build_depend>visualization_msgs</build_depend>
   <build_depend>nav2_util</build_depend>
   <build_depend>nav2_msgs</build_depend>
-  <build_depend>nav2_robot</build_depend>
   <build_depend>nav_msgs</build_depend>
   <build_depend>geometry_msgs</build_depend>
   <build_depend>builtin_interfaces</build_depend>
@@ -27,7 +26,6 @@
   <exec_depend>rclcpp_lifecycle</exec_depend>
   <exec_depend>visualization_msgs</exec_depend>
   <exec_depend>nav2_msgs</exec_depend>
-  <exec_depend>nav2_robot</exec_depend>
   <exec_depend>nav2_util</exec_depend>
   <exec_depend>nav_msgs</exec_depend>
   <exec_depend>geometry_msgs</exec_depend>

nav2_navfn_planner/src/navfn_planner.cpp

@@ -60,7 +60,7 @@ NavfnPlanner::~NavfnPlanner()
 }
 
 nav2_util::CallbackReturn
-NavfnPlanner::on_configure(const rclcpp_lifecycle::State & state)
+NavfnPlanner::on_configure(const rclcpp_lifecycle::State & /*state*/)
 {
   RCLCPP_INFO(get_logger(), "Configuring");
 
@@ -86,10 +86,6 @@ NavfnPlanner::on_configure(const rclcpp_lifecycle::State & state)
 
   auto node = shared_from_this();
 
-  // Initialize supporting objects
-  robot_ = std::make_unique<nav2_robot::Robot>(node);
-  robot_->on_configure(state);
-
   // Create the action server that we implement with our navigateToPose method
   action_server_ = std::make_unique<ActionServer>(rclcpp_node_, "ComputePathToPose",
       std::bind(&NavfnPlanner::computePathToPose, this, std::placeholders::_1));
@@ -98,39 +94,34 @@ NavfnPlanner::on_configure(const rclcpp_lifecycle::State & state)
 }
 
 nav2_util::CallbackReturn
-NavfnPlanner::on_activate(const rclcpp_lifecycle::State & state)
+NavfnPlanner::on_activate(const rclcpp_lifecycle::State & /*state*/)
 {
   RCLCPP_INFO(get_logger(), "Activating");
 
   plan_publisher_->on_activate();
   plan_marker_publisher_->on_activate();
-  robot_->on_activate(state);
 
   return nav2_util::CallbackReturn::SUCCESS;
 }
 
 nav2_util::CallbackReturn
-NavfnPlanner::on_deactivate(const rclcpp_lifecycle::State & state)
+NavfnPlanner::on_deactivate(const rclcpp_lifecycle::State & /*state*/)
 {
   RCLCPP_INFO(get_logger(), "Deactivating");
 
   plan_publisher_->on_deactivate();
   plan_marker_publisher_->on_deactivate();
-  robot_->on_deactivate(state);
 
   return nav2_util::CallbackReturn::SUCCESS;
 }
 
 nav2_util::CallbackReturn
-NavfnPlanner::on_cleanup(const rclcpp_lifecycle::State & state)
+NavfnPlanner::on_cleanup(const rclcpp_lifecycle::State & /*state*/)
 {
   RCLCPP_INFO(get_logger(), "Cleaning up");
 
-  robot_->on_cleanup(state);
-
   plan_publisher_.reset();
   plan_marker_publisher_.reset();
-  robot_.reset();
 
   action_server_.reset();
   planner_.reset();
@@ -167,28 +158,21 @@ NavfnPlanner::computePathToPose(const std::shared_ptr<GoalHandle> goal_handle)
     RCLCPP_DEBUG(get_logger(), "Costmap size: %d,%d",
       costmap_.metadata.size_x, costmap_.metadata.size_y);
 
+    auto start = getRobotPose();
+
     // Update planner based on the new costmap size
     if (isPlannerOutOfDate()) {
       current_costmap_size_[0] = costmap_.metadata.size_x;
       current_costmap_size_[1] = costmap_.metadata.size_y;
       planner_->setNavArr(costmap_.metadata.size_x, costmap_.metadata.size_y);
     }
 
-    // Get the current pose from the robot
-    auto start = std::make_shared<geometry_msgs::msg::PoseWithCovarianceStamped>();
-
-    if (!robot_->getCurrentPose(start)) {
-      RCLCPP_ERROR(get_logger(), "Current robot pose is not available.");
-      goal_handle->abort(result);
-      return;
-    }
-
     RCLCPP_DEBUG(get_logger(), "Attempting to a find path from (%.2f, %.2f) to "
-      "(%.2f, %.2f).", start->pose.pose.position.x, start->pose.pose.position.y,
+      "(%.2f, %.2f).", start.position.x, start.position.y,
       goal->pose.pose.position.x, goal->pose.pose.position.y);
 
     // Make the plan for the provided goal pose
-    bool foundPath = makePlan(start->pose.pose, goal->pose.pose, tolerance_, result->path);
+    bool foundPath = makePlan(start, goal->pose.pose, tolerance_, result->path);
 
     // TODO(orduno): should check for cancel within the makePlan() method?
     if (goal_handle->is_canceling()) {
@@ -209,7 +193,7 @@ NavfnPlanner::computePathToPose(const std::shared_ptr<GoalHandle> goal_handle)
     // Publish the plan for visualization purposes
     RCLCPP_DEBUG(get_logger(), "Publishing the valid path");
     publishPlan(result->path);
-    publishEndpoints(start->pose.pose, goal->pose.pose);
+    publishEndpoints(start, goal->pose.pose);
 
     // TODO(orduno): Enable potential visualization
 
@@ -546,16 +530,16 @@ NavfnPlanner::clearRobotCell(unsigned int mx, unsigned int my)
 
 void
 NavfnPlanner::getCostmap(
-  nav2_msgs::msg::Costmap & costmap, const std::string /*layer*/,
-  const std::chrono::milliseconds /*waitTime*/)
+  nav2_msgs::msg::Costmap & costmap,
+  const std::string /*layer*/)
 {
   // TODO(orduno): explicitly provide specifications for costmap using the costmap on the request,
   //               including master (aggregate) layer
 
   auto request = std::make_shared<nav2_util::CostmapServiceClient::CostmapServiceRequest>();
   request->specs.resolution = 1.0;
 
-  auto result = costmap_client_.invoke(request);
+  auto result = costmap_client_.invoke(request, 5s);
   costmap = result.get()->map;
 }
 
@@ -673,4 +657,16 @@ NavfnPlanner::publishPlan(const nav2_msgs::msg::Path & path)
   plan_publisher_->publish(rviz_path);
 }
 
+geometry_msgs::msg::Pose
+NavfnPlanner::getRobotPose()
+{
+  auto request = std::make_shared<nav2_util::GetRobotPoseClient::GetRobotPoseRequest>();
+
+  auto result = get_robot_pose_client_.invoke(request, 5s);
+  if (!result.get()->is_pose_valid) {
+    throw std::runtime_error("Current robot pose is not available.");
+  }
+  return result.get()->pose.pose;
+}
+
 }  // namespace nav2_navfn_planner

nav2_util/include/nav2_util/costmap_service_client.hpp

@@ -23,23 +23,23 @@
 namespace nav2_util
 {
 
-class CostmapServiceClient : public nav2_util::ServiceClient<nav2_msgs::srv::GetCostmap>
+class CostmapServiceClient : public ServiceClient<nav2_msgs::srv::GetCostmap>
 {
 public:
   explicit CostmapServiceClient(const std::string & parent_node_name)
-  : nav2_util::ServiceClient<nav2_msgs::srv::GetCostmap>("GetCostmap", parent_node_name)
+  : ServiceClient<nav2_msgs::srv::GetCostmap>("GetCostmap", parent_node_name)
   {
   }
 
-  explicit CostmapServiceClient(rclcpp::Node::SharedPtr node)
+  explicit CostmapServiceClient(rclcpp::Node::SharedPtr & node)
   : ServiceClient<nav2_msgs::srv::GetCostmap>("GetCostmap", node)
   {
   }
 
   using CostmapServiceRequest =
-    nav2_util::ServiceClient<nav2_msgs::srv::GetCostmap>::RequestType;
+    ServiceClient<nav2_msgs::srv::GetCostmap>::RequestType;
   using CostmapServiceResponse =
-    nav2_util::ServiceClient<nav2_msgs::srv::GetCostmap>::ResponseType;
+    ServiceClient<nav2_msgs::srv::GetCostmap>::ResponseType;
 };
 
 }  // namespace nav2_util

nav2_util/include/nav2_util/get_robot_pose_client.hpp

@@ -0,0 +1,45 @@
+// Copyright (c) 2019 Intel Corporation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef NAV2_UTIL__GET_ROBOT_POSE_CLIENT_HPP_
+#define NAV2_UTIL__GET_ROBOT_POSE_CLIENT_HPP_
+
+#include "nav2_util/service_client.hpp"
+#include "nav2_msgs/srv/get_robot_pose.hpp"
+
+namespace nav2_util
+{
+
+class GetRobotPoseClient : public ServiceClient<nav2_msgs::srv::GetRobotPose>
+{
+public:
+  explicit GetRobotPoseClient(const std::string & parent_node_name)
+  : ServiceClient<nav2_msgs::srv::GetRobotPose>("GetRobotPose", parent_node_name)
+  {
+  }
+
+  explicit GetRobotPoseClient(rclcpp::Node::SharedPtr & node)
+  : ServiceClient<nav2_msgs::srv::GetRobotPose>("GetRobotPose", node)
+  {
+  }
+
+  using GetRobotPoseRequest =
+    ServiceClient<nav2_msgs::srv::GetRobotPose>::RequestType;
+  using GetRobotPoseResponse =
+    ServiceClient<nav2_msgs::srv::GetRobotPose>::ResponseType;
+};
+
+}  // namespace nav2_tasks
+
+#endif  // NAV2_UTIL__GET_ROBOT_POSE_CLIENT_HPP_

nav2_util/include/nav2_util/service_client.hpp

@@ -55,7 +55,7 @@ class ServiceClient
 
   typename ResponseType::SharedPtr invoke(
     typename RequestType::SharedPtr & request,
-    const std::chrono::seconds timeout = std::chrono::seconds::max())
+    const std::chrono::nanoseconds timeout = std::chrono::nanoseconds::max())
   {
     while (!client_->wait_for_service(std::chrono::seconds(1))) {
       if (!rclcpp::ok()) {
@@ -108,7 +108,7 @@ class ServiceClient
     return response.get();
   }
 
-  void wait_for_service(const std::chrono::seconds timeout = std::chrono::seconds::max())
+  void wait_for_service(const std::chrono::nanoseconds timeout = std::chrono::nanoseconds::max())
   {
     while (!client_->wait_for_service(timeout)) {
       if (!rclcpp::ok()) {

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 @@ class WorldModel : public nav2_util::LifecycleNode
   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