[TF2] Add a signal that says whether the transform is available.

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");

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;
   }