From b3923effabf52c73361629364212231b5f14a001 Mon Sep 17 00:00:00 2001
From: Thomas Moulard <thomas.moulard@gmail.com>
Date: Sat, 27 Nov 2010 01:14:14 +0100
Subject: [PATCH] Fix implementation.
---
src/converter.hh | 18 +++++++++---------
src/ros_export.cpp | 4 +++-
src/ros_export.hh | 12 +++++++++---
src/ros_export.hxx | 39 ++++++++++++++++++++++++++++-----------
src/sot_to_ros.hh | 3 +++
5 files changed, 52 insertions(+), 24 deletions(-)
diff --git a/src/converter.hh b/src/converter.hh
index cd05e0c..66b2351 100644
--- a/src/converter.hh
+++ b/src/converter.hh
@@ -8,22 +8,22 @@ namespace dynamicgraph
void converter (D& dst, const S& src);
template <>
- void converter (SotToRos<double>::ros_t& dst,
- const SotToRos<double>::sot_t& src)
+ inline void converter (SotToRos<double>::ros_t& dst,
+ const SotToRos<double>::sot_t& src)
{
dst.data = src;
}
template <>
- void converter (SotToRos<double>::sot_t& dst,
- const SotToRos<double>::ros_t& src)
+ inline void converter (SotToRos<double>::sot_t& dst,
+ const SotToRos<double>::ros_const_ptr_t& src)
{
- dst = src.data;
+ dst = src->data;
}
template <>
- void converter (SotToRos<ml::Matrix>::ros_t& dst,
- const SotToRos<ml::Matrix>::sot_t& src)
+ inline void converter (SotToRos<ml::Matrix>::ros_t& dst,
+ const SotToRos<ml::Matrix>::sot_t& src)
{
dst.width = src.nbRows ();
dst.data.resize (src.nbCols () * src.nbRows ());
@@ -32,8 +32,8 @@ namespace dynamicgraph
}
template <>
- void converter (SotToRos<ml::Vector>::ros_t& dst,
- const SotToRos<ml::Vector>::sot_t& src)
+ inline void converter (SotToRos<ml::Vector>::ros_t& dst,
+ const SotToRos<ml::Vector>::sot_t& src)
{
dst.data.resize (src.size ());
for (unsigned i = 0; i < src.size (); ++i)
diff --git a/src/ros_export.cpp b/src/ros_export.cpp
index 12fded3..aae723c 100644
--- a/src/ros_export.cpp
+++ b/src/ros_export.cpp
@@ -64,11 +64,13 @@ namespace dynamicgraph
{
cmdArgs >> type >> signal >> topic;
if (type == "double")
- ;
+ add<double> (signal, topic);
else if (type == "matrix")
;
+ //add<ml::Matrix> (signal, topic);
else if (type == "vector")
;
+ //add<ml::Vector> (signal, topic);
else
throw "bad type";
}
diff --git a/src/ros_export.hh b/src/ros_export.hh
index 25cbfcb..522326e 100644
--- a/src/ros_export.hh
+++ b/src/ros_export.hh
@@ -45,9 +45,15 @@ namespace dynamicgraph
template <typename T>
void add (const std::string& signal, const std::string& topic);
- template <typename T>
- void callback (boost::shared_ptr<dynamicgraph::SignalBase<int> > signal,
- const T& message);
+ // template <typename R, typename S>
+ // void callback
+ // (boost::shared_ptr<dynamicgraph::SignalTimeDependent<S, int> > signal,
+ // const R& data);
+
+ template <typename R, typename S>
+ void callback
+ (boost::shared_ptr<dynamicgraph::SignalTimeDependent<S, int> > signal,
+ const R& data);
ros::NodeHandle nh_;
std::map<std::string, bindedSignal_t> bindedSignal_;
diff --git a/src/ros_export.hxx b/src/ros_export.hxx
index 08a75a4..236ead6 100644
--- a/src/ros_export.hxx
+++ b/src/ros_export.hxx
@@ -12,24 +12,37 @@ namespace ml = maal::boost;
namespace dynamicgraph
{
- template <typename T>
+ // template <typename R, typename S>
+ // void
+ // RosExport::callback
+ // (boost::shared_ptr<dynamicgraph::SignalTimeDependent<S, int> > signal,
+ // const R& data)
+ // {
+ // // typedef S sot_t;
+ // // sot_t value;
+ // // converter (value, data);
+ // // (*signal) (value);
+ // }
+
+ template <typename R, typename S>
void
- RosExport::callback (boost::shared_ptr<dynamicgraph::SignalBase<int> > signal,
- const T& data)
+ RosExport::callback
+ (boost::shared_ptr<dynamicgraph::SignalTimeDependent<S, int> > signal,
+ const R& data)
{
- typedef typename SotToRos<T>::sot_t sot_t;
+ typedef S sot_t;
sot_t value;
converter (value, data);
(*signal) (value);
}
+
template <typename T>
void RosExport::add (const std::string& signal, const std::string& topic)
{
typedef typename SotToRos<T>::sot_t sot_t;
- typedef typename SotToRos<T>::ros_t ros_t;
+ typedef typename SotToRos<T>::ros_const_ptr_t ros_const_ptr_t;
typedef typename SotToRos<T>::signal_t signal_t;
- typedef typename SotToRos<T>::callback_t callback_t;
// Initialize the bindedSignal object.
bindedSignal_t bindedSignal;
@@ -38,15 +51,19 @@ namespace dynamicgraph
boost::format signalName ("RosExport(%1%)::%2%");
signalName % name % signal;
- bindedSignal.first = boost::make_shared<signal_t>(0, signalName.str ());
+ boost::shared_ptr<signal_t> signal_ =
+ boost::make_shared<signal_t>(0, signalName.str ());
+ bindedSignal.first = signal_;
signalRegistration (*bindedSignal.first);
// Initialize the publisher.
+ typedef boost::function<void (const ros_const_ptr_t& data)> callback_t;
+ callback_t callback = boost::bind
+ (&RosExport::callback<ros_const_ptr_t, sot_t>,
+ this, signal_, _1);
+
bindedSignal.second =
- boost::make_shared<ros::Publisher>
- (nh_.subscribe
- (topic, 1, boost::bind (&RosExport::callback<T>,
- this, bindedSignal.first)));
+ boost::make_shared<ros::Subscriber> (nh_.subscribe (topic, 1, callback));
bindedSignal_[signal] = bindedSignal;
}
diff --git a/src/sot_to_ros.hh b/src/sot_to_ros.hh
index 324266b..f5ac0cd 100644
--- a/src/sot_to_ros.hh
+++ b/src/sot_to_ros.hh
@@ -18,6 +18,7 @@ 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 boost::function<sot_t& (sot_t&, int)> callback_t;
};
@@ -27,6 +28,7 @@ namespace dynamicgraph
{
typedef ml::Matrix sot_t;
typedef dynamic_graph::Matrix ros_t;
+ typedef dynamic_graph::MatrixConstPtr ros_const_ptr_t;
typedef dynamicgraph::SignalTimeDependent<sot_t, int> signal_t;
typedef boost::function<sot_t& (sot_t&, int)> callback_t;
};
@@ -36,6 +38,7 @@ namespace dynamicgraph
{
typedef ml::Vector sot_t;
typedef dynamic_graph::Vector ros_t;
+ typedef dynamic_graph::VectorConstPtr ros_const_ptr_t;
typedef dynamicgraph::SignalTimeDependent<sot_t, int> signal_t;
typedef boost::function<sot_t& (sot_t&, int)> callback_t;
};
--
GitLab