Merge remote-tracking branch 'origin/main' into (binary-filter)-chang…

…e-boolean-ros-params
ros-navigation · May 19, 2024 · 63dd864 · 63dd864
2 parents aa40776 + bf291d7
commit 63dd864
Show file tree

Hide file tree

Showing 128 changed files with 2,531 additions and 405 deletions.
diff --git a/.devcontainer/caddy/srv/nav2/index.md b/.devcontainer/caddy/srv/nav2/index.md
@@ -14,9 +14,9 @@
 
 For more related documentation:
 
-- [Nav2 Documentation](https://navigation.ros.org)
-  - [Development Guides](https://navigation.ros.org/development_guides)
-    - [Dev Containers](https://navigation.ros.org/development_guides/devcontainer_docs)
+- [Nav2 Documentation](https://docs.nav2.org)
+  - [Development Guides](https://docs.nav2.org/development_guides)
+    - [Dev Containers](https://docs.nav2.org/development_guides/devcontainer_docs)
 
 ## Session Info
 

diff --git a/.github/PULL_REQUEST_TEMPLATE.md b/.github/PULL_REQUEST_TEMPLATE.md
@@ -38,7 +38,7 @@
 -->
 
 #### For Maintainers: <!-- DO NOT EDIT OR REMOVE -->
-- [ ] Check that any new parameters added are updated in navigation.ros.org
+- [ ] Check that any new parameters added are updated in docs.nav2.org
 - [ ] Check that any significant change is added to the migration guide
 - [ ] Check that any new features **OR** changes to existing behaviors are reflected in the tuning guide
 - [ ] Check that any new functions have Doxygen added

diff --git a/.github/mergify.yml b/.github/mergify.yml
@@ -83,7 +83,7 @@ pull_request_rules:
         comment:
           message: |
             @{{author}}, please properly fill in PR template in the future. @stevemacenski, use this instead.
-            - [ ] Check that any new parameters added are updated in navigation.ros.org
+            - [ ] Check that any new parameters added are updated in docs.nav2.org
             - [ ] Check that any significant change is added to the migration guide
             - [ ] Check that any new features **OR** changes to existing behaviors are reflected in the tuning guide
             - [ ] Check that any new functions have Doxygen added

diff --git a/README.md b/README.md
@@ -1,24 +1,24 @@
 # Nav2
-[![GitHub Workflow Status](https://github.com/ros-planning/navigation2/actions/workflows/update_ci_image.yaml/badge.svg)](https://github.com/ros-planning/navigation2/actions/workflows/update_ci_image.yaml)
-[![codecov](https://codecov.io/gh/ros-planning/navigation2/branch/main/graph/badge.svg?token=S3iRmypwlg)](https://codecov.io/gh/ros-planning/navigation2)
+[![GitHub Workflow Status](https://github.com/ros-navigation/navigation2/actions/workflows/update_ci_image.yaml/badge.svg)](https://github.com/ros-navigation/navigation2/actions/workflows/update_ci_image.yaml)
+[![codecov](https://codecov.io/gh/ros-navigation/navigation2/branch/main/graph/badge.svg?token=S3iRmypwlg)](https://codecov.io/gh/ros-navigation/navigation2)
 
 <p align="center">
   <img height="300" src="doc/nav2_logo.png" />
 </p>
 
 For detailed instructions on how to:
-- [Getting Started](https://navigation.ros.org/getting_started/index.html)
-- [Concepts](https://navigation.ros.org/concepts/index.html)
-- [Build](https://navigation.ros.org/development_guides/build_docs/index.html#build)
-- [Install](https://navigation.ros.org/development_guides/build_docs/index.html#install)
-- [General Tutorials](https://navigation.ros.org/tutorials/index.html) and [Algorithm Developer Tutorials](https://navigation.ros.org/plugin_tutorials/index.html)
-- [Configure](https://navigation.ros.org/configuration/index.html)
-- [Navigation Plugins](https://navigation.ros.org/plugins/index.html)
-- [Migration Guides](https://navigation.ros.org/migration/index.html)
+- [Getting Started](https://docs.nav2.org/getting_started/index.html)
+- [Concepts](https://docs.nav2.org/concepts/index.html)
+- [Build](https://docs.nav2.org/development_guides/build_docs/index.html#build)
+- [Install](https://docs.nav2.org/development_guides/build_docs/index.html#install)
+- [General Tutorials](https://docs.nav2.org/tutorials/index.html) and [Algorithm Developer Tutorials](https://docs.nav2.org/plugin_tutorials/index.html)
+- [Configure](https://docs.nav2.org/configuration/index.html)
+- [Navigation Plugins](https://docs.nav2.org/plugins/index.html)
+- [Migration Guides](https://docs.nav2.org/migration/index.html)
 - [Container Images for Building Nav2](https://github.com/orgs/ros-planning/packages/container/package/navigation2)
-- [Contribute](https://navigation.ros.org/development_guides/involvement_docs/index.html)
+- [Contribute](https://docs.nav2.org/development_guides/involvement_docs/index.html)
 
-Please visit our [documentation site](https://navigation.ros.org/). [Please visit our community Slack here](https://join.slack.com/t/navigation2/shared_invite/zt-hu52lnnq-cKYjuhTY~sEMbZXL8p9tOw) (if this link does not work, please contact maintainers to reactivate).
+Please visit our [documentation site](https://docs.nav2.org/). [Please visit our community Slack here](https://join.slack.com/t/navigation2/shared_invite/zt-hu52lnnq-cKYjuhTY~sEMbZXL8p9tOw) (if this link does not work, please contact maintainers to reactivate).
 
 If you need professional services related to Nav2, please contact Open Navigation at [email protected].
 
@@ -117,7 +117,7 @@ If you use the Regulated Pure Pursuit Controller algorithm or software from this
 | Service | Humble | Iron | Main |
 | :---: | :---: | :---: | :---: |
 | ROS Build Farm | [![Build Status](https://build.ros2.org/job/Hdev__navigation2__ubuntu_jammy_amd64/badge/icon)](https://build.ros2.org/job/Hdev__navigation2__ubuntu_jammy_amd64/) | [![Build Status](https://build.ros2.org/job/Idev__navigation2__ubuntu_jammy_amd64/badge/icon)](https://build.ros2.org/job/Idev__navigation2__ubuntu_jammy_amd64/) | N/A |
-| Circle CI | N/A | N/A | [![Build Status](https://circleci.com/gh/ros-planning/navigation2/tree/main.svg?style=svg)](https://circleci.com/gh/ros-planning/navigation2/tree/main) |
+| Circle CI | N/A | N/A | [![Build Status](https://circleci.com/gh/ros-navigation/navigation2/tree/main.svg?style=svg)](https://circleci.com/gh/ros-navigation/navigation2/tree/main) |
 
 
 | Package | Humble Source | Humble Debian | Iron Source | Iron Debian |

diff --git a/nav2_amcl/README.md b/nav2_amcl/README.md
@@ -1,4 +1,4 @@
 # AMCL
 Adaptive Monte Carlo Localization (AMCL) is a probabilistic localization module which estimates the position and orientation (i.e. Pose) of a robot in a given known map using a 2D laser scanner. This is largely a refactored port from ROS 1 without any algorithmic changes.
 
-See the [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-amcl.html) for more details about configurable settings and their meanings.
+See the [Configuration Guide Page](https://docs.nav2.org/configuration/packages/configuring-amcl.html) for more details about configurable settings and their meanings.
diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp
@@ -49,6 +49,7 @@
 #pragma GCC diagnostic pop
 
 #include "nav2_amcl/portable_utils.hpp"
+#include "nav2_util/validate_messages.hpp"
 
 using namespace std::placeholders;
 using rcl_interfaces::msg::ParameterType;
@@ -523,11 +524,8 @@ AmclNode::initialPoseReceived(geometry_msgs::msg::PoseWithCovarianceStamped::Sha
 
   RCLCPP_INFO(get_logger(), "initialPoseReceived");
 
-  if (msg->header.frame_id == "") {
-    // This should be removed at some point
-    RCLCPP_WARN(
-      get_logger(),
-      "Received initial pose with empty frame_id. You should always supply a frame_id.");
+  if (!nav2_util::validateMsg(*msg)) {
+    RCLCPP_ERROR(get_logger(), "Received initialpose message is malformed. Rejecting.");
     return;
   }
   if (nav2_util::strip_leading_slash(msg->header.frame_id) != global_frame_id_) {
@@ -1390,6 +1388,10 @@ void
 AmclNode::mapReceived(const nav_msgs::msg::OccupancyGrid::SharedPtr msg)
 {
   RCLCPP_DEBUG(get_logger(), "AmclNode: A new map was received.");
+  if (!nav2_util::validateMsg(*msg)) {
+    RCLCPP_ERROR(get_logger(), "Received map message is malformed. Rejecting.");
+    return;
+  }
   if (first_map_only_ && first_map_received_) {
     return;
   }

diff --git a/nav2_amcl/src/pf/CMakeLists.txt b/nav2_amcl/src/pf/CMakeLists.txt
@@ -15,6 +15,7 @@ target_include_directories(pf_lib PRIVATE ../include)
 if(HAVE_DRAND48)
   target_compile_definitions(pf_lib PRIVATE "HAVE_DRAND48")
 endif()
+target_link_libraries(pf_lib m)
 
 install(TARGETS
   pf_lib

diff --git a/nav2_behavior_tree/README.md b/nav2_behavior_tree/README.md
@@ -7,9 +7,9 @@ The nav2_behavior_tree module provides:
 * Navigation-specific behavior tree nodes, and
 * a generic BehaviorTreeEngine class that simplifies the integration of BT processing into ROS2 nodes for navigation or higher-level autonomy applications.
 
-See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-bt-xml.html) for additional parameter descriptions and a list of XML nodes made available in this package. Also review the [Nav2 Behavior Tree Explanation](https://navigation.ros.org/behavior_trees/index.html) pages explaining more context on the default behavior trees and examples provided in this package. A [tutorial](https://navigation.ros.org/plugin_tutorials/docs/writing_new_bt_plugin.html) is also provided to explain how to create a simple BT plugin.
+See its [Configuration Guide Page](https://docs.nav2.org/configuration/packages/configuring-bt-xml.html) for additional parameter descriptions and a list of XML nodes made available in this package. Also review the [Nav2 Behavior Tree Explanation](https://docs.nav2.org/behavior_trees/index.html) pages explaining more context on the default behavior trees and examples provided in this package. A [tutorial](https://docs.nav2.org/plugin_tutorials/docs/writing_new_bt_plugin.html) is also provided to explain how to create a simple BT plugin.
 
-See the [Navigation Plugin list](https://navigation.ros.org/plugins/index.html) for a list of the currently known and available planner plugins. 
+See the [Navigation Plugin list](https://docs.nav2.org/plugins/index.html) for a list of the currently known and available planner plugins. 
 
 ## The bt_action_node Template and the Behavior Tree Engine
 

diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp
@@ -144,8 +144,8 @@ bool BtActionServer<ActionT>::on_configure()
   rclcpp::copy_all_parameter_values(node, client_node_);
 
   // set the timeout in seconds for the action server to discard goal handles if not finished
-  double action_server_result_timeout;
-  node->get_parameter("action_server_result_timeout", action_server_result_timeout);
+  double action_server_result_timeout =
+    node->get_parameter("action_server_result_timeout").as_double();
   rcl_action_server_options_t server_options = rcl_action_server_get_default_options();
   server_options.result_timeout.nanoseconds = RCL_S_TO_NS(action_server_result_timeout);
 

diff --git a/nav2_behavior_tree/plugins/decorator/path_longer_on_approach.cpp b/nav2_behavior_tree/plugins/decorator/path_longer_on_approach.cpp
@@ -36,7 +36,7 @@ bool PathLongerOnApproach::isPathUpdated(
 {
   return new_path != old_path && old_path.poses.size() != 0 &&
          new_path.poses.size() != 0 &&
-         old_path.poses.back() == new_path.poses.back();
+         old_path.poses.back().pose == new_path.poses.back().pose;
 }
 
 bool PathLongerOnApproach::isRobotInGoalProximity(
@@ -62,12 +62,11 @@ inline BT::NodeStatus PathLongerOnApproach::tick()
   getInput("prox_len", prox_len_);
   getInput("length_factor", length_factor_);
 
-  if (!BT::isStatusActive(status())) {
-    // Reset the starting point since we're starting a new iteration of
-    // PathLongerOnApproach (moving from IDLE to RUNNING)
-    first_time_ = true;
+  if (first_time_ == false) {
+    if (old_path_.poses.back() != new_path_.poses.back()) {
+      first_time_ = true;
+    }
   }
-
   setStatus(BT::NodeStatus::RUNNING);
 
   // Check if the path is updated and valid, compare the old and the new path length,

diff --git a/nav2_behaviors/README.md b/nav2_behaviors/README.md
@@ -10,9 +10,9 @@ The only required class a behavior must derive from is the `nav2_core/behavior.h
 
 The value of the centralized behavior server is to **share resources** amongst several behaviors that would otherwise be independent nodes. Subscriptions to TF, costmaps, and more can be quite heavy and add non-trivial compute costs to a robot system. By combining these independent behaviors into a single server, they may share these resources while retaining complete independence in execution and interface.
 
-See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-behavior-server.html) for additional parameter descriptions and a [tutorial about writing behaviors](https://navigation.ros.org/plugin_tutorials/docs/writing_new_behavior_plugin.html).
+See its [Configuration Guide Page](https://docs.nav2.org/configuration/packages/configuring-behavior-server.html) for additional parameter descriptions and a [tutorial about writing behaviors](https://docs.nav2.org/plugin_tutorials/docs/writing_new_behavior_plugin.html).
 
-See the [Navigation Plugin list](https://navigation.ros.org/plugins/index.html) for a list of the currently known and available planner plugins.
+See the [Navigation Plugin list](https://docs.nav2.org/plugins/index.html) for a list of the currently known and available planner plugins.
 
 The `TimedBehavior` template makes use of a [nav2_util::TwistPublisher](../nav2_util/README.md#twist-publisher-and-twist-subscriber-for-commanded-velocities).
 The `AssistedTeleop` behavior makes use of a [nav2_util::TwistSubscriber](../nav2_util/README.md#twist-publisher-and-twist-subscriber-for-commanded-velocities).
diff --git a/nav2_behaviors/src/behavior_server.cpp b/nav2_behaviors/src/behavior_server.cpp
@@ -147,8 +147,8 @@ void BehaviorServer::setupResourcesForBehaviorPlugins()
   get_parameter("global_costmap_topic", global_costmap_topic);
   get_parameter("local_footprint_topic", local_footprint_topic);
   get_parameter("global_footprint_topic", global_footprint_topic);
-  get_parameter("transform_tolerance", transform_tolerance);
   get_parameter("robot_base_frame", robot_base_frame);
+  transform_tolerance = get_parameter("transform_tolerance").as_double();
 
   bool need_local_costmap = false;
   bool need_global_costmap = false;

diff --git a/nav2_bringup/README.md b/nav2_bringup/README.md
@@ -10,7 +10,7 @@ Dynamically composed bringup (based on  [ROS2 Composition](https://docs.ros.org/
 
 * Some discussions about performance improvement of composed bringup could be found here: https://discourse.ros.org/t/nav2-composition/22175.
 
-To use, please see the Nav2 [Getting Started Page](https://navigation.ros.org/getting_started/index.html) on our documentation website. Additional [tutorials will help you](https://navigation.ros.org/tutorials/index.html) go from an initial setup in simulation to testing on a hardware robot, using SLAM, and more.
+To use, please see the Nav2 [Getting Started Page](https://docs.nav2.org/getting_started/index.html) on our documentation website. Additional [tutorials will help you](https://docs.nav2.org/tutorials/index.html) go from an initial setup in simulation to testing on a hardware robot, using SLAM, and more.
 
 Note:
 * gazebo should be started with both libgazebo_ros_init.so and libgazebo_ros_factory.so to work correctly.

diff --git a/nav2_bringup/launch/rviz_launch.py b/nav2_bringup/launch/rviz_launch.py
@@ -34,6 +34,7 @@ def generate_launch_description():
     namespace = LaunchConfiguration('namespace')
     use_namespace = LaunchConfiguration('use_namespace')
     rviz_config_file = LaunchConfiguration('rviz_config')
+    use_sim_time = LaunchConfiguration('use_sim_time')
 
     # Declare the launch arguments
     declare_namespace_cmd = DeclareLaunchArgument(
@@ -57,13 +58,19 @@ def generate_launch_description():
         description='Full path to the RVIZ config file to use',
     )
 
+    declare_use_sim_time_cmd = DeclareLaunchArgument(
+        'use_sim_time',
+        default_value='false',
+        description='Use simulation (Gazebo) clock if true')
+
     # Launch rviz
     start_rviz_cmd = Node(
         condition=UnlessCondition(use_namespace),
         package='rviz2',
         executable='rviz2',
         arguments=['-d', rviz_config_file],
         output='screen',
+        parameters=[{'use_sim_time': use_sim_time}],
     )
 
     namespaced_rviz_config_file = ReplaceString(
@@ -77,6 +84,7 @@ def generate_launch_description():
         executable='rviz2',
         namespace=namespace,
         arguments=['-d', namespaced_rviz_config_file],
+        parameters=[{'use_sim_time': use_sim_time}],
         output='screen',
         remappings=[
             ('/map', 'map'),
@@ -111,6 +119,7 @@ def generate_launch_description():
     ld.add_action(declare_namespace_cmd)
     ld.add_action(declare_use_namespace_cmd)
     ld.add_action(declare_rviz_config_file_cmd)
+    ld.add_action(declare_use_sim_time_cmd)
 
     # Add any conditioned actions
     ld.add_action(start_rviz_cmd)

diff --git a/nav2_bringup/launch/slam_launch.py b/nav2_bringup/launch/slam_launch.py
@@ -21,7 +21,7 @@
 from launch.conditions import IfCondition, UnlessCondition
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import LaunchConfiguration
-from launch_ros.actions import Node, SetParameter
+from launch_ros.actions import Node, SetParameter, SetRemap
 from launch_ros.descriptions import ParameterFile
 from nav2_common.launch import HasNodeParams, RewrittenYaml
 
@@ -88,7 +88,6 @@ def generate_launch_description():
     )
 
     # Nodes launching commands
-
     start_map_server = GroupAction(
         actions=[
             SetParameter('use_sim_time', use_sim_time),
@@ -119,19 +118,28 @@ def generate_launch_description():
         source_file=params_file, node_name='slam_toolbox'
     )
 
-    start_slam_toolbox_cmd = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(slam_launch_file),
-        launch_arguments={'use_sim_time': use_sim_time}.items(),
-        condition=UnlessCondition(has_slam_toolbox_params),
-    )
+    start_slam_toolbox_cmd = GroupAction(
+
+        actions=[
+            # Remapping required to have a slam session subscribe & publish in optional namespaces
+            SetRemap(src='/scan', dst='scan'),
+            SetRemap(src='/tf', dst='tf'),
+            SetRemap(src='/tf_static', dst='tf_static'),
+            SetRemap(src='/map', dst='map'),
+
+            IncludeLaunchDescription(
+                PythonLaunchDescriptionSource(slam_launch_file),
+                launch_arguments={'use_sim_time': use_sim_time}.items(),
+                condition=UnlessCondition(has_slam_toolbox_params),
+            ),
 
-    start_slam_toolbox_cmd_with_params = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(slam_launch_file),
-        launch_arguments={
-            'use_sim_time': use_sim_time,
-            'slam_params_file': params_file,
-        }.items(),
-        condition=IfCondition(has_slam_toolbox_params),
+            IncludeLaunchDescription(
+                PythonLaunchDescriptionSource(slam_launch_file),
+                launch_arguments={'use_sim_time': use_sim_time,
+                                  'slam_params_file': params_file}.items(),
+                condition=IfCondition(has_slam_toolbox_params),
+            )
+        ]
     )
 
     ld = LaunchDescription()
@@ -149,6 +157,5 @@ def generate_launch_description():
 
     # Running SLAM Toolbox (Only one of them will be run)
     ld.add_action(start_slam_toolbox_cmd)
-    ld.add_action(start_slam_toolbox_cmd_with_params)
 
     return ld
diff --git a/nav2_bringup/launch/tb3_simulation_launch.py b/nav2_bringup/launch/tb3_simulation_launch.py
@@ -242,6 +242,7 @@ def generate_launch_description():
         launch_arguments={
             'namespace': namespace,
             'use_namespace': use_namespace,
+            'use_sim_time': use_sim_time,
             'rviz_config': rviz_config_file,
         }.items(),
     )

diff --git a/nav2_bt_navigator/README.md b/nav2_bt_navigator/README.md
@@ -2,7 +2,7 @@
 
 The BT Navigator (Behavior Tree Navigator) module implements the NavigateToPose and NavigateThroughPoses task interfaces. It is a [Behavior Tree](https://github.com/BehaviorTree/BehaviorTree.CPP/blob/master/docs/BT_basics.md)-based implementation of navigation that is intended to allow for flexibility in the navigation task and provide a way to easily specify complex robot behaviors.
 
-See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-bt-navigator.html) for additional parameter descriptions, as well as the [Nav2 Behavior Tree Explanation](https://navigation.ros.org/behavior_trees/index.html) pages explaining more context on the default behavior trees and examples provided in this package.
+See its [Configuration Guide Page](https://docs.nav2.org/configuration/packages/configuring-bt-navigator.html) for additional parameter descriptions, as well as the [Nav2 Behavior Tree Explanation](https://docs.nav2.org/behavior_trees/index.html) pages explaining more context on the default behavior trees and examples provided in this package.
 
 ## Overview