diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index cc99e323ff..e5e3587a66 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -245,7 +245,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" @@ -749,7 +749,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, " 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..f0f088cbec 100644 --- a/nav2_costmap_2d/plugins/static_layer.cpp +++ b/nav2_costmap_2d/plugins/static_layer.cpp @@ -117,10 +117,12 @@ void StaticLayer::getParameters() { int temp_lethal_threshold = 0; + double temp_tf_tol; 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(0.0)); node_->get_parameter(name_ + "." + "enabled", enabled_); node_->get_parameter(name_ + "." + "subscribe_to_updates", subscribe_to_updates_); @@ -133,10 +135,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 +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 = 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..f87d59bc25 100644 --- a/nav2_costmap_2d/src/observation_buffer.cpp +++ b/nav2_costmap_2d/src/observation_buffer.cpp @@ -95,11 +95,12 @@ 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( 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 = [