Merge remote-tracking branch 'upstream/noetic-devel' into noetic-devel

tue-robotics · Feb 1, 2023 · 10e077c · 10e077c
2 parents 6712046 + 85e5f33
commit 10e077c
Show file tree

Hide file tree

Showing 55 changed files with 268 additions and 75 deletions.
diff --git a/amcl/CHANGELOG.rst b/amcl/CHANGELOG.rst
@@ -2,6 +2,22 @@
 Changelog for package amcl
 ^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.17.3 (2023-01-10)
+-------------------
+* [AMCL] Add option to force nomotion update after initialpose (`#1226 <https://github.com/ros-planning/navigation/issues/1226>`_)
+  * Adds a new boolean parameter force_update_after_initialpose. When set to true, an update is forced on the next laser scan callback, such as when the /request_nomotion_update service is called. This often results in an improved robot pose after a manual (not very precise) re-localization - without a need for the robot to move.
+  * Fixes a bunch of compiler warnings (unused variable now, catching exceptions by value), normalizes how tf exceptions are caught
+* [ROS-O] various patches (`#1225 <https://github.com/ros-planning/navigation/issues/1225>`_)
+  * do not specify obsolete c++11 standard
+  this breaks with current versions of log4cxx.
+  * update pluginlib include paths
+  the non-hpp headers have been deprecated since kinetic
+  * use lambdas in favor of boost::bind
+  Using boost's _1 as a global system is deprecated since C++11.
+  The ROS packages in Debian removed the implicit support for the global symbols,
+  so this code fails to compile there without the patch.
+* Contributors: Michael Görner, Stephan
+
 1.17.2 (2022-06-20)
 -------------------
 * Update pf.c (`#1161 <https://github.com/ros-planning/navigation/issues/1161>`_)

diff --git a/amcl/CMakeLists.txt b/amcl/CMakeLists.txt
@@ -4,10 +4,6 @@ project(amcl)
 include(CheckIncludeFile)
 include(CheckSymbolExists)
 
