Merge branch 'master' of github.com:ros-planning/navigation2 into reorg_bt_nav

README.md

@@ -13,6 +13,8 @@ ROS2 Navigation System
 # Overview
 The ROS 2 Navigation System is the control system that enables a robot to autonomously reach a goal state, such as a specific position and orientation relative to a specific map. Given a current pose, a map, and a goal, such as a destination pose, the navigation system generates a plan to reach the goal, and outputs commands to autonomously drive the robot, respecting any safety constraints and avoiding obstacles encountered along the way.
 
+![nav2_overview](doc/architecture/navigation_overview.png)
+
 # Documentation
 For detailed instructions on how to install and run the examples, please visit our [documentation site](https://ros-planning.github.io/navigation2/).
 

doc/BUILD.md

@@ -65,7 +65,7 @@ $ colcon build --symlink-install
 ```
 
 ## 3. Conclusion
-After installation of **Navigation2** and required dependencies, test **Navigation2** by following steps in [nav2_bringup README](../nav2_bringup/README.md)
+After installation of **Navigation2** and required dependencies, test **Navigation2** by following steps in [nav2_bringup README](../nav2_bringup/bringup/README.md)
 
 ## 4. Reporting Issue
 If run into any issue, feel free to submit pull request or report issue in this project.

nav2_behavior_tree/plugins/action/compute_path_to_pose_action.cpp

@@ -38,7 +38,7 @@ public:
   void on_tick() override
   {
     getInput("goal", goal_.pose);
-    getInput("planner_property", goal_.planner_property);
+    getInput("planner_id", goal_.planner_id);
   }
 
   void on_success() override
@@ -57,7 +57,7 @@ public:
     return providedBasicPorts({
         BT::OutputPort<nav_msgs::msg::Path>("path", "Path created by ComputePathToPose node"),
         BT::InputPort<geometry_msgs::msg::PoseStamped>("goal", "Destination to plan to"),
-        BT::InputPort<std::string>("planner_property", "")
+        BT::InputPort<std::string>("planner_id", "")
       });
   }
 

nav2_behavior_tree/plugins/action/follow_path_action.cpp

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

nav2_bt_navigator/README.md

@@ -37,10 +37,10 @@ The BT Navigator package has two sample XML-based descriptions of BTs.  These tr
       <RateController hz="1.0">
         <Fallback>
           <GoalReached/>
-          <ComputePathToPose goal="${goal}" planner_property="GridBased"/>
+          <ComputePathToPose goal="${goal}" planner_id="GridBased"/>
         </Fallback>
       </RateController>
-      <FollowPath path="${path}" controller_property="FollowPath"/>
+      <FollowPath path="${path}" controller_id="FollowPath"/>
     </Sequence>
   </BehaviorTree>
 </root>
@@ -55,7 +55,7 @@ Navigate with replanning is composed of the following custom decorator, conditio
 * GoalReached: Checks the distance to the goal, if the distance to goal is less than the pre-defined threshold, the tree returns SUCCESS, which in that case the `ComputePathToPose` action node will not get ticked.
 
 #### Action Nodes
-* ComputePathToPose: When this node is ticked, the goal will be placed on the blackboard which will be shared to the Behavior tree.  The bt action node would then utilizes the action server to send a request to the global planner to recompute the global path.  Once the global path is recomputed, the result will be sent back via action server and then the updated path will be placed on the blackboard. The `planner` parameter will tell the planning server which of the loaded planning plugins to utilize, in case of desiring different parameters, planners, or behaviors. The name of the planner should correspond with the high level task it accomplishes and align with the `planner_property_names` name given to it in the planner server. If no planner name is provided, it will use the only planner in the planner server when only one is available.
+* ComputePathToPose: When this node is ticked, the goal will be placed on the blackboard which will be shared to the Behavior tree.  The bt action node would then utilizes the action server to send a request to the global planner to recompute the global path.  Once the global path is recomputed, the result will be sent back via action server and then the updated path will be placed on the blackboard. The `planner` parameter will tell the planning server which of the loaded planning plugins to utilize, in case of desiring different parameters, planners, or behaviors. The name of the planner should correspond with the high level task it accomplishes and align with the `planner_ids` name given to it in the planner server. If no planner name is provided, it will use the only planner in the planner server when only one is available.
 
 
 The graphical version of this Behavior Tree:
@@ -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 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.
+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_id` 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

@@ -8,10 +8,10 @@
       <RateController hz="1.0">
         <Fallback>
           <GoalReached/>
-          <ComputePathToPose goal="{goal}" planner_property="GridBased"/>
+          <ComputePathToPose goal="{goal}" planner_id="GridBased"/>
         </Fallback>
       </RateController>
-      <FollowPath path="{path}" controller_property="FollowPath"/>
+      <FollowPath path="{path}" controller_id="FollowPath"/>
     </Sequence>
   </BehaviorTree>
 </root>

nav2_bt_navigator/behavior_trees/navigate_w_replanning_and_recovery.xml

@@ -10,13 +10,13 @@
           <Fallback name="GoalReached">
             <GoalReached/>
             <RecoveryNode number_of_retries="1" name="ComputePathToPose">
-              <ComputePathToPose goal="{goal}" path="{path}" planner_property="GridBased"/>
+              <ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
               <ClearEntireCostmap service_name="global_costmap/clear_entirely_global_costmap"/>
             </RecoveryNode>
           </Fallback>
         </RateController>
         <RecoveryNode number_of_retries="1" name="FollowPath">
-          <FollowPath path="{path}" controller_property="FollowPath"/>
+          <FollowPath path="{path}" controller_id="FollowPath"/>
           <ClearEntireCostmap service_name="local_costmap/clear_entirely_local_costmap"/>
         </RecoveryNode>
       </Sequence>

nav2_controller/include/nav2_controller/nav2_controller.hpp

@@ -169,8 +169,8 @@ protected:
   // Controller Plugins
   pluginlib::ClassLoader<nav2_core::LocalPlanner> lp_loader_;
   ControllerMap controllers_;
-  std::vector<std::string> controller_properties_, controller_types_;
-  std::string controller_properties_concat_, current_controller_;
+  std::vector<std::string> controller_ids_, controller_types_;
+  std::string controller_ids_concat_, current_controller_;
 
   std::unique_ptr<ProgressChecker> progress_checker_;
 

nav2_controller/src/nav2_controller.cpp

@@ -35,11 +35,11 @@ ControllerServer::ControllerServer()
   RCLCPP_INFO(get_logger(), "Creating controller server");
 
   declare_parameter("controller_frequency", 20.0);
-  std::vector<std::string> default_property, default_type;
+  std::vector<std::string> default_id, default_type;
   default_type.push_back("dwb_core::DWBLocalPlanner");
-  default_property.push_back("FollowPath");
-  controller_properties_ = declare_parameter("controller_plugin_properties",
-      default_property);
+  default_id.push_back("FollowPath");
+  controller_ids_ = declare_parameter("controller_plugin_ids",
+      default_id);
   controller_types_ = declare_parameter("controller_plugin_types", default_type);
 
   // The costmap node is used in the implementation of the local planner
@@ -66,11 +66,11 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state)
 
   progress_checker_ = std::make_unique<ProgressChecker>(rclcpp_node_);
 
