Skip to content

Warning Message Intervals for canTransform #663

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 4 commits into from
Apr 4, 2024
Merged

Warning Message Intervals for canTransform #663

Show file tree
Hide file tree
Changes from 2 commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
62085be
canTransform warning intervals
CursedRock17 Mar 17, 2024
2417a39
Rounding out all of the buffer classes
CursedRock17 Mar 17, 2024
02003cb
Adding local variable for throttle value
CursedRock17 Mar 25, 2024
9bcf0e6
Fixing Windows Issue
CursedRock17 Apr 3, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
13 changes: 9 additions & 4 deletions tf2/include/tf2/buffer_core.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
{
Expand Down Expand Up @@ -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
Expand All @@ -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.
*/
Expand Down Expand Up @@ -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.
Expand Down
9 changes: 7 additions & 2 deletions tf2/include/tf2/buffer_core_interface.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -28,6 +28,7 @@
#ifndef TF2__BUFFER_CORE_INTERFACE_H_
#define TF2__BUFFER_CORE_INTERFACE_H_

#include <chrono>
#include <string>
#include <vector>

Expand All @@ -36,6 +37,8 @@
#include "tf2/time.h"
#include "tf2/visibility_control.h"

using std::literals::chrono_literals::operator""ms;

namespace tf2
{

Expand Down Expand Up @@ -107,7 +110,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 = 5000ms) const = 0;

/**
* \brief Test if a transform is possible.
Expand All @@ -128,7 +132,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 = 5000ms) const = 0;

/**
* \brief Get all frames that exist in the system.
Expand Down
32 changes: 19 additions & 13 deletions tf2/src/buffer_core.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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());
}
}

Expand All @@ -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;
}
Expand Down Expand Up @@ -759,20 +763,21 @@ 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) {
return true;
}

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;
}
Expand All @@ -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;
}
Expand Down
26 changes: 18 additions & 8 deletions tf2_ros/include/tf2_ros/buffer.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -32,6 +32,7 @@
#ifndef TF2_ROS__BUFFER_H_
#define TF2_ROS__BUFFER_H_

#include <chrono>
#include <future>
#include <memory>
#include <mutex>
Expand All @@ -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
{

Expand Down Expand Up @@ -156,21 +159,25 @@ 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
canTransform(
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
Expand All @@ -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
Expand All @@ -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.
Expand Down
9 changes: 7 additions & 2 deletions tf2_ros/include/tf2_ros/buffer_client.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -38,6 +38,7 @@
#ifndef TF2_ROS__BUFFER_CLIENT_H_
#define TF2_ROS__BUFFER_CLIENT_H_

#include <chrono>
#include <stdexcept>
#include <string>

Expand All @@ -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
{
/**
Expand Down Expand Up @@ -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
Expand All @@ -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.
Expand Down
8 changes: 6 additions & 2 deletions tf2_ros/include/tf2_ros/buffer_interface.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
{

Expand Down Expand Up @@ -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
Expand All @@ -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
Expand Down
15 changes: 9 additions & 6 deletions tf2_ros/src/buffer.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -32,6 +32,7 @@

#include "tf2_ros/buffer.h"

#include <chrono>
#include <exception>
#include <limits>
#include <memory>
Expand Down Expand Up @@ -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;
Expand All @@ -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;
Expand All @@ -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;
Expand All @@ -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)
{
Expand All @@ -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;
Expand Down
Loading