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 <zhenpeng.ge@qq.com> * set use_rclcpp_node false * a cleaner solution using a future and spin_until_future_complete() Signed-off-by: zhenpeng ge <zhenpeng.ge@qq.com> * 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 <zhenpeng.ge@qq.com> * clear unused variables Signed-off-by: zhenpeng ge <zhenpeng.ge@qq.com> * fix lint Signed-off-by: zhenpeng ge <zhenpeng.ge@qq.com> * use dedicated executor thread Signed-off-by: William Woodall <william@osrfoundation.org> * fix building error Signed-off-by: zhenpeng ge <zhenpeng.ge@qq.com> * support to process executor for NodeThread Signed-off-by: zhenpeng ge <zhenpeng.ge@qq.com> * use NodeThread for LifecycleManager Signed-off-by: zhenpeng ge <zhenpeng.ge@qq.com> Co-authored-by: William Woodall <william@osrfoundation.org> * sync with main and use ros_diff_drive in case name changes (#2472) Co-authored-by: YOUSSEF LAHROUNI <youssef.lahrouni@proioxrobots.com> * 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 <youssef.lahrouni@proioxrobots.com> * [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 <kaitaoxie@qq.com> * fix data race: VoxelLayer::matchSize may be executed concurrently (#2513) Co-authored-by: Kai-Tao Xie <kaitaoxie@qq.com> * 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 <kaitaoxie@qq.com> * 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 <kaitaoxie@qq.com> * fix export dependency and library (#2521) Signed-off-by: zhenpeng ge <zhenpeng.ge@qq.com> * 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 <kaitaoxie@qq.com> * 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 <yonghaoz1994@gmail.com> * 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 <kaitaoxie@qq.com> * add semantic checks (#2557) Co-authored-by: Kai-Tao Xie <kaitaoxie@qq.com> * 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 <stevenmacenski@gmail.com> * Fixed vector::reserve exception in smac planner due to precision error (#2563) - Related issue: #2547 Co-authored-by: gezp <zhenpeng.ge@qq.com> Co-authored-by: harderthan <kheo1772@gmail.com> Co-authored-by: William Woodall <william@osrfoundation.org> Co-authored-by: Yousseflah <53279766+Yousseflah@users.noreply.github.com> Co-authored-by: YOUSSEF LAHROUNI <youssef.lahrouni@proioxrobots.com> Co-authored-by: Tim Clephas <tim.clephas@nobleo.nl> Co-authored-by: G.Doisy <doisyg@users.noreply.github.com> Co-authored-by: anaelle-sw <63144493+anaelle-sw@users.noreply.github.com> Co-authored-by: philison <philison.dev@gmail.com> Co-authored-by: Yong-Hao Zou <yonghaoz1994@gmail.com> Co-authored-by: easylyou <38713965+easylyou@users.noreply.github.com> Co-authored-by: Kai-Tao Xie <kaitaoxie@qq.com> Co-authored-by: Adam Aposhian <adam.l.aposhian@gmail.com> Co-authored-by: Pradheep Krishna <padhupradheep@gmail.com> Co-authored-by: Alexey Merzlyakov <60094858+AlexeyMerzlyakov@users.noreply.github.com> Co-authored-by: Luca Bonamini <lucabonamini28@gmail.com> Co-authored-by: Sathakkadhullah <57533054+sathak93@users.noreply.github.com> Co-authored-by: Anshumaan Singh <anshumaan567@gmail.com> Co-authored-by: zoltan-lengyel <79632988+zoltan-lengyel@users.noreply.github.com>
ros-navigation · Sep 15, 2021 · 3ac8819 · 3ac8819
1 parent e48b01a
commit 3ac8819
Show file tree

Hide file tree

Showing 113 changed files with 3,933 additions and 348 deletions.
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>1.0.6</version>
+  <version>1.0.7</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
@@ -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(

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
@@ -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);
 

diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp b/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) {

diff --git a/nav2_behavior_tree/package.xml b/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="michael.jeronimo@intel.com">Michael Jeronimo</maintainer>
   <maintainer email="carlos.a.orduno@intel.com">Carlos Orduno</maintainer>

diff --git a/nav2_bringup/bringup/launch/multi_tb3_simulation_launch.py b/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(

diff --git a/nav2_bringup/bringup/launch/tb3_simulation_launch.py b/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(

diff --git a/nav2_bringup/bringup/package.xml b/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="michael.jeronimo@intel.com">Michael Jeronimo</maintainer>
   <maintainer email="stevenmacenski@gmail.com">Steve Macenski</maintainer>

diff --git a/nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml b/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

diff --git a/nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml b/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

diff --git a/nav2_bringup/nav2_gazebo_spawner/nav2_gazebo_spawner/nav2_gazebo_spawner.py b/nav2_bringup/nav2_gazebo_spawner/nav2_gazebo_spawner/nav2_gazebo_spawner.py
@@ -37,7 +37,8 @@ def main():
     parser.add_argument('-z', type=float, default=0,
                         help='the z component of the initial position [meters]')
     parser.add_argument('-k', '--timeout', type=float, default=10.0,
-                        help="Seconds to wait. Block until the future is complete if negative. Don't wait if 0.")
+                        help="Seconds to wait. Block until the future is complete if negative. \
+                            Don't wait if 0.")
 
     group = parser.add_mutually_exclusive_group(required=True)
     group.add_argument('-t', '--turtlebot_type', type=str,
@@ -67,7 +68,7 @@ def main():
     if args.turtlebot_type is not None:
         sdf_file_path = os.path.join(
             get_package_share_directory('turtlebot3_gazebo'), 'models',
-            'turtlebot3_{}'.format(args.turtlebot_type), 'model.sdf')
+            f'turtlebot3_{args.turtlebot_type}', 'model.sdf')
     else:
         sdf_file_path = args.sdf
 
@@ -78,16 +79,15 @@ def main():
     tree = ET.parse(sdf_file_path)
     root = tree.getroot()
     for plugin in root.iter('plugin'):
-        # TODO(orduno) Handle case if an sdf file from non-turtlebot is provided
-        if 'turtlebot3_diff_drive' in plugin.attrib.values():
+        if 'ros_diff_drive' in plugin.get('filename'):
             # The only plugin we care for now is 'diff_drive' which is
             # broadcasting a transform between`odom` and `base_footprint`
             break
 
     if args.robot_namespace:
         ros_params = plugin.find('ros')
         ros_tf_remap = ET.SubElement(ros_params, 'remapping')
-        ros_tf_remap.text = '/tf:=/' + args.robot_namespace + '/tf'
+        ros_tf_remap.text = f'/tf:=/{args.robot_namespace}/tf'
 
     # Set data for request
     request = SpawnEntity.Request()
@@ -102,10 +102,10 @@ def main():
     future = client.call_async(request)
     rclpy.spin_until_future_complete(node, future, timeout_sec=args.timeout)
     if future.result() is not None:
-        print('response: %r' % future.result())
+        print(f'response: {future.result()!r}')
     else:
         raise RuntimeError(
-            'exception while calling service: %r' % future.exception())
+            f'exception while calling service: {future.exception()!r}')
 
     node.get_logger().info('Done! Shutting down node.')
     node.destroy_node()

diff --git a/nav2_bringup/nav2_gazebo_spawner/package.xml b/nav2_bringup/nav2_gazebo_spawner/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_gazebo_spawner</name>
-  <version>1.0.6</version>
+  <version>1.0.7</version>
   <description>Package for spawning a robot model into Gazebo for Nav2</description>
   <maintainer email="carlos.orduno@intel.com">lkumarbe</maintainer>
   <maintainer email="lalit.kumar.begani@intel.com">lkumarbe</maintainer>

diff --git a/nav2_bt_navigator/package.xml b/nav2_bt_navigator/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_bt_navigator</name>
-  <version>1.0.6</version>
+  <version>1.0.7</version>
   <description>TODO</description>
   <maintainer email="michael.jeronimo@intel.com">Michael Jeronimo</maintainer>
   <license>Apache-2.0</license>

diff --git a/nav2_common/package.xml b/nav2_common/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_common</name>
-  <version>1.0.6</version>
+  <version>1.0.7</version>
   <description>Common support functionality used throughout the navigation 2 stack</description>
   <maintainer email="carl.r.delsey@intel.com">Carl Delsey</maintainer>
   <license>Apache-2.0</license>

diff --git a/nav2_controller/CMakeLists.txt b/nav2_controller/CMakeLists.txt
@@ -110,6 +110,7 @@ ament_export_libraries(simple_progress_checker
   simple_goal_checker
   stopped_goal_checker
   ${library_name})
+ament_export_dependencies(${dependencies})
 pluginlib_export_plugin_description_file(nav2_core plugins.xml)
 
 ament_package()
diff --git a/nav2_controller/package.xml b/nav2_controller/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_controller</name>
-  <version>1.0.6</version>
+  <version>1.0.7</version>
   <description>Controller action interface</description>
   <maintainer email="carl.r.delsey@intel.com">Carl Delsey</maintainer>
   <license>Apache-2.0</license>

diff --git a/nav2_controller/src/nav2_controller.cpp b/nav2_controller/src/nav2_controller.cpp
@@ -71,6 +71,7 @@ ControllerServer::~ControllerServer()
   progress_checker_.reset();
   goal_checkers_.clear();
   controllers_.clear();
+  costmap_thread_.reset();
 }
 
 nav2_util::CallbackReturn