-  if (controller_types_.size() != controller_properties_.size()) {
+  if (controller_types_.size() != controller_ids_.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()));
+      static_cast<int>(controller_ids_.size()));
     exit(-1);
   }
 
@@ -79,17 +79,17 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state)
       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],
+        controller_ids_[i].c_str(), controller_types_[i].c_str());
+      controller->configure(node, controller_ids_[i],
         costmap_ros_->getTfBuffer(), costmap_ros_);
-      controllers_.insert({controller_properties_[i], controller});
+      controllers_.insert({controller_ids_[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(" ");
+  for (uint i = 0; i != controller_ids_.size(); i++) {
+    controller_ids_concat_ += controller_ids_[i] + std::string(" ");
   }
 
   get_parameter("controller_frequency", controller_frequency_);
@@ -183,21 +183,21 @@ void ControllerServer::computeControl()
   RCLCPP_INFO(get_logger(), "Received a goal, begin computing control effort.");
 
   try {
-    std::string c_name = action_server_->get_current_goal()->controller_property;
+    std::string c_name = action_server_->get_current_goal()->controller_id;
 
     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());
+            "This warning will appear once.", controller_ids_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());
+          c_name.c_str(), controller_ids_concat_.c_str());
         action_server_->terminate_goals();
         return;
       }

nav2_msgs/action/ComputePathToPose.action

 #goal definition
 geometry_msgs/PoseStamped pose
-string planner_property
+string planner_id
 ---
 #result definition
 nav_msgs/Path path

nav2_msgs/action/FollowPath.action

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

nav2_planner/include/nav2_planner/planner_server.hpp

@@ -115,8 +115,8 @@ protected:
   // Planner
   PlannerMap planners_;
   pluginlib::ClassLoader<nav2_core::GlobalPlanner> gp_loader_;
-  std::vector<std::string> plugin_properties_, plugin_types_;
-  std::string planner_properties_concat_;
+  std::vector<std::string> plugin_ids_, plugin_types_;
+  std::string planner_ids_concat_;
 
   // TF buffer
   std::shared_ptr<tf2_ros::Buffer> tf_;

nav2_planner/src/planner_server.cpp

@@ -41,10 +41,10 @@ PlannerServer::PlannerServer()
   RCLCPP_INFO(get_logger(), "Creating");
 
   // Declare this node's parameters
