Merge with ros-planning/master

README.md

@@ -12,6 +12,26 @@ For detailed instructions on how to:
 
 Please visit our [documentation site](https://navigation.ros.org/). [Please visit our community Slack here](https://navigation2.slack.com).
 
+## Citation
+
+If you use the navigation framework, an algorithm from this repository, or ideas from it
+please cite this work in your papers!
+
+ - S. Macenski, F. Martín, R. White, J. Clavero. [**The Marathon 2: A Navigation System**](https://arxiv.org/abs/2003.00368). IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2020.
+
+ ```bibtex
+ @InProceedings{macenski2020marathon2,
+   title = {The Marathon 2: A Navigation System},
+   author = {Macenski, Steve and Martín, Francisco and White, Ruffin and Ginés Clavero, Jonatan},
+   year = {2020},
+   booktitle = {2020 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)},
+   url = {https://github.com/ros-planning/navigation2},
+   pdf = {https://arxiv.org/abs/2003.00368}
+ }
+```
+
+## Build Status
+
 | Service | Dashing | Eloquent | Foxy | Master |
 | :---: | :---: | :---: | :---: | :---: |
 | ROS Build Farm | [![Build Status](http://build.ros2.org/job/Ddev__navigation2__ubuntu_bionic_amd64/badge/icon)](http://build.ros2.org/job/Ddev__navigation2__ubuntu_bionic_amd64/) | [![Build Status](http://build.ros2.org/job/Edev__navigation2__ubuntu_bionic_amd64/badge/icon)](http://build.ros2.org/job/Edev__navigation2__ubuntu_bionic_amd64/) | [![Build Status](http://build.ros2.org/job/Fdev__navigation2__ubuntu_focal_amd64/badge/icon)](http://build.ros2.org/job/Fdev__navigation2__ubuntu_focal_amd64/) | N/A |

doc/parameters/param_list.md

@@ -154,12 +154,15 @@ When `plugins` parameter is not overridden, the following default plugins are lo
 | Parameter | Default | Description |
 | ----------| --------| ------------|
 | controller_frequency | 20.0 | Frequency to run controller |
+| progress_checker_plugin | "progress_checker" | Plugin used by the controller to check whether the robot has at least covered a set distance/displacement in a set amount of time, thus checking the progress of the robot. |
+| `<progress_checker_plugin>.plugin` | "nav2_controller::SimpleProgressChecker" | Default plugin |
+| goal_checker_plugin | "goal_checker" | Check if the goal has been reached |
+| `<goal_checker_plugin>.plugin` | "nav2_controller::SimpleGoalChecker" | Default plugin |
 | controller_plugins | ["FollowPath"] | List of mapped names for controller plugins for processing requests and parameters |
+| `<controller_plugins>.plugin` | "dwb_core::DWBLocalPlanner" | Default plugin |
 | min_x_velocity_threshold | 0.0001 | Minimum X velocity to use (m/s) |
 | min_y_velocity_threshold | 0.0001 | Minimum Y velocity to use (m/s) |
 | min_theta_velocity_threshold | 0.0001 | Minimum angular velocity to use (rad/s) |
-| required_movement_radius | 0.5 | Minimum amount a robot must move to be progressing to goal (m) |
-| movement_time_allowance | 10.0 | Maximum amount of time a robot has to move the minimum radius (s) |
 
 **NOTE:** When `controller_plugins` parameter is overridden, each plugin namespace defined in the list needs to have a `plugin` parameter defining the type of plugin to be loaded in the namespace.
 
@@ -173,11 +176,36 @@ controller_server:
       plugin: "dwb_core::DWBLocalPlanner"
 ```
 
-When `controller_plugins` parameter is not overridden, the following default plugins are loaded:
+When `controller_plugins`\`progress_checker_plugin`\`goal_checker_plugin` parameters are not overridden, the following default plugins are loaded:
 
 | Namespace | Plugin |
 | ----------| --------|
 | "FollowPath" | "dwb_core::DWBLocalPlanner" |
+| "progress_checker" | "nav2_controller::SimpleProgressChecker" |
+| "goal_checker" | "nav2_controller::SimpleGoalChecker" |
+
+## simple_progress_checker plugin
+
+| Parameter | Default | Description |
+| ----------| --------| ------------|
+| `<nav2_controller plugin>`.required_movement_radius | 0.5 | Minimum distance to count as progress (m) |
+| `<nav2_controller plugin>`.movement_time_allowance | 10.0 | Maximum time allowence for progress to happen (s) |
+
+
+## simple_goal_checker plugin
+
+| Parameter | Default | Description |
+| ----------| --------| ------------|
+| `<nav2_controller plugin>`.xy_goal_tolerance | 0.25 | Tolerance to meet goal completion criteria (m) |
+| `<nav2_controller plugin>`.yaw_goal_tolerance | 0.25 | Tolerance to meet goal completion criteria (rad) |
+| `<nav2_controller plugin>`.stateful | true | Whether to check for XY position tolerance after rotating to goal orientation in case of minor localization changes |
+
+## stopped_goal_checker plugin
+
+| Parameter | Default | Description |
+| ----------| --------| ------------|
+| `<nav2_controller plugin>`.rot_stopped_velocity | 0.25 | Velocity below is considered to be stopped at tolerance met (rad/s) |
+| `<nav2_controller plugin>`.trans_stopped_velocity | 0.25 | Velocity below is considered to be stopped at tolerance met (m/s) |
 
 # dwb_controller
 
@@ -193,7 +221,6 @@ When `controller_plugins` parameter is not overridden, the following default plu
 | `<dwb plugin>`.prune_distance | 1.0 | Distance (m) to prune backward until |
 | `<dwb plugin>`.debug_trajectory_details | false | Publish debug information |
 | `<dwb plugin>`.trajectory_generator_name | "dwb_plugins::StandardTrajectoryGenerator" | Trajectory generator plugin name |
-| `<dwb plugin>`.goal_checker_name | "dwb_plugins::SimpleGoalChecker" | Goal checker plugin name |
 | `<dwb plugin>`.transform_tolerance | 0.1 | TF transform tolerance |
 | `<dwb plugin>`.short_circuit_trajectory_evaluation | true | Stop evaluating scores after best score is found |
 | `<dwb plugin>`.path_distance_bias | N/A | Old version of `PathAlign.scale`, use that instead |
@@ -350,14 +377,6 @@ When `controller_plugins` parameter is not overridden, the following default plu
 | `<dwb plugin>`.lookahead_time | -1 | If > 0, amount of time to look forward for a collision for. |
 | `<dwb plugin>`.`<name>`.scale | 1.0 | Weight scale |
 
-## simple_goal_checker plugin
-
-| Parameter | Default | Description |
-| ----------| --------| ------------|
-| `<dwb plugin>`.xy_goal_tolerance | 0.25 | Tolerance to meet goal completion criteria (m) |
-| `<dwb plugin>`.yaw_goal_tolerance | 0.25 | Tolerance to meet goal completion criteria (rad) |
-| `<dwb plugin>`.stateful | true | Whether to check for XY position tolerance after rotating to goal orientation in case of minor localization changes |
-
 ## standard_traj_generator plugin
 
 | Parameter | Default | Description |
@@ -375,13 +394,6 @@ When `controller_plugins` parameter is not overridden, the following default plu
 | ----------| --------| ------------|
 | `<dwb plugin>`.sim_time | N/A | Time to simulate ahead by (s) |
 
-## stopped_goal_checker plugin
-
-| Parameter | Default | Description |
-| ----------| --------| ------------|
-| `<dwb plugin>`.rot_stopped_velocity | 0.25 | Velocity below is considered to be stopped at tolerance met (rad/s) |
-| `<dwb plugin>`.trans_stopped_velocity | 0.25 | Velocity below is considered to be stopped at tolerance met (m/s) |
-
 # lifecycle_manager
 
 | Parameter | Default | Description |
@@ -547,6 +559,7 @@ When `recovery_plugins` parameter is not overridden, the following default plugi
 | z_rand | 0.5 | Mixture weight for z_rand part of model, sum of all used z weight must be 1. Beam uses all 4, likelihood model uses z_hit and z_rand. |
 | z_short | 0.05 | Mixture weight for z_short part of model, sum of all used z weight must be 1. Beam uses all 4, likelihood model uses z_hit and z_rand. |
 | always_reset_initial_pose | false | Requires that AMCL is provided an initial pose either via topic or initial_pose* parameter (with parameter set_initial_pose: true) when reset. Otherwise, by default AMCL will use the last known pose to initialize |
+| scan_topic | scan | Topic to subscribe to in order to receive the laser scan for localization |
 
 ---
 
@@ -624,6 +637,14 @@ When `recovery_plugins` parameter is not overridden, the following default plugi
 | server_name | N/A | Action server name |
 | server_timeout | 10 | Action server timeout (ms) |
 
+### BT Node TruncatePath
+
+| Input Port | Default | Description |
+| ---------- | ------- | ----------- |
+| input_path | N/A | Path to be truncated |
+| output_path | N/A | Path truncated |
+| distance | 1.0 | Distance (m) to cut from last pose |
+
 ## Conditions
 
 ### BT Node GoalReached
@@ -674,3 +695,10 @@ When `recovery_plugins` parameter is not overridden, the following default plugi
 | min_speed | 0.0 | Minimum speed (m/s) |
 | max_speed | 0.5 | Maximum speed (m/s) |
 | filter_duration | 0.3 | Duration (secs) for velocity smoothing filter |
+
+### BT Node GoalUpdater
+
+| Input Port | Default | Description |
+| ---------- | ------- | ----------- |
+| input_goal | N/A | The reference goal |
+| output_goal | N/A | The reference goal, or a newer goal received by subscription |

nav2_amcl/include/nav2_amcl/amcl_node.hpp

@@ -167,7 +167,6 @@ class AmclNode : public nav2_util::LifecycleNode
 
   // Laser scan related
   void initLaserScan();
-  const char * scan_topic_{"scan"};
   nav2_amcl::Laser * createLaserObject();
   int scan_error_count_{0};
   std::vector<nav2_amcl::Laser *> lasers_;
@@ -250,6 +249,7 @@ class AmclNode : public nav2_util::LifecycleNode
   double z_max_;
   double z_short_;
   double z_rand_;
+  std::string scan_topic_{"scan"};
 };
 
 }  // namespace nav2_amcl

nav2_amcl/src/amcl_node.cpp

@@ -213,6 +213,10 @@ AmclNode::AmclNode()
     "Requires that AMCL is provided an initial pose either via topic or initial_pose* parameter "
     "(with parameter set_initial_pose: true) when reset. Otherwise, by default AMCL will use the"
     "last known pose to initialize");
+
+  add_parameter(
+    "scan_topic", rclcpp::ParameterValue("scan"),
+    "Topic to subscribe to in order to receive the laser scan for localization");
 }
 
 AmclNode::~AmclNode()
