diff --git a/manifest.xml b/manifest.xml
index af4e7a6f4d2b11e735e00990b3f4cf018c20fedf..13c1bfae6eece288b6dcc2cf2882a6ec70bc878c 100644
--- a/manifest.xml
+++ b/manifest.xml
@@ -11,7 +11,4 @@
<depend package="std_msgs"/>
<depend package="roscpp"/>
-
- <depend package="jrl-mal"/>
- <depend package="dynamic-graph"/>
</package>
diff --git a/src/converter.hh b/src/converter.hh
index 66b23516eeab25e65c3d3366491017e26bdc5b55..dcb23ca018f66df69f20d1434ec2253fcdb68bfc 100644
--- a/src/converter.hh
+++ b/src/converter.hh
@@ -40,6 +40,45 @@ namespace dynamicgraph
dst.data[i] = src.elementAt (i);
}
+ template <>
+ inline void converter
+ (SotToRos<ml::Vector>::sot_t& dst,
+ const SotToRos<ml::Vector>::ros_t& src)
+ {
+ dst.resize (src.data.size ());
+ for (unsigned i = 0; i < src.data.size (); ++i)
+ dst.elementAt (i) = src.data[i];
+ }
+
+ template <>
+ inline void converter
+ (SotToRos<ml::Matrix>::sot_t& dst,
+ const SotToRos<ml::Matrix>::ros_t& src)
+ {
+ dst.resize (src.width, src.data.size () / src.width);
+ for (unsigned i = 0; i < src.data.size (); ++i)
+ dst.elementAt (i) = src.data[i];
+ }
+
+ template <>
+ inline void converter
+ (SotToRos<ml::Vector>::sot_t& dst,
+ const boost::shared_ptr<SotToRos<ml::Vector>::ros_t const>& src)
+ {
+ dst.resize (src->data.size ());
+ for (unsigned i = 0; i < src->data.size (); ++i)
+ dst.elementAt (i) = src->data[i];
+ }
+
+ template <>
+ inline void converter
+ (SotToRos<ml::Matrix>::sot_t& dst,
+ const boost::shared_ptr<SotToRos<ml::Matrix>::ros_t const>& src)
+ {
+ dst.resize (src->width, src->data.size () / src->width);
+ for (unsigned i = 0; i < src->data.size (); ++i)
+ dst.elementAt (i) = src->data[i];
+ }
} // end of namespace dynamicgraph.
diff --git a/src/ros_export.cpp b/src/ros_export.cpp
index aae723c891800f4c189a98151b18a93f73e64973..991a2f3aed45a06e4e221adcb7566e7d96578ee6 100644
--- a/src/ros_export.cpp
+++ b/src/ros_export.cpp
@@ -1,3 +1,4 @@
+#include <boost/assign.hpp>
#include <boost/bind.hpp>
#include <boost/format.hpp>
#include <boost/function.hpp>
@@ -14,6 +15,94 @@ namespace dynamicgraph
{
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(RosExport, "RosExport");
+ namespace command
+ {
+ namespace rosExport
+ {
+ Clear::Clear
+ (RosExport& entity, const std::string& docstring)
+ : Command
+ (entity,
+ std::vector<Value::Type> (),
+ docstring)
+ {}
+
+ Value Clear::doExecute ()
+ {
+ RosExport& entity =
+ static_cast<RosExport&> (owner ());
+
+ entity.clear ();
+ return Value ();
+ }
+
+ List::List
+ (RosExport& entity, const std::string& docstring)
+ : Command
+ (entity,
+ std::vector<Value::Type> (),
+ docstring)
+ {}
+
+ Value List::doExecute ()
+ {
+ RosExport& entity =
+ static_cast<RosExport&> (owner ());
+ entity.list ();
+ return Value ();
+ }
+
+ Add::Add
+ (RosExport& entity, const std::string& docstring)
+ : Command
+ (entity,
+ boost::assign::list_of
+ (Value::STRING) (Value::STRING) (Value::STRING),
+ docstring)
+ {}
+
+ Value Add::doExecute ()
+ {
+ RosExport& entity =
+ static_cast<RosExport&> (owner ());
+ std::vector<Value> values = getParameterValues ();
+
+ const std::string& type = values[0].value ();
+ const std::string& signal = values[1].value ();
+ const std::string& topic = values[2].value ();
+
+ if (type == "double")
+ entity.add<double> (signal, topic);
+ else if (type == "matrix")
+ entity.add<ml::Matrix> (signal, topic);
+ else if (type == "vector")
+ entity.add<ml::Vector> (signal, topic);
+ else
+ throw std::runtime_error("bad type");
+ return Value ();
+ }
+
+ Rm::Rm
+ (RosExport& entity, const std::string& docstring)
+ : Command
+ (entity,
+ boost::assign::list_of (Value::STRING),
+ docstring)
+ {}
+
+ Value Rm::doExecute ()
+ {
+ RosExport& entity =
+ static_cast<RosExport&> (owner ());
+ std::vector<Value> values = getParameterValues ();
+ const std::string& signal = values[0].value ();
+ entity.rm (signal);
+ return Value ();
+ }
+ } // end of errorEstimator.
+ } // end of namespace command.
+
+
const char* rosInit()
{
int argc = 1;
@@ -27,14 +116,29 @@ namespace dynamicgraph
RosExport::RosExport (const std::string& n)
: dynamicgraph::Entity(n),
nh_ (rosInit ()),
- bindedSignal_ ()
+ bindedSignal_ (),
+ spinner_ (1)
{
- ros::AsyncSpinner spinner (1);
- spinner.start ();
+ spinner_.start ();
+
+ std::string docstring;
+ addCommand ("add",
+ new command::rosExport::Add
+ (*this, docstring));
+ addCommand ("rm",
+ new command::rosExport::Rm
+ (*this, docstring));
+ addCommand ("clear",
+ new command::rosExport::Clear
+ (*this, docstring));
+ addCommand ("list",
+ new command::rosExport::List
+ (*this, docstring));
}
RosExport::~RosExport ()
{
+ spinner_.stop ();
ros::waitForShutdown();
}
@@ -43,55 +147,6 @@ namespace dynamicgraph
os << CLASS_NAME << std::endl;
}
- void RosExport::commandLine (const std::string& cmdLine,
- std::istringstream& cmdArgs,
- std::ostream& os)
- {
- std::string type;
- std::string signal;
- std::string topic;
-
- if (cmdLine == "help")
- {
- os << "RosExport: "<< std::endl
- << " - add <TYPE> <SIGNAL> <TOPIC>" << std::endl
- << " - rm <SIGNAL>" << std::endl
- << " - clear" << std::endl
- << " - list" << std::endl;
- Entity::commandLine (cmdLine, cmdArgs, os);
- }
- else if (cmdLine == "add")
- {
- 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";
- }
- else if (cmdLine == "rm")
- {
- cmdArgs >> signal;
- rm (signal);
- }
- else if (cmdLine == "clear")
- clear ();
- else if (cmdLine == "list")
- list ();
- else
- Entity::commandLine (cmdLine, cmdArgs, os);
- }
-
- const std::string& RosExport::getClassName ()
- {
- return CLASS_NAME;
- }
-
void RosExport::rm (const std::string& signal)
{
bindedSignal_.erase (signal);
diff --git a/src/ros_export.hh b/src/ros_export.hh
index 522326e5f55515602e0498c625c7f6b65983faf8..f8f35c5ee8e5e0deaf1c3535a391d46752f4f1bd 100644
--- a/src/ros_export.hh
+++ b/src/ros_export.hh
@@ -7,6 +7,8 @@
# include <dynamic-graph/entity.h>
# include <dynamic-graph/signal-time-dependent.h>
+# include <dynamic-graph/signal-ptr.h>
+# include <dynamic-graph/command.h>
# include <ros/ros.h>
@@ -15,28 +17,48 @@
namespace dynamicgraph
{
+ class RosExport;
+
+ namespace command
+ {
+ namespace rosExport
+ {
+ using ::dynamicgraph::command::Command;
+ using ::dynamicgraph::command::Value;
+
+# define ROS_EXPORT_MAKE_COMMAND(CMD) \
+ class CMD : public Command \
+ { \
+ public: \
+ CMD (RosExport& entity, \
+ const std::string& docstring); \
+ virtual Value doExecute (); \
+ }
+
+ ROS_EXPORT_MAKE_COMMAND(Add);
+ ROS_EXPORT_MAKE_COMMAND(Clear);
+ ROS_EXPORT_MAKE_COMMAND(List);
+ ROS_EXPORT_MAKE_COMMAND(Rm);
+
+#undef ROS_EXPORT_MAKE_COMMAND
+
+ } // end of namespace errorEstimator.
+ } // end of namespace command.
+
+
class RosExport : public dynamicgraph::Entity
{
+ DYNAMIC_GRAPH_ENTITY_DECL();
public:
typedef std::pair<boost::shared_ptr<dynamicgraph::SignalBase<int> >,
boost::shared_ptr<ros::Subscriber> >
bindedSignal_t;
- static const std::string CLASS_NAME;
-
RosExport (const std::string& n);
virtual ~RosExport ();
void display (std::ostream& os) const;
- virtual void
- commandLine (const std::string& cmdLine,
- std::istringstream& cmdArgs,
- std::ostream& os);
-
-
- virtual const std::string& getClassName ();
- private:
void add (const std::string& signal, const std::string& topic);
void rm (const std::string& signal);
void list ();
@@ -45,18 +67,15 @@ namespace dynamicgraph
template <typename T>
void add (const std::string& signal, const std::string& topic);
- // template <typename R, typename S>
- // void callback
- // (boost::shared_ptr<dynamicgraph::SignalTimeDependent<S, int> > signal,
- // const R& data);
-
+ private:
template <typename R, typename S>
void callback
- (boost::shared_ptr<dynamicgraph::SignalTimeDependent<S, int> > signal,
+ (boost::shared_ptr<dynamicgraph::SignalPtr<S, int> > signal,
const R& data);
ros::NodeHandle nh_;
std::map<std::string, bindedSignal_t> bindedSignal_;
+ ros::AsyncSpinner spinner_;
};
} // end of namespace dynamicgraph.
diff --git a/src/ros_export.hxx b/src/ros_export.hxx
index 3423835fbb6983ada1f6610fcb45091826e10dd7..8c78c67171efea0d32aca91cab2f8131a380c162 100644
--- a/src/ros_export.hxx
+++ b/src/ros_export.hxx
@@ -7,33 +7,22 @@
# include "dynamic_graph_bridge/Matrix.h"
# include "dynamic_graph_bridge/Vector.h"
+#include <iostream>//FIXME:
namespace ml = maal::boost;
namespace dynamicgraph
{
- // 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::SignalTimeDependent<S, int> > signal,
+ (boost::shared_ptr<dynamicgraph::SignalPtr<S, int> > signal,
const R& data)
{
typedef S sot_t;
sot_t value;
converter (value, data);
- (*signal) (value);
+ signal->setConstant (value);
}
@@ -42,7 +31,7 @@ namespace dynamicgraph
{
typedef typename SotToRos<T>::sot_t sot_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>::signalIn_t signal_t;
// Initialize the bindedSignal object.
bindedSignal_t bindedSignal;
@@ -51,8 +40,8 @@ namespace dynamicgraph
boost::format signalName ("RosExport(%1%)::%2%");
signalName % name % signal;
- boost::shared_ptr<signal_t> signal_ =
- boost::make_shared<signal_t>(0, signalName.str ());
+ boost::shared_ptr<signal_t> signal_ (new signal_t (0, signalName.str ()));
+ signal_->setConstant (sot_t ());
bindedSignal.first = signal_;
signalRegistration (*bindedSignal.first);
diff --git a/src/ros_import.cpp b/src/ros_import.cpp
index c9c1ce27ebe006f048ebadcf4fa8def552e3290b..63b820c9cca33f25898afd0a68b445bc1bae8247 100644
--- a/src/ros_import.cpp
+++ b/src/ros_import.cpp
@@ -1,3 +1,6 @@
+#include <stdexcept>
+
+#include <boost/assign.hpp>
#include <boost/bind.hpp>
#include <boost/format.hpp>
#include <boost/function.hpp>
@@ -7,6 +10,7 @@
#include <std_msgs/Float64.h>
#include <dynamic-graph/factory.h>
+#include <dynamic-graph/command.h>
#include "ros_import.hh"
@@ -14,6 +18,94 @@ namespace dynamicgraph
{
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(RosImport, "RosImport");
+ namespace command
+ {
+ namespace rosImport
+ {
+ Clear::Clear
+ (RosImport& entity, const std::string& docstring)
+ : Command
+ (entity,
+ std::vector<Value::Type> (),
+ docstring)
+ {}
+
+ Value Clear::doExecute ()
+ {
+ RosImport& entity =
+ static_cast<RosImport&> (owner ());
+
+ entity.clear ();
+ return Value ();
+ }
+
+ List::List
+ (RosImport& entity, const std::string& docstring)
+ : Command
+ (entity,
+ std::vector<Value::Type> (),
+ docstring)
+ {}
+
+ Value List::doExecute ()
+ {
+ RosImport& entity =
+ static_cast<RosImport&> (owner ());
+ entity.list ();
+ return Value ();
+ }
+
+ Add::Add
+ (RosImport& entity, const std::string& docstring)
+ : Command
+ (entity,
+ boost::assign::list_of
+ (Value::STRING) (Value::STRING) (Value::STRING),
+ docstring)
+ {}
+
+ Value Add::doExecute ()
+ {
+ RosImport& entity =
+ static_cast<RosImport&> (owner ());
+ std::vector<Value> values = getParameterValues ();
+
+ const std::string& type = values[0].value ();
+ const std::string& signal = values[1].value ();
+ const std::string& topic = values[2].value ();
+
+ if (type == "double")
+ entity.add<double> (signal, topic);
+ else if (type == "matrix")
+ entity.add<ml::Matrix> (signal, topic);
+ else if (type == "vector")
+ entity.add<ml::Vector> (signal, topic);
+ else
+ throw std::runtime_error("bad type");
+ return Value ();
+ }
+
+ Rm::Rm
+ (RosImport& entity, const std::string& docstring)
+ : Command
+ (entity,
+ boost::assign::list_of (Value::STRING),
+ docstring)
+ {}
+
+ Value Rm::doExecute ()
+ {
+ RosImport& entity =
+ static_cast<RosImport&> (owner ());
+ std::vector<Value> values = getParameterValues ();
+ const std::string& signal = values[0].value ();
+ entity.rm (signal);
+ return Value ();
+ }
+ } // end of errorEstimator.
+ } // end of namespace command.
+
+
const char* rosInit()
{
int argc = 1;
@@ -29,6 +121,19 @@ namespace dynamicgraph
nh_ (rosInit ()),
bindedSignal_ ()
{
+ std::string docstring;
+ addCommand ("add",
+ new command::rosImport::Add
+ (*this, docstring));
+ addCommand ("rm",
+ new command::rosImport::Rm
+ (*this, docstring));
+ addCommand ("clear",
+ new command::rosImport::Clear
+ (*this, docstring));
+ addCommand ("list",
+ new command::rosImport::List
+ (*this, docstring));
}
RosImport::~RosImport ()
@@ -40,53 +145,6 @@ namespace dynamicgraph
os << CLASS_NAME << std::endl;
}
- void RosImport::commandLine (const std::string& cmdLine,
- std::istringstream& cmdArgs,
- std::ostream& os)
- {
- std::string type;
- std::string signal;
- std::string topic;
-
- if (cmdLine == "help")
- {
- os << "RosImport: "<< std::endl
- << " - add <TYPE> <SIGNAL> <TOPIC>" << std::endl
- << " - rm <SIGNAL>" << std::endl
- << " - clear" << std::endl
- << " - list" << std::endl;
- Entity::commandLine (cmdLine, cmdArgs, os);
- }
- else if (cmdLine == "add")
- {
- 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";
- }
- else if (cmdLine == "rm")
- {
- cmdArgs >> signal;
- rm (signal);
- }
- else if (cmdLine == "clear")
- clear ();
- else if (cmdLine == "list")
- list ();
- else
- Entity::commandLine (cmdLine, cmdArgs, os);
- }
-
- const std::string& RosImport::getClassName ()
- {
- return CLASS_NAME;
- }
-
void RosImport::rm (const std::string& signal)
{
bindedSignal_.erase (signal);
diff --git a/src/ros_import.hh b/src/ros_import.hh
index 18b7b4f26f28695c0b758eae5516651c721fe4a0..6f2a17a8d68ec3e6a0abdbeec848b9dc4c4ddedf 100644
--- a/src/ros_import.hh
+++ b/src/ros_import.hh
@@ -7,6 +7,7 @@
# include <dynamic-graph/entity.h>
# include <dynamic-graph/signal-time-dependent.h>
+# include <dynamic-graph/command.h>
# include <ros/ros.h>
@@ -15,28 +16,48 @@
namespace dynamicgraph
{
+ class RosImport;
+
+ namespace command
+ {
+ namespace rosImport
+ {
+ using ::dynamicgraph::command::Command;
+ using ::dynamicgraph::command::Value;
+
+# define ROS_IMPORT_MAKE_COMMAND(CMD) \
+ class CMD : public Command \
+ { \
+ public: \
+ CMD (RosImport& entity, \
+ const std::string& docstring); \
+ virtual Value doExecute (); \
+ }
+
+ ROS_IMPORT_MAKE_COMMAND(Add);
+ ROS_IMPORT_MAKE_COMMAND(Clear);
+ ROS_IMPORT_MAKE_COMMAND(List);
+ ROS_IMPORT_MAKE_COMMAND(Rm);
+
+#undef ROS_IMPORT_MAKE_COMMAND
+
+ } // end of namespace errorEstimator.
+ } // end of namespace command.
+
+
class RosImport : public dynamicgraph::Entity
{
+ DYNAMIC_GRAPH_ENTITY_DECL();
public:
typedef std::pair<boost::shared_ptr<dynamicgraph::SignalBase<int> >,
boost::shared_ptr<ros::Publisher> >
bindedSignal_t;
- static const std::string CLASS_NAME;
-
RosImport (const std::string& n);
virtual ~RosImport ();
void display (std::ostream& os) const;
- virtual void
- commandLine (const std::string& cmdLine,
- std::istringstream& cmdArgs,
- std::ostream& os);
-
- virtual const std::string& getClassName ();
-
- private:
void add (const std::string& signal, const std::string& topic);
void rm (const std::string& signal);
void list ();
@@ -49,6 +70,7 @@ namespace dynamicgraph
template <typename T>
void add (const std::string& signal, const std::string& topic);
+ private:
ros::NodeHandle nh_;
std::map<std::string, bindedSignal_t> bindedSignal_;
};
diff --git a/src/ros_import.hxx b/src/ros_import.hxx
index 57f7ccc9fb8456d858e12088daeaafecdbf24f3a..e628d3d5ac4c43c0c16083f0453e2991349723d6 100644
--- a/src/ros_import.hxx
+++ b/src/ros_import.hxx
@@ -7,6 +7,8 @@
# include "sot_to_ros.hh"
+# include <iostream>
+
namespace dynamicgraph
{
template <typename T>
@@ -42,8 +44,10 @@ namespace dynamicgraph
callback_t signalCallback = boost::bind
(&RosImport::sendData<sot_t>, this, bindedSignal.second, _1, _2);
- bindedSignal.first = boost::make_shared<signal_t>
+ boost::shared_ptr<signal_t> signalPtr = boost::make_shared<signal_t>
(signalCallback, sotNOSIGNAL, signalName.str ());
+ signalPtr->setNeedUpdateFromAllChildren (true);
+ bindedSignal.first = signalPtr;
signalRegistration (*bindedSignal.first);
bindedSignal_[signal] = bindedSignal;
diff --git a/src/sot_to_ros.hh b/src/sot_to_ros.hh
index 3253aacb2e3f8cb3f51c9001cd76581b9cee8637..adeb4cc90b9ace1ab55ae0e3551fdb59c780bdfb 100644
--- a/src/sot_to_ros.hh
+++ b/src/sot_to_ros.hh
@@ -20,6 +20,7 @@ namespace dynamicgraph
typedef std_msgs::Float64 ros_t;
typedef std_msgs::Float64ConstPtr ros_const_ptr_t;
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;
};
@@ -30,6 +31,7 @@ namespace dynamicgraph
typedef dynamic_graph_bridge::Matrix ros_t;
typedef dynamic_graph_bridge::MatrixConstPtr ros_const_ptr_t;
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;
};
@@ -40,6 +42,7 @@ namespace dynamicgraph
typedef dynamic_graph_bridge::Vector ros_t;
typedef dynamic_graph_bridge::VectorConstPtr ros_const_ptr_t;
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;
};
} // end of namespace dynamicgraph.