From a02169c03ab52ba17a3b2c27a5f83c9506ab2b79 Mon Sep 17 00:00:00 2001 From: Guillaume Doisy Date: Fri, 13 Aug 2021 14:36:53 +0200 Subject: [PATCH 1/3] , tf2::durationFromSec(transform_tolerance) --- nav2_costmap_2d/plugins/obstacle_layer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_costmap_2d/plugins/obstacle_layer.cpp b/nav2_costmap_2d/plugins/obstacle_layer.cpp index 164227a21b7..5793d4aa24d 100644 --- a/nav2_costmap_2d/plugins/obstacle_layer.cpp +++ b/nav2_costmap_2d/plugins/obstacle_layer.cpp @@ -218,7 +218,7 @@ void ObstacleLayer::onInitialize() std::shared_ptr> filter( new tf2_ros::MessageFilter( - *sub, *tf_, global_frame_, 50, rclcpp_node_)); + *sub, *tf_, global_frame_, 50, rclcpp_node_, tf2::durationFromSec(transform_tolerance))); if (inf_is_valid) { filter->registerCallback( From 4a1dcced40df7ea171ca63d8bc74a1bc9a532614 Mon Sep 17 00:00:00 2001 From: Guillaume Doisy Date: Tue, 17 Aug 2021 19:17:41 +0200 Subject: [PATCH 2/3] use message_filter timeout in AMCL --- nav2_amcl/src/amcl_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index f1c0a6e5561..76019d47fd9 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -1241,7 +1241,7 @@ AmclNode::initMessageFilters() rclcpp_node_.get(), scan_topic_, rmw_qos_profile_sensor_data); laser_scan_filter_ = std::make_unique>( - *laser_scan_sub_, *tf_buffer_, odom_frame_id_, 10, rclcpp_node_); + *laser_scan_sub_, *tf_buffer_, odom_frame_id_, 10, rclcpp_node_, transform_tolerance_); laser_scan_connection_ = laser_scan_filter_->registerCallback( std::bind( From 16af26a48c11bca9bc736a2c58a1b43d023d6824 Mon Sep 17 00:00:00 2001 From: Guillaume Doisy Date: Tue, 17 Aug 2021 19:42:25 +0200 Subject: [PATCH 3/3] also for sensor_msgs::msg::PointCloud2 --- nav2_costmap_2d/plugins/obstacle_layer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_costmap_2d/plugins/obstacle_layer.cpp b/nav2_costmap_2d/plugins/obstacle_layer.cpp index 5793d4aa24d..0e33aafb3bc 100644 --- a/nav2_costmap_2d/plugins/obstacle_layer.cpp +++ b/nav2_costmap_2d/plugins/obstacle_layer.cpp @@ -252,7 +252,7 @@ void ObstacleLayer::onInitialize() std::shared_ptr> filter( new tf2_ros::MessageFilter( - *sub, *tf_, global_frame_, 50, rclcpp_node_)); + *sub, *tf_, global_frame_, 50, rclcpp_node_, tf2::durationFromSec(transform_tolerance))); filter->registerCallback( std::bind(