From 67be9d98f96902a485294167526c89556b8a740e Mon Sep 17 00:00:00 2001
From: Michael Jeronimo <michael.jeronimo@intel.com>
Date: Thu, 6 Dec 2018 18:26:36 -0800
Subject: [PATCH] Enable DWB to receive path updates (#409)
* Integrate example code into the DWB controller main loop
* Changes to get current robot pose from global planner, required for dynamic replanning
* Address review feedback
* Fix cpplint problem
---
nav2_bt_navigator/CMakeLists.txt | 4 +-
nav2_bt_navigator/behavior_trees/parallel.xml | 10 ++--
.../behavior_trees/simple_sequential.xml | 2 +-
.../nav2_bt_navigator/bt_navigator.hpp | 2 -
nav2_bt_navigator/package.xml | 4 +-
nav2_bt_navigator/src/bt_navigator.cpp | 22 ++------
.../src/navigate_to_pose_behavior_tree.cpp | 1 +
.../dwb_controller/src/dwb_controller.cpp | 35 ++++++-------
nav2_msgs/CMakeLists.txt | 1 -
nav2_msgs/msg/PathEndPoints.msg | 11 ----
nav2_navfn_planner/CMakeLists.txt | 2 +
.../nav2_navfn_planner/navfn_planner.hpp | 9 +++-
nav2_navfn_planner/package.xml | 2 +
nav2_navfn_planner/src/navfn_planner.cpp | 51 ++++++++++++-------
nav2_simple_navigator/CMakeLists.txt | 2 -
.../simple_navigator.hpp | 2 -
nav2_simple_navigator/package.xml | 2 -
.../src/simple_navigator.cpp | 39 +++++---------
.../src/planning/planner_tester.cpp | 48 +++++++----------
.../src/planning/planner_tester.hpp | 4 +-
.../include/nav2_tasks/bt_conversions.hpp | 5 +-
.../compute_path_to_pose_action.hpp | 2 +-
.../nav2_tasks/compute_path_to_pose_task.hpp | 4 +-
.../nav2_tasks/rate_controller_node.hpp | 4 +-
tools/ros2_dependencies.repos | 2 +-
25 files changed, 115 insertions(+), 155 deletions(-)
delete mode 100644 nav2_msgs/msg/PathEndPoints.msg
diff --git a/nav2_bt_navigator/CMakeLists.txt b/nav2_bt_navigator/CMakeLists.txt
index 9adc7857..eaf949a5 100644
--- a/nav2_bt_navigator/CMakeLists.txt
+++ b/nav2_bt_navigator/CMakeLists.txt
@@ -14,8 +14,8 @@ find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED)
find_package(nav2_tasks REQUIRED)
+find_package(nav_msgs REQUIRED)
find_package(nav2_msgs REQUIRED)
-find_package(nav2_robot REQUIRED)
find_package(behaviortree_cpp REQUIRED)
include_directories(
@@ -39,8 +39,8 @@ set(dependencies
rclcpp
std_msgs
nav2_tasks
+ nav_msgs
nav2_msgs
- nav2_robot
behaviortree_cpp
)
diff --git a/nav2_bt_navigator/behavior_trees/parallel.xml b/nav2_bt_navigator/behavior_trees/parallel.xml
index ec6677df..1fdab427 100644
--- a/nav2_bt_navigator/behavior_trees/parallel.xml
+++ b/nav2_bt_navigator/behavior_trees/parallel.xml
@@ -8,8 +8,8 @@
threshold of 1, whenever the FollowPath returns SUCCESS or FAILURE, the parallel
node will return this result.
- The path endpoints (input to the global planner) and the resulting path (output of
- the global planner and input to the local planner) are passed on the blackboard.
+ The goal (input to the global planner) and the resulting path (output of the global
+ planner and input to the local planner) are passed on the blackboard.
The rate at which the ComputePathToPose operation is invoked can be controlled with
the hz parameter to the RateController node.
@@ -18,12 +18,12 @@
<root main_tree_to_execute="MainTree">
<BehaviorTree ID="MainTree">
<SequenceStar name="root">
- <ComputePathToPose endpoints="${endpoints}" path="${path}"/>
+ <ComputePathToPose goal="${goal}" path="${path}"/>
<ParallelNode threshold="1">
<FollowPath path="${path}"/>
<Sequence>
- <RateController hz="2">
- <ComputePathToPose endpoints="${endpoints}" path="${path}"/>
+ <RateController hz="1.0">
+ <ComputePathToPose goal="${goal}" path="${path}"/>
</RateController>
<UpdatePath/>
</Sequence>
diff --git a/nav2_bt_navigator/behavior_trees/simple_sequential.xml b/nav2_bt_navigator/behavior_trees/simple_sequential.xml
index 21e740fe..4f6a441d 100644
--- a/nav2_bt_navigator/behavior_trees/simple_sequential.xml
+++ b/nav2_bt_navigator/behavior_trees/simple_sequential.xml
@@ -9,7 +9,7 @@
<root main_tree_to_execute="MainTree">
<BehaviorTree ID="MainTree">
<SequenceStar name="root">
- <ComputePathToPose endpoints="${endpoints}" path="${path}"/>
+ <ComputePathToPose goal="${goal}" path="${path}"/>
<FollowPath path="${path}"/>
</SequenceStar>
</BehaviorTree>
diff --git a/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp b/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp
index 785c4c66..6a327649 100644
--- a/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp
+++ b/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp
@@ -18,7 +18,6 @@
#include <string>
#include <memory>
#include "nav2_tasks/navigate_to_pose_task.hpp"
-#include "nav2_robot/robot.hpp"
namespace nav2_bt_navigator
{
@@ -32,7 +31,6 @@ public:
const nav2_tasks::NavigateToPoseCommand::SharedPtr command);
private:
- std::unique_ptr<nav2_robot::Robot> robot_;
std::unique_ptr<nav2_tasks::NavigateToPoseTaskServer> task_server_;
std::string bt_xml_filename_;
};
diff --git a/nav2_bt_navigator/package.xml b/nav2_bt_navigator/package.xml
index 4ae7cf21..bf68a8ff 100644
--- a/nav2_bt_navigator/package.xml
+++ b/nav2_bt_navigator/package.xml
@@ -11,13 +11,13 @@
<build_depend>rclcpp</build_depend>
<build_depend>nav2_tasks</build_depend>
+ <build_depend>nav_msgs</build_depend>
<build_depend>nav2_msgs</build_depend>
- <build_depend>nav2_robot</build_depend>
<exec_depend>rclcpp</exec_depend>
<exec_depend>nav2_tasks</exec_depend>
+ <exec_depend>nav_msgs</exec_depend>
<exec_depend>nav2_msgs</exec_depend>
- <exec_depend>nav2_robot</exec_depend>
<test_depend>ament_lint_common</test_depend>
<test_depend>ament_lint_auto</test_depend>
diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp
index a2d28811..139c5e78 100644
--- a/nav2_bt_navigator/src/bt_navigator.cpp
+++ b/nav2_bt_navigator/src/bt_navigator.cpp
@@ -32,8 +32,6 @@ BtNavigator::BtNavigator()
{
auto temp_node = std::shared_ptr<rclcpp::Node>(this, [](rclcpp::Node *) {});
- robot_ = std::make_unique<nav2_robot::Robot>(temp_node);
-
task_server_ = std::make_unique<nav2_tasks::NavigateToPoseTaskServer>(temp_node);
task_server_->setExecuteCallback(
std::bind(&BtNavigator::navigateToPose, this, std::placeholders::_1));
@@ -42,31 +40,17 @@ BtNavigator::BtNavigator()
TaskStatus
BtNavigator::navigateToPose(const nav2_tasks::NavigateToPoseCommand::SharedPtr command)
{
- RCLCPP_INFO(get_logger(), "Start navigating to goal (%.2f, %.2f).",
+ RCLCPP_INFO(get_logger(), "Begin navigating from current location to (%.2f, %.2f)",
command->pose.position.x, command->pose.position.y);
- // Get the current pose from the robot
- auto current = std::make_shared<geometry_msgs::msg::PoseWithCovarianceStamped>();
-
- if (!robot_->getCurrentPose(current)) {
- RCLCPP_ERROR(get_logger(), "Current robot pose is not available.");
- return TaskStatus::FAILED;
- }
-
// Create the blackboard that will be shared by all of the nodes in the tree
BT::Blackboard::Ptr blackboard = BT::Blackboard::create<BT::BlackboardLocal>();
- // Put together the PathEndPoints message for the ComputePathToPose task
- auto endpoints = std::make_shared<nav2_tasks::ComputePathToPoseCommand>();
- endpoints->start = current->pose.pose;
- endpoints->goal = command->pose;
- endpoints->tolerance = 2.0; // TODO(mjeronimo): this will come in the command message
-
- // The path returned from ComputePath and sent to the FollowPath task
+ // Create the path to be returned from ComputePath and sent to the FollowPath task
auto path = std::make_shared<nav2_tasks::ComputePathToPoseResult>();
// Set the shared data (commands/results)
- blackboard->set<nav2_tasks::ComputePathToPoseCommand::SharedPtr>("endpoints", endpoints);
+ blackboard->set<nav2_tasks::ComputePathToPoseCommand::SharedPtr>("goal", command);
blackboard->set<nav2_tasks::ComputePathToPoseResult::SharedPtr>("path", path); // NOLINT
// Get the filename to use from the parameter
diff --git a/nav2_bt_navigator/src/navigate_to_pose_behavior_tree.cpp b/nav2_bt_navigator/src/navigate_to_pose_behavior_tree.cpp
index 1c3bfd1b..4c5ba20d 100644
--- a/nav2_bt_navigator/src/navigate_to_pose_behavior_tree.cpp
+++ b/nav2_bt_navigator/src/navigate_to_pose_behavior_tree.cpp
@@ -57,6 +57,7 @@ BT::NodeStatus NavigateToPoseBehaviorTree::updatePath(BT::TreeNode & tree_node)
// Get the updated path from the blackboard and send to the FollowPath task server
auto path = tree_node.blackboard()->template get<nav2_tasks::ComputePathToPoseResult::SharedPtr>(
"path");
+
follow_path_task_client_->sendUpdate(path);
return BT::NodeStatus::RUNNING;
}
diff --git a/nav2_dwb_controller/dwb_controller/src/dwb_controller.cpp b/nav2_dwb_controller/dwb_controller/src/dwb_controller.cpp
index 04df5bbf..aeb2a01a 100644
--- a/nav2_dwb_controller/dwb_controller/src/dwb_controller.cpp
+++ b/nav2_dwb_controller/dwb_controller/src/dwb_controller.cpp
@@ -55,32 +55,15 @@ DwbController::~DwbController()
TaskStatus
DwbController::followPath(const nav2_tasks::FollowPathCommand::SharedPtr command)
{
-#if 0
- // TODO(mjeronimo): Integrate the following example code into the
- // main loop below
-
- while (true) {
- ...
-
- if (task_server_->updateRequested()) {
- auto new_path = std::make_shared<nav2_tasks::FollowPathCommand>();
- task_server_->getCommandUpdate(new_path);
- task_server_->setUpdated();
-
- // Update the target path
- }
-
- ...
- }
-#endif
-
RCLCPP_INFO(get_logger(), "Starting controller");
try {
auto path = nav_2d_utils::pathToPath2D(*command);
auto nh = shared_from_this();
+
planner_.initialize(nh, shared_ptr<tf2_ros::Buffer>(&tfBuffer_, NO_OP_DELETER), cm_);
planner_.setPlan(path);
RCLCPP_INFO(get_logger(), "Initialized");
+
while (true) {
nav_2d_msgs::msg::Pose2DStamped pose2d;
if (!getRobotPose(pose2d)) {
@@ -94,12 +77,26 @@ DwbController::followPath(const nav2_tasks::FollowPathCommand::SharedPtr command
auto cmd_vel_2d = planner_.computeVelocityCommands(pose2d, velocity);
publishVelocity(cmd_vel_2d);
RCLCPP_INFO(get_logger(), "Publishing velocity");
+
+ // Check if this task has been canceled
if (task_server_->cancelRequested()) {
RCLCPP_INFO(this->get_logger(), "execute: task has been canceled");
task_server_->setCanceled();
publishZeroVelocity();
return TaskStatus::CANCELED;
}
+
+ // Check if there is an update to the path to follow
+ if (task_server_->updateRequested()) {
+ // Get the new, updated path
+ auto path_cmd = std::make_shared<nav2_tasks::FollowPathCommand>();
+ task_server_->getCommandUpdate(path_cmd);
+ task_server_->setUpdated();
+
+ // and pass it to the local planner
+ auto path = nav_2d_utils::pathToPath2D(*path_cmd);
+ planner_.setPlan(path);
+ }
}
std::this_thread::sleep_for(100ms);
}
diff --git a/nav2_msgs/CMakeLists.txt b/nav2_msgs/CMakeLists.txt
index b9ec5fe6..12bbe85e 100644
--- a/nav2_msgs/CMakeLists.txt
+++ b/nav2_msgs/CMakeLists.txt
@@ -20,7 +20,6 @@ rosidl_generate_interfaces(${PROJECT_NAME}
"msg/Costmap.msg"
"msg/CostmapMetaData.msg"
"msg/Path.msg"
- "msg/PathEndPoints.msg"
"msg/MissionPlan.msg"
"msg/TaskStatus.msg"
"msg/VoxelGrid.msg"
diff --git a/nav2_msgs/msg/PathEndPoints.msg b/nav2_msgs/msg/PathEndPoints.msg
deleted file mode 100644
index 7561cec3..00000000
--- a/nav2_msgs/msg/PathEndPoints.msg
+++ /dev/null
@@ -1,11 +0,0 @@
-std_msgs/Header header
-
-# The start pose for the plan
-geometry_msgs/Pose start
-
-# The final pose of the goal
-geometry_msgs/Pose goal
-
-# If the goal is obstructed, how many meters the planner can
-# relax the constraint in x and y before failing.
-float32 tolerance
\ No newline at end of file
diff --git a/nav2_navfn_planner/CMakeLists.txt b/nav2_navfn_planner/CMakeLists.txt
index 9d850f9b..bd8d8630 100644
--- a/nav2_navfn_planner/CMakeLists.txt
+++ b/nav2_navfn_planner/CMakeLists.txt
@@ -17,6 +17,7 @@ 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)
include_directories(
@@ -33,6 +34,7 @@ set(dependencies
nav2_util
nav2_tasks
nav2_msgs
+ nav2_robot
nav_msgs
)
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 11a7d205..d00355d9 100644
--- a/nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp
+++ b/nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp
@@ -29,6 +29,7 @@
#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
{
@@ -103,7 +104,9 @@ private:
// Publish a path for visualization purposes
void publishPlan(const nav2_msgs::msg::Path & path);
- void publishEndpoints(const nav2_tasks::ComputePathToPoseCommand::SharedPtr & endpoints);
+ void publishEndpoints(
+ const geometry_msgs::msg::Pose & start,
+ const geometry_msgs::msg::Pose & goal);
// Determine if a new planner object should be made
bool isPlannerOutOfDate();
@@ -129,10 +132,12 @@ private:
bool allow_unknown_;
// Amount the planner can relax the space constraint
- double default_tolerance_;
+ double tolerance_;
// Whether to use the astar planner or default dijkstras
bool use_astar_;
+
+ std::unique_ptr<nav2_robot::Robot> robot_;
};
} // namespace nav2_navfn_planner
diff --git a/nav2_navfn_planner/package.xml b/nav2_navfn_planner/package.xml
index 7e07ac5e..4060d224 100644
--- a/nav2_navfn_planner/package.xml
+++ b/nav2_navfn_planner/package.xml
@@ -15,12 +15,14 @@
<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>
<exec_depend>rclcpp</exec_depend>
<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>nav_msgs</exec_depend>
<test_depend>ament_lint_common</test_depend>
diff --git a/nav2_navfn_planner/src/navfn_planner.cpp b/nav2_navfn_planner/src/navfn_planner.cpp
index 5d35a144..b6478e8f 100644
--- a/nav2_navfn_planner/src/navfn_planner.cpp
+++ b/nav2_navfn_planner/src/navfn_planner.cpp
@@ -49,9 +49,7 @@ namespace nav2_navfn_planner
NavfnPlanner::NavfnPlanner()
: Node("NavfnPlanner"),
global_frame_("map"),
- allow_unknown_(true),
- default_tolerance_(1.0),
- use_astar_(false)
+ allow_unknown_(true)
{
RCLCPP_INFO(get_logger(), "Initializing.");
@@ -59,6 +57,11 @@ NavfnPlanner::NavfnPlanner()
auto temp_node = std::shared_ptr<rclcpp::Node>(this, [](auto) {});
auto parameters_client = std::make_shared<rclcpp::SyncParametersClient>(temp_node);
+
+ // If the goal is obstructed, the tolerance specifies how many meters the planner
+ // can relax the constraint in x and y before failing
+ tolerance_ = parameters_client->get_parameter("tolerance", 0.0);
+
use_astar_ = parameters_client->get_parameter("use_astar", false);
// TODO(orduno): Enable parameter server and get costmap service name from there
@@ -68,6 +71,8 @@ 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));
@@ -84,10 +89,6 @@ NavfnPlanner::~NavfnPlanner()
TaskStatus
NavfnPlanner::computePathToPose(const nav2_tasks::ComputePathToPoseCommand::SharedPtr command)
{
- RCLCPP_INFO(get_logger(), "Attempting to a find path from (%.2f, %.2f) to "
- "(%.2f, %.2f).", command->start.position.x, command->start.position.y,
- command->goal.position.x, command->goal.position.y);
-
nav2_tasks::ComputePathToPoseResult result;
try {
// Get an updated costmap
@@ -102,8 +103,20 @@ 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,
+ command->pose.position.x, command->pose.position.y);
+
// Make the plan for the provided goal pose
- bool foundPath = makePlan(command->start, command->goal, command->tolerance, result);
+ bool foundPath = makePlan(start->pose.pose, command->pose, tolerance_, result);
// TODO(orduno): should check for cancel within the makePlan() method?
if (task_server_->cancelRequested()) {
@@ -114,7 +127,7 @@ NavfnPlanner::computePathToPose(const nav2_tasks::ComputePathToPoseCommand::Shar
if (!foundPath) {
RCLCPP_WARN(get_logger(), "Planning algorithm failed to generate a valid"
- " path to (%.2f, %.2f)", command->goal.position.x, command->goal.position.y);
+ " path to (%.2f, %.2f)", command->pose.position.x, command->pose.position.y);
return TaskStatus::FAILED;
}
@@ -124,18 +137,18 @@ NavfnPlanner::computePathToPose(const nav2_tasks::ComputePathToPoseCommand::Shar
// Publish the plan for visualization purposes
RCLCPP_INFO(get_logger(), "Publishing the valid path.");
publishPlan(result);
- publishEndpoints(command);
+ publishEndpoints(start->pose.pose, command->pose);
// TODO(orduno): Enable potential visualization
RCLCPP_INFO(get_logger(),
"Successfully navigated to (%.2f, %.2f) with tolerance %.2f",
- command->goal.position.x, command->goal.position.y, command->tolerance);
+ command->pose.position.x, command->pose.position.y, tolerance_);
task_server_->setResult(result);
return TaskStatus::SUCCEEDED;
} catch (std::exception & ex) {
RCLCPP_WARN(get_logger(), "Plan calculation to (%.2f, %.2f) failed: \"%s\"",
- command->goal.position.x, command->goal.position.y, ex.what());
+ command->pose.position.x, command->pose.position.y, ex.what());
// TODO(orduno): provide information about fail error to parent task,
// for example: couldn't get costmap update
@@ -275,8 +288,8 @@ NavfnPlanner::smoothApproachToGoal(
auto last_pose = plan.poses.back();
if (
squared_distance(last_pose, second_to_last_pose) >
- squared_distance(goal, second_to_last_pose)
- ) {
+ squared_distance(goal, second_to_last_pose))
+ {
plan.poses.back() = goal;
} else {
geometry_msgs::msg::Pose goal_copy = goal;
@@ -390,7 +403,7 @@ NavfnPlanner::getPointPotential(const geometry_msgs::msg::Point & world_point)
bool
NavfnPlanner::validPointPotential(const geometry_msgs::msg::Point & world_point)
{
- return validPointPotential(world_point, default_tolerance_);
+ return validPointPotential(world_point, tolerance_);
}
bool
@@ -500,7 +513,9 @@ NavfnPlanner::printCostmap(const nav2_msgs::msg::Costmap & costmap)
}
void
-NavfnPlanner::publishEndpoints(const nav2_tasks::ComputePathToPoseCommand::SharedPtr & endpoints)
+NavfnPlanner::publishEndpoints(
+ const geometry_msgs::msg::Pose & start,
+ const geometry_msgs::msg::Pose & goal)
{
visualization_msgs::msg::Marker marker;
@@ -543,8 +558,8 @@ NavfnPlanner::publishEndpoints(const nav2_tasks::ComputePathToPoseCommand::Share
marker.frame_locked = false;
marker.points.resize(2);
- marker.points[0] = endpoints->start.position;
- marker.points[1] = endpoints->goal.position;
+ marker.points[0] = start.position;
+ marker.points[1] = goal.position;
// Set the color -- be sure to set alpha to something non-zero!
std_msgs::msg::ColorRGBA start_color;
diff --git a/nav2_simple_navigator/CMakeLists.txt b/nav2_simple_navigator/CMakeLists.txt
index 167216a7..15fc49f6 100644
--- a/nav2_simple_navigator/CMakeLists.txt
+++ b/nav2_simple_navigator/CMakeLists.txt
@@ -15,7 +15,6 @@ find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED)
find_package(nav2_tasks REQUIRED)
find_package(nav2_msgs REQUIRED)
-find_package(nav2_robot REQUIRED)
include_directories(
include
@@ -38,7 +37,6 @@ set(dependencies
std_msgs
nav2_tasks
nav2_msgs
- nav2_robot
)
ament_target_dependencies(${executable_name}
diff --git a/nav2_simple_navigator/include/nav2_simple_navigator/simple_navigator.hpp b/nav2_simple_navigator/include/nav2_simple_navigator/simple_navigator.hpp
index 19bf4c78..99e1761f 100644
--- a/nav2_simple_navigator/include/nav2_simple_navigator/simple_navigator.hpp
+++ b/nav2_simple_navigator/include/nav2_simple_navigator/simple_navigator.hpp
@@ -21,7 +21,6 @@
#include "nav2_tasks/navigate_to_pose_task.hpp"
#include "nav2_tasks/compute_path_to_pose_task.hpp"
#include "nav2_tasks/follow_path_task.hpp"
-#include "nav2_robot/robot.hpp"
namespace nav2_simple_navigator
{
@@ -35,7 +34,6 @@ public:
nav2_tasks::TaskStatus navigateToPose(const nav2_tasks::NavigateToPoseCommand::SharedPtr command);
private:
- std::unique_ptr<nav2_robot::Robot> robot_;
std::unique_ptr<nav2_tasks::ComputePathToPoseTaskClient> planner_client_;
std::unique_ptr<nav2_tasks::FollowPathTaskClient> controller_client_;
std::unique_ptr<nav2_tasks::NavigateToPoseTaskServer> task_server_;
diff --git a/nav2_simple_navigator/package.xml b/nav2_simple_navigator/package.xml
index eea54ef2..cef45072 100644
--- a/nav2_simple_navigator/package.xml
+++ b/nav2_simple_navigator/package.xml
@@ -12,12 +12,10 @@
<build_depend>rclcpp</build_depend>
<build_depend>nav2_tasks</build_depend>
<build_depend>nav2_msgs</build_depend>
- <build_depend>nav2_robot</build_depend>
<exec_depend>rclcpp</exec_depend>
<exec_depend>nav2_tasks</exec_depend>
<exec_depend>nav2_msgs</exec_depend>
- <exec_depend>nav2_robot</exec_depend>
<test_depend>ament_lint_common</test_depend>
<test_depend>ament_lint_auto</test_depend>
diff --git a/nav2_simple_navigator/src/simple_navigator.cpp b/nav2_simple_navigator/src/simple_navigator.cpp
index f81a1a42..5d91d3c7 100644
--- a/nav2_simple_navigator/src/simple_navigator.cpp
+++ b/nav2_simple_navigator/src/simple_navigator.cpp
@@ -32,8 +32,6 @@ SimpleNavigator::SimpleNavigator()
auto temp_node = std::shared_ptr<rclcpp::Node>(this, [](auto) {});
- robot_ = std::make_unique<nav2_robot::Robot>(temp_node);
-
planner_client_ =
std::make_unique<nav2_tasks::ComputePathToPoseTaskClient>(temp_node);
@@ -53,35 +51,22 @@ SimpleNavigator::~SimpleNavigator()
TaskStatus
SimpleNavigator::navigateToPose(const nav2_tasks::NavigateToPoseCommand::SharedPtr command)
{
- RCLCPP_INFO(get_logger(), "Begin navigating to (%.2f, %.2f)",
+ RCLCPP_INFO(get_logger(), "Begin navigating from current location to (%.2f, %.2f)",
command->pose.position.x, command->pose.position.y);
- // Get the current pose from the robot
- auto current_pose = std::make_shared<geometry_msgs::msg::PoseWithCovarianceStamped>();
-
- if (!robot_->getCurrentPose(current_pose)) {
- // TODO(mhpanah): use either last known pose, current pose from odom, wait, or try again.
- RCLCPP_WARN(get_logger(), "Current robot pose is not available.");
- return TaskStatus::FAILED;
- }
-
- // Create a PathEndPoints message for the global planner
- auto endpoints = std::make_shared<nav2_tasks::ComputePathToPoseCommand>();
- endpoints->start = current_pose->pose.pose;
- endpoints->goal = command->pose;
- endpoints->tolerance = 2.0; // TODO(mjeronimo): this will come in the command message
+ // Create the path to be returned from ComputePath and sent to the FollowPath task
+ auto path = std::make_shared<nav2_tasks::ComputePathToPoseResult>();
RCLCPP_DEBUG(get_logger(), "Getting the path from the planner");
- RCLCPP_DEBUG(get_logger(), "goal.position.x: %f", endpoints->goal.position.x);
- RCLCPP_DEBUG(get_logger(), "goal.position.y: %f", endpoints->goal.position.y);
- RCLCPP_DEBUG(get_logger(), "goal.position.z: %f", endpoints->goal.position.z);
- RCLCPP_DEBUG(get_logger(), "goal.orientation.x: %f", endpoints->goal.orientation.x);
- RCLCPP_DEBUG(get_logger(), "goal.orientation.y: %f", endpoints->goal.orientation.y);
- RCLCPP_DEBUG(get_logger(), "goal.orientation.z: %f", endpoints->goal.orientation.z);
- RCLCPP_DEBUG(get_logger(), "goal.orientation.w: %f", endpoints->goal.orientation.w);
-
- auto path = std::make_shared<nav2_tasks::ComputePathToPoseResult>();
- planner_client_->sendCommand(endpoints);
+ RCLCPP_DEBUG(get_logger(), "goal->pose.position.x: %f", command->pose.position.x);
+ RCLCPP_DEBUG(get_logger(), "goal->pose.position.y: %f", command->pose.position.y);
+ RCLCPP_DEBUG(get_logger(), "goal->pose.position.z: %f", command->pose.position.z);
+ RCLCPP_DEBUG(get_logger(), "goal->pose.orientation.x: %f", command->pose.orientation.x);
+ RCLCPP_DEBUG(get_logger(), "goal->pose.orientation.y: %f", command->pose.orientation.y);
+ RCLCPP_DEBUG(get_logger(), "goal->pose.orientation.z: %f", command->pose.orientation.z);
+ RCLCPP_DEBUG(get_logger(), "goal->pose.orientation.w: %f", command->pose.orientation.w);
+
+ planner_client_->sendCommand(command);
// Loop until the subtasks are completed
for (;; ) {
diff --git a/nav2_system_tests/src/planning/planner_tester.cpp b/nav2_system_tests/src/planning/planner_tester.cpp
index 20daf794..ee30d9ab 100644
--- a/nav2_system_tests/src/planning/planner_tester.cpp
+++ b/nav2_system_tests/src/planning/planner_tester.cpp
@@ -240,7 +240,7 @@ bool PlannerTester::defaultPlannerTest(
return false;
}
- auto endpoints = std::make_shared<nav2_tasks::ComputePathToPoseCommand>();
+ auto goal = std::make_shared<nav2_tasks::ComputePathToPoseCommand>();
auto costmap_properties = costmap_->getProperties();
// Compose the PathEndPoints message
@@ -248,11 +248,8 @@ bool PlannerTester::defaultPlannerTest(
RCLCPP_INFO(this->get_logger(), "PlannerTester::defaultPlannerTest:"
" planning using a fake costmap");
- endpoints->start.position.x = 1.0;
- endpoints->start.position.y = 1.0;
- endpoints->goal.position.x = 9.0;
- endpoints->goal.position.y = 9.0;
- endpoints->tolerance = 2.0;
+ goal->pose.position.x = 9.0;
+ goal->pose.position.y = 9.0;
} else {
RCLCPP_INFO(this->get_logger(), "PlannerTester::defaultPlannerTest:"
@@ -260,14 +257,11 @@ bool PlannerTester::defaultPlannerTest(
// Defined with respect to world coordinate system
// Planner will do coordinate transformation to map internally
- endpoints->start.position.x = 390.0;
- endpoints->start.position.y = 10.0;
- endpoints->goal.position.x = 10.0;
- endpoints->goal.position.y = 390.0;
- endpoints->tolerance = 2.0;
+ goal->pose.position.x = 10.0;
+ goal->pose.position.y = 390.0;
}
- bool pathIsCollisionFree = plannerTest(endpoints, path);
+ bool pathIsCollisionFree = plannerTest(goal, path);
// TODO(orduno): On a default test, provide the 'right answer' to compare with the planner result
// given that we know the start, end and costmap is either preloaded or coming from
@@ -312,7 +306,7 @@ bool PlannerTester::defaultPlannerRandomTests(const unsigned int number_tests)
return std::make_pair(x, y);
};
- auto endpoints = std::make_shared<nav2_tasks::ComputePathToPoseCommand>();
+ auto goal = std::make_shared<nav2_tasks::ComputePathToPoseCommand>();
auto path = std::make_shared<nav2_tasks::ComputePathToPoseResult>();
bool all_tests_OK = true;
@@ -321,28 +315,22 @@ bool PlannerTester::defaultPlannerRandomTests(const unsigned int number_tests)
RCLCPP_INFO(this->get_logger(), "PlannerTester::defaultPlannerRandomTests:"
" running test #%u", test_num + 1);
- // Compose the path endpoints using random numbers
+ // Compose the goal using random numbers
// Defined with respect to world coordinate system
// Planner will do coordinate transformation to map internally
- auto start = generate_random();
- auto goal = generate_random();
- endpoints->start.position.x = start.first;
- endpoints->start.position.y = start.second;
- endpoints->goal.position.x = goal.first;
- endpoints->goal.position.y = goal.second;
-
- endpoints->tolerance = 2.0;
+ auto vals = generate_random();
+ goal->pose.position.x = vals.first;
+ goal->pose.position.y = vals.second;
// TODO(orduno): Tweak criteria for defining if a path goes into obstacles.
// Current Dijkstra planner will sometimes produce paths that cut corners
// i.e. some points are around the corner are actually inside the obstacle
- bool pathIsCollisionFree = plannerTest(endpoints, path);
+ bool pathIsCollisionFree = plannerTest(goal, path);
if (!pathIsCollisionFree) {
RCLCPP_INFO(this->get_logger(), "PlannerTester::defaultPlannerRandomTests:"
- " failed or found a collision with start at %0.2f, %0.2f and end at %0.2f, %0.2f",
- endpoints->start.position.x, endpoints->start.position.y,
- endpoints->goal.position.x, endpoints->goal.position.y);
+ " failed or found a collision with goal at %0.2f, %0.2f",
+ goal->pose.position.x, goal->pose.position.y);
all_tests_OK = false;
++num_fail;
}
@@ -355,13 +343,13 @@ bool PlannerTester::defaultPlannerRandomTests(const unsigned int number_tests)
}
bool PlannerTester::plannerTest(
- const nav2_tasks::ComputePathToPoseCommand::SharedPtr & endpoints,
+ const nav2_tasks::ComputePathToPoseCommand::SharedPtr & goal,
nav2_tasks::ComputePathToPoseResult::SharedPtr & path)
{
RCLCPP_INFO(this->get_logger(), "PlannerTester::plannerTest:"
" getting the path from the planner");
- TaskStatus status = sendRequest(endpoints, path);
+ TaskStatus status = sendRequest(goal, path);
RCLCPP_INFO(this->get_logger(), "PlannerTester::plannerTest: status: %d", status);
@@ -380,10 +368,10 @@ bool PlannerTester::plannerTest(
}
TaskStatus PlannerTester::sendRequest(
- const nav2_tasks::ComputePathToPoseCommand::SharedPtr & endpoints,
+ const nav2_tasks::ComputePathToPoseCommand::SharedPtr & goal,
nav2_tasks::ComputePathToPoseResult::SharedPtr & path)
{
- planner_client_->sendCommand(endpoints);
+ planner_client_->sendCommand(goal);
// Loop until the subtask is completed
while (true) {
diff --git a/nav2_system_tests/src/planning/planner_tester.hpp b/nav2_system_tests/src/planning/planner_tester.hpp
index 5b752f32..605f3e84 100644
--- a/nav2_system_tests/src/planning/planner_tester.hpp
+++ b/nav2_system_tests/src/planning/planner_tester.hpp
@@ -51,7 +51,7 @@ public:
// Sucess criteria is a collision free path.
// TODO(orduno): assuming a robot the size of a costmap cell
bool plannerTest(
- const nav2_tasks::ComputePathToPoseCommand::SharedPtr & endpoints,
+ const nav2_tasks::ComputePathToPoseCommand::SharedPtr & goal,
nav2_tasks::ComputePathToPoseResult::SharedPtr & path);
// Sends the request to the planner and gets the result.
@@ -74,7 +74,7 @@ private:
void startCostmapServer(std::string serviceName);
nav2_tasks::TaskStatus sendRequest(
- const nav2_tasks::ComputePathToPoseCommand::SharedPtr & endpoints,
+ const nav2_tasks::ComputePathToPoseCommand::SharedPtr & goal,
nav2_tasks::ComputePathToPoseResult::SharedPtr & path
);
diff --git a/nav2_tasks/include/nav2_tasks/bt_conversions.hpp b/nav2_tasks/include/nav2_tasks/bt_conversions.hpp
index 9e988224..1fb8a11b 100644
--- a/nav2_tasks/include/nav2_tasks/bt_conversions.hpp
+++ b/nav2_tasks/include/nav2_tasks/bt_conversions.hpp
@@ -20,8 +20,8 @@
#include "behaviortree_cpp/blackboard/blackboard.h"
#include "geometry_msgs/msg/point.hpp"
#include "geometry_msgs/msg/quaternion.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
#include "nav2_msgs/msg/path.hpp"
-#include "nav2_msgs/msg/path_end_points.hpp"
namespace BT
{
@@ -48,11 +48,12 @@ inline nav2_msgs::msg::Path::SharedPtr convertFromString(const StringView & /*ke
}
template<>
-inline nav2_msgs::msg::PathEndPoints::SharedPtr convertFromString(const StringView & /*key*/)
+inline geometry_msgs::msg::PoseStamped::SharedPtr convertFromString(const StringView & /*key*/)
{
return nullptr;
}
+
// These are needed to be able to set parameters for these types in the BT XML
template<>
diff --git a/nav2_tasks/include/nav2_tasks/compute_path_to_pose_action.hpp b/nav2_tasks/include/nav2_tasks/compute_path_to_pose_action.hpp
index f95390a7..0310b704 100644
--- a/nav2_tasks/include/nav2_tasks/compute_path_to_pose_action.hpp
+++ b/nav2_tasks/include/nav2_tasks/compute_path_to_pose_action.hpp
@@ -38,7 +38,7 @@ public:
void onInit() override
{
command_ =
- blackboard()->template get<nav2_tasks::ComputePathToPoseCommand::SharedPtr>("endpoints");
+ blackboard()->template get<nav2_tasks::ComputePathToPoseCommand::SharedPtr>("goal");
result_ = blackboard()->template get<nav2_tasks::ComputePathToPoseResult::SharedPtr>("path");
}
diff --git a/nav2_tasks/include/nav2_tasks/compute_path_to_pose_task.hpp b/nav2_tasks/include/nav2_tasks/compute_path_to_pose_task.hpp
index 8b3dfafb..0555f002 100644
--- a/nav2_tasks/include/nav2_tasks/compute_path_to_pose_task.hpp
+++ b/nav2_tasks/include/nav2_tasks/compute_path_to_pose_task.hpp
@@ -17,13 +17,13 @@
#include "nav2_tasks/task_client.hpp"
#include "nav2_tasks/task_server.hpp"
-#include "nav2_msgs/msg/path_end_points.hpp"
#include "nav2_msgs/msg/path.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
namespace nav2_tasks
{
-using ComputePathToPoseCommand = nav2_msgs::msg::PathEndPoints;
+using ComputePathToPoseCommand = geometry_msgs::msg::PoseStamped;
using ComputePathToPoseResult = nav2_msgs::msg::Path;
using ComputePathToPoseTaskClient =
diff --git a/nav2_tasks/include/nav2_tasks/rate_controller_node.hpp b/nav2_tasks/include/nav2_tasks/rate_controller_node.hpp
index c3cd133d..5e593a23 100644
--- a/nav2_tasks/include/nav2_tasks/rate_controller_node.hpp
+++ b/nav2_tasks/include/nav2_tasks/rate_controller_node.hpp
@@ -28,8 +28,8 @@ public:
RateController(const std::string & name, const BT::NodeParameters & params)
: BT::DecoratorNode(name, params)
{
- unsigned int hz = 1;
- getParam<unsigned int>("hz", hz);
+ double hz = 1.0;
+ getParam<double>("hz", hz);
period_ = 1.0 / hz;
}
diff --git a/tools/ros2_dependencies.repos b/tools/ros2_dependencies.repos
index 9d1c5e07..86d35bc0 100644
--- a/tools/ros2_dependencies.repos
+++ b/tools/ros2_dependencies.repos
@@ -1,5 +1,5 @@
repositories:
- behavior_tree_core:
+ BehaviorTree.CPP:
type: git
url: https://github.com/BehaviorTree/BehaviorTree.CPP.git
version: ros2
--
GitLab