-  std::vector<std::string> default_property, default_type;
-  default_property.push_back("GridBased");
+  std::vector<std::string> default_id, default_type;
+  default_id.push_back("GridBased");
   default_type.push_back("nav2_navfn_planner/NavfnPlanner");
-  declare_parameter("planner_plugin_properties", default_property);
+  declare_parameter("planner_plugin_ids", default_id);
   declare_parameter("planner_plugin_types", default_type);
 
   // Setup the global costmap
@@ -77,11 +77,11 @@ PlannerServer::on_configure(const rclcpp_lifecycle::State & state)
 
   tf_ = costmap_ros_->getTfBuffer();
 
-  get_parameter("planner_plugin_properties", plugin_properties_);
+  get_parameter("planner_plugin_ids", plugin_ids_);
   get_parameter("planner_plugin_types", plugin_types_);
   auto node = shared_from_this();
 
-  if (plugin_properties_.size() != plugin_types_.size()) {
+  if (plugin_ids_.size() != plugin_types_.size()) {
     RCLCPP_FATAL(get_logger(),
       "Planner plugin names and types sizes do not match!");
     exit(-1);
@@ -92,9 +92,9 @@ PlannerServer::on_configure(const rclcpp_lifecycle::State & state)
       nav2_core::GlobalPlanner::Ptr planner =
         gp_loader_.createUniqueInstance(plugin_types_[i]);
       RCLCPP_INFO(get_logger(), "Created global planner plugin %s of type %s",
-        plugin_properties_[i].c_str(), plugin_types_[i].c_str());
-      planner->configure(node, plugin_properties_[i], tf_, costmap_ros_);
-      planners_.insert({plugin_properties_[i], planner});
+        plugin_ids_[i].c_str(), plugin_types_[i].c_str());
+      planner->configure(node, plugin_ids_[i], tf_, costmap_ros_);
+      planners_.insert({plugin_ids_[i], planner});
     } catch (const pluginlib::PluginlibException & ex) {
       RCLCPP_FATAL(get_logger(), "Failed to create global planner. Exception: %s",
         ex.what());
@@ -103,7 +103,7 @@ PlannerServer::on_configure(const rclcpp_lifecycle::State & state)
   }
 
   for (uint i = 0; i != plugin_types_.size(); i++) {
-    planner_properties_concat_ += plugin_properties_[i] + std::string(" ");
+    planner_ids_concat_ += plugin_ids_[i] + std::string(" ");
   }
 
   // Initialize pubs & subs
@@ -224,27 +224,27 @@ PlannerServer::computePlan()
       "(%.2f, %.2f).", start.pose.position.x, start.pose.position.y,
       goal->pose.pose.position.x, goal->pose.pose.position.y);
 
-    if (planners_.find(goal->planner_property) != planners_.end()) {
-      result->path = planners_[goal->planner_property]->createPlan(start, goal->pose);
+    if (planners_.find(goal->planner_id) != planners_.end()) {
+      result->path = planners_[goal->planner_id]->createPlan(start, goal->pose);
     } else {
-      if (planners_.size() == 1 && goal->planner_property.empty()) {
+      if (planners_.size() == 1 && goal->planner_id.empty()) {
         if (!single_planner_warning_given_) {
           single_planner_warning_given_ = true;
           RCLCPP_WARN(get_logger(), "No planners specified in action call. "
             "Server will use only plugin %s in server."
-            " This warning will appear once.", planner_properties_concat_.c_str());
+            " This warning will appear once.", planner_ids_concat_.c_str());
         }
         result->path = planners_[planners_.begin()->first]->createPlan(start, goal->pose);
       } else {
         RCLCPP_ERROR(get_logger(), "planner %s is not a valid planner. "
-          "Planner names are: %s", goal->planner_property.c_str(),
-          planner_properties_concat_.c_str());
+          "Planner names are: %s", goal->planner_id.c_str(),
+          planner_ids_concat_.c_str());
       }
     }
 
     if (result->path.poses.size() == 0) {
       RCLCPP_WARN(get_logger(), "Planning algorithm %s failed to generate a valid"
-        " path to (%.2f, %.2f)", goal->planner_property.c_str(),
+        " path to (%.2f, %.2f)", goal->planner_id.c_str(),
         goal->pose.pose.position.x, goal->pose.pose.position.y);
       // TODO(orduno): define behavior if a preemption is available
       action_server_->terminate_goals();
@@ -264,7 +264,7 @@ PlannerServer::computePlan()
     return;
   } catch (std::exception & ex) {
     RCLCPP_WARN(get_logger(), "%s plugin failed to plan calculation to (%.2f, %.2f): \"%s\"",
-      goal->planner_property.c_str(), goal->pose.pose.position.x,
+      goal->planner_id.c_str(), goal->pose.pose.position.x,
       goal->pose.pose.position.y, ex.what());
 
     // TODO(orduno): provide information about fail error to parent task,