Foxy sync 2 (July 1 to August 7) (#1932)

* Add slam toolbox as exec dep for nav2_bringup (#1827) * Add slam toolbox as exec dep * Added slam toolbox * Changed version of slam toolbox to foxy-devel * Added modifications to allow for exec depend of slam toolbox without breaking docker / CI * reformatted skip keys * Added tabs to skip key arguments * Removed commented out code block * Add unit tests for follow path, compute path to pose, and navigate to pose BT action nodes (#1844) * Add compute path to pose unit tests Signed-off-by: Sarthak Mittal <sarthakmittal2608@gmail.com> * Add follow path action unit tests Signed-off-by: Sarthak Mittal <sarthakmittal2608@gmail.com> * Add navigate to pose action unit tests Signed-off-by: Sarthak Mittal <sarthakmittal2608@gmail.com> * adding foxy build icons to readme (#1853) * adding foxy build icons * adding dividers * sync master from foxy version updates (#1852) * sync master from foxy version updates * syncing foxy and master * Add unit tests for clear entire costmap and reinitialize global localization BT service nodes (#1845) * Add clear entire costmap service unit tests Signed-off-by: Sarthak Mittal <sarthakmittal2608@gmail.com> * Add reinitialize global localization service unit test Signed-off-by: Sarthak Mittal <sarthakmittal2608@gmail.com> * Temporarily disable dump_params test (#1856) Signed-off-by: Sarthak Mittal <sarthakmittal2608@gmail.com> * commenting out unused validPointPotential (#1854) * Adding ROS2 versions to issue template (#1861) * reload behavior tree before creating RosTopicLogger to prevent nullptr error or no /behavior_tree_log problem (#1849) Signed-off-by: Daisuke Sato <daisukes@cmu.edu> * Add citation for IROS paper (#1867) * Add citation We'll want to add the DOI once published * Bump citation section above build status * Fix line breaks * Changes RCUTILS_CONSOLE_STDOUT_LINE_BUFFERED to RCUTILS_LOGGING_BUFFERED_STREAM (#1868) * Added parameters to configure amcl laser scan topic (#1870) * Added parameters to configure amcl topic * changed parameter to scan_topic and added to all the configuration files * added scan_topic amcl param to docs * Satisfied linter * Move dwb goal/progress checker plugins to nav2_controller (#1857) * Move dwb goal/progress checker plugins to nav2_controller Signed-off-by: Siddarth Gore <siddarth.gore@gmail.com> * Move goal/progress checker plugins to nav2_controller Address review comments Signed-off-by: Siddarth Gore <siddarth.gore@gmail.com> * Move goal/progress checker plugins to nav2_controller Use new plugin declaration format and doc update Signed-off-by: Siddarth Gore <siddarth.gore@gmail.com> * Update bringup.yaml for new plugins in nav2_controller Also remove redundent file from dwb_plugins Signed-off-by: Siddarth Gore <siddarth.gore@gmail.com> * Fix doc errors and update remaining yaml files Signed-off-by: Siddarth Gore <siddarth.gore@gmail.com> * Remove mention of goal_checker from dwb docs Signed-off-by: Siddarth Gore <siddarth.gore@gmail.com> * Add .plugin params to doc Signed-off-by: Siddarth Gore <siddarth.gore@gmail.com> * Tests for progress_checker plugin declare .plugin only if not declared before Signed-off-by: Siddarth Gore <siddarth.gore@gmail.com> * Tweak plugin names/description in doc Signed-off-by: Siddarth Gore <siddarth.gore@gmail.com> * Follow pose (#1859) * Truncate path finished Signed-off-by: Francisco Martin Rico <fmrico@gmail.com> * Follow Pose finished Signed-off-by: Francisco Martin Rico <fmrico@gmail.com> * Change names. Add test and doc Signed-off-by: Francisco Martin Rico <fmrico@gmail.com> * Change names and check atan2 values Signed-off-by: Francisco Martin Rico <fmrico@gmail.com> * Checking Inf/NaNs and trucate path changes Signed-off-by: Francisco Martin Rico <fmrico@gmail.com> * Revert changes in launcher Signed-off-by: Francisco Martin Rico <fmrico@gmail.com> * Documenting Tree Signed-off-by: Francisco Martin Rico <fmrico@gmail.com> * Update nav2_bt_navigator/README.md Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> * Update nav2_bt_navigator/README.md Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> * Change TruncatePath from decorator to action node Signed-off-by: Francisco Martin Rico <fmrico@gmail.com> * Update nav2_bt_navigator/README.md Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> * Remove Deprecated Declaration (#1884) * Remove deprecated rclcpp::executor::FutureReturnCode::SUCCESS in favor of rclcpp::FutureReturnCode::SUCCESS Signed-off-by: Hunter L. Allen <hunter.allen@ghostrobotics.io> * Update nav2_util/include/nav2_util/service_client.hpp Co-authored-by: Sarthak Mittal <sarthakmittal2608@gmail.com> Co-authored-by: Sarthak Mittal <sarthakmittal2608@gmail.com> * Added new costmap buffer to resize map safely (#1837) * Added new costmap buffer to resize map safely Signed-off-by: Aitor Miguel Blanco <aitormibl@gmail.com> * Update map if the layer is not being processed. Signed-off-by: Aitor Miguel Blanco <aitormibl@gmail.com> * Updated bool name and added buffer initialization Signed-off-by: Aitor Miguel Blanco <aitormibl@gmail.com> * Fix dump params tests (#1886) Signed-off-by: Sarthak Mittal <sarthakmittal2608@gmail.com> * Move definitions in tf_help to separate src cpp file (#1890) * Move definitions of functions in tf_help to separate src cpp file to avoid multiple definition error * Fix linting issues * Make uncrustify happier * More format fixing * resolved the simulated motion into its x & y components (#1887) Signed-off-by: Marwan Taher <marokhaled99@gmail.com> * Fix nav2_bt_navigator cleanup (#1901) Signed-off-by: Sarthak Mittal <sarthakmittal2608@gmail.com> * Adding some more test coverage (#1903) * more tests * removing old cmake * remove on_errors in specific servers in favor of a general lifecycle warning (#1904) * remove on_errors in favor of a general lifecycle warning * adding removed thing * simply nodes to remove lines (#1907) * Bunches of random new tests (#1909) * trying to get A* to work again * make flake 8 happy * adding cancel and preempt test * planner tests use A* * adding A* note * test with topic * Adding failure to navigate test (#1912) * failures tests * adding copyrights * cancel test in recoveries * Costmap lock while copying data in navfn planner (#1913) * acquire the costmap lock while copying data in navfn planner Signed-off-by: Daisuke Sato <daisukes@cmu.edu> * add costmap lock to dwb local plannger Signed-off-by: Daisuke Sato <daisukes@cmu.edu> * Added map_topic parameters to static layer and amcl (#1910) * see if spin some in cancel will allow result callback (#1914) * Adding near-complete voxel grid test coverage and more to controller server (#1915) * remove erraneous handling done by prior * adding a bunch of voxel unit tests * retrigger * adding waypoint follower failure test, voxel layer integration tests, etc (#1916) * adding waypoint follower failure test * adding voxel, more logging * reset -> clear * minor changes to lower redundent warnings (#1918) * Costmap plugins declare if not declared for reset capabilities (#1919) * fixing #1917 on declare if not declared * fix API * adding CLI test (#1920) * More coverage in map server tests (#1921) * adding CLI test * adding a bunch of new coverages for map_server * Add in range sensor costmap layer (#1888) * range costmap building * range sensor layer working * nav2 costmap pass linter and uncrustify tests * Added back semicolon to range class * Added docs * Added angles dependency * Added BSD license * Added BSD license * Made functions inline * revmoed get_clock * added input_sensor_type to docs * Made defualt topic name empty to cause error * using chorno literal to denote time units * Added small initial test * Fixed linter error, line breaks, enum, logger level, and transform_tolerance issue * fixed segmentation fault in test and added transfrom tolerances to tf_->transform * Got test to pass * Added more tests * removed incorrect parameter declaration * Improved marking while moving tests and added clearing tests * removed general clearing test * changed testing helper import in test * [Test sprint] push nav2_map_server over 90% (#1922) * adding CLI test * adding tests for more lines to map_server * fix last test * adding out of bounds and higher bounds checks * adding planner tests for plugins working * add cleanup * working * ping * [testing sprint] final test coverage and debug logging in planner/controller servers (#1924) * adding CLI test * adding tests for more lines to map_server * fix last test * adding out of bounds and higher bounds checks * adding planner tests for plugins working * add cleanup * working * ping * adding more testing and debug info messages * [testing sprint] remove dead code not used in 10 years from navfn (#1926) * remove dead code not used in 10 years from navfn * ping * inverting period to rate * removing debug statement that could never be triggered by a single non-looping action server * type change * adding removed files * bump to 0.4.2 * add another missing file * add missing files * remove some tests * lint * removing nav2_bringup from binaries Co-authored-by: Michael Equi <32988490+Michael-Equi@users.noreply.github.com> Co-authored-by: Sarthak Mittal <sarthakmittal2608@gmail.com> Co-authored-by: Daisuke Sato <43101027+daisukes@users.noreply.github.com> Co-authored-by: Ruffin <roxfoxpox@gmail.com> Co-authored-by: Siddarth Gore <siddarth.gore@gmail.com> Co-authored-by: Francisco Martín Rico <fmrico@gmail.com> Co-authored-by: Hunter L. Allen <hunter.allen@ghostrobotics.io> Co-authored-by: Aitor Miguel Blanco <aitormibl@gmail.com> Co-authored-by: Joe Smallman <ijsmallman@gmail.com> Co-authored-by: Marwan Taher <marokhaled99@gmail.com>
ros-navigation · Aug 11, 2020 · 671dde6 · 671dde6
1 parent aa9394a
commit 671dde6
Show file tree

Hide file tree

Showing 168 changed files with 5,823 additions and 937 deletions.
diff --git a/.circleci/config.yml b/.circleci/config.yml
@@ -75,6 +75,9 @@ references:
                 rosdep install -q -y \
                   --from-paths src \
                   --ignore-src \
+                  --skip-keys " \
+                    slam_toolbox \
+                    " \
                   --verbose | \
                 awk '$1 ~ /^resolution\:/' | \
                 awk -F'[][]' '{print $2}' | \

diff --git a/.github/ISSUE_TEMPLATE.md b/.github/ISSUE_TEMPLATE.md
@@ -11,6 +11,8 @@ For Bug report or feature requests, please fill out the relevant category below
 
 - Operating System:
   - <!-- OS and version (e.g. Windows 10, Ubuntu 16.04...) -->
+- ROS2 Version:
+  - <!-- ROS2 distribution and install method (e.g. Foxy binaries, Dashing source...) -->
 - Version or commit hash:
   - <!-- from source: output of `git -C navigation2 rev-parse HEAD
          apt binaries: output of: dpkg-query --show "ros-$ROS_DISTRO-navigation2"

diff --git a/Dockerfile b/Dockerfile
@@ -50,6 +50,9 @@ COPY --from=cacher /tmp/$UNDERLAY_WS ./
 RUN . /opt/ros/$ROS_DISTRO/setup.sh && \
     apt-get update && rosdep install -q -y \
       --from-paths src \
+      --skip-keys " \
+        slam_toolbox \
+        " \
       --ignore-src \
     && rm -rf /var/lib/apt/lists/*
 
@@ -75,6 +78,9 @@ RUN . $UNDERLAY_WS/install/setup.sh && \
     apt-get update && rosdep install -q -y \
       --from-paths src \
         $UNDERLAY_WS/src \
+      --skip-keys " \
+        slam_toolbox \
+        "\
       --ignore-src \
     && rm -rf /var/lib/apt/lists/*
 

diff --git a/README.md b/README.md
diff --git a/doc/parameters/param_list.md b/doc/parameters/param_list.md
@@ -51,11 +51,13 @@ For example:
 ```yaml
 local_costmap:
   ros__parameters:
-    plugins: ["obstacle_layer", "voxel_layer", "inflation_layer"]
+    plugins: ["obstacle_layer", "voxel_layer", "sonar_layer", "inflation_layer"]
     obstacle_layer:
       plugin: "nav2_costmap_2d::ObstacleLayer"
     voxel_layer:
       plugin: "nav2_costmap_2d::VoxelLayer"
+    sonar_layer:
+      plugin: "nav2_costmap_2d::RangeSensorLayer"
     inflation_layer:
       plugin: "nav2_costmap_2d::InflationLayer"
 ```
@@ -78,6 +80,7 @@ When `plugins` parameter is not overridden, the following default plugins are lo
 | `<static layer>`.subscribe_to_updates | false | Subscribe to static map updates after receiving first |
 | `<static layer>`.map_subscribe_transient_local | true | QoS settings for map topic |
 | `<static layer>`.transform_tolerance | 0.0 | TF tolerance |
+| `<static layer>`.map_topic | "" | Name of the map topic to subscribe to (empty means use the map_topic defined by `costmap_2d_ros`) |
 
 ## inflation_layer plugin
 
@@ -114,7 +117,23 @@ When `plugins` parameter is not overridden, the following default plugins are lo
 | `<data source>`.marking | true | Whether source should mark in costmap |
 | `<data source>`.clearing | false | Whether source should raytrace clear in costmap |
 | `<data source>`.obstacle_range | 2.5 | Maximum range to mark obstacles in costmap |
-| `<data source>`.raytrace_range | 3.0 | Maximum range to raytrace clear obstacles from costmap | 
+| `<data source>`.raytrace_range | 3.0 | Maximum range to raytrace clear obstacles from costmap |
+
+## range_sensor_layer plugin
+
+* `<range layer>`: Name corresponding to the `nav2_costmap_2d::RangeSensorLayer` plugin. This name gets defined in `plugin_names`.
+
+| Parameter | Default | Description |
+| ----------| --------| ------------|
+| `<range layer>`.enabled | true | Whether it is enabled |
+| `<range layer>`.topics | [""] | Range topics to subscribe to |
+| `<range layer>`.phi | 1.2 | Phi value |
+| `<range layer>`.inflate_cone | 1.0 | Inflate the triangular area covered by the sensor (percentage) |
+| `<range layer>`.no_readings_timeout | 0.0 | No Readings Timeout |
+| `<range layer>`.clear_threshold | 0.2 | Probability below which cells are marked as free |
+| `<range layer>`.mark_threshold | 0.8 | Probability above which cells are marked as occupied |
+| `<range layer>`.clear_on_max_reading | false | Clear on max reading |
+| `<range layer>`.input_sensor_type | ALL | Input sensor type either ALL (automatic selection), VARIABLE (min range != max range), or FIXED (min range == max range) |
 
 ## voxel_layer plugin
 
@@ -154,12 +173,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 +195,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 +240,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 +396,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 +413,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 +578,8 @@ 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 |
+| map_topic | map | Topic to subscribe to in order to receive the map for localization |
 
 ---
 
@@ -624,6 +657,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 +715,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 |
diff --git a/nav2_amcl/include/nav2_amcl/amcl_node.hpp b/nav2_amcl/include/nav2_amcl/amcl_node.hpp
@@ -66,7 +66,6 @@ class AmclNode : public nav2_util::LifecycleNode
   nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override;
   nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override;
   nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override;
-  nav2_util::CallbackReturn on_error(const rclcpp_lifecycle::State & state) override;
 
   // Since the sensor data from gazebo or the robot is not lifecycle enabled, we won't
   // respond until we're in the active state
@@ -167,7 +166,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 +248,8 @@ class AmclNode : public nav2_util::LifecycleNode
   double z_max_;
   double z_short_;
   double z_rand_;
+  std::string scan_topic_{"scan"};
+  std::string map_topic_{"map"};
 };
 
 }  // namespace nav2_amcl

diff --git a/nav2_amcl/package.xml b/nav2_amcl/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_amcl</name>
-  <version>0.4.1</version>
+  <version>0.4.2</version>
   <description>
     <p>
       amcl is a probabilistic localization system for a robot moving in

diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp
@@ -213,6 +213,14 @@ 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");
+
+  add_parameter(
+    "map_topic", rclcpp::ParameterValue("map"),
+    "Topic to subscribe to in order to receive the map to localize on");
 }
 
 AmclNode::~AmclNode()
@@ -379,13 +387,6 @@ AmclNode::on_cleanup(const rclcpp_lifecycle::State & /*state*/)
   return nav2_util::CallbackReturn::SUCCESS;
 }
 
-nav2_util::CallbackReturn
-AmclNode::on_error(const rclcpp_lifecycle::State & /*state*/)
-{
-  RCLCPP_FATAL(get_logger(), "Lifecycle node entered error state");
-  return nav2_util::CallbackReturn::SUCCESS;
-}
-
 nav2_util::CallbackReturn
 AmclNode::on_shutdown(const rclcpp_lifecycle::State & /*state*/)
 {
@@ -1098,6 +1099,8 @@ 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_);
+  get_parameter("map_topic", map_topic_);
 
   save_pose_period_ = tf2::durationFromSec(1.0 / save_pose_rate);
   transform_tolerance_ = tf2::durationFromSec(tmp_tol);
@@ -1273,7 +1276,7 @@ AmclNode::initPubSub()
     std::bind(&AmclNode::initialPoseReceived, this, std::placeholders::_1));
 
   map_sub_ = create_subscription<nav_msgs::msg::OccupancyGrid>(
-    "map", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(),
+    map_topic_, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(),
     std::bind(&AmclNode::mapReceived, this, std::placeholders::_1));
 
   RCLCPP_INFO(get_logger(), "Subscribed to map topic.");

diff --git a/nav2_behavior_tree/CMakeLists.txt b/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)
 

diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/back_up_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/back_up_action.hpp
@@ -0,0 +1,48 @@
+// Copyright (c) 2018 Intel Corporation
+//
+// 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__BACK_UP_ACTION_HPP_
+#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__BACK_UP_ACTION_HPP_
+
+#include <string>
+
+#include "nav2_behavior_tree/bt_action_node.hpp"
+#include "nav2_msgs/action/back_up.hpp"
+
+namespace nav2_behavior_tree
+{
+
+class BackUpAction : public BtActionNode<nav2_msgs::action::BackUp>
+{
+public:
+  BackUpAction(
+    const std::string & xml_tag_name,
+    const std::string & action_name,
+    const BT::NodeConfiguration & conf);
+
+  void on_tick() override;
+
+  static BT::PortsList providedPorts()
+  {
+    return providedBasicPorts(
+      {
+        BT::InputPort<double>("backup_dist", 0.15, "Distance to backup"),
+        BT::InputPort<double>("backup_speed", 0.025, "Speed at which to backup")
+      });
+  }
+};
+
+}  // namespace nav2_behavior_tree
+
+#endif  // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__BACK_UP_ACTION_HPP_
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/clear_costmap_service.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/clear_costmap_service.hpp
@@ -0,0 +1,38 @@
+// Copyright (c) 2019 Samsung Research America
+//
+// 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__CLEAR_COSTMAP_SERVICE_HPP_
+#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__CLEAR_COSTMAP_SERVICE_HPP_
+
+#include <string>
+
+#include "nav2_behavior_tree/bt_service_node.hpp"
+#include "nav2_msgs/srv/clear_entire_costmap.hpp"
+
+namespace nav2_behavior_tree
+{
+
+class ClearEntireCostmapService : public BtServiceNode<nav2_msgs::srv::ClearEntireCostmap>
+{
+public:
+  ClearEntireCostmapService(
+    const std::string & service_node_name,
+    const BT::NodeConfiguration & conf);
+
+  void on_tick() override;
+};
+
+}  // namespace nav2_behavior_tree
+
+#endif  // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__CLEAR_COSTMAP_SERVICE_HPP_