From 3946b6e797a1fd04731411a1b0e4c2f7a39115d8 Mon Sep 17 00:00:00 2001 From: Grigoriy Lipenko Date: Mon, 31 Jan 2022 12:29:35 +0300 Subject: [PATCH 1/3] suppress spam in canTransform --- tf2_ros/src/buffer.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/tf2_ros/src/buffer.cpp b/tf2_ros/src/buffer.cpp index 3e83f50e0..80fb0793d 100644 --- a/tf2_ros/src/buffer.cpp +++ b/tf2_ros/src/buffer.cpp @@ -146,7 +146,8 @@ Buffer::canTransform( // poll for transform if timeout is set rclcpp::Time start_time = clock_->now(); while (clock_->now() < start_time + rclcpp_timeout && - !canTransform(target_frame, source_frame, time) && + !canTransform(target_frame, source_frame, time, + tf2::Duration(std::chrono::nanoseconds::zero()), errstr) && (clock_->now() + rclcpp::Duration(3, 0) >= start_time) && // don't wait bag loop detected (rclcpp::ok())) // Make sure we haven't been stopped (won't work for pytf) { @@ -174,7 +175,8 @@ Buffer::canTransform( // poll for transform if timeout is set rclcpp::Time start_time = clock_->now(); while (clock_->now() < start_time + rclcpp_timeout && - !canTransform(target_frame, target_time, source_frame, source_time, fixed_frame) && + !canTransform(target_frame, target_time, source_frame, source_time, fixed_frame, + tf2::Duration(std::chrono::nanoseconds::zero()), errstr) && (clock_->now() + rclcpp::Duration(3, 0) >= start_time) && // don't wait bag loop detected (rclcpp::ok())) // Make sure we haven't been stopped (won't work for pytf) { From b35dc1a9fe40e3e8135d566a59169fcd4c74e111 Mon Sep 17 00:00:00 2001 From: Gonzalo de Pedro Date: Wed, 1 Jun 2022 15:49:27 -0300 Subject: [PATCH 2/3] Uncrustify Signed-off-by: Gonzalo de Pedro --- tf2_ros/src/buffer.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/tf2_ros/src/buffer.cpp b/tf2_ros/src/buffer.cpp index 80fb0793d..ce5af42b7 100644 --- a/tf2_ros/src/buffer.cpp +++ b/tf2_ros/src/buffer.cpp @@ -146,8 +146,9 @@ Buffer::canTransform( // poll for transform if timeout is set rclcpp::Time start_time = clock_->now(); while (clock_->now() < start_time + rclcpp_timeout && - !canTransform(target_frame, source_frame, time, - tf2::Duration(std::chrono::nanoseconds::zero()), errstr) && + !canTransform( + target_frame, source_frame, time, + tf2::Duration(std::chrono::nanoseconds::zero()), errstr) && (clock_->now() + rclcpp::Duration(3, 0) >= start_time) && // don't wait bag loop detected (rclcpp::ok())) // Make sure we haven't been stopped (won't work for pytf) { @@ -175,8 +176,9 @@ Buffer::canTransform( // poll for transform if timeout is set rclcpp::Time start_time = clock_->now(); while (clock_->now() < start_time + rclcpp_timeout && - !canTransform(target_frame, target_time, source_frame, source_time, fixed_frame, - tf2::Duration(std::chrono::nanoseconds::zero()), errstr) && + !canTransform( + target_frame, target_time, source_frame, source_time, fixed_frame, + tf2::Duration(std::chrono::nanoseconds::zero()), errstr) && (clock_->now() + rclcpp::Duration(3, 0) >= start_time) && // don't wait bag loop detected (rclcpp::ok())) // Make sure we haven't been stopped (won't work for pytf) { From 00fa84285691893c32786e1113b00aad81ebe2c6 Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Mon, 20 Jun 2022 08:33:58 -0700 Subject: [PATCH 3/3] Suppress console spam in other calls to canTransform The user has few options to workaround potential console spam from canTransform. If we want, it would be better to update the API to optional return the error string. Signed-off-by: Jacob Perron --- tf2_ros/src/buffer.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/tf2_ros/src/buffer.cpp b/tf2_ros/src/buffer.cpp index ce5af42b7..2131de689 100644 --- a/tf2_ros/src/buffer.cpp +++ b/tf2_ros/src/buffer.cpp @@ -91,7 +91,9 @@ Buffer::lookupTransform( const std::string & target_frame, const std::string & source_frame, const tf2::TimePoint & lookup_time, const tf2::Duration timeout) const { - canTransform(target_frame, source_frame, lookup_time, timeout); + // Pass error string to suppress console spam + std::string error; + canTransform(target_frame, source_frame, lookup_time, timeout, &error); return lookupTransform(target_frame, source_frame, lookup_time); } @@ -114,7 +116,9 @@ Buffer::lookupTransform( const std::string & source_frame, const tf2::TimePoint & source_time, const std::string & fixed_frame, const tf2::Duration timeout) const { - canTransform(target_frame, target_time, source_frame, source_time, fixed_frame, timeout); + // Pass error string to suppress console spam + std::string error; + canTransform(target_frame, target_time, source_frame, source_time, fixed_frame, timeout, &error); return lookupTransform(target_frame, target_time, source_frame, source_time, fixed_frame); }