Merge remote-tracking branch 'origin/master' into ci-docs

ros-navigation · Aug 11, 2020 · a8df3f0 · a8df3f0
2 parents afc973b + 4c9b43f
commit a8df3f0
Show file tree

Hide file tree

Showing 139 changed files with 4,480 additions and 772 deletions.
diff --git a/.circleci/config.yml b/.circleci/config.yml
@@ -2,17 +2,7 @@ version: 2.1
 orbs:
   codecov: codecov/[email protected]
 
-references:
-  common_environment: &common_environment
-    ROS_WS: "/opt/ros_ws"
-    UNDERLAY_WS: "/opt/underlay_ws"
-    OVERLAY_WS: "/opt/overlay_ws"
-    CCACHE_LOGFILE: "/tmp/ccache.log"
-    CCACHE_MAXSIZE: "200M"
-    MAKEFLAGS: "-j 1 -l 2"
-    RCUTILS_CONSOLE_STDOUT_LINE_BUFFERED: "1"
-    RETEST_UNTIL_PASS: "2"
-    DEBIAN_FRONTEND: "noninteractive"
+_commands:
   common_commands: &common_commands
     restore_from_cache:
       description: "Restore From Cache"
@@ -155,11 +145,8 @@ references:
                     --show-counts | \
                   xargs)
               set -o xtrace
-              colcon test \
-                --packages-select ${TEST_PACKAGES}
               colcon test \
                 --packages-select ${TEST_PACKAGES} \
-                --packages-select-test-failures \
                 --retest-until-pass ${RETEST_UNTIL_PASS} \
                 --ctest-args --test-regex "test_.*"
               colcon test-result \
@@ -182,6 +169,8 @@ references:
             path: << parameters.workspace >>/test_results
         - store_artifacts:
             path: << parameters.workspace >>/test_results
+
+_steps:
   pre_checkout: &pre_checkout
     run:
       name: Pre Checkout
@@ -195,6 +184,7 @@ references:
           (echo ros_entrypoint && cat) >> checksum.txt
         sha256sum $PWD/checksum.txt >> checksum.txt
         rm -rf $OVERLAY_WS/*
+        mv ~/.ccache /mnt/ramdisk/.ccache
   on_checkout: &on_checkout
     checkout:
       path: src/navigation2
@@ -261,7 +251,7 @@ references:
     save_to_cache:
       key: ccache
       workspace: /opt/underlay_ws
-      path: ~/.ccache
+      path: /mnt/ramdisk/.ccache
       when: always
   test_overlay_workspace: &test_overlay_workspace
     test_workspace:
@@ -320,6 +310,19 @@ commands:
       - *collect_overlay_coverage
       - *upload_overlay_coverage
 
+_environments:
+  common_environment: &common_environment
+    ROS_WS: "/opt/ros_ws"
+    UNDERLAY_WS: "/opt/underlay_ws"
+    OVERLAY_WS: "/opt/overlay_ws"
+    CCACHE_DIR: "/mnt/ramdisk/.ccache"
+    CCACHE_LOGFILE: "/tmp/ccache.log"
+    CCACHE_MAXSIZE: "200M"
+    MAKEFLAGS: "-j 1 -l 2"
+    RCUTILS_CONSOLE_STDOUT_LINE_BUFFERED: "0"
+    RETEST_UNTIL_PASS: "2"
+    DEBIAN_FRONTEND: "noninteractive"
+
 executors:
   debug_exec:
     docker:
@@ -340,6 +343,16 @@ executors:
       OVERLAY_MIXINS: "release ccache"
       UNDERLAY_MIXINS: "release ccache"
 
+_jobs:
+  job_test: &job_test
+    parameters:
+      rmw:
+        default: "rmw_fastrtps_cpp"
+        type: string
+    parallelism: 1
+    environment:
+      RMW_IMPLEMENTATION: << parameters.rmw >>
+
 jobs:
   debug_build: &debug_build
     executor: debug_exec
@@ -351,30 +364,18 @@ jobs:
     executor: release_exec
     <<: *debug_build
   debug_test:
+    <<: *job_test
     executor: debug_exec
-    parallelism: 1
     steps:
       - restore_build
       - test_build
       - report_coverage
   release_test: &release_test
+    <<: *job_test
     executor: release_exec
-    parallelism: 1
     steps:
       - restore_build
       - test_build
-  test_rmw_connext_cpp:
-    <<: *release_test
-    environment:
-      RMW_IMPLEMENTATION: "rmw_connext_cpp"
-  test_rmw_cyclonedds_cpp:
-    <<: *release_test
-    environment:
-      RMW_IMPLEMENTATION: "rmw_cyclonedds_cpp"
-  test_rmw_fastrtps_cpp:
-    <<: *release_test
-    environment:
-      RMW_IMPLEMENTATION: "rmw_fastrtps_cpp"
 
 workflows:
   version: 2
@@ -395,15 +396,15 @@ workflows:
           requires:
             - debug_build
       - release_build
-      # - test_rmw_connext_cpp:
-      #     requires:
-      #       - release_build
-      - test_rmw_cyclonedds_cpp:
-          requires:
-            - release_build
-      - test_rmw_fastrtps_cpp:
+      - release_test:
           requires:
             - release_build
+          matrix:
+            parameters:
+              rmw:
+                # - rmw_connext_cpp
+                - rmw_cyclonedds_cpp
+                - rmw_fastrtps_cpp
     triggers:
       - schedule:
           cron: "0 0 * * *"

diff --git a/README.md b/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 |

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,19 +413,13 @@ 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 |
 | ----------| --------| ------------|
 | node_names | N/A | Ordered list of node names to bringup through lifecycle transition |
 | autostart | false | Whether to transition nodes to active state on startup |
+| bond_timeout_ms | 4000 | Timeout for bond to fail if no heartbeat can be found, in milliseconds. If set to 0, it will be disabled. Must be larger than 300ms for stable bringup. |
 
 # map_server
 
@@ -547,6 +579,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 |
 
 ---
 
@@ -596,8 +630,7 @@ When `recovery_plugins` parameter is not overridden, the following default plugi
 
 | Input Port | Default | Description |
 | ---------- | ------- | ----------- |
-| position | N/A | Position |
-| orientation | N/A | Orientation |
+| goal | N/A | Goal |
 | server_name | N/A | Action server name |
 | server_timeout | 10 | Action server timeout (ms) |
 
@@ -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