Navfn gets robot pose from costmap service instead of robot class.

nav2_navfn_planner/CMakeLists.txt

@@ -9,7 +9,6 @@ find_package(visualization_msgs REQUIRED)
 find_package(nav2_tasks 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)
@@ -30,7 +29,6 @@ set(dependencies
   nav2_util
   nav2_tasks
   nav2_msgs
-  nav2_robot
   nav_msgs
   geometry_msgs
   builtin_interfaces

nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp

@@ -29,7 +29,6 @@
 #include "geometry_msgs/msg/point.hpp"
 #include "nav_msgs/msg/path.hpp"
 #include "visualization_msgs/msg/marker.hpp"
-#include "nav2_robot/robot.hpp"
 
 namespace nav2_navfn_planner
 {
@@ -96,7 +95,9 @@ class NavfnPlanner : public rclcpp::Node
 
   // Request costmap from world model
   void getCostmap(
-    nav2_msgs::msg::Costmap & costmap, const std::string layer = "master",
+    nav2_msgs::msg::Costmap & costmap,
+    geometry_msgs::msg::PoseStamped & starting_pose,
+    const std::string layer = "master",
     const std::chrono::nanoseconds waitTime = std::chrono::milliseconds(100));
 
   // Print costmap to terminal
@@ -136,8 +137,6 @@ class NavfnPlanner : public rclcpp::Node
 
   // Whether to use the astar planner or default dijkstras
   bool use_astar_;
-
-  std::unique_ptr<nav2_robot::Robot> robot_;
 };
 
 }  // namespace nav2_navfn_planner

nav2_navfn_planner/package.xml

@@ -16,7 +16,6 @@
   <build_depend>nav2_tasks</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>
@@ -25,7 +24,6 @@
   <exec_depend>visualization_msgs</exec_depend>
   <exec_depend>nav2_tasks</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

@@ -71,8 +71,6 @@ NavfnPlanner::NavfnPlanner()
   plan_marker_publisher_ = this->create_publisher<visualization_msgs::msg::Marker>(
     "endpoints", 1);
 
-  robot_ = std::make_unique<nav2_robot::Robot>(temp_node);
-
   task_server_ = std::make_unique<nav2_tasks::ComputePathToPoseTaskServer>(temp_node, false),
   task_server_->setExecuteCallback(
     std::bind(&NavfnPlanner::computePathToPose, this, std::placeholders::_1));
@@ -91,8 +89,9 @@ NavfnPlanner::computePathToPose(const nav2_tasks::ComputePathToPoseCommand::Shar
 {
   nav2_tasks::ComputePathToPoseResult result;
   try {
-    // Get an updated costmap
-    getCostmap(costmap_);
+    // Get an updated costmap and pose
+    geometry_msgs::msg::PoseStamped start;
+    getCostmap(costmap_, start, "master", 5s);
     RCLCPP_DEBUG(get_logger(), "Costmap size: %d,%d",
       costmap_.metadata.size_x, costmap_.metadata.size_y);
 
@@ -103,20 +102,12 @@ NavfnPlanner::computePathToPose(const nav2_tasks::ComputePathToPoseCommand::Shar
       planner_ = std::make_unique<NavFn>(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.");
-      return TaskStatus::FAILED;
-    }
-
     RCLCPP_INFO(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.pose.position.x, start.pose.position.y,
       command->pose.position.x, command->pose.position.y);
 
     // Make the plan for the provided goal pose
-    bool foundPath = makePlan(start->pose.pose, command->pose, tolerance_, result);
+    bool foundPath = makePlan(start.pose, command->pose, tolerance_, result);
 
     // TODO(orduno): should check for cancel within the makePlan() method?
     if (task_server_->cancelRequested()) {
@@ -137,7 +128,7 @@ NavfnPlanner::computePathToPose(const nav2_tasks::ComputePathToPoseCommand::Shar
     // Publish the plan for visualization purposes
     RCLCPP_INFO(get_logger(), "Publishing the valid path.");
     publishPlan(result);
-    publishEndpoints(start->pose.pose, command->pose);
+    publishEndpoints(start.pose, command->pose);
 
     // TODO(orduno): Enable potential visualization
 
@@ -472,17 +463,23 @@ 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,
+  geometry_msgs::msg::PoseStamped & pose,
+  const std::string /*layer*/,
+  const std::chrono::nanoseconds waitTime)
 {
   // TODO(orduno): explicitly provide specifications for costmap using the costmap on the request,
   //               including master (aggregate) layer
 
   auto request = std::make_shared<nav2_tasks::CostmapServiceClient::CostmapServiceRequest>();
   request->specs.resolution = 1.0;
 
-  auto result = costmap_client_.invoke(request);
+  auto result = costmap_client_.invoke(request, waitTime);
   costmap = result.get()->map;
+  pose = result.get()->pose;
+  if (!result.get()->is_pose_valid) {
+    throw std::runtime_error("Current robot pose is not available.");
+  }
 }
 
 void