@@ -1098,6 +1102,7 @@ AmclNode::initParameters()
   get_parameter("z_short", z_short_);
   get_parameter("first_map_only_", first_map_only_);
   get_parameter("always_reset_initial_pose", always_reset_initial_pose_);
+  get_parameter("scan_topic", scan_topic_);
 
   save_pose_period_ = tf2::durationFromSec(1.0 / save_pose_rate);
   transform_tolerance_ = tf2::durationFromSec(tmp_tol);

nav2_behavior_tree/CMakeLists.txt

@@ -101,6 +101,12 @@ list(APPEND plugin_libs nav2_distance_controller_bt_node)
 add_library(nav2_speed_controller_bt_node SHARED plugins/decorator/speed_controller.cpp)
 list(APPEND plugin_libs nav2_speed_controller_bt_node)
 
+add_library(nav2_truncate_path_action_bt_node SHARED plugins/action/truncate_path_action.cpp)
+list(APPEND plugin_libs nav2_truncate_path_action_bt_node)
+
+add_library(nav2_goal_updater_node_bt_node SHARED plugins/decorator/goal_updater_node.cpp)
+list(APPEND plugin_libs nav2_goal_updater_node_bt_node)
+
 add_library(nav2_recovery_node_bt_node SHARED plugins/control/recovery_node.cpp)
 list(APPEND plugin_libs nav2_recovery_node_bt_node)
 

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/truncate_path_action.hpp

