From c390b3a8009cd829ddf3de9d7090fd72b4b27d62 Mon Sep 17 00:00:00 2001
From: Thomas Moulard <thomas.moulard@gmail.com>
Date: Thu, 10 Nov 2011 14:07:30 +0100
Subject: [PATCH] Fix Ros import class.
---
CMakeLists.txt | 2 ++
src/ros_import.cpp | 21 ++++++++++++++++++++-
src/ros_import.hh | 19 ++++++++++++++-----
src/ros_import.hxx | 36 +++++++++++++++++++-----------------
src/sot_to_ros.cpp | 14 ++++++++++++++
src/sot_to_ros.hh | 33 ++++++++++++++++++++++++++++++++-
tests/test_import.py | 15 +++++++++++++++
7 files changed, 116 insertions(+), 24 deletions(-)
create mode 100644 src/sot_to_ros.cpp
create mode 100755 tests/test_import.py
diff --git a/CMakeLists.txt b/CMakeLists.txt
index d3f6f11..a65358c 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -36,6 +36,7 @@ pkg_check_modules(DYNAMIC_GRAPH REQUIRED dynamic-graph)
pkg_check_modules(SOT_CORE REQUIRED sot-core)
rosbuild_add_library(ros_import
+ src/sot_to_ros.cpp
src/ros_import.cpp src/ros_import.hh src/converter.hh src/sot_to_ros.hh)
rosbuild_add_compile_flags(ros_import ${JRL_MAL_CFLAGS} ${DYNAMIC_GRAPH_CFLAGS}
${SOT_CORE_CFLAGS})
@@ -45,6 +46,7 @@ target_link_libraries(ros_import
${JRL_MAL_LIBRARIES} ${DYNAMIC_GRAPH_LIBRARIES} ${SOT_CORE_LIBRARIES})
rosbuild_add_library(ros_export
+ src/sot_to_ros.cpp
src/ros_export.cpp src/ros_export.hh src/converter.hh src/sot_to_ros.hh)
rosbuild_add_compile_flags(ros_export ${JRL_MAL_CFLAGS} ${DYNAMIC_GRAPH_CFLAGS}
${SOT_CORE_CFLAGS})
diff --git a/src/ros_import.cpp b/src/ros_import.cpp
index 63b820c..e709bac 100644
--- a/src/ros_import.cpp
+++ b/src/ros_import.cpp
@@ -2,6 +2,7 @@
#include <boost/assign.hpp>
#include <boost/bind.hpp>
+#include <boost/foreach.hpp>
#include <boost/format.hpp>
#include <boost/function.hpp>
#include <boost/make_shared.hpp>
@@ -119,8 +120,14 @@ namespace dynamicgraph
RosImport::RosImport (const std::string& n)
: dynamicgraph::Entity(n),
nh_ (rosInit ()),
- bindedSignal_ ()
+ bindedSignal_ (),
+ trigger_ (boost::bind (&RosImport::trigger, this, _1, _2),
+ sotNOSIGNAL,
+ MAKE_SIGNAL_STRING(name, true, "int", "trigger"))
{
+ signalRegistration (trigger_);
+ trigger_.setNeedUpdateFromAllChildren (true);
+
std::string docstring;
addCommand ("add",
new command::rosImport::Add
@@ -160,4 +167,16 @@ namespace dynamicgraph
bindedSignal_.clear ();
}
+ int& RosImport::trigger (int& dummy, int t)
+ {
+ typedef std::map<std::string, bindedSignal_t>::iterator iterator_t;
+
+ for (iterator_t it = bindedSignal_.begin ();
+ it != bindedSignal_.end (); ++it)
+ {
+ boost::get<2>(it->second) (t);
+ }
+ return dummy;
+ }
+
} // end of namespace dynamicgraph.
diff --git a/src/ros_import.hh b/src/ros_import.hh
index e45695a..c7f5025 100644
--- a/src/ros_import.hh
+++ b/src/ros_import.hh
@@ -4,6 +4,7 @@
# include <map>
# include <boost/shared_ptr.hpp>
+# include <boost/tuple/tuple.hpp>
# include <dynamic-graph/entity.h>
# include <dynamic-graph/signal-time-dependent.h>
@@ -50,9 +51,13 @@ namespace dynamicgraph
{
DYNAMIC_GRAPH_ENTITY_DECL();
public:
- typedef std::pair<boost::shared_ptr<dynamicgraph::SignalBase<int> >,
- boost::shared_ptr<ros::Publisher> >
- bindedSignal_t;
+ typedef boost::function<void (int)> callback_t;
+
+ typedef boost::tuple<
+ boost::shared_ptr<dynamicgraph::SignalBase<int> >,
+ boost::shared_ptr<ros::Publisher>,
+ callback_t>
+ bindedSignal_t;
RosImport (const std::string& n);
virtual ~RosImport ();
@@ -64,9 +69,12 @@ namespace dynamicgraph
void list ();
void clear ();
+ int& trigger (int&, int);
+
template <typename T>
- T& sendData (boost::shared_ptr<ros::Publisher> publisher,
- T& data, int time);
+ void sendData (boost::shared_ptr<ros::Publisher> publisher,
+ boost::shared_ptr<typename SotToRos<T>::signal_t> signal,
+ int time);
template <typename T>
void add (const std::string& signal, const std::string& topic);
@@ -74,6 +82,7 @@ namespace dynamicgraph
private:
ros::NodeHandle nh_;
std::map<std::string, bindedSignal_t> bindedSignal_;
+ dynamicgraph::SignalTimeDependent<int,int> trigger_;
};
} // end of namespace dynamicgraph.
diff --git a/src/ros_import.hxx b/src/ros_import.hxx
index e628d3d..5b7b8cf 100644
--- a/src/ros_import.hxx
+++ b/src/ros_import.hxx
@@ -12,14 +12,14 @@
namespace dynamicgraph
{
template <typename T>
- T&
+ void
RosImport::sendData (boost::shared_ptr<ros::Publisher> publisher,
- T& data, int time)
+ boost::shared_ptr<typename SotToRos<T>::signal_t> signal,
+ int time)
{
typename SotToRos<T>::ros_t result;
- converter (result, data);
+ converter (result, signal->access (time));
publisher->publish (result);
- return data;
}
template <typename T>
@@ -28,27 +28,29 @@ namespace dynamicgraph
typedef typename SotToRos<T>::sot_t sot_t;
typedef typename SotToRos<T>::ros_t ros_t;
typedef typename SotToRos<T>::signal_t signal_t;
- typedef typename SotToRos<T>::callback_t callback_t;
// Initialize the bindedSignal object.
bindedSignal_t bindedSignal;
// Initialize the publisher.
- bindedSignal.second =
+ boost::get<1> (bindedSignal) =
boost::make_shared<ros::Publisher> (nh_.advertise<ros_t>(topic, 1));
// Initialize the signal.
- boost::format signalName ("RosImport(%1%)::%2%");
- signalName % name % signal;
-
- callback_t signalCallback = boost::bind
- (&RosImport::sendData<sot_t>, this, bindedSignal.second, _1, _2);
-
- boost::shared_ptr<signal_t> signalPtr = boost::make_shared<signal_t>
- (signalCallback, sotNOSIGNAL, signalName.str ());
- signalPtr->setNeedUpdateFromAllChildren (true);
- bindedSignal.first = signalPtr;
- signalRegistration (*bindedSignal.first);
+ boost::shared_ptr<signal_t> signalPtr =
+ boost::make_shared<signal_t>
+ (MAKE_SIGNAL_STRING(name, true, SotToRos<T>::signalTypeName, signal));
+ boost::get<0> (bindedSignal) = signalPtr;
+ signalRegistration (*boost::get<0> (bindedSignal));
+
+ // Initialize the callback.
+ callback_t callback = boost::bind
+ (&RosImport::sendData<T>,
+ this,
+ boost::get<1> (bindedSignal),
+ signalPtr,
+ _1);
+ boost::get<2> (bindedSignal) = callback;
bindedSignal_[signal] = bindedSignal;
}
diff --git a/src/sot_to_ros.cpp b/src/sot_to_ros.cpp
new file mode 100644
index 0000000..89df1c0
--- /dev/null
+++ b/src/sot_to_ros.cpp
@@ -0,0 +1,14 @@
+#include "sot_to_ros.hh"
+
+namespace dynamicgraph
+{
+
+ const char* SotToRos<double>::signalTypeName = "Double";
+ const char* SotToRos<ml::Matrix>::signalTypeName = "Matrix";
+ const char* SotToRos<ml::Vector>::signalTypeName = "Vector";
+ const char* SotToRos<sot::MatrixHomogeneous>::signalTypeName = "MatrixHomo";
+ const char* SotToRos
+ <std::pair<sot::MatrixHomogeneous, ml::Vector> >::signalTypeName
+ = "MatrixHomo";
+
+} // end of namespace dynamicgraph.
diff --git a/src/sot_to_ros.hh b/src/sot_to_ros.hh
index c228799..bf5752f 100644
--- a/src/sot_to_ros.hh
+++ b/src/sot_to_ros.hh
@@ -3,6 +3,8 @@
# include <vector>
# include <utility>
+# include <boost/format.hpp>
+
# include <jrl/mal/boost.hh>
# include <std_msgs/Float64.h>
# include "dynamic_graph_bridge/Matrix.h"
@@ -15,6 +17,10 @@
# include <dynamic-graph/signal-ptr.h>
# include <sot/core/matrix-homogeneous.hh>
+# define MAKE_SIGNAL_STRING(NAME, IS_INPUT, OUTPUT_TYPE, SIGNAL_NAME) \
+ makeSignalString (typeid (*this).name (), NAME, \
+ IS_INPUT, OUTPUT_TYPE, SIGNAL_NAME)
+
namespace dynamicgraph
{
namespace ml = maal::boost;
@@ -34,9 +40,11 @@ namespace dynamicgraph
typedef double sot_t;
typedef std_msgs::Float64 ros_t;
typedef std_msgs::Float64ConstPtr ros_const_ptr_t;
- typedef dynamicgraph::SignalTimeDependent<sot_t, int> signal_t;
+ typedef dynamicgraph::Signal<sot_t, int> signal_t;
typedef dynamicgraph::SignalPtr<sot_t, int> signalIn_t;
typedef boost::function<sot_t& (sot_t&, int)> callback_t;
+
+ static const char* signalTypeName;
};
template <>
@@ -48,6 +56,8 @@ namespace dynamicgraph
typedef dynamicgraph::SignalTimeDependent<sot_t, int> signal_t;
typedef dynamicgraph::SignalPtr<sot_t, int> signalIn_t;
typedef boost::function<sot_t& (sot_t&, int)> callback_t;
+
+ static const char* signalTypeName;
};
template <>
@@ -59,6 +69,8 @@ namespace dynamicgraph
typedef dynamicgraph::SignalTimeDependent<sot_t, int> signal_t;
typedef dynamicgraph::SignalPtr<sot_t, int> signalIn_t;
typedef boost::function<sot_t& (sot_t&, int)> callback_t;
+
+ static const char* signalTypeName;
};
template <>
@@ -70,6 +82,8 @@ namespace dynamicgraph
typedef dynamicgraph::SignalTimeDependent<sot_t, int> signal_t;
typedef dynamicgraph::SignalPtr<sot_t, int> signalIn_t;
typedef boost::function<sot_t& (sot_t&, int)> callback_t;
+
+ static const char* signalTypeName;
};
// Stamped transformation
@@ -82,8 +96,25 @@ namespace dynamicgraph
typedef dynamicgraph::SignalTimeDependent<sot_t, int> signal_t;
typedef dynamicgraph::SignalPtr<sot_t, int> signalIn_t;
typedef boost::function<sot_t& (sot_t&, int)> callback_t;
+
+ static const char* signalTypeName;
};
+ inline std::string
+ makeSignalString (const std::string& className,
+ const std::string& instanceName,
+ bool isInputSignal,
+ const std::string& signalType,
+ const std::string& signalName)
+ {
+ boost::format fmt ("%s(%s)::%s(%s)::%s");
+ fmt % className
+ % instanceName
+ % (isInputSignal ? "input" : "output")
+ % signalType
+ % signalName;
+ return fmt.str ();
+ }
} // end of namespace dynamicgraph.
#endif //! DYNAMIC_GRAPH_ROS_SOT_TO_ROS_HH
diff --git a/tests/test_import.py b/tests/test_import.py
new file mode 100755
index 0000000..b55a818
--- /dev/null
+++ b/tests/test_import.py
@@ -0,0 +1,15 @@
+#!/usr/bin/python
+
+from dynamic_graph.ros import RosImport
+
+ri = RosImport('rosimport')
+
+ri.add('double', 'doubleS', 'doubleT')
+ri.add('vector', 'vectorS', 'vectorT')
+ri.add('matrix', 'matrixS', 'matrixT')
+
+ri.doubleS.value = 42.
+ri.vectorS.value = (42., 42.,)
+ri.matrixS.value = ((42., 42.,),(42., 42.,),)
+
+ri.trigger.recompute(ri.trigger.time + 1)
--
GitLab