Galactic sync 4 (Sept 15)

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="michael.jeronimo@intel.com">Michael Jeronimo</maintainer>
   <maintainer email="carlos.a.orduno@intel.com">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="michael.jeronimo@intel.com">Michael Jeronimo</maintainer>
   <maintainer email="stevenmacenski@gmail.com">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

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()

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>

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>

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>

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()

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>

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

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>

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

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

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

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

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