Skip to content

Commit

Permalink
Extend TimeCache API to provide rich ExtrapolationException infos (#586)
Browse files Browse the repository at this point in the history 
* Extend TimeCache API to provide rich ExtrapolationException infos
  • Loading branch information
@roncapat
roncapat authored Mar 15, 2023
1 parent 8920381 commit 3bac620
Show file tree
Hide file tree
Showing 6 changed files with 169 additions and 28 deletions.
42 changes: 42 additions & 0 deletions tf2/include/tf2/exceptions.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -66,6 +66,9 @@ enum class TF2Error : std::uint8_t
TF2_INVALID_ARGUMENT_ERROR = 4,
TF2_TIMEOUT_ERROR = 5,
TF2_TRANSFORM_ERROR = 6,
TF2_BACKWARD_EXTRAPOLATION_ERROR = 7,
TF2_FORWARD_EXTRAPOLATION_ERROR = 8,
TF2_NO_DATA_FOR_EXTRAPOLATION_ERROR = 9,

NO_ERROR [[deprecated("Use TF2_NO_ERROR instead")]] = 0,
LOOKUP_ERROR [[deprecated("Use TF2_LOOKUP_ERROR instead")]] = 1,
Expand Down Expand Up @@ -147,6 +150,45 @@ class ExtrapolationException : public TransformException
}
};

/** \brief An exception class to notify that the requested value would have required extrapolation in the past.
*
*/
class BackwardExtrapolationException : public ExtrapolationException
{
public:
TF2_PUBLIC
explicit BackwardExtrapolationException(const std::string errorDescription)
: ExtrapolationException(errorDescription)
{
}
};

/** \brief An exception class to notify that the requested value would have required extrapolation in the future.
*
*/
class ForwardExtrapolationException : public ExtrapolationException
{
public:
TF2_PUBLIC
explicit ForwardExtrapolationException(const std::string errorDescription)
: ExtrapolationException(errorDescription)
{
}
};

/** \brief An exception class to notify that the requested value would have required extrapolation, but only zero or one data is available, so not enough for extrapolation.
*
*/
class NoDataForExtrapolationException : public ExtrapolationException
{
public:
TF2_PUBLIC
explicit NoDataForExtrapolationException(const std::string errorDescription)
: ExtrapolationException(errorDescription)
{
}
};

/** \brief An exception class to notify that one of the arguments is invalid
*
* usually it's an uninitalized Quaternion (0,0,0,0)
Expand Down
20 changes: 13 additions & 7 deletions tf2/include/tf2/time_cache.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -39,6 +39,7 @@

#include "tf2/visibility_control.h"
#include "tf2/transform_storage.h"
#include "tf2/exceptions.h"

namespace tf2
{
Expand All @@ -56,7 +57,7 @@ class TimeCacheInterface
TF2_PUBLIC
virtual bool getData(
tf2::TimePoint time, tf2::TransformStorage & data_out,
std::string * error_str = 0) = 0;
std::string * error_str = 0, TF2Error * error_code = 0) = 0;

/** \brief Insert data into the cache */
TF2_PUBLIC
Expand All @@ -68,7 +69,8 @@ class TimeCacheInterface

/** \brief Retrieve the parent at a specific time */
TF2_PUBLIC
virtual CompactFrameID getParent(tf2::TimePoint time, std::string * error_str) = 0;
virtual CompactFrameID getParent(
tf2::TimePoint time, std::string * error_str = 0, TF2Error * error_code = 0) = 0;

