From 62085bec17b7fe3c53ef3bbd8546531fd37170c6 Mon Sep 17 00:00:00 2001 From: CursedRock17 Date: Sat, 16 Mar 2024 23:12:43 -0400 Subject: [PATCH 1/4] canTransform warning intervals Signed-off-by: CursedRock17 --- tf2/include/tf2/buffer_core.h | 13 ++++++---- tf2/include/tf2/buffer_core_interface.h | 7 ++++-- tf2/src/buffer_core.cpp | 32 +++++++++++++++---------- 3 files changed, 33 insertions(+), 19 deletions(-) diff --git a/tf2/include/tf2/buffer_core.h b/tf2/include/tf2/buffer_core.h index 6d75127da..225f62175 100644 --- a/tf2/include/tf2/buffer_core.h +++ b/tf2/include/tf2/buffer_core.h @@ -46,11 +46,13 @@ #include "LinearMath/Transform.h" #include "geometry_msgs/msg/transform_stamped.hpp" +#include "rcutils/logging_macros.h" #include "tf2/buffer_core_interface.h" #include "tf2/exceptions.h" #include "tf2/transform_storage.h" #include "tf2/visibility_control.h" -#include "rcutils/logging_macros.h" + +using std::literals::chrono_literals::operator""ms; namespace tf2 { @@ -167,7 +169,8 @@ class BufferCore : public BufferCoreInterface TF2_PUBLIC bool canTransform( const std::string & target_frame, const std::string & source_frame, - const TimePoint & time, std::string * error_msg = nullptr) const override; + const TimePoint & time, std::string * error_msg = nullptr, + std::chrono::milliseconds warning_interval = 5000ms) const override; /** \brief Test if a transform is possible * \param target_frame The frame into which to transform @@ -182,7 +185,8 @@ class BufferCore : public BufferCoreInterface bool canTransform( const std::string & target_frame, const TimePoint & target_time, const std::string & source_frame, const TimePoint & source_time, - const std::string & fixed_frame, std::string * error_msg = nullptr) const override; + const std::string & fixed_frame, std::string * error_msg = nullptr, + std::chrono::milliseconds warning_interval = 5000ms) const override; /** \brief Get all frames that exist in the system. */ @@ -390,7 +394,8 @@ class BufferCore : public BufferCoreInterface CompactFrameID validateFrameId( const char * function_name_arg, const std::string & frame_id, - std::string * error_msg) const; + std::string * error_msg, + std::chrono::milliseconds warning_interval) const; /** \brief Validate a frame ID format and look it up its compact ID. * Raise an exception for invalid cases. diff --git a/tf2/include/tf2/buffer_core_interface.h b/tf2/include/tf2/buffer_core_interface.h index f68512a6c..70f8bc3d4 100644 --- a/tf2/include/tf2/buffer_core_interface.h +++ b/tf2/include/tf2/buffer_core_interface.h @@ -28,6 +28,7 @@ #ifndef TF2__BUFFER_CORE_INTERFACE_H_ #define TF2__BUFFER_CORE_INTERFACE_H_ +#include #include #include @@ -107,7 +108,8 @@ class BufferCoreInterface const std::string & target_frame, const std::string & source_frame, const tf2::TimePoint & time, - std::string * error_msg) const = 0; + std::string * error_msg, + std::chrono::milliseconds warning_interval) const = 0; /** * \brief Test if a transform is possible. @@ -128,7 +130,8 @@ class BufferCoreInterface const std::string & source_frame, const tf2::TimePoint & source_time, const std::string & fixed_frame, - std::string * error_msg) const = 0; + std::string * error_msg, + std::chrono::milliseconds warning_interval) const = 0; /** * \brief Get all frames that exist in the system. diff --git a/tf2/src/buffer_core.cpp b/tf2/src/buffer_core.cpp index 4c270800e..c68be582c 100644 --- a/tf2/src/buffer_core.cpp +++ b/tf2/src/buffer_core.cpp @@ -82,14 +82,15 @@ void fillOrWarnMessageForInvalidFrame( const char * function_name_arg, const std::string & frame_id, std::string * error_msg, - const char * rationale) + const char * rationale, + std::chrono::milliseconds warning_interval) { std::string s = "Invalid frame ID \"" + frame_id + "\" passed to " + function_name_arg + " - " + rationale; if (error_msg != nullptr) { *error_msg = s; } else { - RCUTILS_LOG_WARN("%s", s.c_str()); + RCUTILS_LOG_WARN_THROTTLE(RCUTILS_STEADY_TIME, warning_interval.count(), "%s", s.c_str()); } } @@ -98,24 +99,27 @@ void fillOrWarnMessageForInvalidFrame( CompactFrameID BufferCore::validateFrameId( const char * function_name_arg, const std::string & frame_id, - std::string * error_msg) const + std::string * error_msg, + std::chrono::milliseconds warning_interval) const { if (frame_id.empty()) { fillOrWarnMessageForInvalidFrame( - function_name_arg, frame_id, error_msg, "in tf2 frame_ids cannot be empty"); + function_name_arg, frame_id, error_msg, "in tf2 frame_ids cannot be empty", + warning_interval); return 0; } if (startsWithSlash(frame_id)) { fillOrWarnMessageForInvalidFrame( - function_name_arg, frame_id, error_msg, "in tf2 frame_ids cannot start with a '/'"); + function_name_arg, frame_id, error_msg, "in tf2 frame_ids cannot start with a '/'", + warning_interval); return 0; } CompactFrameID id = lookupFrameNumber(frame_id); if (id == 0) { fillOrWarnMessageForInvalidFrame( - function_name_arg, frame_id, error_msg, "frame does not exist"); + function_name_arg, frame_id, error_msg, "frame does not exist", warning_interval); } return id; } @@ -759,7 +763,8 @@ bool BufferCore::canTransformInternal( bool BufferCore::canTransform( const std::string & target_frame, const std::string & source_frame, - const TimePoint & time, std::string * error_msg) const + const TimePoint & time, std::string * error_msg, + std::chrono::milliseconds warning_interval) const { // Short circuit if target_frame == source_frame if (target_frame == source_frame) { @@ -767,12 +772,12 @@ bool BufferCore::canTransform( } CompactFrameID target_id = validateFrameId( - "canTransform argument target_frame", target_frame, error_msg); + "canTransform argument target_frame", target_frame, error_msg, warning_interval); if (target_id == 0) { return false; } CompactFrameID source_id = validateFrameId( - "canTransform argument source_frame", source_frame, error_msg); + "canTransform argument source_frame", source_frame, error_msg, warning_interval); if (source_id == 0) { return false; } @@ -783,20 +788,21 @@ bool BufferCore::canTransform( bool BufferCore::canTransform( const std::string & target_frame, const TimePoint & target_time, const std::string & source_frame, const TimePoint & source_time, - const std::string & fixed_frame, std::string * error_msg) const + const std::string & fixed_frame, std::string * error_msg, + std::chrono::milliseconds warning_interval) const { CompactFrameID target_id = validateFrameId( - "canTransform argument target_frame", target_frame, error_msg); + "canTransform argument target_frame", target_frame, error_msg, warning_interval); if (target_id == 0) { return false; } CompactFrameID source_id = validateFrameId( - "canTransform argument source_frame", source_frame, error_msg); + "canTransform argument source_frame", source_frame, error_msg, warning_interval); if (source_id == 0) { return false; } CompactFrameID fixed_id = validateFrameId( - "canTransform argument fixed_frame", fixed_frame, error_msg); + "canTransform argument fixed_frame", fixed_frame, error_msg, warning_interval); if (fixed_id == 0) { return false; } From 2417a398ca4537d61781f22acb4559067390460d Mon Sep 17 00:00:00 2001 From: CursedRock17 Date: Sun, 17 Mar 2024 03:26:56 -0400 Subject: [PATCH 2/4] Rounding out all of the buffer classes Signed-off-by: CursedRock17 --- tf2/include/tf2/buffer_core_interface.h | 6 +++-- tf2_ros/include/tf2_ros/buffer.h | 26 +++++++++++++++------- tf2_ros/include/tf2_ros/buffer_client.h | 9 ++++++-- tf2_ros/include/tf2_ros/buffer_interface.h | 8 +++++-- tf2_ros/src/buffer.cpp | 15 ++++++++----- tf2_ros/src/buffer_client.cpp | 8 +++++-- 6 files changed, 50 insertions(+), 22 deletions(-) diff --git a/tf2/include/tf2/buffer_core_interface.h b/tf2/include/tf2/buffer_core_interface.h index 70f8bc3d4..1e7110bb3 100644 --- a/tf2/include/tf2/buffer_core_interface.h +++ b/tf2/include/tf2/buffer_core_interface.h @@ -37,6 +37,8 @@ #include "tf2/time.h" #include "tf2/visibility_control.h" +using std::literals::chrono_literals::operator""ms; + namespace tf2 { @@ -109,7 +111,7 @@ class BufferCoreInterface const std::string & source_frame, const tf2::TimePoint & time, std::string * error_msg, - std::chrono::milliseconds warning_interval) const = 0; + std::chrono::milliseconds warning_interval = 5000ms) const = 0; /** * \brief Test if a transform is possible. @@ -131,7 +133,7 @@ class BufferCoreInterface const tf2::TimePoint & source_time, const std::string & fixed_frame, std::string * error_msg, - std::chrono::milliseconds warning_interval) const = 0; + std::chrono::milliseconds warning_interval = 5000ms) const = 0; /** * \brief Get all frames that exist in the system. diff --git a/tf2_ros/include/tf2_ros/buffer.h b/tf2_ros/include/tf2_ros/buffer.h index fad6f0f4d..69de8615b 100644 --- a/tf2_ros/include/tf2_ros/buffer.h +++ b/tf2_ros/include/tf2_ros/buffer.h @@ -32,6 +32,7 @@ #ifndef TF2_ROS__BUFFER_H_ #define TF2_ROS__BUFFER_H_ +#include #include #include #include @@ -49,6 +50,8 @@ #include "tf2_msgs/srv/frame_graph.hpp" #include "rclcpp/rclcpp.hpp" +using std::literals::chrono_literals::operator""ms; + namespace tf2_ros { @@ -156,11 +159,13 @@ class Buffer : public BufferInterface, public AsyncBufferInterface, public tf2:: canTransform( const std::string & target_frame, const std::string & source_frame, const tf2::TimePoint & target_time, const tf2::Duration timeout, - std::string * errstr = NULL) const override; + std::string * errstr = NULL, + std::chrono::milliseconds warning_interval = 5000ms) const override; /** \brief Test if a transform is possible * \sa canTransform(const std::string&, const std::string&, - * const tf2::TimePoint&, const tf2::Duration, std::string*) + * const tf2::TimePoint&, const tf2::Duration, std::string* + * std::chrono::milliseconds*) */ TF2_ROS_PUBLIC bool @@ -168,9 +173,11 @@ class Buffer : public BufferInterface, public AsyncBufferInterface, public tf2:: const std::string & target_frame, const std::string & source_frame, const rclcpp::Time & time, const rclcpp::Duration timeout = rclcpp::Duration::from_nanoseconds(0), - std::string * errstr = NULL) const + std::string * errstr = NULL, + std::chrono::milliseconds warning_interval = 5000ms) const { - return canTransform(target_frame, source_frame, fromRclcpp(time), fromRclcpp(timeout), errstr); + return canTransform( + target_frame, source_frame, fromRclcpp(time), fromRclcpp(timeout), errstr, warning_interval); } /** \brief Test if a transform is possible @@ -189,13 +196,15 @@ class Buffer : public BufferInterface, public AsyncBufferInterface, public tf2:: const std::string & target_frame, const tf2::TimePoint & target_time, const std::string & source_frame, const tf2::TimePoint & source_time, const std::string & fixed_frame, const tf2::Duration timeout, - std::string * errstr = NULL) const override; + std::string * errstr = NULL, + std::chrono::milliseconds warning_interval = 5000ms) const override; /** \brief Test if a transform is possible * \sa * canTransform(const std::string&, const tf2::TimePoint&, * const std::string&, const tf2::TimePoint&, - * const std::string&, const tf2::Duration, std::string*) + * const std::string&, const tf2::Duration, std::string* + * std::chrono::milliseconds*) */ TF2_ROS_PUBLIC bool @@ -204,13 +213,14 @@ class Buffer : public BufferInterface, public AsyncBufferInterface, public tf2:: const std::string & source_frame, const rclcpp::Time & source_time, const std::string & fixed_frame, const rclcpp::Duration timeout = rclcpp::Duration::from_nanoseconds(0), - std::string * errstr = NULL) const + std::string * errstr = NULL, + std::chrono::milliseconds warning_interval = 5000ms) const { return canTransform( target_frame, fromRclcpp(target_time), source_frame, fromRclcpp(source_time), fixed_frame, fromRclcpp(timeout), - errstr); + errstr, warning_interval); } /** \brief Wait for a transform between two frames to become available. diff --git a/tf2_ros/include/tf2_ros/buffer_client.h b/tf2_ros/include/tf2_ros/buffer_client.h index ba2818115..e22091800 100644 --- a/tf2_ros/include/tf2_ros/buffer_client.h +++ b/tf2_ros/include/tf2_ros/buffer_client.h @@ -38,6 +38,7 @@ #ifndef TF2_ROS__BUFFER_CLIENT_H_ #define TF2_ROS__BUFFER_CLIENT_H_ +#include #include #include @@ -49,6 +50,8 @@ #include "rclcpp_action/rclcpp_action.hpp" #include "tf2_msgs/action/lookup_transform.hpp" +using std::literals::chrono_literals::operator""ms; + namespace tf2_ros { /** @@ -214,7 +217,8 @@ class BufferClient : public BufferInterface const std::string & source_frame, const tf2::TimePoint & time, const tf2::Duration timeout = tf2::durationFromSec(0.0), - std::string * errstr = nullptr) const override; + std::string * errstr = nullptr, + std::chrono::milliseconds warning_interval = 5000ms) const override; /** \brief Test if a transform is possible * \param target_frame The frame into which to transform @@ -235,7 +239,8 @@ class BufferClient : public BufferInterface const tf2::TimePoint & source_time, const std::string & fixed_frame, const tf2::Duration timeout = tf2::durationFromSec(0.0), - std::string * errstr = nullptr) const override; + std::string * errstr = nullptr, + std::chrono::milliseconds warning_interval = 5000ms) const override; /** \brief Block until the action server is ready to respond to requests. * \param timeout Time to wait for the server. diff --git a/tf2_ros/include/tf2_ros/buffer_interface.h b/tf2_ros/include/tf2_ros/buffer_interface.h index 4f5e4c31f..e5b6fc7ed 100644 --- a/tf2_ros/include/tf2_ros/buffer_interface.h +++ b/tf2_ros/include/tf2_ros/buffer_interface.h @@ -47,6 +47,8 @@ #include "geometry_msgs/msg/transform_stamped.hpp" #include "rclcpp/rclcpp.hpp" +using std::literals::chrono_literals::operator""ms; + namespace tf2_ros { @@ -174,7 +176,8 @@ class BufferInterface canTransform( const std::string & target_frame, const std::string & source_frame, const tf2::TimePoint & time, const tf2::Duration timeout, - std::string * errstr = NULL) const = 0; + std::string * errstr = NULL, + std::chrono::milliseconds warning_interval = 5000ms) const = 0; /** \brief Test if a transform is possible * \param target_frame The frame into which to transform @@ -192,7 +195,8 @@ class BufferInterface const std::string & target_frame, const tf2::TimePoint & target_time, const std::string & source_frame, const tf2::TimePoint & source_time, const std::string & fixed_frame, const tf2::Duration timeout, - std::string * errstr = NULL) const = 0; + std::string * errstr = NULL, + std::chrono::milliseconds warning_interval = 5000ms) const = 0; /** \brief Transform an input into the target frame. * This function is templated and can take as input any valid mathematical object that tf knows diff --git a/tf2_ros/src/buffer.cpp b/tf2_ros/src/buffer.cpp index c3e89ebd1..54aba05e9 100644 --- a/tf2_ros/src/buffer.cpp +++ b/tf2_ros/src/buffer.cpp @@ -32,6 +32,7 @@ #include "tf2_ros/buffer.h" +#include #include #include #include @@ -139,7 +140,8 @@ void conditionally_append_timeout_info( bool Buffer::canTransform( const std::string & target_frame, const std::string & source_frame, - const tf2::TimePoint & time, const tf2::Duration timeout, std::string * errstr) const + const tf2::TimePoint & time, const tf2::Duration timeout, std::string * errstr, + std::chrono::milliseconds warning_interval) const { if (timeout != tf2::durationFromSec(0.0) && !checkAndErrorDedicatedThreadPresent(errstr)) { return false; @@ -152,14 +154,14 @@ Buffer::canTransform( while (clock_->now() < start_time + rclcpp_timeout && !canTransform( target_frame, source_frame, time, - tf2::Duration(std::chrono::nanoseconds::zero()), errstr) && + tf2::Duration(std::chrono::nanoseconds::zero()), errstr, warning_interval) && (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) { // TODO(sloretz) sleep using clock_->sleep_for when implemented std::this_thread::sleep_for(std::chrono::milliseconds(10)); } - bool retval = canTransform(target_frame, source_frame, time, errstr); + bool retval = canTransform(target_frame, source_frame, time, errstr, warning_interval); rclcpp::Time current_time = clock_->now(); conditionally_append_timeout_info(errstr, start_time, current_time, rclcpp_timeout); return retval; @@ -169,7 +171,8 @@ bool Buffer::canTransform( const std::string & target_frame, const tf2::TimePoint & target_time, const std::string & source_frame, const tf2::TimePoint & source_time, - const std::string & fixed_frame, const tf2::Duration timeout, std::string * errstr) const + const std::string & fixed_frame, const tf2::Duration timeout, std::string * errstr, + std::chrono::milliseconds warning_interval) const { if (timeout != tf2::durationFromSec(0.0) && !checkAndErrorDedicatedThreadPresent(errstr)) { return false; @@ -182,7 +185,7 @@ Buffer::canTransform( 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) && + tf2::Duration(std::chrono::nanoseconds::zero()), errstr, warning_interval) && (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) { @@ -191,7 +194,7 @@ Buffer::canTransform( } bool retval = canTransform( target_frame, target_time, - source_frame, source_time, fixed_frame, errstr); + source_frame, source_time, fixed_frame, errstr, warning_interval); rclcpp::Time current_time = clock_->now(); conditionally_append_timeout_info(errstr, start_time, current_time, rclcpp_timeout); return retval; diff --git a/tf2_ros/src/buffer_client.cpp b/tf2_ros/src/buffer_client.cpp index 32702ab32..3fea8232c 100644 --- a/tf2_ros/src/buffer_client.cpp +++ b/tf2_ros/src/buffer_client.cpp @@ -180,8 +180,10 @@ bool BufferClient::canTransform( const std::string & source_frame, const tf2::TimePoint & time, const tf2::Duration timeout, - std::string * errstr) const + std::string * errstr, + std::chrono::milliseconds warning_interval) const { + (void)warning_interval; try { lookupTransform(target_frame, source_frame, time, timeout); return true; @@ -205,8 +207,10 @@ bool BufferClient::canTransform( const tf2::TimePoint & source_time, const std::string & fixed_frame, const tf2::Duration timeout, - std::string * errstr) const + std::string * errstr, + std::chrono::milliseconds warning_interval) const { + (void)warning_interval; try { lookupTransform(target_frame, target_time, source_frame, source_time, fixed_frame, timeout); return true; From 02003cba7dac59747d3d16afebfc27fd8da68c8a Mon Sep 17 00:00:00 2001 From: CursedRock17 Date: Sun, 24 Mar 2024 22:29:35 -0400 Subject: [PATCH 3/4] Adding local variable for throttle value Signed-off-by: CursedRock17 --- tf2/include/tf2/buffer_core.h | 9 +++---- tf2/include/tf2/buffer_core_interface.h | 9 ++----- tf2/src/buffer_core.cpp | 31 +++++++++------------- tf2_ros/include/tf2_ros/buffer.h | 26 ++++++------------ tf2_ros/include/tf2_ros/buffer_client.h | 9 ++----- tf2_ros/include/tf2_ros/buffer_interface.h | 8 ++---- tf2_ros/src/buffer.cpp | 15 +++++------ tf2_ros/src/buffer_client.cpp | 8 ++---- 8 files changed, 38 insertions(+), 77 deletions(-) diff --git a/tf2/include/tf2/buffer_core.h b/tf2/include/tf2/buffer_core.h index 225f62175..598e60a7a 100644 --- a/tf2/include/tf2/buffer_core.h +++ b/tf2/include/tf2/buffer_core.h @@ -169,8 +169,7 @@ class BufferCore : public BufferCoreInterface TF2_PUBLIC bool canTransform( const std::string & target_frame, const std::string & source_frame, - const TimePoint & time, std::string * error_msg = nullptr, - std::chrono::milliseconds warning_interval = 5000ms) const override; + const TimePoint & time, std::string * error_msg = nullptr) const override; /** \brief Test if a transform is possible * \param target_frame The frame into which to transform @@ -185,8 +184,7 @@ class BufferCore : public BufferCoreInterface bool canTransform( const std::string & target_frame, const TimePoint & target_time, const std::string & source_frame, const TimePoint & source_time, - const std::string & fixed_frame, std::string * error_msg = nullptr, - std::chrono::milliseconds warning_interval = 5000ms) const override; + const std::string & fixed_frame, std::string * error_msg = nullptr) const override; /** \brief Get all frames that exist in the system. */ @@ -394,8 +392,7 @@ class BufferCore : public BufferCoreInterface CompactFrameID validateFrameId( const char * function_name_arg, const std::string & frame_id, - std::string * error_msg, - std::chrono::milliseconds warning_interval) const; + std::string * error_msg) const; /** \brief Validate a frame ID format and look it up its compact ID. * Raise an exception for invalid cases. diff --git a/tf2/include/tf2/buffer_core_interface.h b/tf2/include/tf2/buffer_core_interface.h index 1e7110bb3..f68512a6c 100644 --- a/tf2/include/tf2/buffer_core_interface.h +++ b/tf2/include/tf2/buffer_core_interface.h @@ -28,7 +28,6 @@ #ifndef TF2__BUFFER_CORE_INTERFACE_H_ #define TF2__BUFFER_CORE_INTERFACE_H_ -#include #include #include @@ -37,8 +36,6 @@ #include "tf2/time.h" #include "tf2/visibility_control.h" -using std::literals::chrono_literals::operator""ms; - namespace tf2 { @@ -110,8 +107,7 @@ class BufferCoreInterface const std::string & target_frame, const std::string & source_frame, const tf2::TimePoint & time, - std::string * error_msg, - std::chrono::milliseconds warning_interval = 5000ms) const = 0; + std::string * error_msg) const = 0; /** * \brief Test if a transform is possible. @@ -132,8 +128,7 @@ class BufferCoreInterface const std::string & source_frame, const tf2::TimePoint & source_time, const std::string & fixed_frame, - std::string * error_msg, - std::chrono::milliseconds warning_interval = 5000ms) const = 0; + std::string * error_msg) const = 0; /** * \brief Get all frames that exist in the system. diff --git a/tf2/src/buffer_core.cpp b/tf2/src/buffer_core.cpp index c68be582c..d65ab8234 100644 --- a/tf2/src/buffer_core.cpp +++ b/tf2/src/buffer_core.cpp @@ -82,14 +82,14 @@ void fillOrWarnMessageForInvalidFrame( const char * function_name_arg, const std::string & frame_id, std::string * error_msg, - const char * rationale, - std::chrono::milliseconds warning_interval) + const char * rationale) { std::string s = "Invalid frame ID \"" + frame_id + "\" passed to " + function_name_arg + " - " + rationale; if (error_msg != nullptr) { *error_msg = s; } else { + std::chrono::milliseconds warning_interval = 2500ms; RCUTILS_LOG_WARN_THROTTLE(RCUTILS_STEADY_TIME, warning_interval.count(), "%s", s.c_str()); } } @@ -99,27 +99,24 @@ void fillOrWarnMessageForInvalidFrame( CompactFrameID BufferCore::validateFrameId( const char * function_name_arg, const std::string & frame_id, - std::string * error_msg, - std::chrono::milliseconds warning_interval) const + std::string * error_msg) const { if (frame_id.empty()) { fillOrWarnMessageForInvalidFrame( - function_name_arg, frame_id, error_msg, "in tf2 frame_ids cannot be empty", - warning_interval); + function_name_arg, frame_id, error_msg, "in tf2 frame_ids cannot be empty"); return 0; } if (startsWithSlash(frame_id)) { fillOrWarnMessageForInvalidFrame( - function_name_arg, frame_id, error_msg, "in tf2 frame_ids cannot start with a '/'", - warning_interval); + function_name_arg, frame_id, error_msg, "in tf2 frame_ids cannot start with a '/'"); return 0; } CompactFrameID id = lookupFrameNumber(frame_id); if (id == 0) { fillOrWarnMessageForInvalidFrame( - function_name_arg, frame_id, error_msg, "frame does not exist", warning_interval); + function_name_arg, frame_id, error_msg, "frame does not exist"); } return id; } @@ -763,8 +760,7 @@ bool BufferCore::canTransformInternal( bool BufferCore::canTransform( const std::string & target_frame, const std::string & source_frame, - const TimePoint & time, std::string * error_msg, - std::chrono::milliseconds warning_interval) const + const TimePoint & time, std::string * error_msg) const { // Short circuit if target_frame == source_frame if (target_frame == source_frame) { @@ -772,12 +768,12 @@ bool BufferCore::canTransform( } CompactFrameID target_id = validateFrameId( - "canTransform argument target_frame", target_frame, error_msg, warning_interval); + "canTransform argument target_frame", target_frame, error_msg); if (target_id == 0) { return false; } CompactFrameID source_id = validateFrameId( - "canTransform argument source_frame", source_frame, error_msg, warning_interval); + "canTransform argument source_frame", source_frame, error_msg); if (source_id == 0) { return false; } @@ -788,21 +784,20 @@ bool BufferCore::canTransform( bool BufferCore::canTransform( const std::string & target_frame, const TimePoint & target_time, const std::string & source_frame, const TimePoint & source_time, - const std::string & fixed_frame, std::string * error_msg, - std::chrono::milliseconds warning_interval) const + const std::string & fixed_frame, std::string * error_msg) const { CompactFrameID target_id = validateFrameId( - "canTransform argument target_frame", target_frame, error_msg, warning_interval); + "canTransform argument target_frame", target_frame, error_msg); if (target_id == 0) { return false; } CompactFrameID source_id = validateFrameId( - "canTransform argument source_frame", source_frame, error_msg, warning_interval); + "canTransform argument source_frame", source_frame, error_msg); if (source_id == 0) { return false; } CompactFrameID fixed_id = validateFrameId( - "canTransform argument fixed_frame", fixed_frame, error_msg, warning_interval); + "canTransform argument fixed_frame", fixed_frame, error_msg); if (fixed_id == 0) { return false; } diff --git a/tf2_ros/include/tf2_ros/buffer.h b/tf2_ros/include/tf2_ros/buffer.h index 69de8615b..fad6f0f4d 100644 --- a/tf2_ros/include/tf2_ros/buffer.h +++ b/tf2_ros/include/tf2_ros/buffer.h @@ -32,7 +32,6 @@ #ifndef TF2_ROS__BUFFER_H_ #define TF2_ROS__BUFFER_H_ -#include #include #include #include @@ -50,8 +49,6 @@ #include "tf2_msgs/srv/frame_graph.hpp" #include "rclcpp/rclcpp.hpp" -using std::literals::chrono_literals::operator""ms; - namespace tf2_ros { @@ -159,13 +156,11 @@ class Buffer : public BufferInterface, public AsyncBufferInterface, public tf2:: canTransform( const std::string & target_frame, const std::string & source_frame, const tf2::TimePoint & target_time, const tf2::Duration timeout, - std::string * errstr = NULL, - std::chrono::milliseconds warning_interval = 5000ms) const override; + std::string * errstr = NULL) const override; /** \brief Test if a transform is possible * \sa canTransform(const std::string&, const std::string&, - * const tf2::TimePoint&, const tf2::Duration, std::string* - * std::chrono::milliseconds*) + * const tf2::TimePoint&, const tf2::Duration, std::string*) */ TF2_ROS_PUBLIC bool @@ -173,11 +168,9 @@ class Buffer : public BufferInterface, public AsyncBufferInterface, public tf2:: const std::string & target_frame, const std::string & source_frame, const rclcpp::Time & time, const rclcpp::Duration timeout = rclcpp::Duration::from_nanoseconds(0), - std::string * errstr = NULL, - std::chrono::milliseconds warning_interval = 5000ms) const + std::string * errstr = NULL) const { - return canTransform( - target_frame, source_frame, fromRclcpp(time), fromRclcpp(timeout), errstr, warning_interval); + return canTransform(target_frame, source_frame, fromRclcpp(time), fromRclcpp(timeout), errstr); } /** \brief Test if a transform is possible @@ -196,15 +189,13 @@ class Buffer : public BufferInterface, public AsyncBufferInterface, public tf2:: const std::string & target_frame, const tf2::TimePoint & target_time, const std::string & source_frame, const tf2::TimePoint & source_time, const std::string & fixed_frame, const tf2::Duration timeout, - std::string * errstr = NULL, - std::chrono::milliseconds warning_interval = 5000ms) const override; + std::string * errstr = NULL) const override; /** \brief Test if a transform is possible * \sa * canTransform(const std::string&, const tf2::TimePoint&, * const std::string&, const tf2::TimePoint&, - * const std::string&, const tf2::Duration, std::string* - * std::chrono::milliseconds*) + * const std::string&, const tf2::Duration, std::string*) */ TF2_ROS_PUBLIC bool @@ -213,14 +204,13 @@ class Buffer : public BufferInterface, public AsyncBufferInterface, public tf2:: const std::string & source_frame, const rclcpp::Time & source_time, const std::string & fixed_frame, const rclcpp::Duration timeout = rclcpp::Duration::from_nanoseconds(0), - std::string * errstr = NULL, - std::chrono::milliseconds warning_interval = 5000ms) const + std::string * errstr = NULL) const { return canTransform( target_frame, fromRclcpp(target_time), source_frame, fromRclcpp(source_time), fixed_frame, fromRclcpp(timeout), - errstr, warning_interval); + errstr); } /** \brief Wait for a transform between two frames to become available. diff --git a/tf2_ros/include/tf2_ros/buffer_client.h b/tf2_ros/include/tf2_ros/buffer_client.h index e22091800..ba2818115 100644 --- a/tf2_ros/include/tf2_ros/buffer_client.h +++ b/tf2_ros/include/tf2_ros/buffer_client.h @@ -38,7 +38,6 @@ #ifndef TF2_ROS__BUFFER_CLIENT_H_ #define TF2_ROS__BUFFER_CLIENT_H_ -#include #include #include @@ -50,8 +49,6 @@ #include "rclcpp_action/rclcpp_action.hpp" #include "tf2_msgs/action/lookup_transform.hpp" -using std::literals::chrono_literals::operator""ms; - namespace tf2_ros { /** @@ -217,8 +214,7 @@ class BufferClient : public BufferInterface const std::string & source_frame, const tf2::TimePoint & time, const tf2::Duration timeout = tf2::durationFromSec(0.0), - std::string * errstr = nullptr, - std::chrono::milliseconds warning_interval = 5000ms) const override; + std::string * errstr = nullptr) const override; /** \brief Test if a transform is possible * \param target_frame The frame into which to transform @@ -239,8 +235,7 @@ class BufferClient : public BufferInterface const tf2::TimePoint & source_time, const std::string & fixed_frame, const tf2::Duration timeout = tf2::durationFromSec(0.0), - std::string * errstr = nullptr, - std::chrono::milliseconds warning_interval = 5000ms) const override; + std::string * errstr = nullptr) const override; /** \brief Block until the action server is ready to respond to requests. * \param timeout Time to wait for the server. diff --git a/tf2_ros/include/tf2_ros/buffer_interface.h b/tf2_ros/include/tf2_ros/buffer_interface.h index e5b6fc7ed..4f5e4c31f 100644 --- a/tf2_ros/include/tf2_ros/buffer_interface.h +++ b/tf2_ros/include/tf2_ros/buffer_interface.h @@ -47,8 +47,6 @@ #include "geometry_msgs/msg/transform_stamped.hpp" #include "rclcpp/rclcpp.hpp" -using std::literals::chrono_literals::operator""ms; - namespace tf2_ros { @@ -176,8 +174,7 @@ class BufferInterface canTransform( const std::string & target_frame, const std::string & source_frame, const tf2::TimePoint & time, const tf2::Duration timeout, - std::string * errstr = NULL, - std::chrono::milliseconds warning_interval = 5000ms) const = 0; + std::string * errstr = NULL) const = 0; /** \brief Test if a transform is possible * \param target_frame The frame into which to transform @@ -195,8 +192,7 @@ class BufferInterface const std::string & target_frame, const tf2::TimePoint & target_time, const std::string & source_frame, const tf2::TimePoint & source_time, const std::string & fixed_frame, const tf2::Duration timeout, - std::string * errstr = NULL, - std::chrono::milliseconds warning_interval = 5000ms) const = 0; + std::string * errstr = NULL) const = 0; /** \brief Transform an input into the target frame. * This function is templated and can take as input any valid mathematical object that tf knows diff --git a/tf2_ros/src/buffer.cpp b/tf2_ros/src/buffer.cpp index 54aba05e9..c3e89ebd1 100644 --- a/tf2_ros/src/buffer.cpp +++ b/tf2_ros/src/buffer.cpp @@ -32,7 +32,6 @@ #include "tf2_ros/buffer.h" -#include #include #include #include @@ -140,8 +139,7 @@ void conditionally_append_timeout_info( bool Buffer::canTransform( const std::string & target_frame, const std::string & source_frame, - const tf2::TimePoint & time, const tf2::Duration timeout, std::string * errstr, - std::chrono::milliseconds warning_interval) const + const tf2::TimePoint & time, const tf2::Duration timeout, std::string * errstr) const { if (timeout != tf2::durationFromSec(0.0) && !checkAndErrorDedicatedThreadPresent(errstr)) { return false; @@ -154,14 +152,14 @@ Buffer::canTransform( while (clock_->now() < start_time + rclcpp_timeout && !canTransform( target_frame, source_frame, time, - tf2::Duration(std::chrono::nanoseconds::zero()), errstr, warning_interval) && + 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) { // TODO(sloretz) sleep using clock_->sleep_for when implemented std::this_thread::sleep_for(std::chrono::milliseconds(10)); } - bool retval = canTransform(target_frame, source_frame, time, errstr, warning_interval); + bool retval = canTransform(target_frame, source_frame, time, errstr); rclcpp::Time current_time = clock_->now(); conditionally_append_timeout_info(errstr, start_time, current_time, rclcpp_timeout); return retval; @@ -171,8 +169,7 @@ bool Buffer::canTransform( const std::string & target_frame, const tf2::TimePoint & target_time, const std::string & source_frame, const tf2::TimePoint & source_time, - const std::string & fixed_frame, const tf2::Duration timeout, std::string * errstr, - std::chrono::milliseconds warning_interval) const + const std::string & fixed_frame, const tf2::Duration timeout, std::string * errstr) const { if (timeout != tf2::durationFromSec(0.0) && !checkAndErrorDedicatedThreadPresent(errstr)) { return false; @@ -185,7 +182,7 @@ Buffer::canTransform( 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, warning_interval) && + 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) { @@ -194,7 +191,7 @@ Buffer::canTransform( } bool retval = canTransform( target_frame, target_time, - source_frame, source_time, fixed_frame, errstr, warning_interval); + source_frame, source_time, fixed_frame, errstr); rclcpp::Time current_time = clock_->now(); conditionally_append_timeout_info(errstr, start_time, current_time, rclcpp_timeout); return retval; diff --git a/tf2_ros/src/buffer_client.cpp b/tf2_ros/src/buffer_client.cpp index 3fea8232c..32702ab32 100644 --- a/tf2_ros/src/buffer_client.cpp +++ b/tf2_ros/src/buffer_client.cpp @@ -180,10 +180,8 @@ bool BufferClient::canTransform( const std::string & source_frame, const tf2::TimePoint & time, const tf2::Duration timeout, - std::string * errstr, - std::chrono::milliseconds warning_interval) const + std::string * errstr) const { - (void)warning_interval; try { lookupTransform(target_frame, source_frame, time, timeout); return true; @@ -207,10 +205,8 @@ bool BufferClient::canTransform( const tf2::TimePoint & source_time, const std::string & fixed_frame, const tf2::Duration timeout, - std::string * errstr, - std::chrono::milliseconds warning_interval) const + std::string * errstr) const { - (void)warning_interval; try { lookupTransform(target_frame, target_time, source_frame, source_time, fixed_frame, timeout); return true; From 9bcf0e6e9b39dba31d1d1494378a46527198d939 Mon Sep 17 00:00:00 2001 From: CursedRock17 Date: Wed, 3 Apr 2024 19:47:34 -0400 Subject: [PATCH 4/4] Fixing Windows Issue Signed-off-by: CursedRock17 --- tf2/include/tf2/buffer_core.h | 2 -- tf2/src/buffer_core.cpp | 4 +++- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/tf2/include/tf2/buffer_core.h b/tf2/include/tf2/buffer_core.h index 598e60a7a..cb6f78c44 100644 --- a/tf2/include/tf2/buffer_core.h +++ b/tf2/include/tf2/buffer_core.h @@ -52,8 +52,6 @@ #include "tf2/transform_storage.h" #include "tf2/visibility_control.h" -using std::literals::chrono_literals::operator""ms; - namespace tf2 { diff --git a/tf2/src/buffer_core.cpp b/tf2/src/buffer_core.cpp index d65ab8234..3da2b2d48 100644 --- a/tf2/src/buffer_core.cpp +++ b/tf2/src/buffer_core.cpp @@ -89,7 +89,9 @@ void fillOrWarnMessageForInvalidFrame( if (error_msg != nullptr) { *error_msg = s; } else { - std::chrono::milliseconds warning_interval = 2500ms; + static constexpr std::chrono::milliseconds warning_interval = + std::chrono::milliseconds(2500); + RCUTILS_LOG_WARN_THROTTLE(RCUTILS_STEADY_TIME, warning_interval.count(), "%s", s.c_str()); } }