From dc93370f383767e1febbb30ef3febe5f39b228d6 Mon Sep 17 00:00:00 2001
From: stevemacenski <stevenmacenski@gmail.com>
Date: Wed, 23 Oct 2019 17:22:56 -0700
Subject: [PATCH] nav2 controller accepting multiple plugin types
---
.../nav2_controller/nav2_controller.hpp | 16 ++--
nav2_controller/src/nav2_controller.cpp | 96 +++++++++++++------
nav2_core/include/nav2_core/local_planner.hpp | 2 +-
nav2_dwb_controller/README.md | 8 +-
.../include/dwb_core/dwb_local_planner.hpp | 2 +-
.../dwb_core/src/dwb_local_planner.cpp | 2 +-
.../src/dummy_controller/dummy_controller.cpp | 6 +-
.../src/dummy_controller/dummy_controller.hpp | 6 +-
8 files changed, 92 insertions(+), 46 deletions(-)
diff --git a/nav2_controller/include/nav2_controller/nav2_controller.hpp b/nav2_controller/include/nav2_controller/nav2_controller.hpp
index 26c39283..4366c1a1 100644
--- a/nav2_controller/include/nav2_controller/nav2_controller.hpp
+++ b/nav2_controller/include/nav2_controller/nav2_controller.hpp
@@ -18,11 +18,12 @@
#include <memory>
#include <string>
#include <thread>
+#include <unordered_map>
#include "nav2_core/local_planner.hpp"
#include "nav2_costmap_2d/costmap_2d_ros.hpp"
#include "tf2_ros/transform_listener.h"
-#include "nav2_msgs/action/follow_path.hpp"
+#include "nav2_msgs/action/compute_control.hpp"
#include "nav_2d_utils/odom_subscriber.hpp"
#include "nav2_util/lifecycle_node.hpp"
#include "nav2_util/simple_action_server.hpp"
@@ -38,6 +39,7 @@ class ProgressChecker;
class ControllerServer : public nav2_util::LifecycleNode
{
public:
+ typedef std::unordered_map<std::string, nav2_core::LocalPlanner::Ptr> ControllerMap;
ControllerServer();
~ControllerServer();
@@ -50,13 +52,13 @@ protected:
nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override;
nav2_util::CallbackReturn on_error(const rclcpp_lifecycle::State & state) override;
- using ActionServer = nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath>;
+ using ActionServer = nav2_util::SimpleActionServer<nav2_msgs::action::ComputeControl>;
- // Our action server implements the FollowPath action
+ // Our action server implements the ComputeControl action
std::unique_ptr<ActionServer> action_server_;
// The action server callback
- void followPath();
+ void computeControl();
void setPlannerPath(const nav_msgs::msg::Path & path);
void computeAndPublishVelocity();
@@ -74,9 +76,11 @@ protected:
std::unique_ptr<nav_2d_utils::OdomSubscriber> odom_sub_;
rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::Twist>::SharedPtr vel_publisher_;
- // Local Planner Plugin
+ // Controller Plugins
pluginlib::ClassLoader<nav2_core::LocalPlanner> lp_loader_;
- nav2_core::LocalPlanner::Ptr local_planner_;
+ ControllerMap controllers_;
+ std::vector<std::string> controller_names_, controller_types_;
+ std::string controller_names_concat_, current_controller_;
std::unique_ptr<ProgressChecker> progress_checker_;
diff --git a/nav2_controller/src/nav2_controller.cpp b/nav2_controller/src/nav2_controller.cpp
index da63aff0..dba6b603 100644
--- a/nav2_controller/src/nav2_controller.cpp
+++ b/nav2_controller/src/nav2_controller.cpp
@@ -31,10 +31,14 @@ ControllerServer::ControllerServer()
: LifecycleNode("controller_server", "", true),
lp_loader_("nav2_core", "nav2_core::LocalPlanner")
{
- RCLCPP_INFO(get_logger(), "Creating controller");
+ RCLCPP_INFO(get_logger(), "Creating controller server");
declare_parameter("controller_frequency", 20.0);
- declare_parameter("local_controller_plugin", "dwb_core::DWBLocalPlanner");
+ std::vector<std::string> default_name, default_type;
+ default_type.push_back("dwb_core::DWBLocalPlanner");
+ default_name.push_back("FollowPath");
+ controller_names_ = declare_parameter("controller_plugin_names", default_name);
+ controller_types_ = declare_parameter("controller_plugin_types", default_type);
// The costmap node is used in the implementation of the local planner
costmap_ros_ = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
@@ -52,9 +56,7 @@ ControllerServer::~ControllerServer()
nav2_util::CallbackReturn
ControllerServer::on_configure(const rclcpp_lifecycle::State & state)
{
- std::string local_planner_name;
-
- RCLCPP_INFO(get_logger(), "Configuring local controller interface");
+ RCLCPP_INFO(get_logger(), "Configuring controller interface");
costmap_ros_->on_configure(state);
@@ -62,14 +64,30 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state)
progress_checker_ = std::make_unique<ProgressChecker>(rclcpp_node_);
- try {
- std::string local_controller_plugin;
- get_parameter("local_controller_plugin", local_controller_plugin);
- local_planner_ = lp_loader_.createUniqueInstance(local_controller_plugin);
- RCLCPP_INFO(get_logger(), "Created local controller : %s", local_controller_plugin.c_str());
- local_planner_->configure(node, costmap_ros_->getTfBuffer(), costmap_ros_);
- } catch (const pluginlib::PluginlibException & ex) {
- RCLCPP_FATAL(get_logger(), "Failed to create local planner. Exception: %s", ex.what());
+ if (controller_types_.size() != controller_names_.size()) {
+ RCLCPP_FATAL(get_logger(), "Size of controller names (%i) and "
+ "controller types (%i) are not the same!",
+ static_cast<int>(controller_types_.size()),
+ static_cast<int>(controller_names_.size()));
+ exit(-1);
+ }
+
+ for (uint i = 0; i != controller_names_.size(); i++) {
+ try {
+ nav2_core::LocalPlanner::Ptr controller =
+ lp_loader_.createUniqueInstance(controller_types_[i]);
+ RCLCPP_INFO(get_logger(), "Created controller : %s of type %s",
+ controller_names_[i].c_str(), controller_types_[i].c_str());
+ controller->configure(node, controller_names_[i],
+ costmap_ros_->getTfBuffer(), costmap_ros_);
+ controllers_.insert({controller_names_[i], controller});
+ } catch (const pluginlib::PluginlibException & ex) {
+ RCLCPP_FATAL(get_logger(), "Failed to create controller. Exception: %s", ex.what());
+ }
+ }
+
+ for (uint i = 0; i != controller_names_.size(); i++) {
+ controller_names_concat_ += controller_names_[i] + std::string(" ");
}
get_parameter("controller_frequency", controller_frequency_);
@@ -79,8 +97,8 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state)
vel_publisher_ = create_publisher<geometry_msgs::msg::Twist>("/cmd_vel", 1);
// Create the action server that we implement with our followPath method
- action_server_ = std::make_unique<ActionServer>(rclcpp_node_, "FollowPath",
- std::bind(&ControllerServer::followPath, this));
+ action_server_ = std::make_unique<ActionServer>(rclcpp_node_, "ComputeControl",
+ std::bind(&ControllerServer::computeControl, this));
return nav2_util::CallbackReturn::SUCCESS;
}
@@ -91,7 +109,10 @@ ControllerServer::on_activate(const rclcpp_lifecycle::State & state)
RCLCPP_INFO(get_logger(), "Activating");
costmap_ros_->on_activate(state);
- local_planner_->activate();
+ ControllerMap::iterator it;
+ for (it = controllers_.begin(); it != controllers_.end(); ++it) {
+ it->second->activate();
+ }
vel_publisher_->on_activate();
action_server_->activate();
@@ -104,7 +125,10 @@ ControllerServer::on_deactivate(const rclcpp_lifecycle::State & state)
RCLCPP_INFO(get_logger(), "Deactivating");
action_server_->deactivate();
- local_planner_->deactivate();
+ ControllerMap::iterator it;
+ for (it = controllers_.begin(); it != controllers_.end(); ++it) {
+ it->second->deactivate();
+ }
costmap_ros_->on_deactivate(state);
publishZeroVelocity();
@@ -119,12 +143,17 @@ ControllerServer::on_cleanup(const rclcpp_lifecycle::State & state)
RCLCPP_INFO(get_logger(), "Cleaning up");
// Cleanup the helper classes
- local_planner_->cleanup();
+ ControllerMap::iterator it;
+ for (it = controllers_.begin(); it != controllers_.end(); ++it) {
+ it->second->cleanup();
+ }
costmap_ros_->on_cleanup(state);
// Release any allocated resources
action_server_.reset();
- local_planner_.reset();
+ for (it = controllers_.begin(); it != controllers_.end(); ++it) {
+ it->second.reset();
+ }
odom_sub_.reset();
vel_publisher_.reset();
@@ -147,14 +176,25 @@ ControllerServer::on_shutdown(const rclcpp_lifecycle::State &)
return nav2_util::CallbackReturn::SUCCESS;
}
-void ControllerServer::followPath()
+void ControllerServer::computeControl()
{
- RCLCPP_INFO(get_logger(), "Received a goal, begin following path");
-
+ RCLCPP_INFO(get_logger(), "Received a goal, begin computing control effort.");
+ //TODO STEVE msg string, check and use
try {
setPlannerPath(action_server_->get_current_goal()->path);
progress_checker_->reset();
+ std::string c_name = action_server_->get_current_goal()->controller_name;
+
+ if (controllers_.find(c_name) == controllers_.end()) {
+ RCLCPP_ERROR(get_logger(), "ComputeControl called with controller name %s, "
+ "which does not exist. Available controllers are %s.",
+ c_name.c_str(), controller_names_concat_.c_str());
+ action_server_->terminate_goals();
+ } else {
+ current_controller_ = c_name;
+ }
+
rclcpp::Rate loop_rate(controller_frequency_);
while (rclcpp::ok()) {
if (action_server_ == nullptr) {
@@ -199,14 +239,15 @@ void ControllerServer::followPath()
publishZeroVelocity();
- // TODO(orduno) #861 Handle a pending preemption.
+ // TODO(orduno) #861 Handle a pending preemption and set controller name
action_server_->succeeded_current();
}
void ControllerServer::setPlannerPath(const nav_msgs::msg::Path & path)
{
- RCLCPP_DEBUG(get_logger(), "Providing path to the local planner");
- local_planner_->setPlan(path);
+ RCLCPP_DEBUG(get_logger(),
+ "Providing path to the controller %s", current_controller_);
+ controllers_[current_controller_]->setPlan(path);
auto end_pose = *(path.poses.end() - 1);
@@ -224,7 +265,8 @@ void ControllerServer::computeAndPublishVelocity()
progress_checker_->check(pose);
- auto cmd_vel_2d = local_planner_->computeVelocityCommands(pose,
+ auto cmd_vel_2d =
+ controllers_[current_controller_]->computeVelocityCommands(pose,
nav_2d_utils::twist2Dto3D(odom_sub_->getTwist()));
RCLCPP_DEBUG(get_logger(), "Publishing velocity at time %.2f", now().seconds());
@@ -266,7 +308,7 @@ bool ControllerServer::isGoalReached()
}
geometry_msgs::msg::Twist velocity = nav_2d_utils::twist2Dto3D(odom_sub_->getTwist());
- return local_planner_->isGoalReached(pose, velocity);
+ return controllers_[current_controller_]->isGoalReached(pose, velocity);
}
bool ControllerServer::getRobotPose(geometry_msgs::msg::PoseStamped & pose)
diff --git a/nav2_core/include/nav2_core/local_planner.hpp b/nav2_core/include/nav2_core/local_planner.hpp
index 14b953c8..0ab8f9ee 100644
--- a/nav2_core/include/nav2_core/local_planner.hpp
+++ b/nav2_core/include/nav2_core/local_planner.hpp
@@ -73,7 +73,7 @@ public:
*/
virtual void configure(
const rclcpp_lifecycle::LifecycleNode::SharedPtr &,
- const std::shared_ptr<tf2_ros::Buffer> &,
+ std::string name, const std::shared_ptr<tf2_ros::Buffer> &,
const std::shared_ptr<nav2_costmap_2d::Costmap2DROS> &) = 0;
/**
diff --git a/nav2_dwb_controller/README.md b/nav2_dwb_controller/README.md
index bf4f03dd..e0a5aca5 100644
--- a/nav2_dwb_controller/README.md
+++ b/nav2_dwb_controller/README.md
@@ -27,12 +27,12 @@ The advantages of DWB were:
For the most part, the DWB codebase is unchanged, other than what was required for ROS2.
-The main architectural change is due to the replacement of `MoveBase` by the `FollowPath` task interface.
+The main architectural change is due to the replacement of `MoveBase` by the `ComputeControl` task interface.
-To support the new FollowPath task interface, the `MoveBase` adapter layer and `nav_core2` interfaces were removed. They were replaced by an an alternate adapter between
-the `FollowPath` task interface and the `DWBLocalPlanner` component.
+To support the new ComputeControl task interface, the `MoveBase` adapter layer and `nav_core2` interfaces were removed. They were replaced by an an alternate adapter between
+the `ComputeControl` task interface and the `DWBLocalPlanner` component.
## New local planner interface
@@ -66,7 +66,7 @@ TaskStatus execute(path)
std::this_thread::sleep_for(10ms);
}
- nav2_behavior_tree::FollowPathResult result;
+ nav2_behavior_tree::ComputeControlResult result;
setResult(result);
return TaskStatus::SUCCEEDED;
diff --git a/nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp b/nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp
index ddaf88c5..4d905f75 100644
--- a/nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp
+++ b/nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp
@@ -68,7 +68,7 @@ public:
void configure(
const rclcpp_lifecycle::LifecycleNode::SharedPtr & node,
- const std::shared_ptr<tf2_ros::Buffer> & tf,
+ std::string name, const std::shared_ptr<tf2_ros::Buffer> & tf,
const std::shared_ptr<nav2_costmap_2d::Costmap2DROS> & costmap_ros) override;
virtual ~DWBLocalPlanner() {}
diff --git a/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp b/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp
index 28ea0959..4527bec1 100644
--- a/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp
+++ b/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp
@@ -66,7 +66,7 @@ DWBLocalPlanner::DWBLocalPlanner()
void DWBLocalPlanner::configure(
const rclcpp_lifecycle::LifecycleNode::SharedPtr & node,
- const std::shared_ptr<tf2_ros::Buffer> & tf,
+ std::string name, const std::shared_ptr<tf2_ros::Buffer> & tf,
const std::shared_ptr<nav2_costmap_2d::Costmap2DROS> & costmap_ros)
{
node_ = node;
diff --git a/nav2_system_tests/src/dummy_controller/dummy_controller.cpp b/nav2_system_tests/src/dummy_controller/dummy_controller.cpp
index b5ffc935..a0a8824f 100644
--- a/nav2_system_tests/src/dummy_controller/dummy_controller.cpp
+++ b/nav2_system_tests/src/dummy_controller/dummy_controller.cpp
@@ -34,7 +34,7 @@ DummyController::DummyController()
vel_pub_ =
this->create_publisher<geometry_msgs::msg::Twist>("cmd_vel", 1);
- task_server_ = std::make_unique<nav2_behavior_tree::FollowPathTaskServer>(temp_node, false),
+ task_server_ = std::make_unique<nav2_behavior_tree::ComputeControlTaskServer>(temp_node, false),
task_server_->setExecuteCallback(
std::bind(&DummyController::followPath, this, std::placeholders::_1));
@@ -50,7 +50,7 @@ DummyController::~DummyController()
}
void
-DummyController::followPath(const nav2_behavior_tree::FollowPathCommand::SharedPtr /*command*/)
+DummyController::followPath(const nav2_behavior_tree::ComputeControlCommand::SharedPtr /*command*/)
{
RCLCPP_INFO(get_logger(), "Starting controller ");
@@ -87,7 +87,7 @@ DummyController::followPath(const nav2_behavior_tree::FollowPathCommand::SharedP
}
}
- nav2_behavior_tree::FollowPathResult result;
+ nav2_behavior_tree::ComputeControlResult result;
task_server_->setResult(result);
}
diff --git a/nav2_system_tests/src/dummy_controller/dummy_controller.hpp b/nav2_system_tests/src/dummy_controller/dummy_controller.hpp
index 70dd67e9..30824128 100644
--- a/nav2_system_tests/src/dummy_controller/dummy_controller.hpp
+++ b/nav2_system_tests/src/dummy_controller/dummy_controller.hpp
@@ -17,7 +17,7 @@
#include <memory>
-#include "nav2_behavior_tree/follow_path_task.hpp"
+#include "nav2_behavior_tree/compute_control_task.hpp"
#include "geometry_msgs/msg/twist.hpp"
namespace nav2_system_tests
@@ -30,12 +30,12 @@ public:
~DummyController();
nav2_behavior_tree::TaskStatus followPath(
- const nav2_behavior_tree::FollowPathCommand::SharedPtr command);
+ const nav2_behavior_tree::ComputeControlCommand::SharedPtr command);
private:
void setZeroVelocity();
- std::unique_ptr<nav2_behavior_tree::FollowPathTaskServer> task_server_;
+ std::unique_ptr<nav2_behavior_tree::ComputeControlTaskServer> task_server_;
std::shared_ptr<rclcpp::Publisher<geometry_msgs::msg::Twist>> vel_pub_;
};
--
GitLab