From f94f7b0f31b094e097c0203be4be859529d612f7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Michael=20G=C3=B6rner?= Date: Wed, 9 Nov 2022 18:23:26 +0100 Subject: [PATCH 1/6] [ROS-O] various patches (#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. --- amcl/CMakeLists.txt | 4 ---- base_local_planner/src/odometry_helper_ros.cpp | 2 +- .../src/trajectory_planner_ros.cpp | 10 +++++++--- carrot_planner/src/carrot_planner.cpp | 2 +- .../src/clear_costmap_recovery.cpp | 2 +- costmap_2d/plugins/inflation_layer.cpp | 6 +++--- costmap_2d/plugins/obstacle_layer.cpp | 17 +++++++---------- costmap_2d/plugins/static_layer.cpp | 6 +++--- costmap_2d/plugins/voxel_layer.cpp | 6 +++--- costmap_2d/src/costmap_2d_cloud.cpp | 2 +- costmap_2d/src/costmap_2d_markers.cpp | 2 +- costmap_2d/src/costmap_2d_ros.cpp | 3 +-- dwa_local_planner/src/dwa_planner.cpp | 6 +++++- dwa_local_planner/src/dwa_planner_ros.cpp | 6 +++--- fake_localization/fake_localization.cpp | 4 ++-- global_planner/src/planner_core.cpp | 6 +++--- map_server/test/rtest.cpp | 6 +++--- move_base/src/move_base.cpp | 6 +++--- move_slow_and_clear/src/move_slow_and_clear.cpp | 2 +- navfn/src/navfn_ros.cpp | 2 +- rotate_recovery/src/rotate_recovery.cpp | 2 +- 21 files changed, 51 insertions(+), 51 deletions(-) diff --git a/amcl/CMakeLists.txt b/amcl/CMakeLists.txt index c10a9a5340..63071195f5 100644 --- 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/base_local_planner/src/odometry_helper_ros.cpp b/base_local_planner/src/odometry_helper_ros.cpp index 15636642bf..cc497c4add 100644 --- 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( odom_topic_, 1, boost::bind( &OdometryHelperRos::odomCallback, this, _1 )); + odom_sub_ = gn.subscribe( 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 index b7aaea6190..e1ff245188 100644 --- a/base_local_planner/src/trajectory_planner_ros.cpp +++ b/base_local_planner/src/trajectory_planner_ros.cpp @@ -48,7 +48,7 @@ #include -#include +#include #include #include @@ -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(private_nh); - dynamic_reconfigure::Server::CallbackType cb = boost::bind(&TrajectoryPlannerROS::reconfigureCB, this, _1, _2); + dynamic_reconfigure::Server::CallbackType cb = [this](auto& config, auto level){ reconfigureCB(config, level); }; dsrv_->setCallback(cb); } else { diff --git a/carrot_planner/src/carrot_planner.cpp b/carrot_planner/src/carrot_planner.cpp index c801cedf43..4d6fb9bcaf 100644 --- a/carrot_planner/src/carrot_planner.cpp +++ b/carrot_planner/src/carrot_planner.cpp @@ -36,7 +36,7 @@ *********************************************************************/ #include #include -#include +#include #include #include #include diff --git a/clear_costmap_recovery/src/clear_costmap_recovery.cpp b/clear_costmap_recovery/src/clear_costmap_recovery.cpp index c2457f58aa..cc9ffc868b 100644 --- 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 -#include +#include #include #include diff --git a/costmap_2d/plugins/inflation_layer.cpp b/costmap_2d/plugins/inflation_layer.cpp index e428c4f472..4f6a4cde4a 100644 --- a/costmap_2d/plugins/inflation_layer.cpp +++ b/costmap_2d/plugins/inflation_layer.cpp @@ -40,7 +40,7 @@ #include #include #include -#include +#include PLUGINLIB_EXPORT_CLASS(costmap_2d::InflationLayer, costmap_2d::Layer) @@ -83,8 +83,8 @@ void InflationLayer::onInitialize() seen_size_ = 0; need_reinflation_ = false; - dynamic_reconfigure::Server::CallbackType cb = boost::bind( - &InflationLayer::reconfigureCB, this, _1, _2); + dynamic_reconfigure::Server::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 index 2bcdecdaf2..358dc9b460 100644 --- a/costmap_2d/plugins/obstacle_layer.cpp +++ b/costmap_2d/plugins/obstacle_layer.cpp @@ -39,7 +39,7 @@ #include #include -#include +#include #include 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 > filter(new tf2_ros::MessageFilter(*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 > filter(new tf2_ros::MessageFilter(*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(nh); - dynamic_reconfigure::Server::CallbackType cb = boost::bind( - &ObstacleLayer::reconfigureCB, this, _1, _2); + dynamic_reconfigure::Server::CallbackType cb = + [this](auto& config, auto level){ reconfigureCB(config, level); }; dsrv_->setCallback(cb); } diff --git a/costmap_2d/plugins/static_layer.cpp b/costmap_2d/plugins/static_layer.cpp index f6ea332843..845c401f4f 100644 --- a/costmap_2d/plugins/static_layer.cpp +++ b/costmap_2d/plugins/static_layer.cpp @@ -38,7 +38,7 @@ *********************************************************************/ #include #include -#include +#include #include #include @@ -118,8 +118,8 @@ void StaticLayer::onInitialize() } dsrv_ = new dynamic_reconfigure::Server(nh); - dynamic_reconfigure::Server::CallbackType cb = boost::bind( - &StaticLayer::reconfigureCB, this, _1, _2); + dynamic_reconfigure::Server::CallbackType cb = + [this](auto& config, auto level){ reconfigureCB(config, level); }; dsrv_->setCallback(cb); } diff --git a/costmap_2d/plugins/voxel_layer.cpp b/costmap_2d/plugins/voxel_layer.cpp index eca7a32d5f..9927ddda9a 100644 --- a/costmap_2d/plugins/voxel_layer.cpp +++ b/costmap_2d/plugins/voxel_layer.cpp @@ -36,7 +36,7 @@ * David V. Lu!! *********************************************************************/ #include -#include +#include #include #define VOXEL_BITS 16 @@ -67,8 +67,8 @@ void VoxelLayer::onInitialize() void VoxelLayer::setupDynamicReconfigure(ros::NodeHandle& nh) { voxel_dsrv_ = new dynamic_reconfigure::Server(nh); - dynamic_reconfigure::Server::CallbackType cb = boost::bind( - &VoxelLayer::reconfigureCB, this, _1, _2); + dynamic_reconfigure::Server::CallbackType cb = + [this](auto& config, auto level){ reconfigureCB(config, level); }; voxel_dsrv_->setCallback(cb); } diff --git a/costmap_2d/src/costmap_2d_cloud.cpp b/costmap_2d/src/costmap_2d_cloud.cpp index 0eb62587df..98dec020b4 100644 --- a/costmap_2d/src/costmap_2d_cloud.cpp +++ b/costmap_2d/src/costmap_2d_cloud.cpp @@ -199,7 +199,7 @@ int main(int argc, char** argv) ros::Publisher pub_marked = n.advertise < sensor_msgs::PointCloud > ("voxel_marked_cloud", 2); ros::Publisher pub_unknown = n.advertise < sensor_msgs::PointCloud > ("voxel_unknown_cloud", 2); ros::Subscriber sub = n.subscribe < costmap_2d::VoxelGrid - > ("voxel_grid", 1, boost::bind(voxelCallback, pub_marked, pub_unknown, _1)); + > ("voxel_grid", 1, [&pub_marked,&pub_unknown](auto& msg){ voxelCallback(pub_marked, pub_unknown, msg); }); ros::spin(); diff --git a/costmap_2d/src/costmap_2d_markers.cpp b/costmap_2d/src/costmap_2d_markers.cpp index 63b072efe4..4da5f78f3c 100644 --- a/costmap_2d/src/costmap_2d_markers.cpp +++ b/costmap_2d/src/costmap_2d_markers.cpp @@ -146,7 +146,7 @@ int main(int argc, char** argv) ROS_DEBUG("Startup"); ros::Publisher pub = n.advertise < visualization_msgs::Marker > ("visualization_marker", 1); - ros::Subscriber sub = n.subscribe < costmap_2d::VoxelGrid > ("voxel_grid", 1, boost::bind(voxelCallback, pub, _1)); + ros::Subscriber sub = n.subscribe < costmap_2d::VoxelGrid > ("voxel_grid", 1, [&pub](auto& msg){ voxelCallback(pub, msg); }); g_marker_ns = n.resolveName("voxel_grid"); ros::spin(); diff --git a/costmap_2d/src/costmap_2d_ros.cpp b/costmap_2d/src/costmap_2d_ros.cpp index d9e9588828..304c5f0a93 100644 --- a/costmap_2d/src/costmap_2d_ros.cpp +++ b/costmap_2d/src/costmap_2d_ros.cpp @@ -165,8 +165,7 @@ Costmap2DROS::Costmap2DROS(const std::string& name, tf2_ros::Buffer& tf) : stopped_ = false; dsrv_ = new dynamic_reconfigure::Server(ros::NodeHandle("~/" + name)); - dynamic_reconfigure::Server::CallbackType cb = boost::bind(&Costmap2DROS::reconfigureCB, this, _1, - _2); + dynamic_reconfigure::Server::CallbackType cb = [this](auto& config, auto level){ reconfigureCB(config, level); }; dsrv_->setCallback(cb); } diff --git a/dwa_local_planner/src/dwa_planner.cpp b/dwa_local_planner/src/dwa_planner.cpp index d1592c57e1..5abeb6cbb3 100644 --- a/dwa_local_planner/src/dwa_planner.cpp +++ b/dwa_local_planner/src/dwa_planner.cpp @@ -154,7 +154,11 @@ namespace dwa_local_planner { private_nh.param("publish_cost_grid_pc", publish_cost_grid_pc_, false); - map_viz_.initialize(name, planner_util->getGlobalFrame(), boost::bind(&DWAPlanner::getCellCosts, this, _1, _2, _3, _4, _5, _6)); + map_viz_.initialize(name, + planner_util->getGlobalFrame(), + [this](int cx, int cy, float &path_cost, float &goal_cost, float &occ_cost, float &total_cost){ + return getCellCosts(cx, cy, path_cost, goal_cost, occ_cost, total_cost); + }); private_nh.param("global_frame_id", frame_id_, std::string("odom")); diff --git a/dwa_local_planner/src/dwa_planner_ros.cpp b/dwa_local_planner/src/dwa_planner_ros.cpp index d024dcc1e9..4b21ef0fee 100644 --- a/dwa_local_planner/src/dwa_planner_ros.cpp +++ b/dwa_local_planner/src/dwa_planner_ros.cpp @@ -41,7 +41,7 @@ #include -#include +#include #include #include @@ -131,7 +131,7 @@ namespace dwa_local_planner { nav_core::warnRenamedParameter(private_nh, "theta_stopped_vel", "rot_stopped_vel"); dsrv_ = new dynamic_reconfigure::Server(private_nh); - dynamic_reconfigure::Server::CallbackType cb = boost::bind(&DWAPlannerROS::reconfigureCB, this, _1, _2); + dynamic_reconfigure::Server::CallbackType cb = [this](auto& config, auto level){ reconfigureCB(config, level); }; dsrv_->setCallback(cb); } else{ @@ -297,7 +297,7 @@ namespace dwa_local_planner { &planner_util_, odom_helper_, current_pose_, - boost::bind(&DWAPlanner::checkTrajectory, dp_, _1, _2, _3)); + [this](auto pos, auto vel, auto vel_samples){ return dp_->checkTrajectory(pos, vel, vel_samples); }); } else { bool isOk = dwaComputeVelocityCommands(current_pose_, cmd_vel); if (isOk) { diff --git a/fake_localization/fake_localization.cpp b/fake_localization/fake_localization.cpp index 3e7d374f3b..1e1df08e77 100644 --- a/fake_localization/fake_localization.cpp +++ b/fake_localization/fake_localization.cpp @@ -123,12 +123,12 @@ class FakeOdomNode stuff_sub_ = nh.subscribe("base_pose_ground_truth", 100, &FakeOdomNode::stuffFilter, this); filter_sub_ = new message_filters::Subscriber(nh, "", 100); filter_ = new tf2_ros::MessageFilter(*filter_sub_, *m_tfBuffer, base_frame_id_, 100, nh); - filter_->registerCallback(boost::bind(&FakeOdomNode::update, this, _1)); + filter_->registerCallback([this](auto& msg){ update(msg); }); // subscription to "2D Pose Estimate" from RViz: m_initPoseSub = new message_filters::Subscriber(nh, "initialpose", 1); m_initPoseFilter = new tf2_ros::MessageFilter(*m_initPoseSub, *m_tfBuffer, global_frame_id_, 1, nh); - m_initPoseFilter->registerCallback(boost::bind(&FakeOdomNode::initPoseReceived, this, _1)); + m_initPoseFilter->registerCallback([this](auto& msg){ initPoseReceived(msg); }); } ~FakeOdomNode(void) diff --git a/global_planner/src/planner_core.cpp b/global_planner/src/planner_core.cpp index 0aa7f62a0e..ac104cae3e 100644 --- a/global_planner/src/planner_core.cpp +++ b/global_planner/src/planner_core.cpp @@ -36,7 +36,7 @@ * David V. Lu!! *********************************************************************/ #include -#include +#include #include #include @@ -149,8 +149,8 @@ void GlobalPlanner::initialize(std::string name, costmap_2d::Costmap2D* costmap, make_plan_srv_ = private_nh.advertiseService("make_plan", &GlobalPlanner::makePlanService, this); dsrv_ = new dynamic_reconfigure::Server(ros::NodeHandle("~/" + name)); - dynamic_reconfigure::Server::CallbackType cb = boost::bind( - &GlobalPlanner::reconfigureCB, this, _1, _2); + dynamic_reconfigure::Server::CallbackType cb = + [this](auto& config, auto level){ reconfigureCB(config, level); }; dsrv_->setCallback(cb); initialized_ = true; diff --git a/map_server/test/rtest.cpp b/map_server/test/rtest.cpp index 5178952357..27f607f487 100644 --- a/map_server/test/rtest.cpp +++ b/map_server/test/rtest.cpp @@ -97,7 +97,7 @@ TEST_F(MapClientTest, call_service) /* Try to retrieve the map via topic, and compare to ground truth */ TEST_F(MapClientTest, subscribe_topic) { - ros::Subscriber sub = n_->subscribe("map", 1, boost::bind(&MapClientTest::mapCallback, this, _1)); + ros::Subscriber sub = n_->subscribe("map", 1, [this](auto& map){ mapCallback(map); }); // Try a few times, because the server may not be up yet. int i=20; @@ -120,7 +120,7 @@ TEST_F(MapClientTest, subscribe_topic) /* Try to retrieve the metadata via topic, and compare to ground truth */ TEST_F(MapClientTest, subscribe_topic_metadata) { - ros::Subscriber sub = n_->subscribe("map_metadata", 1, boost::bind(&MapClientTest::mapMetaDataCallback, this, _1)); + ros::Subscriber sub = n_->subscribe("map_metadata", 1, [this](auto& map_metadata){ mapMetaDataCallback(map_metadata); }); // Try a few times, because the server may not be up yet. int i=20; @@ -140,7 +140,7 @@ TEST_F(MapClientTest, subscribe_topic_metadata) /* Change the map, retrieve the map via topic, and compare to ground truth */ TEST_F(MapClientTest, change_map) { - ros::Subscriber sub = n_->subscribe("map", 1, boost::bind(&MapClientTest::mapCallback, this, _1)); + ros::Subscriber sub = n_->subscribe("map", 1, [this](auto& map){ mapCallback(map); }); // Try a few times, because the server may not be up yet. for (int i = 20; i > 0 && !got_map_; i--) diff --git a/move_base/src/move_base.cpp b/move_base/src/move_base.cpp index bd14df571d..dceacbe8c0 100644 --- a/move_base/src/move_base.cpp +++ b/move_base/src/move_base.cpp @@ -58,7 +58,7 @@ namespace move_base { planner_plan_(NULL), latest_plan_(NULL), controller_plan_(NULL), runPlanner_(false), setup_(false), p_freq_change_(false), c_freq_change_(false), new_global_plan_(false) { - as_ = new MoveBaseActionServer(ros::NodeHandle(), "move_base", boost::bind(&MoveBase::executeCb, this, _1), false); + as_ = new MoveBaseActionServer(ros::NodeHandle(), "move_base", [this](auto& goal){ executeCb(goal); }, false); ros::NodeHandle private_nh("~"); ros::NodeHandle nh; @@ -104,7 +104,7 @@ namespace move_base { //they won't get any useful information back about its status, but this is useful for tools //like nav_view and rviz ros::NodeHandle simple_nh("move_base_simple"); - goal_sub_ = simple_nh.subscribe("goal", 1, boost::bind(&MoveBase::goalCB, this, _1)); + goal_sub_ = simple_nh.subscribe("goal", 1, [this](auto& goal){ goalCB(goal); }); //we'll assume the radius of the robot to be consistent with what's specified for the costmaps private_nh.param("local_costmap/inscribed_radius", inscribed_radius_, 0.325); @@ -175,7 +175,7 @@ namespace move_base { as_->start(); dsrv_ = new dynamic_reconfigure::Server(ros::NodeHandle("~")); - dynamic_reconfigure::Server::CallbackType cb = boost::bind(&MoveBase::reconfigureCB, this, _1, _2); + dynamic_reconfigure::Server::CallbackType cb = [this](auto& config, auto level){ reconfigureCB(config, level); }; dsrv_->setCallback(cb); } diff --git a/move_slow_and_clear/src/move_slow_and_clear.cpp b/move_slow_and_clear/src/move_slow_and_clear.cpp index 5fc6c96112..53b9ae660b 100644 --- a/move_slow_and_clear/src/move_slow_and_clear.cpp +++ b/move_slow_and_clear/src/move_slow_and_clear.cpp @@ -35,7 +35,7 @@ * Author: Eitan Marder-Eppstein *********************************************************************/ #include -#include +#include #include PLUGINLIB_EXPORT_CLASS(move_slow_and_clear::MoveSlowAndClear, nav_core::RecoveryBehavior) diff --git a/navfn/src/navfn_ros.cpp b/navfn/src/navfn_ros.cpp index d78ca6d8a0..709a0ba8a9 100644 --- a/navfn/src/navfn_ros.cpp +++ b/navfn/src/navfn_ros.cpp @@ -35,7 +35,7 @@ * Author: Eitan Marder-Eppstein *********************************************************************/ #include -#include +#include #include #include #include diff --git a/rotate_recovery/src/rotate_recovery.cpp b/rotate_recovery/src/rotate_recovery.cpp index 2d0bdfe74f..b144736166 100644 --- a/rotate_recovery/src/rotate_recovery.cpp +++ b/rotate_recovery/src/rotate_recovery.cpp @@ -35,7 +35,7 @@ * Author: Eitan Marder-Eppstein *********************************************************************/ #include -#include +#include #include #include #include From 4122c1b84296b7c49799ef1aee742565090753d9 Mon Sep 17 00:00:00 2001 From: Stephan <1481786+stwirth@users.noreply.github.com> Date: Tue, 29 Nov 2022 15:25:29 +0100 Subject: [PATCH 2/6] [AMCL] Add option to force nomotion update after initialpose (#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 --- amcl/cfg/AMCL.cfg | 2 ++ amcl/src/amcl_node.cpp | 25 +++++++++++++++++++------ 2 files changed, 21 insertions(+), 6 deletions(-) diff --git a/amcl/cfg/AMCL.cfg b/amcl/cfg/AMCL.cfg index 79e2524044..4695df72fb 100755 --- 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/src/amcl_node.cpp b/amcl/src/amcl_node.cpp index 20f3ea5c02..b0e11d1bbe 100644 --- 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 From 82e12a94ad88634f9478be15005adbb7c8e1b9f0 Mon Sep 17 00:00:00 2001 From: Michael Ferguson Date: Tue, 10 Jan 2023 12:09:02 -0500 Subject: [PATCH 3/6] update changelogs --- amcl/CHANGELOG.rst | 16 ++++++++++++++++ base_local_planner/CHANGELOG.rst | 13 +++++++++++++ carrot_planner/CHANGELOG.rst | 13 +++++++++++++ clear_costmap_recovery/CHANGELOG.rst | 13 +++++++++++++ costmap_2d/CHANGELOG.rst | 13 +++++++++++++ dwa_local_planner/CHANGELOG.rst | 13 +++++++++++++ fake_localization/CHANGELOG.rst | 13 +++++++++++++ global_planner/CHANGELOG.rst | 13 +++++++++++++ map_server/CHANGELOG.rst | 13 +++++++++++++ move_base/CHANGELOG.rst | 13 +++++++++++++ move_slow_and_clear/CHANGELOG.rst | 13 +++++++++++++ nav_core/CHANGELOG.rst | 3 +++ navfn/CHANGELOG.rst | 13 +++++++++++++ navigation/CHANGELOG.rst | 3 +++ rotate_recovery/CHANGELOG.rst | 13 +++++++++++++ voxel_grid/CHANGELOG.rst | 3 +++ 16 files changed, 181 insertions(+) diff --git a/amcl/CHANGELOG.rst b/amcl/CHANGELOG.rst index ab4b140fa3..698fd36e98 100644 --- a/amcl/CHANGELOG.rst +++ b/amcl/CHANGELOG.rst @@ -2,6 +2,22 @@ Changelog for package amcl ^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [AMCL] Add option to force nomotion update after initialpose (`#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 `_) + * 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 `_) diff --git a/base_local_planner/CHANGELOG.rst b/base_local_planner/CHANGELOG.rst index 75f5ec477a..b8713f767f 100644 --- a/base_local_planner/CHANGELOG.rst +++ b/base_local_planner/CHANGELOG.rst @@ -2,6 +2,19 @@ Changelog for package base_local_planner ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [ROS-O] various patches (`#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 `_) (`#1204 `_) diff --git a/carrot_planner/CHANGELOG.rst b/carrot_planner/CHANGELOG.rst index e4b4aaa3fc..e6bd0031cb 100644 --- a/carrot_planner/CHANGELOG.rst +++ b/carrot_planner/CHANGELOG.rst @@ -2,6 +2,19 @@ Changelog for package carrot_planner ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [ROS-O] various patches (`#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 `_) diff --git a/clear_costmap_recovery/CHANGELOG.rst b/clear_costmap_recovery/CHANGELOG.rst index 7cd123c72c..f79435899f 100644 --- a/clear_costmap_recovery/CHANGELOG.rst +++ b/clear_costmap_recovery/CHANGELOG.rst @@ -2,6 +2,19 @@ Changelog for package clear_costmap_recovery ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [ROS-O] various patches (`#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 `_) diff --git a/costmap_2d/CHANGELOG.rst b/costmap_2d/CHANGELOG.rst index 864de8c82c..7621874d83 100644 --- a/costmap_2d/CHANGELOG.rst +++ b/costmap_2d/CHANGELOG.rst @@ -2,6 +2,19 @@ Changelog for package costmap_2d ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [ROS-O] various patches (`#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 `_) diff --git a/dwa_local_planner/CHANGELOG.rst b/dwa_local_planner/CHANGELOG.rst index ff85883af4..cdecd84697 100644 --- a/dwa_local_planner/CHANGELOG.rst +++ b/dwa_local_planner/CHANGELOG.rst @@ -2,6 +2,19 @@ Changelog for package dwa_local_planner ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [ROS-O] various patches (`#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) ------------------- diff --git a/fake_localization/CHANGELOG.rst b/fake_localization/CHANGELOG.rst index 98ed167537..5c62d51e35 100644 --- a/fake_localization/CHANGELOG.rst +++ b/fake_localization/CHANGELOG.rst @@ -2,6 +2,19 @@ Changelog for package fake_localization ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [ROS-O] various patches (`#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) ------------------- diff --git a/global_planner/CHANGELOG.rst b/global_planner/CHANGELOG.rst index 98bc8d2300..d4f8357722 100644 --- a/global_planner/CHANGELOG.rst +++ b/global_planner/CHANGELOG.rst @@ -2,6 +2,19 @@ Changelog for package global_planner ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [ROS-O] various patches (`#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) ------------------- * Throttled failed to get plan error to 5 seconds (`#1102 `_) diff --git a/map_server/CHANGELOG.rst b/map_server/CHANGELOG.rst index 613b60793b..34c333b0dc 100644 --- a/map_server/CHANGELOG.rst +++ b/map_server/CHANGELOG.rst @@ -2,6 +2,19 @@ Changelog for package map_server ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [ROS-O] various patches (`#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) ------------------- * Change_map service to map_server [Rebase/Noetic] (`#1029 `_) diff --git a/move_base/CHANGELOG.rst b/move_base/CHANGELOG.rst index f8e0ebd748..b005637d40 100644 --- a/move_base/CHANGELOG.rst +++ b/move_base/CHANGELOG.rst @@ -2,6 +2,19 @@ Changelog for package move_base ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [ROS-O] various patches (`#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 stamp of transformed pose is non-zero (`#1200 `_) diff --git a/move_slow_and_clear/CHANGELOG.rst b/move_slow_and_clear/CHANGELOG.rst index d69f55092d..8302411369 100644 --- a/move_slow_and_clear/CHANGELOG.rst +++ b/move_slow_and_clear/CHANGELOG.rst @@ -2,6 +2,19 @@ Changelog for package move_slow_and_clear ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [ROS-O] various patches (`#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) ------------------- * [move_slow_and_clear] Add rosparam representing max vel rosparam name (`#1039 `_) diff --git a/nav_core/CHANGELOG.rst b/nav_core/CHANGELOG.rst index cec214ac68..0114d2abd8 100644 --- a/nav_core/CHANGELOG.rst +++ b/nav_core/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package nav_core ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 1.17.2 (2022-06-20) ------------------- diff --git a/navfn/CHANGELOG.rst b/navfn/CHANGELOG.rst index f8b43d2d66..e5f74e61d1 100644 --- a/navfn/CHANGELOG.rst +++ b/navfn/CHANGELOG.rst @@ -2,6 +2,19 @@ Changelog for package navfn ^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [ROS-O] various patches (`#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) ------------------- * navfn: stop installing test headers (`#1085 `_) diff --git a/navigation/CHANGELOG.rst b/navigation/CHANGELOG.rst index 7df9273d6a..2c515032e9 100644 --- a/navigation/CHANGELOG.rst +++ b/navigation/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package navigation ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 1.17.2 (2022-06-20) ------------------- diff --git a/rotate_recovery/CHANGELOG.rst b/rotate_recovery/CHANGELOG.rst index fe5d64e173..34986046c3 100644 --- a/rotate_recovery/CHANGELOG.rst +++ b/rotate_recovery/CHANGELOG.rst @@ -2,6 +2,19 @@ Changelog for package rotate_recovery ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [ROS-O] various patches (`#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) ------------------- diff --git a/voxel_grid/CHANGELOG.rst b/voxel_grid/CHANGELOG.rst index a7039cb01b..054c13ed93 100644 --- a/voxel_grid/CHANGELOG.rst +++ b/voxel_grid/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package voxel_grid ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 1.17.2 (2022-06-20) ------------------- From f13af47ee2bca6c3bf99db2965e46e55303f6e66 Mon Sep 17 00:00:00 2001 From: Michael Ferguson Date: Tue, 10 Jan 2023 12:09:10 -0500 Subject: [PATCH 4/6] 1.17.3 --- amcl/CHANGELOG.rst | 4 ++-- amcl/package.xml | 2 +- base_local_planner/CHANGELOG.rst | 4 ++-- base_local_planner/package.xml | 2 +- carrot_planner/CHANGELOG.rst | 4 ++-- carrot_planner/package.xml | 2 +- clear_costmap_recovery/CHANGELOG.rst | 4 ++-- clear_costmap_recovery/package.xml | 2 +- costmap_2d/CHANGELOG.rst | 4 ++-- costmap_2d/package.xml | 2 +- dwa_local_planner/CHANGELOG.rst | 4 ++-- dwa_local_planner/package.xml | 2 +- fake_localization/CHANGELOG.rst | 4 ++-- fake_localization/package.xml | 2 +- global_planner/CHANGELOG.rst | 4 ++-- global_planner/package.xml | 2 +- map_server/CHANGELOG.rst | 4 ++-- map_server/package.xml | 2 +- move_base/CHANGELOG.rst | 4 ++-- move_base/package.xml | 2 +- move_slow_and_clear/CHANGELOG.rst | 4 ++-- move_slow_and_clear/package.xml | 2 +- nav_core/CHANGELOG.rst | 4 ++-- nav_core/package.xml | 2 +- navfn/CHANGELOG.rst | 4 ++-- navfn/package.xml | 2 +- navigation/CHANGELOG.rst | 4 ++-- navigation/package.xml | 2 +- rotate_recovery/CHANGELOG.rst | 4 ++-- rotate_recovery/package.xml | 2 +- voxel_grid/CHANGELOG.rst | 4 ++-- voxel_grid/package.xml | 2 +- 32 files changed, 48 insertions(+), 48 deletions(-) diff --git a/amcl/CHANGELOG.rst b/amcl/CHANGELOG.rst index 698fd36e98..2faef832ce 100644 --- a/amcl/CHANGELOG.rst +++ b/amcl/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package amcl ^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.17.3 (2023-01-10) +------------------- * [AMCL] Add option to force nomotion update after initialpose (`#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 diff --git a/amcl/package.xml b/amcl/package.xml index 0c587b01e3..8bed780d30 100644 --- a/amcl/package.xml +++ b/amcl/package.xml @@ -2,7 +2,7 @@ amcl - 1.17.2 + 1.17.3

amcl is a probabilistic localization system for a robot moving in diff --git a/base_local_planner/CHANGELOG.rst b/base_local_planner/CHANGELOG.rst index b8713f767f..257807e3e0 100644 --- a/base_local_planner/CHANGELOG.rst +++ b/base_local_planner/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package base_local_planner ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.17.3 (2023-01-10) +------------------- * [ROS-O] various patches (`#1225 `_) * do not specify obsolete c++11 standard this breaks with current versions of log4cxx. diff --git a/base_local_planner/package.xml b/base_local_planner/package.xml index db0cdab811..a0926f7572 100644 --- a/base_local_planner/package.xml +++ b/base_local_planner/package.xml @@ -2,7 +2,7 @@ base_local_planner - 1.17.2 + 1.17.3 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 nav_core package. diff --git a/carrot_planner/CHANGELOG.rst b/carrot_planner/CHANGELOG.rst index e6bd0031cb..12fa2feb5f 100644 --- a/carrot_planner/CHANGELOG.rst +++ b/carrot_planner/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package carrot_planner ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.17.3 (2023-01-10) +------------------- * [ROS-O] various patches (`#1225 `_) * do not specify obsolete c++11 standard this breaks with current versions of log4cxx. diff --git a/carrot_planner/package.xml b/carrot_planner/package.xml index 39f266803e..db215677b8 100644 --- a/carrot_planner/package.xml +++ b/carrot_planner/package.xml @@ -2,7 +2,7 @@ carrot_planner - 1.17.2 + 1.17.3 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/clear_costmap_recovery/CHANGELOG.rst b/clear_costmap_recovery/CHANGELOG.rst index f79435899f..d3d40a95d9 100644 --- a/clear_costmap_recovery/CHANGELOG.rst +++ b/clear_costmap_recovery/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package clear_costmap_recovery ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.17.3 (2023-01-10) +------------------- * [ROS-O] various patches (`#1225 `_) * do not specify obsolete c++11 standard this breaks with current versions of log4cxx. diff --git a/clear_costmap_recovery/package.xml b/clear_costmap_recovery/package.xml index 2d62f984f4..69ebf07c6c 100644 --- a/clear_costmap_recovery/package.xml +++ b/clear_costmap_recovery/package.xml @@ -2,7 +2,7 @@ clear_costmap_recovery - 1.17.2 + 1.17.3 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/costmap_2d/CHANGELOG.rst b/costmap_2d/CHANGELOG.rst index 7621874d83..a3358ca8f5 100644 --- a/costmap_2d/CHANGELOG.rst +++ b/costmap_2d/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package costmap_2d ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.17.3 (2023-01-10) +------------------- * [ROS-O] various patches (`#1225 `_) * do not specify obsolete c++11 standard this breaks with current versions of log4cxx. diff --git a/costmap_2d/package.xml b/costmap_2d/package.xml index 8f9bbf9a4a..e83c37c42f 100644 --- a/costmap_2d/package.xml +++ b/costmap_2d/package.xml @@ -2,7 +2,7 @@ costmap_2d - 1.17.2 + 1.17.3 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/dwa_local_planner/CHANGELOG.rst b/dwa_local_planner/CHANGELOG.rst index cdecd84697..54bf37698b 100644 --- a/dwa_local_planner/CHANGELOG.rst +++ b/dwa_local_planner/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package dwa_local_planner ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.17.3 (2023-01-10) +------------------- * [ROS-O] various patches (`#1225 `_) * do not specify obsolete c++11 standard this breaks with current versions of log4cxx. diff --git a/dwa_local_planner/package.xml b/dwa_local_planner/package.xml index 14dad421f0..dfd9d0a0d1 100644 --- a/dwa_local_planner/package.xml +++ b/dwa_local_planner/package.xml @@ -2,7 +2,7 @@ dwa_local_planner - 1.17.2 + 1.17.3 This package provides an implementation of the Dynamic Window Approach to diff --git a/fake_localization/CHANGELOG.rst b/fake_localization/CHANGELOG.rst index 5c62d51e35..a3f975250a 100644 --- a/fake_localization/CHANGELOG.rst +++ b/fake_localization/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package fake_localization ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.17.3 (2023-01-10) +------------------- * [ROS-O] various patches (`#1225 `_) * do not specify obsolete c++11 standard this breaks with current versions of log4cxx. diff --git a/fake_localization/package.xml b/fake_localization/package.xml index 76c6b3b18e..dab80627e3 100644 --- a/fake_localization/package.xml +++ b/fake_localization/package.xml @@ -2,7 +2,7 @@ fake_localization - 1.17.2 + 1.17.3 A ROS node that simply forwards odometry information. Ioan A. Sucan contradict@gmail.com diff --git a/global_planner/CHANGELOG.rst b/global_planner/CHANGELOG.rst index d4f8357722..316c2a8d05 100644 --- a/global_planner/CHANGELOG.rst +++ b/global_planner/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package global_planner ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.17.3 (2023-01-10) +------------------- * [ROS-O] various patches (`#1225 `_) * do not specify obsolete c++11 standard this breaks with current versions of log4cxx. diff --git a/global_planner/package.xml b/global_planner/package.xml index eb14127fdb..9b62295da3 100644 --- a/global_planner/package.xml +++ b/global_planner/package.xml @@ -2,7 +2,7 @@ global_planner - 1.17.2 + 1.17.3 A path planner library and node. diff --git a/map_server/CHANGELOG.rst b/map_server/CHANGELOG.rst index 34c333b0dc..7c16d455bf 100644 --- a/map_server/CHANGELOG.rst +++ b/map_server/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package map_server ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.17.3 (2023-01-10) +------------------- * [ROS-O] various patches (`#1225 `_) * do not specify obsolete c++11 standard this breaks with current versions of log4cxx. diff --git a/map_server/package.xml b/map_server/package.xml index c1be448a90..640cc76872 100644 --- a/map_server/package.xml +++ b/map_server/package.xml @@ -2,7 +2,7 @@ map_server - 1.17.2 + 1.17.3 map_server provides the map_server ROS Node, which offers map data as a ROS Service. It also provides the map_saver command-line utility, which allows dynamically generated maps to be saved to file. diff --git a/move_base/CHANGELOG.rst b/move_base/CHANGELOG.rst index b005637d40..e671c9ac23 100644 --- a/move_base/CHANGELOG.rst +++ b/move_base/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package move_base ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.17.3 (2023-01-10) +------------------- * [ROS-O] various patches (`#1225 `_) * do not specify obsolete c++11 standard this breaks with current versions of log4cxx. diff --git a/move_base/package.xml b/move_base/package.xml index 1e3d2e5743..00fd76f55f 100644 --- a/move_base/package.xml +++ b/move_base/package.xml @@ -2,7 +2,7 @@ move_base - 1.17.2 + 1.17.3 The move_base package provides an implementation of an action (see the actionlib package) that, given a goal in the world, will attempt to reach it with a mobile base. The move_base node links together a global and local planner to accomplish its global navigation task. It supports any global planner adhering to the nav_core::BaseGlobalPlanner interface specified in the nav_core package and any local planner adhering to the nav_core::BaseLocalPlanner interface specified in the nav_core package. The move_base node also maintains two costmaps, one for the global planner, and one for a local planner (see the costmap_2d package) that are used to accomplish navigation tasks. diff --git a/move_slow_and_clear/CHANGELOG.rst b/move_slow_and_clear/CHANGELOG.rst index 8302411369..137c381549 100644 --- a/move_slow_and_clear/CHANGELOG.rst +++ b/move_slow_and_clear/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package move_slow_and_clear ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.17.3 (2023-01-10) +------------------- * [ROS-O] various patches (`#1225 `_) * do not specify obsolete c++11 standard this breaks with current versions of log4cxx. diff --git a/move_slow_and_clear/package.xml b/move_slow_and_clear/package.xml index 0147c34867..eddf751abd 100644 --- a/move_slow_and_clear/package.xml +++ b/move_slow_and_clear/package.xml @@ -2,7 +2,7 @@ move_slow_and_clear - 1.17.2 + 1.17.3 move_slow_and_clear diff --git a/nav_core/CHANGELOG.rst b/nav_core/CHANGELOG.rst index 0114d2abd8..1817763371 100644 --- a/nav_core/CHANGELOG.rst +++ b/nav_core/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package nav_core ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.17.3 (2023-01-10) +------------------- 1.17.2 (2022-06-20) ------------------- diff --git a/nav_core/package.xml b/nav_core/package.xml index c16bbb1937..866276a897 100644 --- a/nav_core/package.xml +++ b/nav_core/package.xml @@ -2,7 +2,7 @@ nav_core - 1.17.2 + 1.17.3 This package provides common interfaces for navigation specific robot actions. Currently, this package provides the BaseGlobalPlanner, BaseLocalPlanner, and RecoveryBehavior interfaces, which can be used to build actions that can easily swap their planner, local controller, or recovery behavior for new versions adhering to the same interface. diff --git a/navfn/CHANGELOG.rst b/navfn/CHANGELOG.rst index e5f74e61d1..a34edb5618 100644 --- a/navfn/CHANGELOG.rst +++ b/navfn/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package navfn ^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.17.3 (2023-01-10) +------------------- * [ROS-O] various patches (`#1225 `_) * do not specify obsolete c++11 standard this breaks with current versions of log4cxx. diff --git a/navfn/package.xml b/navfn/package.xml index 3e0c8e2ff1..49b6cd0458 100644 --- a/navfn/package.xml +++ b/navfn/package.xml @@ -2,7 +2,7 @@ navfn - 1.17.2 + 1.17.3 navfn provides a fast interpolated navigation function that can be used to create plans for diff --git a/navigation/CHANGELOG.rst b/navigation/CHANGELOG.rst index 2c515032e9..c530279c14 100644 --- a/navigation/CHANGELOG.rst +++ b/navigation/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package navigation ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.17.3 (2023-01-10) +------------------- 1.17.2 (2022-06-20) ------------------- diff --git a/navigation/package.xml b/navigation/package.xml index 1179a5b707..e03599d9d1 100644 --- a/navigation/package.xml +++ b/navigation/package.xml @@ -2,7 +2,7 @@ navigation - 1.17.2 + 1.17.3 A 2D navigation stack that takes in information from odometry, sensor streams, and a goal pose and outputs safe velocity commands that are sent diff --git a/rotate_recovery/CHANGELOG.rst b/rotate_recovery/CHANGELOG.rst index 34986046c3..f9fd962e60 100644 --- a/rotate_recovery/CHANGELOG.rst +++ b/rotate_recovery/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rotate_recovery ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.17.3 (2023-01-10) +------------------- * [ROS-O] various patches (`#1225 `_) * do not specify obsolete c++11 standard this breaks with current versions of log4cxx. diff --git a/rotate_recovery/package.xml b/rotate_recovery/package.xml index 5eca961b79..cfec387b94 100644 --- a/rotate_recovery/package.xml +++ b/rotate_recovery/package.xml @@ -2,7 +2,7 @@ rotate_recovery - 1.17.2 + 1.17.3 This package provides a recovery behavior for the navigation stack that attempts to clear space by performing a 360 degree rotation of the robot. diff --git a/voxel_grid/CHANGELOG.rst b/voxel_grid/CHANGELOG.rst index 054c13ed93..110eed493d 100644 --- a/voxel_grid/CHANGELOG.rst +++ b/voxel_grid/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package voxel_grid ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.17.3 (2023-01-10) +------------------- 1.17.2 (2022-06-20) ------------------- diff --git a/voxel_grid/package.xml b/voxel_grid/package.xml index 0c6e7ee95e..21790eee94 100644 --- a/voxel_grid/package.xml +++ b/voxel_grid/package.xml @@ -2,7 +2,7 @@ voxel_grid - 1.17.2 + 1.17.3 voxel_grid provides an implementation of an efficient 3D voxel grid. The occupancy grid can support 3 different representations for the state of a cell: marked, free, or unknown. Due to the underlying implementation relying on bitwise and and or integer operations, the voxel grid only supports 16 different levels per voxel column. However, this limitation yields raytracing and cell marking performance in the grid comparable to standard 2D structures making it quite fast compared to most 3D structures. From 6d6ce3df4484218776904fef38bb17c2cc17ad0c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Jo=C3=A3o=20Aguizo?= Date: Mon, 16 Jan 2023 18:04:27 +0000 Subject: [PATCH 5/6] Throttle some warnings. (#1231) Throttle planner and controller warnings. Avoids high frequency outputs to terminal. This is specially valuable in the following contexts: * long term experiments, in which terminal outputs' are saved for later analysis (without throttling they will take several GB of disk space). * or simply, the active monitoring of a running system. --- global_planner/src/planner_core.cpp | 2 +- navfn/src/navfn_ros.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/global_planner/src/planner_core.cpp b/global_planner/src/planner_core.cpp index ac104cae3e..5cfdd6790f 100644 --- a/global_planner/src/planner_core.cpp +++ b/global_planner/src/planner_core.cpp @@ -250,7 +250,7 @@ bool GlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, const geom double start_x, start_y, goal_x, goal_y; if (!costmap_->worldToMap(wx, wy, start_x_i, start_y_i)) { - ROS_WARN( + ROS_WARN_THROTTLE(1.0, "The robot's start position is off the global costmap. Planning will always fail, are you sure the robot has been properly localized?"); return false; } diff --git a/navfn/src/navfn_ros.cpp b/navfn/src/navfn_ros.cpp index 709a0ba8a9..2a4cbbb5e8 100644 --- a/navfn/src/navfn_ros.cpp +++ b/navfn/src/navfn_ros.cpp @@ -226,7 +226,7 @@ namespace navfn { unsigned int mx, my; if(!costmap_->worldToMap(wx, wy, mx, my)){ - ROS_WARN("The robot's start position is off the global costmap. Planning will always fail, are you sure the robot has been properly localized?"); + ROS_WARN_THROTTLE(1.0, "The robot's start position is off the global costmap. Planning will always fail, are you sure the robot has been properly localized?"); return false; } From 85e5f334cbc9e62e7f476bc2dd875f54ade0ecbb Mon Sep 17 00:00:00 2001 From: Stephan Wirth <1481786+stwirth@users.noreply.github.com> Date: Tue, 17 Jan 2023 17:26:59 +0100 Subject: [PATCH 6/6] include roslib_INCLUDE_DIRS for unit tests (#1232) --- map_server/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/map_server/CMakeLists.txt b/map_server/CMakeLists.txt index 37a67cc360..147c9ae506 100644 --- a/map_server/CMakeLists.txt +++ b/map_server/CMakeLists.txt @@ -94,6 +94,7 @@ if(CATKIN_ENABLE_TESTING) ) find_package(roslib REQUIRED) + include_directories(${roslib_INCLUDE_DIRS}) add_executable(rtest test/rtest.cpp test/test_constants.cpp) add_dependencies(rtest ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) add_dependencies(tests rtest)