Merge pull request #1292 from SteveMacenski/mult_control

Multiple controller plugins in server for contextual control 

nav2_behavior_tree/README.md

@@ -41,7 +41,7 @@ Once a new node is registered with the factory, it is now available to the Behav
   <BehaviorTree ID="MainTree">
     <Sequence name="root">
       <ComputePathToPose goal="${goal}"/>
-      <FollowPath path="${path}"/>
+      <FollowPath path="${path}" controller_property="FollowPath"/>
     </Sequence>
   </BehaviorTree>
 </root>
@@ -67,7 +67,7 @@ The nav2_behavior_tree package provides several navigation-specific nodes that a
 |----------|:-------------|------|
 | Backup |  Action | Invokes the BackUp ROS2 action server, which causes the robot to back up to a specific pose. This is used in nav2 Behavior Trees as a recovery behavior. The nav2_recoveries module implements the BackUp action server. | 
 | ComputePathToPose |    Action   | Invokes the ComputePathToPose ROS2 action server, which is implemented by the nav2_planner module. |
-| FollowPath | Action |Invokes the FollowPath ROS2 action server, which is implemented by the nav2_dwb_controller module. |
+| FollowPath | Action |Invokes the FollowPath ROS2 action server, which is implemented by the controller plugin modules loaded. |
 | GoalReached | Condition | Checks the distance to the goal, if the distance to goal is less than the pre-defined threshold, the tree returns SUCCESS, otherwise it returns FAILURE. |
 | IsStuck | Condition | Determines if the robot is not progressing towards the goal. If the robot is stuck and not progressing, the condition returns SUCCESS, otherwise it returns FAILURE. |
 | NavigateToPose | Action | Invokes the NavigateToPose ROS2 action server, which is implemented by the bt_navigator module. |

nav2_behavior_tree/include/nav2_behavior_tree/follow_path_action.hpp

@@ -38,6 +38,7 @@ public:
   void on_tick() override
   {
     getInput("path", goal_.path);
+    getInput("controller_property", goal_.controller_property);
   }
 
   void on_server_timeout() override
@@ -58,6 +59,7 @@ public:
   {
     return providedBasicPorts({
         BT::InputPort<nav_msgs::msg::Path>("path", "Path to follow"),
+        BT::InputPort<std::string>("controller_property", ""),
       });
   }
 };

nav2_bt_navigator/README.md

@@ -40,7 +40,7 @@ The BT Navigator package has two sample XML-based descriptions of BTs.  These tr
           <ComputePathToPose goal="${goal}" planner_property="GridBased"/>
         </Fallback>
       </RateController>
-      <FollowPath path="${path}"/>
+      <FollowPath path="${path}" controller_property="FollowPath"/>
     </Sequence>
   </BehaviorTree>
 </root>
@@ -63,7 +63,7 @@ The graphical version of this Behavior Tree:
 <img src="./doc/simple_parallel.png" title="" width="65%" align="middle">
 <br/>
 
-The navigate with replanning BT first ticks the `RateController` node which specifies how frequently the `GoalReached` and `ComputePathToPose` should be invoked. Then the `GoalReached` nodes check the distance to the goal to determine if the `ComputePathToPose` should be ticked or not. The `ComputePathToPose` gets the incoming goal pose from the blackboard, computes the path and puts the result back on the blackboard, where `FollowPath` picks it up. Each time a new path is computed, the blackboard gets updated and then `FollowPath` picks up the new goal.
+The navigate with replanning BT first ticks the `RateController` node which specifies how frequently the `GoalReached` and `ComputePathToPose` should be invoked. Then the `GoalReached` nodes check the distance to the goal to determine if the `ComputePathToPose` should be ticked or not. The `ComputePathToPose` gets the incoming goal pose from the blackboard, computes the path and puts the result back on the blackboard, where `FollowPath` picks it up. Each time a new path is computed, the blackboard gets updated and then `FollowPath` picks up the new goal to compute a control effort for. `controller_property` will specify the type of control effort you'd like to compute such as `FollowPath` `FollowPathSlow` `FollowPathExactly`, etc.
 
 ### Recovery Node
 In this section, the recovery node is being introduced to the navigation package.

nav2_bt_navigator/behavior_trees/navigate_w_replanning.xml

@@ -11,7 +11,7 @@
           <ComputePathToPose goal="{goal}" planner_property="GridBased"/>
         </Fallback>
       </RateController>
-      <FollowPath path="{path}"/>
+      <FollowPath path="{path}" controller_property="FollowPath"/>
     </Sequence>
   </BehaviorTree>
 </root>

nav2_bt_navigator/behavior_trees/navigate_w_replanning_and_recovery.xml

@@ -16,7 +16,7 @@
           </Fallback>
         </RateController>
         <RecoveryNode number_of_retries="1" name="FollowPath">
-          <FollowPath path="{path}"/>
+          <FollowPath path="{path}" controller_property="FollowPath"/>
           <ClearEntireCostmap service_name="local_costmap/clear_entirely_local_costmap"/>
         </RecoveryNode>
       </Sequence>

nav2_controller/include/nav2_controller/nav2_controller.hpp

@@ -18,6 +18,8 @@
 #include <memory>
 #include <string>
 #include <thread>
+#include <unordered_map>
+#include <vector>
 
 #include "nav2_core/local_planner.hpp"
 #include "nav2_costmap_2d/costmap_2d_ros.hpp"