-if(NOT CMAKE_CXX_STANDARD)
-  set(CMAKE_CXX_STANDARD 11)
-endif()
-
 find_package(catkin REQUIRED
   COMPONENTS
     diagnostic_updater

diff --git a/amcl/cfg/AMCL.cfg b/amcl/cfg/AMCL.cfg
@@ -28,6 +28,8 @@ gen.add("beam_skip_distance", double_t, 0, "Distance from a valid map point befo
 gen.add("beam_skip_threshold", double_t, 0, "Ratio of samples for which the scans are valid to consider as valid scan", 0.3, 0, 1)
 
 gen.add("tf_broadcast", bool_t, 0, "When true (the default), publish results via TF.  When false, do not.", True)
+gen.add("force_update_after_initialpose", bool_t, 0, "When true, force a pose update after a new /initialpose is received.", False)
+gen.add("force_update_after_set_map", bool_t, 0, "When true, force a pose update after a new map (and pose) has been set via the /set_map service.", False)
 gen.add("gui_publish_rate", double_t, 0, "Maximum rate (Hz) at which scans and paths are published for visualization, -1.0 to disable.", -1, -1, 100)
 gen.add("save_pose_rate", double_t, 0, "Maximum rate (Hz) at which to store the last estimated pose and covariance to the parameter server, in the variables ~initial_pose_* and ~initial_cov_*. This saved pose will be used on subsequent runs to initialize the filter. -1.0 to disable.", .5, -1, 10)
 

diff --git a/amcl/package.xml b/amcl/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="2">
     <name>amcl</name>
-    <version>1.17.2</version>
+    <version>1.17.3</version>
     <description>
         <p>
             amcl is a probabilistic localization system for a robot moving in

diff --git a/amcl/src/amcl_node.cpp b/amcl/src/amcl_node.cpp
@@ -284,6 +284,8 @@ class AmclNode
     double init_cov_[3];
     laser_model_t laser_model_type_;
     bool tf_broadcast_;
+    bool force_update_after_initialpose_;
+    bool force_update_after_set_map_;
     bool selective_resampling_;
 
     void reconfigureCB(amcl::AMCLConfig &config, uint32_t level);
@@ -445,6 +447,8 @@ AmclNode::AmclNode() :
   private_nh_.param("recovery_alpha_slow", alpha_slow_, 0.001);
   private_nh_.param("recovery_alpha_fast", alpha_fast_, 0.1);
   private_nh_.param("tf_broadcast", tf_broadcast_, true);
+  private_nh_.param("force_update_after_initialpose", force_update_after_initialpose_, false);
+  private_nh_.param("force_update_after_set_map", force_update_after_set_map_, false);
 
   // For diagnostics
   private_nh_.param("std_warn_level_x", std_warn_level_x_, 0.2);
@@ -584,6 +588,8 @@ void AmclNode::reconfigureCB(AMCLConfig &config, uint32_t level)
   alpha_slow_ = config.recovery_alpha_slow;
   alpha_fast_ = config.recovery_alpha_fast;
   tf_broadcast_ = config.tf_broadcast;
+  force_update_after_initialpose_ = config.force_update_after_initialpose;
+  force_update_after_set_map_ = config.force_update_after_set_map;
 
   do_beamskip_= config.do_beamskip; 
   beam_skip_distance_ = config.beam_skip_distance; 
@@ -1028,7 +1034,7 @@ AmclNode::getOdomPose(geometry_msgs::PoseStamped& odom_pose,
   {
     this->tf_->transform(ident, odom_pose, odom_frame_id_);
   }
-  catch(tf2::TransformException e)
+  catch(const tf2::TransformException& e)
   {
     ROS_WARN("Failed to compute odom pose, skipping scan (%s)", e.what());
     return false;
@@ -1111,6 +1117,10 @@ AmclNode::setMapCallback(nav_msgs::SetMap::Request& req,
 {
   handleMapMessage(req.map);
   handleInitialPoseMessage(req.initial_pose);
+  if (force_update_after_set_map_)
+  {
+    m_force_update = true;
+  }
   res.success = true;
   return true;
 }
@@ -1144,7 +1154,7 @@ AmclNode::laserReceived(const sensor_msgs::LaserScanConstPtr& laser_scan)
     {
       this->tf_->transform(ident, laser_pose, base_frame_id_);
     }
-    catch(tf2::TransformException& e)
+    catch(const tf2::TransformException& e)
     {
       ROS_ERROR("Couldn't transform from %s to %s, "
                 "even though the message notifier is in use",
@@ -1267,7 +1277,7 @@ AmclNode::laserReceived(const sensor_msgs::LaserScanConstPtr& laser_scan)
       tf_->transform(min_q, min_q, base_frame_id_);
       tf_->transform(inc_q, inc_q, base_frame_id_);
     }
-    catch(tf2::TransformException& e)
+    catch(const tf2::TransformException& e)
     {
       ROS_WARN("Unable to transform min/max laser angles into base frame: %s",
                e.what());
@@ -1459,7 +1469,7 @@ AmclNode::laserReceived(const sensor_msgs::LaserScanConstPtr& laser_scan)
 
         this->tf_->transform(tmp_tf_stamped, odom_to_map, odom_frame_id_);
       }
-      catch(tf2::TransformException)
+      catch(const tf2::TransformException&)
       {
         ROS_DEBUG("Failed to subtract base to odom transform");
         return;
@@ -1522,6 +1532,10 @@ void
 AmclNode::initialPoseReceived(const geometry_msgs::PoseWithCovarianceStampedConstPtr& msg)
 {
   handleInitialPoseMessage(*msg);
+  if (force_update_after_initialpose_)
+  {
+    m_force_update = true;
+  }
 }
 
 void
@@ -1547,13 +1561,12 @@ AmclNode::handleInitialPoseMessage(const geometry_msgs::PoseWithCovarianceStampe
   geometry_msgs::TransformStamped tx_odom;
   try
   {
-    ros::Time now = ros::Time::now();
     // wait a little for the latest tf to become available
     tx_odom = tf_->lookupTransform(base_frame_id_, msg.header.stamp,
                                    base_frame_id_, ros::Time::now(),
                                    odom_frame_id_, ros::Duration(0.5));
   }
-  catch(tf2::TransformException e)
+  catch(const tf2::TransformException& e)
   {
     // If we've never sent a transform, then this is normal, because the
     // global_frame_id_ frame doesn't exist.  We only care about in-time

diff --git a/base_local_planner/CHANGELOG.rst b/base_local_planner/CHANGELOG.rst
@@ -2,6 +2,19 @@
 Changelog for package base_local_planner
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.17.3 (2023-01-10)
+-------------------
+* [ROS-O] various patches (`#1225 <https://github.com/ros-planning/navigation/issues/1225>`_)
+  * do not specify obsolete c++11 standard
+  this breaks with current versions of log4cxx.
+  * update pluginlib include paths
+  the non-hpp headers have been deprecated since kinetic
+  * use lambdas in favor of boost::bind
+  Using boost's _1 as a global system is deprecated since C++11.
+  The ROS packages in Debian removed the implicit support for the global symbols,
+  so this code fails to compile there without the patch.
+* Contributors: Michael Görner
+
 1.17.2 (2022-06-20)
 -------------------
 * Commit 89a8593 removed footprint scaling. This brings it back. (`#886 <https://github.com/ros-planning/navigation/issues/886>`_) (`#1204 <https://github.com/ros-planning/navigation/issues/1204>`_)

diff --git a/base_local_planner/package.xml b/base_local_planner/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="2">
     <name>base_local_planner</name>
-    <version>1.17.2</version>
+    <version>1.17.3</version>
     <description>
 
         This package provides implementations of the Trajectory Rollout and Dynamic Window approaches to local robot navigation on a plane. Given a plan to follow and a costmap, the controller produces velocity commands to send to a mobile base. This package supports both holonomic and non-holonomic robots, any robot footprint that can be represented as a convex polygon or circle, and exposes its configuration as ROS parameters that can be set in a launch file. This package's ROS wrapper adheres to the BaseLocalPlanner interface specified in the <a href="http://wiki.ros.org/nav_core">nav_core</a> package. 

diff --git a/base_local_planner/src/odometry_helper_ros.cpp b/base_local_planner/src/odometry_helper_ros.cpp
@@ -94,7 +94,7 @@ void OdometryHelperRos::setOdomTopic(std::string odom_topic)
     if( odom_topic_ != "" )
     {
       ros::NodeHandle gn;
-      odom_sub_ = gn.subscribe<nav_msgs::Odometry>( odom_topic_, 1, boost::bind( &OdometryHelperRos::odomCallback, this, _1 ));
+      odom_sub_ = gn.subscribe<nav_msgs::Odometry>( odom_topic_, 1, [this](auto& msg){ odomCallback(msg); });
     }
     else
     {

diff --git a/base_local_planner/src/trajectory_planner_ros.cpp b/base_local_planner/src/trajectory_planner_ros.cpp
@@ -48,7 +48,7 @@
 
 #include <ros/console.h>
 
-#include <pluginlib/class_list_macros.h>
+#include <pluginlib/class_list_macros.hpp>
 
 #include <base_local_planner/goal_functions.h>
 #include <nav_msgs/Path.h>
@@ -256,11 +256,15 @@ namespace base_local_planner {
           max_vel_x, min_vel_x, max_vel_th_, min_vel_th_, min_in_place_vel_th_, backup_vel,
           dwa, heading_scoring, heading_scoring_timestep, meter_scoring, simple_attractor, y_vels, stop_time_buffer, sim_period_, angular_sim_granularity);
 
-      map_viz_.initialize(name, global_frame_, boost::bind(&TrajectoryPlanner::getCellCosts, tc_, _1, _2, _3, _4, _5, _6));
+      map_viz_.initialize(name,
+                          global_frame_,
+                          [this](int cx, int cy, float &path_cost, float &goal_cost, float &occ_cost, float &total_cost){
+                              return tc_->getCellCosts(cx, cy, path_cost, goal_cost, occ_cost, total_cost);
+                          });
       initialized_ = true;
 
       dsrv_ = new dynamic_reconfigure::Server<BaseLocalPlannerConfig>(private_nh);
-      dynamic_reconfigure::Server<BaseLocalPlannerConfig>::CallbackType cb = boost::bind(&TrajectoryPlannerROS::reconfigureCB, this, _1, _2);
+      dynamic_reconfigure::Server<BaseLocalPlannerConfig>::CallbackType cb = [this](auto& config, auto level){ reconfigureCB(config, level); };
       dsrv_->setCallback(cb);
 
     } else {

diff --git a/carrot_planner/CHANGELOG.rst b/carrot_planner/CHANGELOG.rst
@@ -2,6 +2,19 @@
 Changelog for package carrot_planner
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.17.3 (2023-01-10)
+-------------------
+* [ROS-O] various patches (`#1225 <https://github.com/ros-planning/navigation/issues/1225>`_)
+  * do not specify obsolete c++11 standard
+  this breaks with current versions of log4cxx.
+  * update pluginlib include paths
+  the non-hpp headers have been deprecated since kinetic
+  * use lambdas in favor of boost::bind
+  Using boost's _1 as a global system is deprecated since C++11.
+  The ROS packages in Debian removed the implicit support for the global symbols,
+  so this code fails to compile there without the patch.
+* Contributors: Michael Görner
+
 1.17.2 (2022-06-20)
 -------------------
 * Fix carrot planner (`#1056 <https://github.com/ros-planning/navigation/issues/1056>`_)

diff --git a/carrot_planner/package.xml b/carrot_planner/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="2">
     <name>carrot_planner</name>
-    <version>1.17.2</version>
+    <version>1.17.3</version>
     <description>
 
         This planner attempts to find a legal place to put a carrot for the robot to follow. It does this by moving back along the vector between the robot and the goal point.

diff --git a/carrot_planner/src/carrot_planner.cpp b/carrot_planner/src/carrot_planner.cpp
@@ -36,7 +36,7 @@
 *********************************************************************/
 #include <angles/angles.h>
 #include <carrot_planner/carrot_planner.h>
-#include <pluginlib/class_list_macros.h>
+#include <pluginlib/class_list_macros.hpp>
 #include <tf2/convert.h>
 #include <tf2/utils.h>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>

diff --git a/clear_costmap_recovery/CHANGELOG.rst b/clear_costmap_recovery/CHANGELOG.rst
@@ -2,6 +2,19 @@
 Changelog for package clear_costmap_recovery
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.17.3 (2023-01-10)
+-------------------
+* [ROS-O] various patches (`#1225 <https://github.com/ros-planning/navigation/issues/1225>`_)
+  * do not specify obsolete c++11 standard
+  this breaks with current versions of log4cxx.
+  * update pluginlib include paths
+  the non-hpp headers have been deprecated since kinetic
+  * use lambdas in favor of boost::bind
+  Using boost's _1 as a global system is deprecated since C++11.
+  The ROS packages in Debian removed the implicit support for the global symbols,
+  so this code fails to compile there without the patch.
+* Contributors: Michael Görner
+
 1.17.2 (2022-06-20)
 -------------------
 * check if the layer is derived from costmap_2d::CostmapLayer (`#1054 <https://github.com/ros-planning/navigation/issues/1054>`_)

diff --git a/clear_costmap_recovery/package.xml b/clear_costmap_recovery/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="2">
     <name>clear_costmap_recovery</name>
-    <version>1.17.2</version>
+    <version>1.17.3</version>
     <description>
 
         This package provides a recovery behavior for the navigation stack that attempts to clear space by reverting the costmaps used by the navigation stack to the static map outside of a given area.

diff --git a/clear_costmap_recovery/src/clear_costmap_recovery.cpp b/clear_costmap_recovery/src/clear_costmap_recovery.cpp
@@ -35,7 +35,7 @@
 * Author: Eitan Marder-Eppstein
 *********************************************************************/
 #include <clear_costmap_recovery/clear_costmap_recovery.h>
-#include <pluginlib/class_list_macros.h>
+#include <pluginlib/class_list_macros.hpp>
 #include <boost/pointer_cast.hpp>
 #include <vector>
 

diff --git a/costmap_2d/CHANGELOG.rst b/costmap_2d/CHANGELOG.rst
@@ -2,6 +2,19 @@
 Changelog for package costmap_2d
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.17.3 (2023-01-10)
+-------------------
+* [ROS-O] various patches (`#1225 <https://github.com/ros-planning/navigation/issues/1225>`_)
+  * do not specify obsolete c++11 standard
+  this breaks with current versions of log4cxx.
+  * update pluginlib include paths
+  the non-hpp headers have been deprecated since kinetic
+  * use lambdas in favor of boost::bind
+  Using boost's _1 as a global system is deprecated since C++11.
+  The ROS packages in Debian removed the implicit support for the global symbols,
+  so this code fails to compile there without the patch.
+* Contributors: Michael Görner
+
 1.17.2 (2022-06-20)
 -------------------
 * Removed unused variables in costmap_2d_ros (`#1126 <https://github.com/ros-planning/navigation/issues/1126>`_)

diff --git a/costmap_2d/package.xml b/costmap_2d/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="2">
     <name>costmap_2d</name>
-    <version>1.17.2</version>
+    <version>1.17.3</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/costmap_2d/plugins/inflation_layer.cpp b/costmap_2d/plugins/inflation_layer.cpp
@@ -40,7 +40,7 @@
 #include <costmap_2d/costmap_math.h>
 #include <costmap_2d/footprint.h>
 #include <boost/thread.hpp>
-#include <pluginlib/class_list_macros.h>
+#include <pluginlib/class_list_macros.hpp>
 
 PLUGINLIB_EXPORT_CLASS(costmap_2d::InflationLayer, costmap_2d::Layer)
 
@@ -87,8 +87,8 @@ void InflationLayer::onInitialize()
     seen_size_ = 0;
     need_reinflation_ = false;
 
-    dynamic_reconfigure::Server<costmap_2d::InflationPluginConfig>::CallbackType cb = boost::bind(
-        &InflationLayer::reconfigureCB, this, _1, _2);
+    dynamic_reconfigure::Server<costmap_2d::InflationPluginConfig>::CallbackType cb =
+        [this](auto& config, auto level){ reconfigureCB(config, level); };
 
     if (dsrv_ != NULL){
       dsrv_->clearCallback();

diff --git a/costmap_2d/plugins/obstacle_layer.cpp b/costmap_2d/plugins/obstacle_layer.cpp
@@ -39,7 +39,7 @@
 #include <costmap_2d/costmap_math.h>
 #include <tf2_ros/message_filter.h>
 
-#include <pluginlib/class_list_macros.h>
+#include <pluginlib/class_list_macros.hpp>
 #include <sensor_msgs/point_cloud2_iterator.h>
 
 PLUGINLIB_EXPORT_CLASS(costmap_2d::ObstacleLayer, costmap_2d::Layer)
@@ -158,12 +158,11 @@ void ObstacleLayer::onInitialize()
 
       if (inf_is_valid)
       {
-        filter->registerCallback(boost::bind(&ObstacleLayer::laserScanValidInfCallback, this, _1,
-                                            observation_buffers_.back()));
+        filter->registerCallback([this,buffer=observation_buffers_.back()](auto& msg){ laserScanValidInfCallback(msg, buffer); });
       }
       else
       {
-        filter->registerCallback(boost::bind(&ObstacleLayer::laserScanCallback, this, _1, observation_buffers_.back()));
+        filter->registerCallback([this,buffer=observation_buffers_.back()](auto& msg){ laserScanCallback(msg, buffer); });
       }
 
       observation_subscribers_.push_back(sub);
@@ -183,8 +182,7 @@ void ObstacleLayer::onInitialize()
 
         boost::shared_ptr < tf2_ros::MessageFilter<sensor_msgs::PointCloud>
         > filter(new tf2_ros::MessageFilter<sensor_msgs::PointCloud>(*sub, *tf_, global_frame_, 50, g_nh));
-        filter->registerCallback(
-          boost::bind(&ObstacleLayer::pointCloudCallback, this, _1, observation_buffers_.back()));
+        filter->registerCallback([this,buffer=observation_buffers_.back()](auto& msg){ pointCloudCallback(msg, buffer); });
 
       observation_subscribers_.push_back(sub);
       observation_notifiers_.push_back(filter);
@@ -201,8 +199,7 @@ void ObstacleLayer::onInitialize()
 
       boost::shared_ptr < tf2_ros::MessageFilter<sensor_msgs::PointCloud2>
       > filter(new tf2_ros::MessageFilter<sensor_msgs::PointCloud2>(*sub, *tf_, global_frame_, 50, g_nh));
-      filter->registerCallback(
-          boost::bind(&ObstacleLayer::pointCloud2Callback, this, _1, observation_buffers_.back()));
+      filter->registerCallback([this,buffer=observation_buffers_.back()](auto& msg){ pointCloud2Callback(msg, buffer); });
 
       observation_subscribers_.push_back(sub);
       observation_notifiers_.push_back(filter);
@@ -224,8 +221,8 @@ void ObstacleLayer::onInitialize()
 void ObstacleLayer::setupDynamicReconfigure(ros::NodeHandle& nh)
 {
   dsrv_ = new dynamic_reconfigure::Server<costmap_2d::ObstaclePluginConfig>(nh);
-  dynamic_reconfigure::Server<costmap_2d::ObstaclePluginConfig>::CallbackType cb = boost::bind(
-      &ObstacleLayer::reconfigureCB, this, _1, _2);
+  dynamic_reconfigure::Server<costmap_2d::ObstaclePluginConfig>::CallbackType cb =
+      [this](auto& config, auto level){ reconfigureCB(config, level); };
   dsrv_->setCallback(cb);
 }