diff --git a/src/ros_tf_listener.cpp b/src/ros_tf_listener.cpp index 87332493b30e798a2135883925837e440de913db..48137b30edab04e4515d2ee913f57aa920ef16ed 100644 --- a/src/ros_tf_listener.cpp +++ b/src/ros_tf_listener.cpp @@ -11,10 +11,12 @@ sot::MatrixHomogeneous& TransformListenerData::getTransform( try { transform = buffer.lookupTransform(toFrame, fromFrame, rosTime); } catch (const tf2::TransformException& ex) { - res.setIdentity(); std::ostringstream oss; oss << "Enable to get transform at time " << time << ": " << ex.what(); entity->SEND_WARNING_STREAM_MSG(oss.str()); + + failbackSig.recompute(time); + res = failbackSig.accessCopy(); return res; } diff --git a/src/ros_tf_listener.hh b/src/ros_tf_listener.hh index adf4b5db66fe3641a52b361577975b4df9e151b1..256a6d3bff113b3040bf0fada4458dd0e054c55d 100644 --- a/src/ros_tf_listener.hh +++ b/src/ros_tf_listener.hh @@ -8,6 +8,7 @@ #include <dynamic-graph/entity.h> #include <dynamic-graph/signal.h> +#include <dynamic-graph/signal-ptr.h> #include <dynamic-graph/command-bind.h> #include <sot/core/matrix-geometry.hh> @@ -18,19 +19,28 @@ class RosTfListener; namespace internal { struct TransformListenerData { typedef Signal<sot::MatrixHomogeneous, int> signal_t; + typedef SignalPtr<sot::MatrixHomogeneous, int> signalIn_t; RosTfListener* entity; tf2_ros::Buffer& buffer; const std::string toFrame, fromFrame; geometry_msgs::TransformStamped transform; signal_t signal; + signalIn_t failbackSig; TransformListenerData(RosTfListener* e, tf2_ros::Buffer& b, const std::string& to, const std::string& from, const std::string& signame) - : entity(e), buffer(b), toFrame(to), fromFrame(from), signal(signame) { + : entity(e) + , buffer(b) + , toFrame(to) + , fromFrame(from) + , signal(signame) + , failbackSig(NULL, signame+"_failback") + { signal.setFunction( boost::bind(&TransformListenerData::getTransform, this, _1, _2)); + failbackSig.setConstant (sot::MatrixHomogeneous::Identity()); } sot::MatrixHomogeneous& getTransform(sot::MatrixHomogeneous& res, int time); @@ -79,7 +89,7 @@ class RosTfListener : public Entity { TransformListenerData* tld = new TransformListenerData(this, buffer, to, from, signalName.str()); - signalRegistration(tld->signal); + signalRegistration(tld->signal << tld->failbackSig); listenerDatas[signame] = tld; }