diff --git a/nav2_core/package.xml b/nav2_core/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_core</name>
-  <version>1.0.6</version>
+  <version>1.0.7</version>
   <description>A set of headers for plugins core to the Nav2 stack</description>
   <maintainer email="stevenmacenski@gmail.com">Steve Macenski</maintainer>
   <maintainer email="carl.r.delsey@intel.com">Carl Delsey</maintainer>

diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/layered_costmap.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/layered_costmap.hpp
@@ -146,10 +146,8 @@ class LayeredCostmap
   /**
    * @brief Add a new plugin to the plugins vector to process
    */
-  void addPlugin(std::shared_ptr<Layer> plugin)
-  {
-    plugins_.push_back(plugin);
-  }
+  void addPlugin(std::shared_ptr<Layer> plugin);
+
 
   /**
    * @brief Add a new costmap filter plugin to the filters vector to process

diff --git a/nav2_costmap_2d/package.xml b/nav2_costmap_2d/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_costmap_2d</name>
-  <version>1.0.6</version>
+  <version>1.0.7</version>
   <description>
     This package provides an implementation of a 2D costmap that takes in sensor
     data from the world, builds a 2D or 3D occupancy grid of the data (depending

diff --git a/nav2_costmap_2d/plugins/obstacle_layer.cpp b/nav2_costmap_2d/plugins/obstacle_layer.cpp
@@ -218,7 +218,7 @@ void ObstacleLayer::onInitialize()
 
       std::shared_ptr<tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan>> filter(
         new tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan>(
-          *sub, *tf_, global_frame_, 50, rclcpp_node_));
+          *sub, *tf_, global_frame_, 50, rclcpp_node_, tf2::durationFromSec(transform_tolerance)));
 
       if (inf_is_valid) {
         filter->registerCallback(
@@ -252,7 +252,7 @@ void ObstacleLayer::onInitialize()
 
       std::shared_ptr<tf2_ros::MessageFilter<sensor_msgs::msg::PointCloud2>> filter(
         new tf2_ros::MessageFilter<sensor_msgs::msg::PointCloud2>(
-          *sub, *tf_, global_frame_, 50, rclcpp_node_));
+          *sub, *tf_, global_frame_, 50, rclcpp_node_, tf2::durationFromSec(transform_tolerance)));
 
       filter->registerCallback(
         std::bind(
@@ -291,6 +291,13 @@ ObstacleLayer::laserScanCallback(
       global_frame_.c_str(),
       ex.what());
     projector_.projectLaser(*message, cloud);
+  } catch (std::runtime_error & ex) {
+    RCLCPP_WARN(
+      logger_,
+      "transformLaserScanToPointCloud error, it seems the message from laser is malformed."
+      " Ignore this message. what(): %s",
+      ex.what());
+    return;
   }
 
   // buffer the point cloud
@@ -327,6 +334,13 @@ ObstacleLayer::laserScanValidInfCallback(
       "High fidelity enabled, but TF returned a transform exception to frame %s: %s",
       global_frame_.c_str(), ex.what());
     projector_.projectLaser(message, cloud);
+  } catch (std::runtime_error & ex) {
+    RCLCPP_WARN(
+      logger_,
+      "transformLaserScanToPointCloud error, it seems the message from laser is malformed."
+      " Ignore this message. what(): %s",
+      ex.what());
+    return;
   }
 
   // buffer the point cloud

diff --git a/nav2_costmap_2d/plugins/voxel_layer.cpp b/nav2_costmap_2d/plugins/voxel_layer.cpp
@@ -110,6 +110,7 @@ VoxelLayer::~VoxelLayer()
 
 void VoxelLayer::matchSize()
 {
+  std::lock_guard<Costmap2D::mutex_t> guard(*getMutex());
   ObstacleLayer::matchSize();
   voxel_grid_.resize(size_x_, size_y_, size_z_);
   assert(voxel_grid_.sizeX() == size_x_ && voxel_grid_.sizeY() == size_y_);

diff --git a/nav2_costmap_2d/src/clear_costmap_service.cpp b/nav2_costmap_2d/src/clear_costmap_service.cpp
@@ -129,8 +129,8 @@ void ClearCostmapService::clearLayerRegion(
   double end_point_y = start_point_y + reset_distance;
 
   int start_x, start_y, end_x, end_y;
-  costmap->worldToMapNoBounds(start_point_x, start_point_y, start_x, start_y);
-  costmap->worldToMapNoBounds(end_point_x, end_point_y, end_x, end_y);
+  costmap->worldToMapEnforceBounds(start_point_x, start_point_y, start_x, start_y);
+  costmap->worldToMapEnforceBounds(end_point_x, end_point_y, end_x, end_y);
 
   costmap->clearArea(start_x, start_y, end_x, end_y, invert);