Galactic sync 4 (Sept 15) (#2561)

* Adding launch_testing_ros dep on nav2 utils to install (#2450)

* Reduce map saver nodes (#2454)

* reduce MapSaver nodes by using callback group/executor combo

Signed-off-by: zhenpeng ge <[email protected]>

* set use_rclcpp_node false

* a cleaner solution using a future and spin_until_future_complete()

Signed-off-by: zhenpeng ge <[email protected]>

* Update nav2_controller.cpp (#2462)

Add `costmap_thread_.reset()` on the destructor of ControllerServer class

* Reduce lifecycle manager nodes (#2456)

* remove bond_client_node_ in class LifecycleManager

Signed-off-by: zhenpeng ge <[email protected]>

* clear unused variables

Signed-off-by: zhenpeng ge <[email protected]>

* fix lint

Signed-off-by: zhenpeng ge <[email protected]>

* use dedicated executor thread

Signed-off-by: William Woodall <[email protected]>

* fix building error

Signed-off-by: zhenpeng ge <[email protected]>

* support to process executor for NodeThread

Signed-off-by: zhenpeng ge <[email protected]>

* use  NodeThread for LifecycleManager

Signed-off-by: zhenpeng ge <[email protected]>

Co-authored-by: William Woodall <[email protected]>

* sync with main and use ros_diff_drive in case name changes (#2472)

Co-authored-by: YOUSSEF LAHROUNI <[email protected]>

* Nav2 Simple (Python3) Commander Library (#2411)

* adding nav2_python_commander package

* adding readme

* launch files for the python commander examples

* renaming to nav2_simple_commander

* resolve review comments

* fixing rosdep key

* fixing up linters

* Python string format (#2466)

* Convert to python format strings for readability

* Merge concatenated strings

* Revert converting generated files

* Single quotes for consistency

* Just print the exception

* Fix Smac cleanup (#2477)

* fix smac2d cleanup

* same for hybrid

* Naming BT client node after action name (#2470)

* Put action name in node namespace instead of node name

* Put namespace remapping in node options

* Rename client node with action name replacing "/" by "_"

* Code formatting

* Code formatting

* fix nav2 params and launch file to publish Local and global costmaps in multi robots example (#2471)

* adjust launch file with needed gazebo plugin and set groot to False for multi-robot params

* correct unwanted changes

* change port and set groot to false

* fix lints

Co-authored-by: YOUSSEF LAHROUNI <[email protected]>

* [SmacPlanner2D] make tolerance parameter dynamic (#2475)

* make tolerance dyn param

* full reconfigure

* fix typo

* Place function above the variables

* lock param callback

* Modify the BtServiceNode to include an on_success call. (#2481)

* Modify the BtServiceNode to include an on_success call.

* PR: Fix linter error by removing trailing whitespaces.

* PR: Rename on_success() to on_completion() to improve understandability.

* Accept path of length 1 (#2495)

* Fix null pointer in amcl on_cleanup (#2503)

* fix data race: addPlugin() and resizeMap() can be executed concurrently (#2512)

Co-authored-by: Kai-Tao Xie <[email protected]>

* fix data race: VoxelLayer::matchSize may be executed concurrently (#2513)

Co-authored-by: Kai-Tao Xie <[email protected]>

* catch runtime_error if the message from laser is malformed (#2511)

* catch runtime_error if the message from laser is malformed

* fix styling

Co-authored-by: Kai-Tao Xie <[email protected]>

* Smac planner bad alloc (#2516)

* test(nav2_smac_planner): show short path bad_alloc

When given a goal that is one or zero costmap
cells away, AStarAlgorithm throws std::bad_alloc

* fix(nav2_smac_planner): fixed bad_alloc

* [ObstacleLayer] Use message_filter timeout (#2518)

* , tf2::durationFromSec(transform_tolerance)

* use message_filter timeout in AMCL

* also for sensor_msgs::msg::PointCloud2

* fix possible use-after-free: unsafe shared_ptr in multithread (#2510)

Co-authored-by: Kai-Tao Xie <[email protected]>

* fix export dependency and library (#2521)

Signed-off-by: zhenpeng ge <[email protected]>

* Add more semantic checks for amcl parameters (#2528)

* Fix null pointer in amcl on_cleanup

* Add more semantic checks for amcl

* fix possible use-after-free: unsafe shared_ptr in multithread (#2530)

Co-authored-by: Kai-Tao Xie <[email protected]>

* Hot fix rrp slow (#2526)

* review update

* updated the 2nd review comments

* String formatting

* Fix out of voxel grid array regression (#2460)

* Update dwb_local_planner.hpp (#2533)

Add remarks

* Add new test for smac_planner_2d (#2531)

* Add new test for smac_planner_2d

* Fix costmap initialization

* Change set_parameters() with set_parameters_atomically()

* Improving coverage

* Remove debug helper method

* Fix linting issue introduced in #2533 (#2539)

* [All 2D planners] Fix last pose orientation, fix small path and add ignore_goal_orientation parameter (#2488)

* end point orientation

* add ignore_goal_orientation to dyn param

* back()  instead of [path.poses.size() - 1]

* Smac2d use_final_approach_orientation name

* add in NavFn

* add in theta_star

* wip

* deal with navfn double end pose case

* add tests

* back and revert test smac2d

* get path hotfix

* navfn wip

* Corner cases

* Checks consistency accross the 3 2D planners

* comment

* interpolate final orientation in planner instead of smoother

* clarify and homogenize comments

* using same cell test instead of distance compared to resolution

* remove unneeded else

* lint

* fix smac2d goal orientation override

* fix and add tests

* update comment

* typo

* simplify if (_use_final_approach_orientation) block

* Use worldToMapEnforceBounds in clear_costmap_service (#2544)

to avoid buffer overflow

Signed-off-by: zouyonghao <[email protected]>

* Add new test for nav2_regulated_pure_pursuit (#2542)

* unit test for findDirChg

* lint fix

* add test for unchanged direction

* fix  for loop ending

* Enabled runtime configuration of parameters for Hybrid A* (#2540)

* Enabled runtime configuration of parameters for Hybrid A*

* Fix parameter name

* Fix parameter type

* fix pf_ use-before-initial in laserReceived() callback (#2550)

Co-authored-by: Kai-Tao Xie <[email protected]>

* add semantic checks (#2557)

Co-authored-by: Kai-Tao Xie <[email protected]>

* bumping galactic to 1.0.7

* Updates to Nav2 Theta Star Planner docs (#2559)

* - removing 'w_heuristic_cost_' from Nav2 Theta Star Planner as a user configurable parameter
    - updates to `nav2_theta_star_planner/README.md` to remove its reference wherever applicable
    - updates to `nav2_theta_star_planner/src/theta_star_planner.cpp` to set `w_heuristic_cost` as `std::min(1.0, w_euc_cost_)`
- fixed an incorrect check of whether the start and goal pose are the same in `nav2_theta_star_planner/src/theta_star_planner.cpp`

* Update theta_star_planner.hpp

clean up stuff from forced merge

* Update theta_star_planner.cpp

clean up stuff from a forced merge

* Update README.md

* Update theta_star_planner.cpp

Re-adding the goal to ensure, that the last pose, is the goal exactly

* fix linting issues

* Update theta_star_planner.cpp

Co-authored-by: Steve Macenski <[email protected]>

* Fixed vector::reserve exception in smac planner due to precision error (#2563)

- Related issue: #2547

Co-authored-by: gezp <[email protected]>
Co-authored-by: harderthan <[email protected]>
Co-authored-by: William Woodall <[email protected]>
Co-authored-by: Yousseflah <[email protected]>
Co-authored-by: YOUSSEF LAHROUNI <[email protected]>
Co-authored-by: Tim Clephas <[email protected]>
Co-authored-by: G.Doisy <[email protected]>
Co-authored-by: anaelle-sw <[email protected]>
Co-authored-by: philison <[email protected]>
Co-authored-by: Yong-Hao Zou <[email protected]>
Co-authored-by: easylyou <[email protected]>
Co-authored-by: Kai-Tao Xie <[email protected]>
Co-authored-by: Adam Aposhian <[email protected]>
Co-authored-by: Pradheep Krishna <[email protected]>
Co-authored-by: Alexey Merzlyakov <[email protected]>
Co-authored-by: Luca Bonamini <[email protected]>
Co-authored-by: Sathakkadhullah <[email protected]>
Co-authored-by: Anshumaan Singh <[email protected]>
Co-authored-by: zoltan-lengyel <[email protected]>

Author: 20 people

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

nav2_amcl/src/amcl_node.cpp

@@ -234,12 +234,12 @@ AmclNode::on_configure(const rclcpp_lifecycle::State & /*state*/)
 
   initParameters();
   initTransforms();
+  initParticleFilter();
+  initLaserScan();
   initMessageFilters();
   initPubSub();
   initServices();
   initOdometry();
-  initParticleFilter();
-  initLaserScan();
 
   return nav2_util::CallbackReturn::SUCCESS;
 }
@@ -335,8 +335,10 @@ AmclNode::on_cleanup(const rclcpp_lifecycle::State & /*state*/)
   laser_scan_sub_.reset();
 
   // Map
-  map_free(map_);
-  map_ = nullptr;
+  if (map_ != NULL) {
+    map_free(map_);
+    map_ = nullptr;
+  }
   first_map_received_ = false;
   free_space_indices.resize(0);
 
@@ -1105,6 +1107,25 @@ AmclNode::initParameters()
   last_time_printed_msg_ = now();
 
   // Semantic checks
+  if (laser_likelihood_max_dist_ < 0) {
+    RCLCPP_WARN(
+      get_logger(), "You've set laser_likelihood_max_dist to be negtive,"
+      " this isn't allowed so it will be set to default value 2.0.");
+    laser_likelihood_max_dist_ = 2.0;
+  }
+  if (max_particles_ < 0) {
+    RCLCPP_WARN(
+      get_logger(), "You've set max_particles to be negtive,"
+      " this isn't allowed so it will be set to default value 2000.");
+    max_particles_ = 2000;
+  }
+
+  if (min_particles_ < 0) {
+    RCLCPP_WARN(
+      get_logger(), "You've set min_particles to be negtive,"
+      " this isn't allowed so it will be set to default value 500.");
+    min_particles_ = 500;
+  }
 
   if (min_particles_ > max_particles_) {
     RCLCPP_WARN(
@@ -1113,6 +1134,13 @@ AmclNode::initParameters()
     max_particles_ = min_particles_;
   }
 
+  if (resample_interval_ <= 0) {
+    RCLCPP_WARN(
+      get_logger(), "You've set resample_interval to be zero or negtive,"
+      " this isn't allowed so it will be set to default value to 1.");
+    resample_interval_ = 1;
+  }
+
   if (always_reset_initial_pose_) {
     initial_pose_is_known_ = false;
   }
@@ -1239,7 +1267,7 @@ AmclNode::initMessageFilters()
     rclcpp_node_.get(), scan_topic_, rmw_qos_profile_sensor_data);
 
   laser_scan_filter_ = std::make_unique<tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan>>(
-    *laser_scan_sub_, *tf_buffer_, odom_frame_id_, 10, rclcpp_node_);
+    *laser_scan_sub_, *tf_buffer_, odom_frame_id_, 10, rclcpp_node_, transform_tolerance_);
 
   laser_scan_connection_ = laser_scan_filter_->registerCallback(
     std::bind(

nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp

@@ -82,11 +82,16 @@ bool BtActionServer<ActionT>::on_configure()
     throw std::runtime_error{"Failed to lock node"};
   }
 
-  // use suffix '_rclcpp_node' to keep parameter file consistency #1773
+  // Name client node after action name
+  std::string client_node_name = action_name_;
+  std::replace(client_node_name.begin(), client_node_name.end(), '/', '_');
+  // Use suffix '_rclcpp_node' to keep parameter file consistency #1773
   auto options = rclcpp::NodeOptions().arguments(
     {"--ros-args",
-      "-r", std::string("__node:=") + std::string(node->get_name()) + action_name_ + "_rclcpp_node",
+      "-r",
+      std::string("__node:=") + std::string(node->get_name()) + client_node_name + "_rclcpp_node",
       "--"});
+
   // Support for handling the topic-based goal pose from rviz
   client_node_ = std::make_shared<rclcpp::Node>("_", options);
 

nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp

@@ -142,6 +142,16 @@ class BtServiceNode : public BT::ActionNodeBase
   {
   }
 
+  /**
+   * @brief Function to perform some user-defined operation upon successful
+   * completion of the service. Could put a value on the blackboard.
+   * @return BT::NodeStatus Returns SUCCESS by default, user may override to return another value
+   */
+  virtual BT::NodeStatus on_completion()
+  {
+    return BT::NodeStatus::SUCCESS;
+  }
+
   /**
    * @brief Check the future and decide the status of BT
    * @return BT::NodeStatus SUCCESS if future complete before timeout, FAILURE otherwise
@@ -158,7 +168,8 @@ class BtServiceNode : public BT::ActionNodeBase
       rc = callback_group_executor_.spin_until_future_complete(future_result_, server_timeout_);
       if (rc == rclcpp::FutureReturnCode::SUCCESS) {
         request_sent_ = false;
-        return BT::NodeStatus::SUCCESS;
+        BT::NodeStatus status = on_completion();
+        return status;
       }
 
       if (rc == rclcpp::FutureReturnCode::TIMEOUT) {

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.0.6</version>
+  <version>1.0.7</version>
   <description>TODO</description>
   <maintainer email="[email protected]">Michael Jeronimo</maintainer>
   <maintainer email="[email protected]">Carlos Orduno</maintainer>

nav2_bringup/bringup/launch/multi_tb3_simulation_launch.py

@@ -102,7 +102,8 @@ def generate_launch_description():
 
     # Start Gazebo with plugin providing the robot spawing service
     start_gazebo_cmd = ExecuteProcess(
-        cmd=[simulator, '--verbose', '-s', 'libgazebo_ros_factory.so', world],
+        cmd=[simulator, '--verbose', '-s', 'libgazebo_ros_init.so',
+                                     '-s', 'libgazebo_ros_factory.so', world],
         output='screen')
 
     # Define commands for spawing the robots into Gazebo
@@ -123,7 +124,7 @@ def generate_launch_description():
     # Define commands for launching the navigation instances
     nav_instances_cmds = []
     for robot in robots:
-        params_file = LaunchConfiguration(robot['name'] + '_params_file')
+        params_file = LaunchConfiguration(f"{robot['name']}_params_file")
 
         group = GroupAction([
             IncludeLaunchDescription(

nav2_bringup/bringup/launch/tb3_simulation_launch.py

@@ -131,7 +131,7 @@ def generate_launch_description():
     # Specify the actions
     start_gazebo_server_cmd = ExecuteProcess(
         condition=IfCondition(use_simulator),
-        cmd=['gzserver', '-s', 'libgazebo_ros_init.so', world],
+        cmd=['gzserver', '-s', 'libgazebo_ros_init.so',  '-s', 'libgazebo_ros_factory.so', world],
         cwd=[launch_dir], output='screen')
 
     start_gazebo_client_cmd = ExecuteProcess(

nav2_bringup/bringup/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_bringup</name>
-  <version>1.0.6</version>
+  <version>1.0.7</version>
   <description>Bringup scripts and configurations for the Nav2 stack</description>
   <maintainer email="[email protected]">Michael Jeronimo</maintainer>
   <maintainer email="[email protected]">Steve Macenski</maintainer>

nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml

@@ -59,6 +59,10 @@ bt_navigator:
     # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml
     # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml
     # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2.
+    # if enable_groot_monitoring is set to True, ports need to be different for each robot !!
+    enable_groot_monitoring: False
+    groot_zmq_publisher_port: 1666
+    groot_zmq_server_port: 1667
     plugin_lib_names:
     - nav2_compute_path_to_pose_action_bt_node
     - nav2_compute_path_through_poses_action_bt_node

nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml

@@ -59,6 +59,10 @@ bt_navigator:
     # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml
     # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml
     # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2.
+    # if enable_groot_monitoring is set to True, ports need to be different for each robot !!
+    enable_groot_monitoring: False
+    groot_zmq_publisher_port: 1789
+    groot_zmq_server_port: 1887
     plugin_lib_names:
     - nav2_compute_path_to_pose_action_bt_node
     - nav2_compute_path_through_poses_action_bt_node