Change navfn to use new get robot pose service

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::nanoseconds 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::nanoseconds /*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