diff --git a/CMakeLists.txt b/CMakeLists.txt index d8b177fe3b956a3c90d69b746e34ac51299a098e..b9bf4edaeb97c5a9dd7bb8e725beda340243c36f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -34,13 +34,15 @@ rosbuild_genmsg() pkg_check_modules(JRL_MAL REQUIRED jrl-mal) pkg_check_modules(DYNAMIC_GRAPH REQUIRED dynamic-graph) -rosbuild_add_library(ros_import src/ros_import.cpp) +rosbuild_add_library(ros_import + 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}) rosbuild_add_link_flags(ros_import ${JRL_MAL_LDFLAGS} ${DYNAMIC_GRAPH_LDFLAGS}) target_link_libraries(ros_import ${JRL_MAL_LIBRARIES} ${DYNAMIC_GRAPH_LIBRARIES}) -rosbuild_add_library(ros_export src/ros_export.cpp) +rosbuild_add_library(ros_export + 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}) rosbuild_add_link_flags(ros_export ${JRL_MAL_LDFLAGS} ${DYNAMIC_GRAPH_LDFLAGS}) target_link_libraries(ros_export diff --git a/src/converter.hh b/src/converter.hh new file mode 100644 index 0000000000000000000000000000000000000000..cd05e0c3d3818832a525b0ee1c6d78312cc7dd21 --- /dev/null +++ b/src/converter.hh @@ -0,0 +1,46 @@ +#ifndef DYNAMIC_GRAPH_ROS_CONVERTER_HH +# define DYNAMIC_GRAPH_ROS_CONVERTER_HH +# include "sot_to_ros.hh" + +namespace dynamicgraph +{ + template <typename D, typename S> + void converter (D& dst, const S& src); + + template <> + 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) + { + dst = src.data; + } + + template <> + 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 ()); + for (unsigned i = 0; i < src.nbCols () * src.nbRows (); ++i) + dst.data[i] = src.elementAt (i); + } + + template <> + 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) + dst.data[i] = src.elementAt (i); + } + + +} // end of namespace dynamicgraph. + +#endif //! DYNAMIC_GRAPH_ROS_CONVERTER_HH diff --git a/src/ros_export.cpp b/src/ros_export.cpp index 74458fa418ba7821fa3756612abf841e188ed28f..12fded3779916b5993f9e006cdd7de1d8901bbac 100644 --- a/src/ros_export.cpp +++ b/src/ros_export.cpp @@ -29,10 +29,13 @@ namespace dynamicgraph nh_ (rosInit ()), bindedSignal_ () { + ros::AsyncSpinner spinner (1); + spinner.start (); } RosExport::~RosExport () { + ros::waitForShutdown(); } void RosExport::display (std::ostream& os) const diff --git a/src/ros_export.hh b/src/ros_export.hh index 12c915c3455b180ccefe05cf15b058fb97e58f6d..25cbfcb2ff0a6adac09b7045459e4c9fc5e132d0 100644 --- a/src/ros_export.hh +++ b/src/ros_export.hh @@ -10,13 +10,16 @@ # include <ros/ros.h> +# include "converter.hh" +# include "sot_to_ros.hh" + namespace dynamicgraph { class RosExport : public dynamicgraph::Entity { public: typedef std::pair<boost::shared_ptr<dynamicgraph::SignalBase<int> >, - boost::shared_ptr<ros::Publisher> > + boost::shared_ptr<ros::Subscriber> > bindedSignal_t; static const std::string CLASS_NAME; @@ -39,6 +42,13 @@ namespace dynamicgraph void list (); void clear (); + 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); + ros::NodeHandle nh_; std::map<std::string, bindedSignal_t> bindedSignal_; }; diff --git a/src/ros_export.hxx b/src/ros_export.hxx index 97154a0d41ef08f75d52b683c44c7fedf2f2a089..08a75a4ba3ac4dc1aeecab368bec34a4aff07f88 100644 --- a/src/ros_export.hxx +++ b/src/ros_export.hxx @@ -1,6 +1,7 @@ #ifndef DYNAMIC_GRAPH_ROS_EXPORT_HXX # define DYNAMIC_GRAPH_ROS_EXPORT_HXX # include <vector> +# include <boost/bind.hpp> # include <jrl/mal/boost.hh> # include <std_msgs/Float64.h> # include "dynamic_graph/Matrix.h" @@ -11,6 +12,44 @@ namespace ml = maal::boost; namespace dynamicgraph { + template <typename T> + void + RosExport::callback (boost::shared_ptr<dynamicgraph::SignalBase<int> > signal, + const T& data) + { + typedef typename SotToRos<T>::sot_t 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>::signal_t signal_t; + typedef typename SotToRos<T>::callback_t callback_t; + + // Initialize the bindedSignal object. + bindedSignal_t bindedSignal; + + // Initialize the signal. + boost::format signalName ("RosExport(%1%)::%2%"); + signalName % name % signal; + + bindedSignal.first = boost::make_shared<signal_t>(0, signalName.str ()); + signalRegistration (*bindedSignal.first); + + // Initialize the publisher. + bindedSignal.second = + boost::make_shared<ros::Publisher> + (nh_.subscribe + (topic, 1, boost::bind (&RosExport::callback<T>, + this, bindedSignal.first))); + + bindedSignal_[signal] = bindedSignal; + } } // end of namespace dynamicgraph. #endif //! DYNAMIC_GRAPH_ROS_EXPORT_HXX diff --git a/src/ros_import.hh b/src/ros_import.hh index 6d5352901569e7a92a67ce596d1a88aa452ecedf..18b7b4f26f28695c0b758eae5516651c721fe4a0 100644 --- a/src/ros_import.hh +++ b/src/ros_import.hh @@ -10,14 +10,11 @@ # include <ros/ros.h> +# include "converter.hh" +# include "sot_to_ros.hh" + namespace dynamicgraph { - template <typename SotType> - class SotToRos; - - template <typename D, typename S> - void converter (D& dst, const S& src); - class RosImport : public dynamicgraph::Entity { public: diff --git a/src/ros_import.hxx b/src/ros_import.hxx index f4f4656eaee0cebe97d083dbc2ce68d3339e735a..3b8b5024effa53f41ba13f8d932019715efd500d 100644 --- a/src/ros_import.hxx +++ b/src/ros_import.hxx @@ -1,71 +1,14 @@ #ifndef DYNAMIC_GRAPH_ROS_IMPORT_HXX # define DYNAMIC_GRAPH_ROS_IMPORT_HXX # include <vector> -# include <jrl/mal/boost.hh> # include <std_msgs/Float64.h> # include "dynamic_graph/Matrix.h" # include "dynamic_graph/Vector.h" - -namespace ml = maal::boost; +# include "sot_to_ros.hh" namespace dynamicgraph { - template <> - struct SotToRos<double> - { - typedef double sot_t; - typedef std_msgs::Float64 ros_t; - typedef dynamicgraph::SignalTimeDependent<sot_t, int> signal_t; - typedef boost::function<sot_t& (sot_t&, int)> callback_t; - }; - - template <> - struct SotToRos<ml::Matrix> - { - typedef ml::Matrix sot_t; - typedef dynamic_graph::Matrix ros_t; - typedef dynamicgraph::SignalTimeDependent<sot_t, int> signal_t; - typedef boost::function<sot_t& (sot_t&, int)> callback_t; - }; - - template <> - struct SotToRos<ml::Vector> - { - typedef ml::Vector sot_t; - typedef dynamic_graph::Vector ros_t; - typedef dynamicgraph::SignalTimeDependent<sot_t, int> signal_t; - typedef boost::function<sot_t& (sot_t&, int)> callback_t; - }; - - - template <> - void converter (SotToRos<double>::ros_t& dst, - const SotToRos<double>::sot_t& src) - { - dst.data = src; - } - - template <> - 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 ()); - for (unsigned i = 0; i < src.nbCols () * src.nbRows (); ++i) - dst.data[i] = src.elementAt (i); - } - - template <> - 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) - dst.data[i] = src.elementAt (i); - } - - template <typename T> T& RosImport::sendData (boost::shared_ptr<ros::Publisher> publisher, diff --git a/src/sot_to_ros.hh b/src/sot_to_ros.hh new file mode 100644 index 0000000000000000000000000000000000000000..324266bd40d627e86130e0100e40284708ce7cbd --- /dev/null +++ b/src/sot_to_ros.hh @@ -0,0 +1,44 @@ +#ifndef DYNAMIC_GRAPH_ROS_SOT_TO_ROS_HH +# define DYNAMIC_GRAPH_ROS_SOT_TO_ROS_HH +# include <vector> +# include <jrl/mal/boost.hh> +# include <std_msgs/Float64.h> +# include "dynamic_graph/Matrix.h" +# include "dynamic_graph/Vector.h" + +namespace dynamicgraph +{ + namespace ml = maal::boost; + + template <typename SotType> + class SotToRos; + + template <> + struct SotToRos<double> + { + typedef double sot_t; + typedef std_msgs::Float64 ros_t; + typedef dynamicgraph::SignalTimeDependent<sot_t, int> signal_t; + typedef boost::function<sot_t& (sot_t&, int)> callback_t; + }; + + template <> + struct SotToRos<ml::Matrix> + { + typedef ml::Matrix sot_t; + typedef dynamic_graph::Matrix ros_t; + typedef dynamicgraph::SignalTimeDependent<sot_t, int> signal_t; + typedef boost::function<sot_t& (sot_t&, int)> callback_t; + }; + + template <> + struct SotToRos<ml::Vector> + { + typedef ml::Vector sot_t; + typedef dynamic_graph::Vector ros_t; + typedef dynamicgraph::SignalTimeDependent<sot_t, int> signal_t; + typedef boost::function<sot_t& (sot_t&, int)> callback_t; + }; +} // end of namespace dynamicgraph. + +#endif //! DYNAMIC_GRAPH_ROS_SOT_TO_ROS_HH