From 4008b02554b4877b543cf0df3a6d58a448fedf4b Mon Sep 17 00:00:00 2001 From: Marwan Taher Date: Tue, 12 May 2020 22:27:46 +0100 Subject: [PATCH 1/6] Parameterized tf_timeout in amcl * Refactored the existing transform_tolerance parameter in amcl to transform_publish_shift. * Refactored tf_buffer method calls to use transform_tolerance according to [978]. --- nav2_amcl/include/nav2_amcl/amcl_node.hpp | 3 ++- nav2_amcl/src/amcl_node.cpp | 15 +++++++++++---- 2 files changed, 13 insertions(+), 5 deletions(-) diff --git a/nav2_amcl/include/nav2_amcl/amcl_node.hpp b/nav2_amcl/include/nav2_amcl/amcl_node.hpp index 669ea98835..d9710b4668 100644 --- a/nav2_amcl/include/nav2_amcl/amcl_node.hpp +++ b/nav2_amcl/include/nav2_amcl/amcl_node.hpp @@ -243,7 +243,8 @@ class AmclNode : public nav2_util::LifecycleNode tf2::Duration save_pose_period_; double sigma_hit_; bool tf_broadcast_; - tf2::Duration transform_tolerance_; + tf2::Duration transform_tolerance_; // tf timeout duration + tf2::Duration transform_publish_shift_; // post-date the transform that is published, this was formerly called transform_tolerance_ double a_thresh_; double d_thresh_; double z_hit_; diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index cc99e323ff..1c36f6b241 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -190,6 +190,10 @@ AmclNode::AmclNode() add_parameter( "transform_tolerance", rclcpp::ParameterValue(1.0), + "Waiting duration in seconds for transform (tf) data to be available."); + + add_parameter( + "transform_publish_shift", rclcpp::ParameterValue(1.0), "Time with which to post-date the transform that is published, to indicate that this transform " "is valid into the future"); @@ -245,7 +249,7 @@ AmclNode::waitForTransforms() while (rclcpp::ok() && !tf_buffer_->canTransform( global_frame_id_, odom_frame_id_, tf2::TimePointZero, - tf2::durationFromSec(1.0), &tf_error)) + transform_tolerance_, &tf_error)) { RCLCPP_INFO( get_logger(), "Timed out waiting for transform from %s to %s" @@ -716,7 +720,7 @@ AmclNode::laserReceived(sensor_msgs::msg::LaserScan::ConstSharedPtr laser_scan) // We want to send a transform that is good up until a // tolerance time so that odom can be used auto stamp = tf2_ros::fromMsg(laser_scan->header.stamp); - tf2::TimePoint transform_expiration = stamp + transform_tolerance_; + tf2::TimePoint transform_expiration = stamp + transform_publish_shift_; sendMapToOdomTransform(transform_expiration); sent_first_transform_ = true; } @@ -728,7 +732,7 @@ AmclNode::laserReceived(sensor_msgs::msg::LaserScan::ConstSharedPtr laser_scan) // Nothing changed, so we'll just republish the last transform, to keep // everybody happy. tf2::TimePoint transform_expiration = tf2_ros::fromMsg(laser_scan->header.stamp) + - transform_tolerance_; + transform_publish_shift_; sendMapToOdomTransform(transform_expiration); } } @@ -749,7 +753,7 @@ bool AmclNode::addNewScanner( ident.header.stamp = rclcpp::Time(); tf2::toMsg(tf2::Transform::getIdentity(), ident.pose); try { - tf_buffer_->transform(ident, laser_pose, base_frame_id_); + tf_buffer_->transform(ident, laser_pose, base_frame_id_, transform_tolerance_); } catch (tf2::TransformException & e) { RCLCPP_ERROR( get_logger(), "Couldn't transform from %s to %s, " @@ -1052,6 +1056,7 @@ AmclNode::initParameters() { double save_pose_rate; double tmp_tol; + double tmp_tf_shift; get_parameter("alpha1", alpha1_); get_parameter("alpha2", alpha2_); @@ -1088,6 +1093,7 @@ AmclNode::initParameters() get_parameter("sigma_hit", sigma_hit_); get_parameter("tf_broadcast", tf_broadcast_); get_parameter("transform_tolerance", tmp_tol); + get_parameter("transform_publish_shift", tmp_tf_shift); get_parameter("update_min_a", a_thresh_); get_parameter("update_min_d", d_thresh_); get_parameter("z_hit", z_hit_); @@ -1099,6 +1105,7 @@ AmclNode::initParameters() save_pose_period_ = tf2::durationFromSec(1.0 / save_pose_rate); transform_tolerance_ = tf2::durationFromSec(tmp_tol); + transform_publish_shift_ = tf2::durationFromSec(tmp_tf_shift); odom_frame_id_ = nav2_util::strip_leading_slash(odom_frame_id_); base_frame_id_ = nav2_util::strip_leading_slash(base_frame_id_); From 64b9bbd6fb1a8827771449d0882e66de21c58b48 Mon Sep 17 00:00:00 2001 From: Marwan Taher Date: Tue, 12 May 2020 23:09:35 +0100 Subject: [PATCH 2/6] Added tf_timeout to static_layer and observation_buffer --- nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp | 1 + nav2_costmap_2d/plugins/static_layer.cpp | 6 +++++- nav2_costmap_2d/src/observation_buffer.cpp | 4 ++-- 3 files changed, 8 insertions(+), 3 deletions(-) diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp index 05a5185583..f6f01e8c56 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp @@ -110,6 +110,7 @@ class StaticLayer : public CostmapLayer unsigned char unknown_cost_value_; bool trinary_costmap_; bool map_received_{false}; + tf2::Duration transform_tolerance_; }; } // namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/plugins/static_layer.cpp b/nav2_costmap_2d/plugins/static_layer.cpp index f84d8d1f40..6a588c21a4 100644 --- a/nav2_costmap_2d/plugins/static_layer.cpp +++ b/nav2_costmap_2d/plugins/static_layer.cpp @@ -117,6 +117,7 @@ void StaticLayer::getParameters() { int temp_lethal_threshold = 0; + double temp_tf_tol; declareParameter("enabled", rclcpp::ParameterValue(true)); declareParameter("subscribe_to_updates", rclcpp::ParameterValue(false)); @@ -133,10 +134,13 @@ StaticLayer::getParameters() node_->get_parameter("lethal_cost_threshold", temp_lethal_threshold); node_->get_parameter("unknown_cost_value", unknown_cost_value_); node_->get_parameter("trinary_costmap", trinary_costmap_); + node_->get_parameter("transform_tolerance", temp_tf_tol); // Enforce bounds lethal_threshold_ = std::max(std::min(temp_lethal_threshold, 100), 0); map_received_ = false; + + transform_tolerance_ = tf2::durationFromSec(temp_tf_tol); } void @@ -359,7 +363,7 @@ StaticLayer::updateCosts( // Might even be in a different frame geometry_msgs::msg::TransformStamped transform; try { - transform = tf_->lookupTransform(map_frame_, global_frame_, tf2::TimePointZero); + transform = tf_->lookupTransform(map_frame_, global_frame_, tf2::TimePointZero, transform_tolerance_); } catch (tf2::TransformException & ex) { RCLCPP_ERROR(node_->get_logger(), "StaticLayer: %s", ex.what()); return; diff --git a/nav2_costmap_2d/src/observation_buffer.cpp b/nav2_costmap_2d/src/observation_buffer.cpp index 4a4ec21138..2fd83dae6c 100644 --- a/nav2_costmap_2d/src/observation_buffer.cpp +++ b/nav2_costmap_2d/src/observation_buffer.cpp @@ -95,11 +95,11 @@ bool ObservationBuffer::setGlobalFrame(const std::string new_global_frame) origin.point = obs.origin_; // we need to transform the origin of the observation to the new global frame - tf2_buffer_.transform(origin, origin, new_global_frame); + tf2_buffer_.transform(origin, origin, new_global_frame, tf2::durationFromSec(tf_tolerance_)); obs.origin_ = origin.point; // we also need to transform the cloud of the observation to the new global frame - tf2_buffer_.transform(*(obs.cloud_), *(obs.cloud_), new_global_frame); + tf2_buffer_.transform(*(obs.cloud_), *(obs.cloud_), new_global_frame, tf2::durationFromSec(tf_tolerance_)); } catch (tf2::TransformException & ex) { RCLCPP_ERROR( rclcpp::get_logger( From b3ec76ca7e036c99f73bc66a0274d4fe206fe699 Mon Sep 17 00:00:00 2001 From: Marwan Taher Date: Wed, 13 May 2020 20:17:59 +0100 Subject: [PATCH 3/6] declared transform_tolerance parameter --- nav2_costmap_2d/plugins/static_layer.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/nav2_costmap_2d/plugins/static_layer.cpp b/nav2_costmap_2d/plugins/static_layer.cpp index 6a588c21a4..bc99342d8b 100644 --- a/nav2_costmap_2d/plugins/static_layer.cpp +++ b/nav2_costmap_2d/plugins/static_layer.cpp @@ -122,6 +122,7 @@ StaticLayer::getParameters() declareParameter("enabled", rclcpp::ParameterValue(true)); declareParameter("subscribe_to_updates", rclcpp::ParameterValue(false)); declareParameter("map_subscribe_transient_local", rclcpp::ParameterValue(true)); + declareParameter("transform_tolerance", rclcpp::ParameterValue(1.0)); node_->get_parameter(name_ + "." + "enabled", enabled_); node_->get_parameter(name_ + "." + "subscribe_to_updates", subscribe_to_updates_); From a324e1c61d37ccfe7a31c3ed3c38abbda9ff9e6d Mon Sep 17 00:00:00 2001 From: Marwan Taher Date: Wed, 13 May 2020 20:41:29 +0100 Subject: [PATCH 4/6] change transform_tolerance default value Co-authored-by: Steve Macenski --- nav2_costmap_2d/plugins/static_layer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_costmap_2d/plugins/static_layer.cpp b/nav2_costmap_2d/plugins/static_layer.cpp index bc99342d8b..b945e86795 100644 --- a/nav2_costmap_2d/plugins/static_layer.cpp +++ b/nav2_costmap_2d/plugins/static_layer.cpp @@ -122,7 +122,7 @@ StaticLayer::getParameters() declareParameter("enabled", rclcpp::ParameterValue(true)); declareParameter("subscribe_to_updates", rclcpp::ParameterValue(false)); declareParameter("map_subscribe_transient_local", rclcpp::ParameterValue(true)); - declareParameter("transform_tolerance", rclcpp::ParameterValue(1.0)); + declareParameter("transform_tolerance", rclcpp::ParameterValue(0.0)); node_->get_parameter(name_ + "." + "enabled", enabled_); node_->get_parameter(name_ + "." + "subscribe_to_updates", subscribe_to_updates_); From 19ca42c2cbe5766c8af758a03f542e84908ecf56 Mon Sep 17 00:00:00 2001 From: Marwan Taher Date: Wed, 13 May 2020 20:51:03 +0100 Subject: [PATCH 5/6] Removed transform_publish_shift param --- nav2_amcl/include/nav2_amcl/amcl_node.hpp | 3 +-- nav2_amcl/src/amcl_node.cpp | 11 ++--------- 2 files changed, 3 insertions(+), 11 deletions(-) diff --git a/nav2_amcl/include/nav2_amcl/amcl_node.hpp b/nav2_amcl/include/nav2_amcl/amcl_node.hpp index d9710b4668..669ea98835 100644 --- a/nav2_amcl/include/nav2_amcl/amcl_node.hpp +++ b/nav2_amcl/include/nav2_amcl/amcl_node.hpp @@ -243,8 +243,7 @@ class AmclNode : public nav2_util::LifecycleNode tf2::Duration save_pose_period_; double sigma_hit_; bool tf_broadcast_; - tf2::Duration transform_tolerance_; // tf timeout duration - tf2::Duration transform_publish_shift_; // post-date the transform that is published, this was formerly called transform_tolerance_ + tf2::Duration transform_tolerance_; double a_thresh_; double d_thresh_; double z_hit_; diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index 1c36f6b241..e5e3587a66 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -190,10 +190,6 @@ AmclNode::AmclNode() add_parameter( "transform_tolerance", rclcpp::ParameterValue(1.0), - "Waiting duration in seconds for transform (tf) data to be available."); - - add_parameter( - "transform_publish_shift", rclcpp::ParameterValue(1.0), "Time with which to post-date the transform that is published, to indicate that this transform " "is valid into the future"); @@ -720,7 +716,7 @@ AmclNode::laserReceived(sensor_msgs::msg::LaserScan::ConstSharedPtr laser_scan) // We want to send a transform that is good up until a // tolerance time so that odom can be used auto stamp = tf2_ros::fromMsg(laser_scan->header.stamp); - tf2::TimePoint transform_expiration = stamp + transform_publish_shift_; + tf2::TimePoint transform_expiration = stamp + transform_tolerance_; sendMapToOdomTransform(transform_expiration); sent_first_transform_ = true; } @@ -732,7 +728,7 @@ AmclNode::laserReceived(sensor_msgs::msg::LaserScan::ConstSharedPtr laser_scan) // Nothing changed, so we'll just republish the last transform, to keep // everybody happy. tf2::TimePoint transform_expiration = tf2_ros::fromMsg(laser_scan->header.stamp) + - transform_publish_shift_; + transform_tolerance_; sendMapToOdomTransform(transform_expiration); } } @@ -1056,7 +1052,6 @@ AmclNode::initParameters() { double save_pose_rate; double tmp_tol; - double tmp_tf_shift; get_parameter("alpha1", alpha1_); get_parameter("alpha2", alpha2_); @@ -1093,7 +1088,6 @@ AmclNode::initParameters() get_parameter("sigma_hit", sigma_hit_); get_parameter("tf_broadcast", tf_broadcast_); get_parameter("transform_tolerance", tmp_tol); - get_parameter("transform_publish_shift", tmp_tf_shift); get_parameter("update_min_a", a_thresh_); get_parameter("update_min_d", d_thresh_); get_parameter("z_hit", z_hit_); @@ -1105,7 +1099,6 @@ AmclNode::initParameters() save_pose_period_ = tf2::durationFromSec(1.0 / save_pose_rate); transform_tolerance_ = tf2::durationFromSec(tmp_tol); - transform_publish_shift_ = tf2::durationFromSec(tmp_tf_shift); odom_frame_id_ = nav2_util::strip_leading_slash(odom_frame_id_); base_frame_id_ = nav2_util::strip_leading_slash(base_frame_id_); From cc56a13c209035793f6bea7ae4895e6b1b9c1517 Mon Sep 17 00:00:00 2001 From: Marwan Taher Date: Wed, 13 May 2020 23:58:25 +0100 Subject: [PATCH 6/6] Fixed linting errors --- nav2_costmap_2d/plugins/static_layer.cpp | 4 +++- nav2_costmap_2d/src/observation_buffer.cpp | 3 ++- nav2_system_tests/src/system/test_multi_robot_launch.py | 4 ++-- 3 files changed, 7 insertions(+), 4 deletions(-) diff --git a/nav2_costmap_2d/plugins/static_layer.cpp b/nav2_costmap_2d/plugins/static_layer.cpp index b945e86795..f0f088cbec 100644 --- a/nav2_costmap_2d/plugins/static_layer.cpp +++ b/nav2_costmap_2d/plugins/static_layer.cpp @@ -364,7 +364,9 @@ StaticLayer::updateCosts( // Might even be in a different frame geometry_msgs::msg::TransformStamped transform; try { - transform = tf_->lookupTransform(map_frame_, global_frame_, tf2::TimePointZero, transform_tolerance_); + transform = tf_->lookupTransform( + map_frame_, global_frame_, tf2::TimePointZero, + transform_tolerance_); } catch (tf2::TransformException & ex) { RCLCPP_ERROR(node_->get_logger(), "StaticLayer: %s", ex.what()); return; diff --git a/nav2_costmap_2d/src/observation_buffer.cpp b/nav2_costmap_2d/src/observation_buffer.cpp index 2fd83dae6c..f87d59bc25 100644 --- a/nav2_costmap_2d/src/observation_buffer.cpp +++ b/nav2_costmap_2d/src/observation_buffer.cpp @@ -99,7 +99,8 @@ bool ObservationBuffer::setGlobalFrame(const std::string new_global_frame) obs.origin_ = origin.point; // we also need to transform the cloud of the observation to the new global frame - tf2_buffer_.transform(*(obs.cloud_), *(obs.cloud_), new_global_frame, tf2::durationFromSec(tf_tolerance_)); + tf2_buffer_.transform( + *(obs.cloud_), *(obs.cloud_), new_global_frame, tf2::durationFromSec(tf_tolerance_)); } catch (tf2::TransformException & ex) { RCLCPP_ERROR( rclcpp::get_logger( diff --git a/nav2_system_tests/src/system/test_multi_robot_launch.py b/nav2_system_tests/src/system/test_multi_robot_launch.py index 5adfa97f17..6f6f9f2e5e 100755 --- a/nav2_system_tests/src/system/test_multi_robot_launch.py +++ b/nav2_system_tests/src/system/test_multi_robot_launch.py @@ -41,9 +41,9 @@ def generate_launch_description(): bringup_dir = get_package_share_directory('nav2_bringup') robot1_params_file = os.path.join(bringup_dir, # noqa: F841 - 'params/nav2_multirobot_params_1.yaml') + 'params/nav2_multirobot_params_1.yaml') robot2_params_file = os.path.join(bringup_dir, # noqa: F841 - 'params/nav2_multirobot_params_2.yaml') + 'params/nav2_multirobot_params_2.yaml') # Names and poses of the robots robots = [