@@ -42,6 +44,8 @@ class ProgressChecker;
 class ControllerServer : public nav2_util::LifecycleNode
 {
 public:
+  using ControllerMap = std::unordered_map<std::string, nav2_core::LocalPlanner::Ptr>;
+
   /**
    * @brief Constructor for nav2_controller::ControllerServer
    */
@@ -107,6 +111,7 @@ protected:
 
   // Our action server implements the FollowPath action
   std::unique_ptr<ActionServer> action_server_;
+
   /**
    * @brief FollowPath action server callback. Handles action server updates and
    * spins server until goal is reached
@@ -116,7 +121,8 @@ protected:
    * the specified rate till the goal is reached.
    * @throw nav2_core::PlannerException
    */
-  void followPath();
+  void computeControl();
+
   /**
    * @brief Assigns path to controller
    * @param path Path received from action server
@@ -160,13 +166,18 @@ 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_properties_, controller_types_;
+  std::string controller_properties_concat_, current_controller_;
 
   std::unique_ptr<ProgressChecker> progress_checker_;
 
   double controller_frequency_;
+
+  // Whether we've published the single controller warning yet
+  bool single_controller_warning_given_{false};
 };
 
 }  // namespace nav2_controller

nav2_controller/src/nav2_controller.cpp

@@ -13,6 +13,7 @@
 // limitations under the License.
 
 #include <chrono>
+#include <vector>
 #include <memory>
 #include <string>
 
@@ -31,10 +32,15 @@ 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_property, default_type;
+  default_type.push_back("dwb_core::DWBLocalPlanner");
+  default_property.push_back("FollowPath");
+  controller_properties_ = declare_parameter("controller_plugin_properties",
+      default_property);
+  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 +58,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 +66,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_properties_.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_properties_.size()));
+    exit(-1);
+  }
+
+  for (uint i = 0; i != controller_types_.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_properties_[i].c_str(), controller_types_[i].c_str());
+      controller->configure(node, controller_properties_[i],
+        costmap_ros_->getTfBuffer(), costmap_ros_);
+      controllers_.insert({controller_properties_[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_properties_.size(); i++) {
+    controller_properties_concat_ += controller_properties_[i] + std::string(" ");
   }
 
   get_parameter("controller_frequency", controller_frequency_);
@@ -80,7 +100,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state)
 
   // 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));
+      std::bind(&ControllerServer::computeControl, this));
 
   return nav2_util::CallbackReturn::SUCCESS;
 }
@@ -91,7 +111,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 +127,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 +145,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,11 +178,33 @@ 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.");
 
   try {
+    std::string c_name = action_server_->get_current_goal()->controller_property;
+
+    if (controllers_.find(c_name) == controllers_.end()) {
+      if (controllers_.size() == 1 && c_name.empty()) {
+        if (!single_controller_warning_given_) {
+          RCLCPP_WARN(get_logger(), "No controller was specified in action call."
+            " Server will use only plugin loaded %s. "
+            "This warning will appear once.", controller_properties_concat_.c_str());
+          single_controller_warning_given_ = true;
+        }
+        current_controller_ = controllers_.begin()->first;
+      } else {
+        RCLCPP_ERROR(get_logger(), "FollowPath called with controller name %s, "
+          "which does not exist. Available controllers are %s.",
+          c_name.c_str(), controller_properties_concat_.c_str());
+        action_server_->terminate_goals();
+        return;
+      }
+    } else {
+      current_controller_ = c_name;
+    }
+
     setPlannerPath(action_server_->get_current_goal()->path);
     progress_checker_->reset();
 
@@ -199,14 +252,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 +278,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 +321,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,52 +27,10 @@ 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 `FollowPath` action server.
 
 ![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.
-
 ## New local planner interface
 
-For the local planner, the new task interface consist of
-initialization/destruction code and one core method - `execute`. This method
-gets called with the path produced by the global planner. The local planner
-keeps processing the path until:
-1. It reaches the goal.
-2. It gets a new plan which preempts the current plan.
-3. It fails to produce an adequate plan, in which case it fails and leaves it to the caller to hand off to a recovery behavior.
-
-This is in contrast to the local planner in ROS1, where the local planner is
-queried at each iteration for a velocity command and the iteration and
-publishing of the command were handled by `MoveBase`
-
-The new task interface looks something like this in pseudocode
-```c++
-TaskStatus execute(path)
-{
-    planner_.initialize(nodeHandle);
-    planner_.setPlan(path);
-    while (true) {
-      auto pose = getRobotPose(pose2d);
-      if (isGoalReached(pose)) {
-        break;
-      }
-      auto velocity = getTwist();
-      auto cmd_vel_2d = planner_.computeVelocityCommands(pose, velocity);
-      publishVelocity(cmd_vel_2d);
-
-      std::this_thread::sleep_for(10ms);
-    }
-
-  nav2_behavior_tree::FollowPathResult result;
-  setResult(result);
-
-  return TaskStatus::SUCCEEDED;
-}
-
-```
-## Future Plans
-
-* Port another planner to refine the interfaces to the planner and to the world model. There are not many local planners available for the navigation stack, it seems. However `Time Elastic Band` seems like an interesting possibility of the few that are out there, as it interprets the dynamic obstacles very differently. https://github.com/ros-planning/navigation2/issues/202
+See `nav2_core` `LocalPlanner` interface. They are loaded into the `nav2_controller_server` to compute control commands from requests to the ROS2 action server.

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_msgs/action/FollowPath.action

 #goal definition
 nav_msgs/Path path
+string controller_property
 ---
 #result definition
 std_msgs/Empty result