diff --git a/nav2_navfn_planner/CMakeLists.txt b/nav2_navfn_planner/CMakeLists.txt
index b0a745bd4c3d25f830327acbd87e1f4c3092b96d..eeacc51cf7b788ae0a420c02a9573512d170120e 100644
--- a/nav2_navfn_planner/CMakeLists.txt
+++ b/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
diff --git a/nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp b/nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp
index 2cfa35e9d85684076d77af229b233eff3f8b6ebb..cf9f0e206f8c4d80926d088eadd7abc863716364 100644
--- a/nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp
+++ b/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 @@ protected:
// 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 @@ protected:
// 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_;
diff --git a/nav2_navfn_planner/package.xml b/nav2_navfn_planner/package.xml
index ef4d2819053a35edca4ab80a7270f0f1687367f9..452a6d641f490dd04f30009eec7d4f7142d4d3fb 100644
--- a/nav2_navfn_planner/package.xml
+++ b/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>
diff --git a/nav2_navfn_planner/src/navfn_planner.cpp b/nav2_navfn_planner/src/navfn_planner.cpp
index 8b837a1b9059d55f664f07d1146821e95af3f89a..529e2e2388388d3ceb4af71035827edf14b85621 100644
--- a/nav2_navfn_planner/src/navfn_planner.cpp
+++ b/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,6 +158,8 @@ 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;
@@ -174,21 +167,12 @@ NavfnPlanner::computePathToPose(const std::shared_ptr<GoalHandle> goal_handle)
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,8 +530,8 @@ 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
@@ -555,7 +539,7 @@ NavfnPlanner::getCostmap(
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