From ac658382edd4856e4c1f6b102f6dbf18e60ef2e2 Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Mon, 25 Jun 2018 10:54:21 +0200
Subject: [PATCH] Add RosTfListener
---
CMakeLists.txt | 1 +
src/ros_tf_listener.cpp | 9 ++++
src/ros_tf_listener.hh | 107 ++++++++++++++++++++++++++++++++++++++++
3 files changed, 117 insertions(+)
create mode 100644 src/ros_tf_listener.cpp
create mode 100644 src/ros_tf_listener.hh
diff --git a/CMakeLists.txt b/CMakeLists.txt
index c58e04e..f58a660 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -133,6 +133,7 @@ endmacro()
compile_plugin(ros_publish)
compile_plugin(ros_subscribe)
compile_plugin(ros_queued_subscribe)
+compile_plugin(ros_tf_listener)
compile_plugin(ros_time)
compile_plugin(ros_joint_state)
pkg_config_use_dependency(ros_joint_state pinocchio)
diff --git a/src/ros_tf_listener.cpp b/src/ros_tf_listener.cpp
new file mode 100644
index 0000000..7dd625a
--- /dev/null
+++ b/src/ros_tf_listener.cpp
@@ -0,0 +1,9 @@
+#include "dynamic_graph_bridge/ros_init.hh"
+#include "ros_tf_listener.hh"
+
+#include <dynamic-graph/factory.h>
+
+namespace dynamicgraph
+{
+ DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(RosTfListener, "RosTfListener");
+}
diff --git a/src/ros_tf_listener.hh b/src/ros_tf_listener.hh
new file mode 100644
index 0000000..d09433e
--- /dev/null
+++ b/src/ros_tf_listener.hh
@@ -0,0 +1,107 @@
+#ifndef DYNAMIC_GRAPH_ROS_TF_LISTENER_HH
+# define DYNAMIC_GRAPH_ROS_TF_LISTENER_HH
+
+# include <boost/bind.hpp>
+
+# include <tf/transform_listener.h>
+
+# include <dynamic-graph/entity.h>
+# include <dynamic-graph/signal.h>
+# include <dynamic-graph/command-bind.h>
+
+# include <sot/core/matrix-geometry.hh>
+
+namespace dynamicgraph {
+ class RosTfListener;
+
+ namespace internal
+ {
+ struct TransformListenerData {
+ typedef Signal<sot::MatrixHomogeneous, int> signal_t;
+
+ tf::TransformListener& listener;
+ const std::string toFrame, fromFrame;
+ tf::StampedTransform transform;
+ signal_t signal;
+
+ TransformListenerData (tf::TransformListener& l,
+ const std::string& to, const std::string& from,
+ const std::string& signame)
+ : listener (l)
+ , toFrame (to)
+ , fromFrame (from)
+ , signal (signame)
+ {
+ signal.setFunction (boost::bind(&TransformListenerData::getTransform, this, _1, _2));
+ }
+
+ sot::MatrixHomogeneous& getTransform (sot::MatrixHomogeneous& res, int time)
+ {
+ static const ros::Time rosTime(0);
+ try {
+ listener.lookupTransform (toFrame, fromFrame, rosTime, transform);
+ } catch (const tf::TransformException& ex) {
+ res.setIdentity();
+ ROS_ERROR("Enable to get transform at time %i: %s",time,ex.what());
+ }
+ for (sot::MatrixHomogeneous::Index r = 0; r < 3; ++r) {
+ for (sot::MatrixHomogeneous::Index c = 0; c < 3; ++c)
+ res.linear ()(r,c) = transform.getBasis().getRow(r)[c];
+ res.translation()[r] = transform.getOrigin()[r];
+ }
+ return res;
+ }
+ };
+ } // end of internal namespace.
+
+ class RosTfListener : public Entity
+ {
+ DYNAMIC_GRAPH_ENTITY_DECL();
+
+ public:
+ typedef internal::TransformListenerData TransformListenerData;
+
+ RosTfListener (const std::string& name) : Entity (name)
+ {
+ std::string docstring =
+ "\n"
+ " Add a signal containing the transform between two frames.\n"
+ "\n"
+ " Input:\n"
+ " - to : frame name\n"
+ " - from: frame name,\n"
+ " - signalName: the signal name in dynamic-graph"
+ "\n";
+ addCommand ("add",
+ command::makeCommandVoid3(*this, &RosTfListener::add, docstring));
+ }
+
+ ~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)
+ {
+ if (listenerDatas.find(signame) != listenerDatas.end())
+ throw std::invalid_argument ("A signal " + signame
+ + " already exists in RosTfListener " + getName());
+
+ boost::format signalName ("RosTfListener(%1%)::output(MatrixHomo)::%2%");
+ signalName % getName () % signame;
+
+ TransformListenerData* tld = new TransformListenerData (
+ listener, to, from, signalName.str());
+ signalRegistration (tld->signal);
+ listenerDatas[signame] = tld;
+ }
+
+ private:
+ typedef std::map<std::string, TransformListenerData*> Map_t;
+ Map_t listenerDatas;
+ tf::TransformListener listener;
+ };
+} // end of namespace dynamicgraph.
+
+#endif // DYNAMIC_GRAPH_ROS_TF_LISTENER_HH
--
GitLab