/**
* \brief Get the latest time stored in this cache, and the parent associated with it. Returns parent = 0 if no data.
Expand Down Expand Up @@ -117,13 +119,14 @@ class TimeCache : public TimeCacheInterface
TF2_PUBLIC
virtual bool getData(
tf2::TimePoint time, tf2::TransformStorage & data_out,
std::string * error_str = 0);
std::string * error_str = 0, TF2Error * error_code = 0);
TF2_PUBLIC
virtual bool insertData(const tf2::TransformStorage & new_data);
TF2_PUBLIC
virtual void clearList();
TF2_PUBLIC
virtual tf2::CompactFrameID getParent(tf2::TimePoint time, std::string * error_str);
virtual tf2::CompactFrameID getParent(
tf2::TimePoint time, std::string * error_str = 0, TF2Error * error_code = 0);
TF2_PUBLIC
virtual P_TimeAndFrameID getLatestTimeAndParent();

Expand All @@ -146,7 +149,7 @@ class TimeCache : public TimeCacheInterface
// Assumes storage is already locked for it
inline uint8_t findClosest(
tf2::TransformStorage * & one, TransformStorage * & two,
tf2::TimePoint target_time, std::string * error_str);
tf2::TimePoint target_time, std::string * error_str = 0, TF2Error * error_code = 0);

inline void interpolate(
const tf2::TransformStorage & one, const tf2::TransformStorage & two,
Expand All @@ -160,14 +163,17 @@ class StaticCache : public TimeCacheInterface
public:
/// Virtual methods
TF2_PUBLIC
virtual bool getData(TimePoint time, TransformStorage & data_out, std::string * error_str = 0);
virtual bool getData(
TimePoint time, TransformStorage & data_out,
std::string * error_str = 0, TF2Error * error_code = 0);
// returns false if data unavailable (should be thrown as lookup exception
TF2_PUBLIC
virtual bool insertData(const TransformStorage & new_data);
TF2_PUBLIC
virtual void clearList();
TF2_PUBLIC
virtual CompactFrameID getParent(TimePoint time, std::string * error_str);
virtual CompactFrameID getParent(
TimePoint time, std::string * error_str = 0, TF2Error * error_code = 0);
TF2_PUBLIC
virtual P_TimeAndFrameID getLatestTimeAndParent();

Expand Down
39 changes: 31 additions & 8 deletions tf2/src/buffer_core.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -351,6 +351,7 @@ tf2::TF2Error BufferCore::walkToTopParent(
CompactFrameID top_parent = frame;
uint32_t depth = 0;

TF2Error error_code = TF2Error::TF2_NO_ERROR;
std::string extrapolation_error_string;
bool extrapolation_might_have_occurred = false;

Expand All @@ -366,7 +367,7 @@ tf2::TF2Error BufferCore::walkToTopParent(
break;
}

CompactFrameID parent = f.gather(cache, time, &extrapolation_error_string);
CompactFrameID parent = f.gather(cache, time, &extrapolation_error_string, &error_code);
if (parent == 0) {
// Just break out here... there may still be a path from source -> target
top_parent = frame;
Expand Down Expand Up @@ -412,7 +413,7 @@ tf2::TF2Error BufferCore::walkToTopParent(
break;
}

CompactFrameID parent = f.gather(cache, time, error_string);
CompactFrameID parent = f.gather(cache, time, error_string, &error_code);
if (parent == 0) {
if (error_string) {
std::stringstream ss;
Expand All @@ -421,7 +422,7 @@ tf2::TF2Error BufferCore::walkToTopParent(
*error_string = ss.str();
}

return tf2::TF2Error::TF2_EXTRAPOLATION_ERROR;
return error_code;
}

// Early out... source frame is a direct parent of the target frame
Expand Down Expand Up @@ -457,7 +458,7 @@ tf2::TF2Error BufferCore::walkToTopParent(
lookupFrameString(source_id) << "] to frame [" << lookupFrameString(target_id) << "]";
*error_string = ss.str();
}
return tf2::TF2Error::TF2_EXTRAPOLATION_ERROR;
return error_code;
}
createConnectivityErrorString(source_id, target_id, error_string);
return tf2::TF2Error::TF2_CONNECTIVITY_ERROR;
Expand Down Expand Up @@ -504,9 +505,11 @@ struct TransformAccum
{
}

CompactFrameID gather(TimeCacheInterfacePtr cache, TimePoint time, std::string * error_string)
CompactFrameID gather(
TimeCacheInterfacePtr cache, TimePoint time,
std::string * error_string, TF2Error * error_code)
{
if (!cache->getData(time, st, error_string)) {
if (!cache->getData(time, st, error_string, error_code)) {
return 0;
}

Expand Down Expand Up @@ -661,6 +664,12 @@ void BufferCore::lookupTransformImpl(
switch (retval) {
case tf2::TF2Error::TF2_CONNECTIVITY_ERROR:
throw ConnectivityException(error_string);
case tf2::TF2Error::TF2_BACKWARD_EXTRAPOLATION_ERROR:
throw BackwardExtrapolationException(error_string);
case tf2::TF2Error::TF2_FORWARD_EXTRAPOLATION_ERROR:
throw ForwardExtrapolationException(error_string);
case tf2::TF2Error::TF2_NO_DATA_FOR_EXTRAPOLATION_ERROR:
throw NoDataForExtrapolationException(error_string);
case tf2::TF2Error::TF2_EXTRAPOLATION_ERROR:
throw ExtrapolationException(error_string);
case tf2::TF2Error::TF2_LOOKUP_ERROR:
Expand Down Expand Up @@ -698,9 +707,11 @@ void BufferCore::lookupTransformImpl(

struct CanTransformAccum
{
CompactFrameID gather(TimeCacheInterfacePtr cache, TimePoint time, std::string * error_string)
CompactFrameID gather(
TimeCacheInterfacePtr cache, TimePoint time,
std::string * error_string, TF2Error * error_code)
{
return cache->getParent(time, error_string);
return cache->getParent(time, error_string, error_code);
}

void accum(bool source)
Expand Down Expand Up @@ -1455,6 +1466,12 @@ void BufferCore::_chainAsVector(
switch (retval) {
case tf2::TF2Error::TF2_CONNECTIVITY_ERROR:
throw ConnectivityException(error_string);
case tf2::TF2Error::TF2_BACKWARD_EXTRAPOLATION_ERROR:
throw BackwardExtrapolationException(error_string);
case tf2::TF2Error::TF2_FORWARD_EXTRAPOLATION_ERROR:
throw ForwardExtrapolationException(error_string);
case tf2::TF2Error::TF2_NO_DATA_FOR_EXTRAPOLATION_ERROR:
throw NoDataForExtrapolationException(error_string);
case tf2::TF2Error::TF2_EXTRAPOLATION_ERROR:
throw ExtrapolationException(error_string);
case tf2::TF2Error::TF2_LOOKUP_ERROR:
Expand All @@ -1475,6 +1492,12 @@ void BufferCore::_chainAsVector(
switch (retval) {
case tf2::TF2Error::TF2_CONNECTIVITY_ERROR:
throw ConnectivityException(error_string);
case tf2::TF2Error::TF2_BACKWARD_EXTRAPOLATION_ERROR:
throw BackwardExtrapolationException(error_string);
case tf2::TF2Error::TF2_FORWARD_EXTRAPOLATION_ERROR:
throw ForwardExtrapolationException(error_string);
case tf2::TF2Error::TF2_NO_DATA_FOR_EXTRAPOLATION_ERROR:
throw NoDataForExtrapolationException(error_string);
case tf2::TF2Error::TF2_EXTRAPOLATION_ERROR:
throw ExtrapolationException(error_string);
case tf2::TF2Error::TF2_LOOKUP_ERROR:
Expand Down
40 changes: 29 additions & 11 deletions tf2/src/cache.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -67,8 +67,12 @@ namespace cache
{
// hoisting these into separate functions causes an ~8% speedup.
// Removing calling them altogether adds another ~10%
void createExtrapolationException1(TimePoint t0, TimePoint t1, std::string * error_str)
void createExtrapolationException1(
TimePoint t0, TimePoint t1, std::string * error_str, TF2Error * error_code)
{
if (error_code) {
*error_code = TF2Error::TF2_NO_DATA_FOR_EXTRAPOLATION_ERROR;
}
if (error_str) {
std::stringstream ss;
ss << "Lookup would require extrapolation at time " << displayTimePoint(t0) <<
Expand All @@ -77,8 +81,12 @@ void createExtrapolationException1(TimePoint t0, TimePoint t1, std::string * err
}
}

void createExtrapolationException2(TimePoint t0, TimePoint t1, std::string * error_str)
void createExtrapolationException2(
TimePoint t0, TimePoint t1, std::string * error_str, TF2Error * error_code)
{
if (error_code) {
*error_code = TF2Error::TF2_FORWARD_EXTRAPOLATION_ERROR;
}
if (error_str) {
std::stringstream ss;
ss << "Lookup would require extrapolation into the future. Requested time " <<
Expand All @@ -87,8 +95,12 @@ void createExtrapolationException2(TimePoint t0, TimePoint t1, std::string * err
}
}

void createExtrapolationException3(TimePoint t0, TimePoint t1, std::string * error_str)
void createExtrapolationException3(
TimePoint t0, TimePoint t1, std::string * error_str, TF2Error * error_code)
{
if (error_code) {
*error_code = TF2Error::TF2_BACKWARD_EXTRAPOLATION_ERROR;
}
if (error_str) {
std::stringstream ss;
ss << "Lookup would require extrapolation into the past. Requested time " << displayTimePoint(
Expand All @@ -100,10 +112,15 @@ void createExtrapolationException3(TimePoint t0, TimePoint t1, std::string * err

uint8_t TimeCache::findClosest(
TransformStorage * & one, TransformStorage * & two,
TimePoint target_time, std::string * error_str)
TimePoint target_time, std::string * error_str, TF2Error * error_code)
{
if (error_code) {
*error_code = TF2Error::TF2_NO_ERROR;
}

// No values stored
if (storage_.empty()) {
*error_code = TF2Error::TF2_NO_DATA_FOR_EXTRAPOLATION_ERROR;
return 0;
}

Expand All @@ -120,7 +137,7 @@ uint8_t TimeCache::findClosest(
one = &ts;
return 1;
} else {
cache::createExtrapolationException1(target_time, ts.stamp_, error_str);
cache::createExtrapolationException1(target_time, ts.stamp_, error_str, error_code);
return 0;
}
}
Expand All @@ -136,11 +153,11 @@ uint8_t TimeCache::findClosest(
return 1;
} else { // Catch cases that would require extrapolation
if (target_time > latest_time) {
cache::createExtrapolationException2(target_time, latest_time, error_str);
cache::createExtrapolationException2(target_time, latest_time, error_str, error_code);
return 0;
} else {
if (target_time < earliest_time) {
cache::createExtrapolationException3(target_time, earliest_time, error_str);
cache::createExtrapolationException3(target_time, earliest_time, error_str, error_code);
return 0;
}
}
Expand Down Expand Up @@ -188,13 +205,13 @@ void TimeCache::interpolate(

bool TimeCache::getData(
TimePoint time, TransformStorage & data_out,
std::string * error_str)
std::string * error_str, TF2Error * error_code)
{
// returns false if data not available
TransformStorage * p_temp_1;
TransformStorage * p_temp_2;

int num_nodes = findClosest(p_temp_1, p_temp_2, time, error_str);
int num_nodes = findClosest(p_temp_1, p_temp_2, time, error_str, error_code);
if (num_nodes == 0) {
return false;
} else if (num_nodes == 1) {
Expand All @@ -211,12 +228,13 @@ bool TimeCache::getData(
return true;
}

CompactFrameID TimeCache::getParent(TimePoint time, std::string * error_str)
CompactFrameID TimeCache::getParent(
TimePoint time, std::string * error_str, TF2Error * error_code)
{
TransformStorage * p_temp_1;
TransformStorage * p_temp_2;

int num_nodes = findClosest(p_temp_1, p_temp_2, time, error_str);
int num_nodes = findClosest(p_temp_1, p_temp_2, time, error_str, error_code);
if (num_nodes == 0) {
return 0;
}
Expand Down
8 changes: 6 additions & 2 deletions tf2/src/static_cache.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,8 +38,9 @@

bool tf2::StaticCache::getData(
tf2::TimePoint time,
tf2::TransformStorage & data_out, std::string * error_str)
tf2::TransformStorage & data_out, std::string * error_str, TF2Error * error_code)
{
(void)error_code;
(void)error_str;
data_out = storage_;
data_out.stamp_ = time;
Expand All @@ -56,9 +57,12 @@ void tf2::StaticCache::clearList() {}

unsigned tf2::StaticCache::getListLength() {return 1;}

tf2::CompactFrameID tf2::StaticCache::getParent(tf2::TimePoint time, std::string * error_str)
tf2::CompactFrameID tf2::StaticCache::getParent(
tf2::TimePoint time, std::string * error_str,
TF2Error * error_code)
{
(void)time;
(void)error_code;
(void)error_str;
return storage_.frame_id_;
}
Expand Down
Loading

0 comments on commit 3bac620

Please sign in to comment.