Iron Sync 3 - Sept 25 (#3837)

* Same orientation of coordinate frames in rviz ang gazebo (#3751)

* rviz view straight in default xy orientation

Signed-off-by: Christian Henkel <christian.henkel2@de.bosch.com>

* gazebo orientation to match rviz

Signed-off-by: Christian Henkel <christian.henkel2@de.bosch.com>

* rotating in direction of view

---------

Signed-off-by: Christian Henkel <christian.henkel2@de.bosch.com>

* Fix flaky costmap filters tests: (#3754)

1. Set forward_prune_distance to 1.0 to robot not getting lost
2. Correct map name for costmap filter tests

* Update smac_planner_hybrid.cpp (#3760)

* Fix missing mutex in PlannerServer::isPathValid (#3756)

Signed-off-by: ymd-stella <world.applepie@gmail.com>

* Rename PushRosNamespace to PushROSNamespace (#3763)

* Rewrite the scan topic costmap plugins for multi-robot(namespace) before launch navigation. (#3572)

* Make it possible to launch namspaced robot which rewrites `<robot_namespace>` to namespace.
- It allows to apply namespace automatically on specific target topic path in costmap plugins.

Add new nav2 params file for multi-robot(rewriting `<robot_namespace>`) as an example.
- nav2_multirobot_params_all.yaml

Modify nav2_common.ReplaceString
- add condition argument

* Update nav2_bringup/launch/bringup_launch.py

Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>

* Add new luanch script for multi-robot bringup

Rename luanch script for multi-robot simulation bringup

Add new nav2_common script
- Parse argument
- Parse multirobot pose

Update README.md

* Update README.md

Apply suggestions from code review

Fix pep257 erors

Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>

---------

Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>

* use ros clock for wait (#3782)

* use ROS clock for wait

* fix backport issue

---------

Co-authored-by: Guillaume Doisy <guillaume@dexory.com>

* fixing external users of the BT action node template (#3792)

* fixing external users of the BT action node template

* Update nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp

Co-authored-by: Guillaume Doisy <doisyg@users.noreply.github.com>

---------

Co-authored-by: Guillaume Doisy <doisyg@users.noreply.github.com>

* Fixing typo in compute path through poses error codes (#3799)

Signed-off-by: Mannucci, Anna (Bosch (CR)) <Anna.Mannucci@de.bosch.com>
Co-authored-by: Mannucci, Anna (Bosch (CR)) <Anna.Mannucci@de.bosch.com>

* Fixes for flaky WPF test (#3785)

* Fixes for flaky WPF test:
* New RewrittenYaml ability to add non-existing parameters
* Prune distance fix for WPF test
* Treat UNKNOWN status as error in WPF
* Clear error codes after BT run
* Remove unnecessary setInitialPose() from WPF test

* Update nav2_waypoint_follower/src/waypoint_follower.cpp

Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>

* Clean error code in any situation

* Fix UNKNOWN WPF status handling

---------

Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>

* Fix `min_points` comparison check (#3795)

* Fix min_points checking

* Expose action server result timeout as a parameter in bt navigator servers (#3787)

* Expose action server default timeout in bt navigator servers

* typo

* duplicated comment

* Expose result timeout in other actions

* Proper timeout in bt node

* Change default timeouts and remove comments

* Remove comment in params file

* uncrustify controller server

* Using Simple Commander API for multi robot systems (#3803)

* support multirobot namespaces

* add docs

* adding copy all params primitive for BT navigator (to ingest into rclcpp) (#3804)

* adding copy all params primitive

* fix linting

* lint

* I swear to god, this better be the last linting issue

* allowing params to be declared from yaml

* Update bt_navigator.cpp

* Fix CD configuration link reference (#3811)

* Fix CD configuration page reference

* Add CM work on 6th ROS Developers Day reference

* some minor optimizations (#3821)

* fix broken behaviortree doc link (#3822)

Signed-off-by: Anton Kesy <antonkesy@gmail.com>

* [MPPI] complete minor optimaization with floating point calculations (#3827)

* floating point calculations

* Update optimizer_unit_tests.cpp

* Update critics_tests.cpp

* Update critics_tests.cpp

* 25% speed up of goal critic; 1% speed up from vy striding when not in use

* Add nav2_gps_waypoint_follower (#2814)

* Add nav2_gps_waypoint_follower

* use correct client node while calling it to spin

* changed after 1'st review

* apply requested changes

* nav2_util::ServiceClient instead of CallbackGroup

* another  iteration to adress issues

* update poses with function in the follower logic

* add deps of robot_localization: diagnostics

* fix typo in underlay.repo

* add deps of robot_localization: geographic_info

* minor clean-ups

* bond_core version has been updated

* rotation should also be considered in GPS WPFing

* use better namings related to gps wpf orientation

* handle cpplint errors

* tf_listener needs to be initialized

* apply requested changes

* apply requested changes 3.0/3.0

* fix misplaced ";"

* use run time param for gps transform timeout

* change timeout var name

* make use of stop_on_failure for GPS too

* passing emptywaypont vectors are seen as failure

* update warning for empty requests

* consider utm -> map yaw offset

* fix missed RCLCPP info

* reorrect action;s name

* waypoint stamps need to be updated

* Fix segmentation fault on waypoint follower

* Parametric frames and matrix multiplications

* Replace oriented navsatfix for geographic_msgs/geopose

* Remove deprecated oriented  navsatfix message

* Update branch name on robot_localization dependency

* Fix parametric frames logic

* Rename functions and adress comments

* fix style in underlay.repos

* remove duplicate word in underlay.repos

* update dependency version of ompl

* Template ServiceClient class to accept lifecycle node

* Remove link to stackoverflow answer

* Remove yaw offset compensation

* Fix API change

* Fix styling

* Minor docs fixes

* Fix style divergences

* Style fixes

* Style fixes v2

* Style fixes v3

* Remove unused variables and timestam overrides

* restore goal timestamp override

* WIP: Add follow gps waypoints test

* Style fixes and gazebo world inertia fix

* Reduce velocity smoother timeout

* empty commit to rerun tests

* Increment circle ci cache idx

* Remove extra space in cmakelists.txt

* Fix wrong usage of the global action server

* update follow gps waypoints action definition

* Fix action definition and looping

* update params for the unit testing

* WIP: update tests

* fix tests

* fixes to nav2 simple commander

* add robot_localization localizer

* Bring back from LL client

* Update nav2_simple_commander/nav2_simple_commander/robot_navigator.py

Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>

* missing argument in test function

* small test error

* style fixes nav2 simple commander

* rename cartesian action server

---------

Co-authored-by: jediofgever <fetulahatas1@gmail.com>
Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>

* bumping iron from 1.2.2 to 1.2.3 for release

* Update waypoint_follower.cpp

---------

Signed-off-by: Christian Henkel <christian.henkel2@de.bosch.com>
Signed-off-by: ymd-stella <world.applepie@gmail.com>
Signed-off-by: Mannucci, Anna (Bosch (CR)) <Anna.Mannucci@de.bosch.com>
Signed-off-by: Anton Kesy <antonkesy@gmail.com>
Co-authored-by: Christian Henkel <6976069+ct2034@users.noreply.github.com>
Co-authored-by: Alexey Merzlyakov <60094858+AlexeyMerzlyakov@users.noreply.github.com>
Co-authored-by: ymd-stella <7959916+ymd-stella@users.noreply.github.com>
Co-authored-by: Tony Najjar <tony.najjar@logivations.com>
Co-authored-by: Hyunseok <yanghyunseok@me.com>
Co-authored-by: Guillaume Doisy <doisyg@users.noreply.github.com>
Co-authored-by: Guillaume Doisy <guillaume@dexory.com>
Co-authored-by: Anna Mannucci <19239559+annamannucci@users.noreply.github.com>
Co-authored-by: Mannucci, Anna (Bosch (CR)) <Anna.Mannucci@de.bosch.com>
Co-authored-by: Tony Najjar <tony.najjar.1997@gmail.com>
Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com>
Co-authored-by: Anton Kesy <antonkesy@gmail.com>
Co-authored-by: jediofgever <fetulahatas1@gmail.com>

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>1.2.2</version>
+  <version>1.2.3</version>
   <description>
     <p>
       amcl is a probabilistic localization system for a robot moving in

nav2_behavior_tree/README.md

@@ -63,4 +63,4 @@ The BehaviorTree engine has a run method that accepts an XML description of a BT
 
 See the code in the [BT Navigator](../nav2_bt_navigator/src/bt_navigator.cpp) for an example usage of the BehaviorTreeEngine.
 
-For more information about the behavior tree nodes that are available in the default BehaviorTreeCPP library, see documentation here: https://www.behaviortree.dev/bt_basics/
+For more information about the behavior tree nodes that are available in the default BehaviorTreeCPP library, see documentation here: https://www.behaviortree.dev/docs/learn-the-basics/bt_basics/

nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp

@@ -196,6 +196,11 @@ class BtActionServer
    */
   void populateErrorCode(typename std::shared_ptr<typename ActionT::Result> result);
 
+  /**
+   * @brief Setting BT error codes to success. Used to clean blackboard between different BT runs
+   */
+  void cleanErrorCodes();
+
   // Action name
   std::string action_name_;
 

nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp

@@ -26,6 +26,7 @@
 #include "nav2_msgs/action/navigate_to_pose.hpp"
 #include "nav2_behavior_tree/bt_action_server.hpp"
 #include "ament_index_cpp/get_package_share_directory.hpp"
+#include "nav2_util/node_utils.hpp"
 
 namespace nav2_behavior_tree
 {
@@ -60,6 +61,9 @@ BtActionServer<ActionT>::BtActionServer(
   if (!node->has_parameter("default_server_timeout")) {
     node->declare_parameter("default_server_timeout", 20);
   }
+  if (!node->has_parameter("action_server_result_timeout")) {
+    node->declare_parameter("action_server_result_timeout", 900.0);
+  }
 
   std::vector<std::string> error_code_names = {
     "follow_path_error_code",
@@ -122,19 +126,38 @@ bool BtActionServer<ActionT>::on_configure()
   // Support for handling the topic-based goal pose from rviz
   client_node_ = std::make_shared<rclcpp::Node>("_", options);
 
+  // Declare parameters for common client node applications to share with BT nodes
+  // Declare if not declared in case being used an external application, then copying
+  // all of the main node's parameters to the client for BT nodes to obtain
+  nav2_util::declare_parameter_if_not_declared(
+    node, "global_frame", rclcpp::ParameterValue(std::string("map")));
+  nav2_util::declare_parameter_if_not_declared(
+    node, "robot_base_frame", rclcpp::ParameterValue(std::string("base_link")));
+  nav2_util::declare_parameter_if_not_declared(
+    node, "transform_tolerance", rclcpp::ParameterValue(0.1));
+  nav2_util::copy_all_parameters(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);
+  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);
+
   action_server_ = std::make_shared<ActionServer>(
     node->get_node_base_interface(),
     node->get_node_clock_interface(),
     node->get_node_logging_interface(),
     node->get_node_waitables_interface(),
-    action_name_, std::bind(&BtActionServer<ActionT>::executeCallback, this));
+    action_name_, std::bind(&BtActionServer<ActionT>::executeCallback, this),
+    nullptr, std::chrono::milliseconds(500), false, server_options);
 
   // Get parameters for BT timeouts
-  int timeout;
-  node->get_parameter("bt_loop_duration", timeout);
-  bt_loop_duration_ = std::chrono::milliseconds(timeout);
-  node->get_parameter("default_server_timeout", timeout);
-  default_server_timeout_ = std::chrono::milliseconds(timeout);
+  int bt_loop_duration;
+  node->get_parameter("bt_loop_duration", bt_loop_duration);
+  bt_loop_duration_ = std::chrono::milliseconds(bt_loop_duration);
+  int default_server_timeout;
+  node->get_parameter("default_server_timeout", default_server_timeout);
+  default_server_timeout_ = std::chrono::milliseconds(default_server_timeout);
 
   // Get error code id names to grab off of the blackboard
   error_code_names_ = node->get_parameter("error_code_names").as_string_array();
@@ -224,6 +247,7 @@ void BtActionServer<ActionT>::executeCallback()
 {
   if (!on_goal_received_callback_(action_server_->get_current_goal())) {
     action_server_->terminate_current();
+    cleanErrorCodes();
     return;
   }
 
@@ -278,6 +302,8 @@ void BtActionServer<ActionT>::executeCallback()
       action_server_->terminate_all(result);
       break;
   }
+
+  cleanErrorCodes();
 }
 
 template<class ActionT>
@@ -304,6 +330,14 @@ void BtActionServer<ActionT>::populateErrorCode(
   }
 }
 
+template<class ActionT>
+void BtActionServer<ActionT>::cleanErrorCodes()
+{
+  for (const auto & error_code : error_code_names_) {
+    blackboard_->set<unsigned short>(error_code, 0);  //NOLINT
+  }
+}
+
 }  // namespace nav2_behavior_tree
 
 #endif  // NAV2_BEHAVIOR_TREE__BT_ACTION_SERVER_IMPL_HPP_

nav2_behavior_tree/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_behavior_tree</name>
-  <version>1.2.2</version>
+  <version>1.2.3</version>
   <description>TODO</description>
   <maintainer email="michael.jeronimo@intel.com">Michael Jeronimo</maintainer>
   <maintainer email="carlos.a.orduno@intel.com">Carlos Orduno</maintainer>

nav2_behaviors/include/nav2_behaviors/plugins/wait.hpp

@@ -63,7 +63,7 @@ class Wait : public TimedBehavior<WaitAction>
   CostmapInfoType getResourceInfo() override {return CostmapInfoType::LOCAL;}
 
 protected:
-  std::chrono::time_point<std::chrono::steady_clock> wait_end_;
+  rclcpp::Time wait_end_;
   WaitAction::Feedback::SharedPtr feedback_;
 };
 

nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp

@@ -133,9 +133,19 @@ class TimedBehavior : public nav2_core::Behavior
     node->get_parameter("robot_base_frame", robot_base_frame_);
     node->get_parameter("transform_tolerance", transform_tolerance_);
 
+    if (!node->has_parameter("action_server_result_timeout")) {
+      node->declare_parameter("action_server_result_timeout", 10.0);
+    }
+
+    double action_server_result_timeout;
+    node->get_parameter("action_server_result_timeout", action_server_result_timeout);
+    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);
+
     action_server_ = std::make_shared<ActionServer>(
       node, behavior_name_,
-      std::bind(&TimedBehavior::execute, this));
+      std::bind(&TimedBehavior::execute, this), nullptr, std::chrono::milliseconds(
+        500), false, server_options);
 
     local_collision_checker_ = local_collision_checker;
     global_collision_checker_ = global_collision_checker;

nav2_behaviors/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_behaviors</name>
-  <version>1.2.2</version>
+  <version>1.2.3</version>
   <description>TODO</description>
   <maintainer email="carlos.a.orduno@intel.com">Carlos Orduno</maintainer>
   <maintainer email="stevenmacenski@gmail.com">Steve Macenski</maintainer>

nav2_behaviors/plugins/wait.cpp

@@ -30,21 +30,19 @@ Wait::~Wait() = default;
 
 ResultStatus Wait::onRun(const std::shared_ptr<const WaitAction::Goal> command)
 {
-  wait_end_ = std::chrono::steady_clock::now() +
-    rclcpp::Duration(command->time).to_chrono<std::chrono::nanoseconds>();
+  wait_end_ = node_.lock()->now() + rclcpp::Duration(command->time);
   return ResultStatus{Status::SUCCEEDED};
 }
 
 ResultStatus Wait::onCycleUpdate()
 {
-  auto current_point = std::chrono::steady_clock::now();
-  auto time_left =
-    std::chrono::duration_cast<std::chrono::nanoseconds>(wait_end_ - current_point).count();
+  auto current_point = node_.lock()->now();
+  auto time_left = wait_end_ - current_point;
 
-  feedback_->time_left = rclcpp::Duration(rclcpp::Duration::from_nanoseconds(time_left));
+  feedback_->time_left = time_left;
   action_server_->publish_feedback(feedback_);
 
-  if (time_left > 0) {
+  if (time_left.nanoseconds() > 0) {
     return ResultStatus{Status::RUNNING};
   } else {
     return ResultStatus{Status::SUCCEEDED};

nav2_bringup/README.md

@@ -1,17 +1,44 @@
 # nav2_bringup
 
-The `nav2_bringup` package is an example bringup system for Nav2 applications. 
+The `nav2_bringup` package is an example bringup system for Nav2 applications.
 
-This is a very flexible example for nav2 bringup that can be modified for different maps/robots/hardware/worlds/etc. It is our expectation for an application specific robot system that you're mirroring `nav2_bringup` package and modifying it for your specific maps/robots/bringup needs. This is an applied and working demonstration for the default system bringup with many options that can be easily modified. 
+This is a very flexible example for nav2 bringup that can be modified for different maps/robots/hardware/worlds/etc. It is our expectation for an application specific robot system that you're mirroring `nav2_bringup` package and modifying it for your specific maps/robots/bringup needs. This is an applied and working demonstration for the default system bringup with many options that can be easily modified.
 
 Usual robot stacks will have a `<robot_name>_nav` package with config/bringup files and this is that for the general case to base a specific robot system off of.
 
 Dynamically composed bringup (based on  [ROS2 Composition](https://docs.ros.org/en/galactic/Tutorials/Composition.html)) is optional for users. It can be used to compose all Nav2 nodes in a single process instead of launching these nodes separately, which is useful for embedded systems users that need to make optimizations due to harsh resource constraints. Dynamically composed bringup is used by default, but can be disabled by using the launch argument `use_composition:=False`.
 
 * 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://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.
 
 Note:
 * gazebo should be started with both libgazebo_ros_init.so and libgazebo_ros_factory.so to work correctly.
 * spawn_entity node could not remap /tf and /tf_static to tf and tf_static in the launch file yet, used only for multi-robot situations. Instead it should be done as remapping argument <remapping>/tf:=tf</remapping>  <remapping>/tf_static:=tf_static</remapping> under ros2 tag in each plugin which publishs transforms in the SDF file. It is essential to differentiate the tf's of the different robot.
+
+## Launch
+
+### Multi-robot Simulation
+
+This is how to launch multi-robot simulation with simple command line. Please see the Nav2 documentation for further augments.
+
+#### Cloned
+
+This allows to bring up multiple robots, cloning a single robot N times at different positions in the map. The parameter are loaded from `nav2_multirobot_params_all.yaml` file by default.
+The multiple robots that consists of name and initial pose in YAML format will be set on the command-line. The format for each robot is `robot_name={x: 0.0, y: 0.0, yaw: 0.0, roll: 0.0, pitch: 0.0, yaw: 0.0}`.
+
+Please refer to below examples.
+
+```shell
+ros2 launch nav2_bringup cloned_multi_tb3_simulation_launch.py robots:="robot1={x: 1.0, y: 1.0, yaw: 1.5707}; robot2={x: 1.0, y: 1.0, yaw: 1.5707}"
+```
+
+#### Unique
+
+There are two robots including name and intitial pose are hard-coded in the launch script. Two separated unique robots are required params file (`nav2_multirobot_params_1.yaml`, `nav2_multirobot_params_2.yaml`) for each robot to bring up.
+
+If you want to bringup more than two robots, you should modify the `unique_multi_tb3_simulation_launch.py` script.
+
+```shell
+ros2 launch nav2_bringup unique_multi_tb3_simulation_launch.py
+```

nav2_bringup/launch/bringup_launch.py

@@ -23,9 +23,9 @@
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import LaunchConfiguration, PythonExpression
 from launch_ros.actions import Node
-from launch_ros.actions import PushRosNamespace
+from launch_ros.actions import PushROSNamespace
 from launch_ros.descriptions import ParameterFile
-from nav2_common.launch import RewrittenYaml
+from nav2_common.launch import RewrittenYaml, ReplaceString
 
 
 def generate_launch_description():
@@ -54,6 +54,15 @@ def generate_launch_description():
     remappings = [('/tf', 'tf'),
                   ('/tf_static', 'tf_static')]
 
+    # Only it applys when `use_namespace` is True.
+    # '<robot_namespace>' keyword shall be replaced by 'namespace' launch argument
+    # in config file 'nav2_multirobot_params.yaml' as a default & example.
+    # User defined config file should contain '<robot_namespace>' keyword for the replacements.
+    params_file = ReplaceString(
+        source_file=params_file,
+        replacements={'<robot_namespace>': ('/', namespace)},
+        condition=IfCondition(use_namespace))
+
     configured_params = ParameterFile(
         RewrittenYaml(
             source_file=params_file,
@@ -113,7 +122,7 @@ def generate_launch_description():
 
     # Specify the actions
     bringup_cmd_group = GroupAction([
-        PushRosNamespace(
+        PushROSNamespace(
             condition=IfCondition(use_namespace),
             namespace=namespace),