From b09faf2ec9246d2decb3369301ea6308be97373e Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Thu, 16 Jan 2020 17:30:49 +0100
Subject: [PATCH] [TF2] Add a signal that says whether the transform is
available.
---
src/ros_tf_listener.cpp | 56 +++++++++++++++++++++++++----------------
src/ros_tf_listener.hh | 28 +++++++++++++++------
2 files changed, 55 insertions(+), 29 deletions(-)
diff --git a/src/ros_tf_listener.cpp b/src/ros_tf_listener.cpp
index 5511783..e130779 100644
--- a/src/ros_tf_listener.cpp
+++ b/src/ros_tf_listener.cpp
@@ -5,18 +5,45 @@
namespace dynamicgraph {
namespace internal {
-sot::MatrixHomogeneous& TransformListenerData::getTransform(sot::MatrixHomogeneous& res, int time) {
+sot::MatrixHomogeneous& TransformListenerData::getTransform(sot::MatrixHomogeneous& res, int time)
+{
+ availableSig.recompute(time);
+
+ bool available = availableSig.accessCopy();
+
+ if (!available) {
+ failbackSig.recompute(time);
+ res = failbackSig.accessCopy();
+ return res;
+ }
+
+ const geometry_msgs::TransformStamped::_transform_type::_rotation_type&
+ quat = transform.transform.rotation;
+ const geometry_msgs::TransformStamped::_transform_type::_translation_type&
+ trans = transform.transform.translation;
+ res.linear() = sot::Quaternion(quat.w, quat.x, quat.y, quat.z).matrix();
+ res.translation() << trans.x, trans.y, trans.z;
+ return res;
+}
+
+bool& TransformListenerData::isAvailable(bool& available, int time)
+{
static const ros::Time origin(0);
- bool useFailback = false;
+ available = false;
ros::Duration elapsed;
std::string msg;
if (buffer.canTransform(toFrame, fromFrame, origin, &msg)) {
transform = buffer.lookupTransform(toFrame, fromFrame, origin);
- elapsed = ros::Time::now() - transform.header.stamp;
- useFailback = elapsed > max_elapsed;
+ if (transform.header.stamp == origin) {
+ // This is likely a TF2 static transform.
+ available = true;
+ } else {
+ elapsed = ros::Time::now() - transform.header.stamp;
+ available = (elapsed <= max_elapsed);
+ }
- if (useFailback) {
+ if (!available) {
std::ostringstream oss;
oss << "Use failback " << signal.getName() << " at time " << time
<< ". Time since last update of the transform: " << elapsed;
@@ -27,24 +54,11 @@ sot::MatrixHomogeneous& TransformListenerData::getTransform(sot::MatrixHomogeneo
oss << "Unable to get transform " << signal.getName() << " at time "
<< time << ": " << msg;
entity->SEND_WARNING_STREAM_MSG(oss.str());
- useFailback = true;
- }
-
- if (useFailback)
- {
- failbackSig.recompute(time);
- res = failbackSig.accessCopy();
- return res;
+ available = false;
}
-
- const geometry_msgs::TransformStamped::_transform_type::_rotation_type&
- quat = transform.transform.rotation;
- const geometry_msgs::TransformStamped::_transform_type::_translation_type&
- trans = transform.transform.translation;
- res.linear() = sot::Quaternion(quat.w, quat.x, quat.y, quat.z).matrix();
- res.translation() << trans.x, trans.y, trans.z;
- return res;
+ return available;
}
+
} // namespace internal
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(RosTfListener, "RosTfListener");
diff --git a/src/ros_tf_listener.hh b/src/ros_tf_listener.hh
index 799a683..85b7e80 100644
--- a/src/ros_tf_listener.hh
+++ b/src/ros_tf_listener.hh
@@ -18,16 +18,18 @@ class RosTfListener;
namespace internal {
struct TransformListenerData {
- typedef SignalTimeDependent<sot::MatrixHomogeneous, int> signal_t;
- typedef SignalPtr<sot::MatrixHomogeneous, int> signalIn_t;
+ typedef SignalTimeDependent<bool, int> AvailableSignal_t;
+ typedef SignalTimeDependent<sot::MatrixHomogeneous, int> MatrixSignal_t;
+ typedef SignalPtr<sot::MatrixHomogeneous, int> DefaultSignal_t;
RosTfListener* entity;
tf2_ros::Buffer& buffer;
const std::string toFrame, fromFrame;
geometry_msgs::TransformStamped transform;
ros::Duration max_elapsed;
- signal_t signal;
- signalIn_t failbackSig;
+ AvailableSignal_t availableSig;
+ MatrixSignal_t signal;
+ DefaultSignal_t failbackSig;
TransformListenerData(RosTfListener* e, tf2_ros::Buffer& b,
const std::string& to, const std::string& from,
@@ -37,16 +39,24 @@ struct TransformListenerData {
, toFrame(to)
, fromFrame(from)
, max_elapsed(0.5)
+ , availableSig(signame+"_available")
, signal(signame)
, failbackSig(NULL, signame+"_failback")
{
signal.setFunction(
boost::bind(&TransformListenerData::getTransform, this, _1, _2));
+
+ availableSig.setFunction(
+ boost::bind(&TransformListenerData::isAvailable, this, _1, _2));
+ availableSig.setNeedUpdateFromAllChildren(true);
+
failbackSig.setConstant (sot::MatrixHomogeneous::Identity());
- signal.addDependency(failbackSig);
+ signal.addDependencies(failbackSig << availableSig);
}
sot::MatrixHomogeneous& getTransform(sot::MatrixHomogeneous& res, int time);
+
+ bool& isAvailable(bool& isAvailable, int time);
};
} // namespace internal
@@ -82,11 +92,13 @@ class RosTfListener : public Entity {
addCommand("setMaximumDelay", command::makeCommandVoid2(*this, &RosTfListener::setMaximumDelay, docstring));
}
- ~RosTfListener() {
+ ~RosTfListener()
+ {
for (Map_t::const_iterator _it = listenerDatas.begin(); _it != listenerDatas.end(); ++_it) delete _it->second;
}
- void add(const std::string& to, const std::string& from, const std::string& signame) {
+ void add(const std::string& to, const std::string& from, const std::string& signame)
+ {
if (listenerDatas.find(signame) != listenerDatas.end())
throw std::invalid_argument("A signal " + signame + " already exists in RosTfListener " + getName());
@@ -95,7 +107,7 @@ class RosTfListener : public Entity {
TransformListenerData* tld =
new TransformListenerData(this, buffer, to, from, signalName.str());
- signalRegistration(tld->signal << tld->failbackSig);
+ signalRegistration(tld->signal << tld->availableSig << tld->failbackSig);
listenerDatas[signame] = tld;
}
--
GitLab