nav2 controller accepting multiple plugin types

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_;
 

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)

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;
 
   /**

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.
 
 ![ROS 1 DWB Structure](./images/DWB_Structure_Simplified.svg "ROS 1 DWB Structure")
 
-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;

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() {}

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;

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);
 }
 

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_;
 };