@@ -0,0 +1,55 @@
+// Copyright (c) 2018 Intel Corporation
+// Copyright (c) 2020 Francisco Martin Rico
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__TRUNCATE_PATH_ACTION_HPP_
+#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__TRUNCATE_PATH_ACTION_HPP_
+
+#include <memory>
+#include <string>
+
+#include "nav_msgs/msg/path.hpp"
+
+#include "behaviortree_cpp_v3/action_node.h"
+
+namespace nav2_behavior_tree
+{
+
+class TruncatePath : public BT::ActionNodeBase
+{
+public:
+  TruncatePath(
+    const std::string & xml_tag_name,
+    const BT::NodeConfiguration & conf);
+
+
+  static BT::PortsList providedPorts()
+  {
+    return {
+      BT::InputPort<nav_msgs::msg::Path>("input_path", "Original Path"),
+      BT::OutputPort<nav_msgs::msg::Path>("output_path", "Path truncated to a certain distance"),
+      BT::InputPort<double>("distance", 1.0, "distance"),
+    };
+  }
+
+private:
+  void halt() override {}
+  BT::NodeStatus tick() override;
+
+  double distance_;
+};
+
+}  // namespace nav2_behavior_tree
+
+#endif  // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__TRUNCATE_PATH_ACTION_HPP_

nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp

@@ -0,0 +1,61 @@
+// Copyright (c) 2018 Intel Corporation
+// Copyright (c) 2020 Francisco Martin Rico
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__GOAL_UPDATER_NODE_HPP_
+#define NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__GOAL_UPDATER_NODE_HPP_
+
+#include <memory>
+#include <string>
+
+#include "geometry_msgs/msg/pose_stamped.hpp"
+
+#include "behaviortree_cpp_v3/decorator_node.h"
+
+#include "rclcpp/rclcpp.hpp"
+
+namespace nav2_behavior_tree
+{
+
+class GoalUpdater : public BT::DecoratorNode
+{
+public:
+  GoalUpdater(
+    const std::string & xml_tag_name,
+    const BT::NodeConfiguration & conf);
+
+
+  static BT::PortsList providedPorts()
+  {
+    return {
+      BT::InputPort<geometry_msgs::msg::PoseStamped>("input_goal", "Original Goal"),
+      BT::OutputPort<geometry_msgs::msg::PoseStamped>(
+        "output_goal",
+        "Received Goal by subscription"),
+    };
+  }
+
+private:
+  BT::NodeStatus tick() override;
+
+  void callback_updated_goal(const geometry_msgs::msg::PoseStamped::SharedPtr msg);
+
+  rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr goal_sub_;
+
+  geometry_msgs::msg::PoseStamped last_goal_received_;
+};
+
+}  // namespace nav2_behavior_tree
+
+#endif  // NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__GOAL_UPDATER_NODE_HPP_