diff --git a/CMakeLists.txt b/CMakeLists.txt index a6b22056022cd9cfe79f832fcbe94a1bded54a00..0ac7264404467ea48fd96f2b0f1740b6420863c8 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -71,7 +71,7 @@ link_directories(${DYNAMIC_GRAPH_LIBRARY_DIRS}) compile_plugin(ros_import) compile_plugin(ros_export) -compile_plugin(ros_export) +compile_plugin(ros_time) compile_plugin(ros_joint_state) target_link_libraries(ros_joint_state "${DYNAMIC_GRAPH_PLUGINDIR}/dynamic.so") diff --git a/src/converter.hh b/src/converter.hh index 2e116615d547b7157f14c4a66bccfb9c3c3b182c..4cc8e56bb48d03cb67c8273e591e4d03ab4f7550 100644 --- a/src/converter.hh +++ b/src/converter.hh @@ -4,6 +4,8 @@ # include "sot_to_ros.hh" # include <boost/static_assert.hpp> +# include <boost/date_time/date.hpp> +# include <boost/date_time/posix_time/posix_time.hpp> # include <ros/time.h> # include <std_msgs/Header.h> @@ -270,6 +272,30 @@ namespace dynamicgraph // This will always fail if instantiated. BOOST_STATIC_ASSERT (sizeof (U) == 0); } + + typedef boost::posix_time::ptime ptime; + typedef boost::posix_time::seconds seconds; + typedef boost::posix_time::microseconds microseconds; + typedef boost::posix_time::time_duration time_duration; + typedef boost::gregorian::date date; + + boost::posix_time::ptime rosTimeToPtime (const ros::Time& rosTime) + { + ptime time (date(1970,1,1), seconds (rosTime.sec) + + microseconds (rosTime.nsec/1000)); + return time; + } + + ros::Time pTimeToRostime (const boost::posix_time::ptime& time) + { + static ptime timeStart(date(1970,1,1)); + time_duration diff = time - timeStart; + + uint32_t sec = diff.ticks ()/time_duration::rep_type::res_adjust (); + uint32_t nsec = diff.fractional_seconds(); + + return ros::Time (sec, nsec); + } } // end of namespace dynamicgraph. #endif //! DYNAMIC_GRAPH_ROS_CONVERTER_HH diff --git a/src/dynamic_graph/ros/ros.py b/src/dynamic_graph/ros/ros.py index edfceaddc676194dea217aa94c425407e7a83eb0..043db71383fff50a9153e0b2c14dc23fad985278 100644 --- a/src/dynamic_graph/ros/ros.py +++ b/src/dynamic_graph/ros/ros.py @@ -1,6 +1,7 @@ from ros_import import RosImport from ros_export import RosExport from ros_joint_state import RosJointState +from ros_time import RosTime from dynamic_graph import plug @@ -16,6 +17,7 @@ class Ros(object): self.rosExport = RosExport('rosExport{0}'.format(suffix)) self.rosJointState = RosJointState('rosJointState{0}'.format(suffix)) self.rosJointState.retrieveJointNames(self.robot.dynamic.name) + self.rosTime = RosTime ('rosTime{0}'.format(suffix)) plug(self.robot.device.state, self.rosJointState.state) self.robot.device.after.addSignal('{0}.trigger'.format(self.rosImport.name)) diff --git a/src/ros_export.hh b/src/ros_export.hh index 52bb6213efa7e5562ebb5303bc4e3fe0e99ed48b..e0573b1b44750131163833d5c019645a6eeb65ad 100644 --- a/src/ros_export.hh +++ b/src/ros_export.hh @@ -57,6 +57,7 @@ namespace dynamicgraph class RosExport : public dynamicgraph::Entity { DYNAMIC_GRAPH_ENTITY_DECL(); + typedef boost::posix_time::ptime ptime; public: typedef std::pair<boost::shared_ptr<dynamicgraph::SignalBase<int> >, boost::shared_ptr<ros::Subscriber> > @@ -93,7 +94,7 @@ namespace dynamicgraph template <typename R> void callbackTimestamp - (boost::shared_ptr<dynamicgraph::SignalPtr<ml::Vector, int> > signal, + (boost::shared_ptr<dynamicgraph::SignalPtr<ptime, int> > signal, const R& data); template <typename T> diff --git a/src/ros_export.hxx b/src/ros_export.hxx index 0293e67d432b3b3bce3cc802dd3b32a36221c787..82ad056691aa4e302ce92cd3736d2122383313d0 100644 --- a/src/ros_export.hxx +++ b/src/ros_export.hxx @@ -2,10 +2,14 @@ # define DYNAMIC_GRAPH_ROS_EXPORT_HXX # include <vector> # include <boost/bind.hpp> +# include <boost/date_time/posix_time/posix_time.hpp> +# include <dynamic-graph/signal-caster.h> +# include <dynamic-graph/signal-cast-helper.h> # include <jrl/mal/boost.hh> # include <std_msgs/Float64.h> # include "dynamic_graph_bridge/Matrix.h" # include "dynamic_graph_bridge/Vector.h" +# include "ros_time.hh" namespace ml = ::maal::boost; @@ -26,13 +30,11 @@ namespace dynamicgraph template <typename R> void RosExport::callbackTimestamp - (boost::shared_ptr<dynamicgraph::SignalPtr<ml::Vector, int> > signal, + (boost::shared_ptr<dynamicgraph::SignalPtr<ptime, int> > signal, const R& data) { - ml::Vector time (2); - time (0) = data->header.stamp.sec; - // Convert nanoseconds into microseconds (i.e. timeval structure). - time (1) = data->header.stamp.nsec / 1000.; + ptime time = + rosTimeToPtime (data->header.stamp); signal->setConstant(time); } @@ -116,7 +118,7 @@ namespace dynamicgraph // Timestamp. - typedef dynamicgraph::SignalPtr<ml::Vector, int> + typedef dynamicgraph::SignalPtr<RosExport::ptime, int> signalTimestamp_t; std::string signalTimestamp = (boost::format ("%1%%2%") % signal % "Timestamp").str (); @@ -131,8 +133,7 @@ namespace dynamicgraph boost::shared_ptr<signalTimestamp_t> signalTimestamp_ (new signalTimestamp_t (0, signalNameTimestamp.str ())); - ml::Vector zero (2); - zero.setZero (); + RosExport::ptime zero (rosTimeToPtime (ros::Time (0, 0))); signalTimestamp_->setConstant (zero); bindedSignalTimestamp.first = signalTimestamp_; rosExport.signalRegistration (*bindedSignalTimestamp.first); diff --git a/src/ros_time.cpp b/src/ros_time.cpp new file mode 100644 index 0000000000000000000000000000000000000000..81fdd3e0d4466f6827844dc76a71aa1dcb73c526 --- /dev/null +++ b/src/ros_time.cpp @@ -0,0 +1,35 @@ +/// +/// Copyright 2012 CNRS +/// +/// Author: Florent Lamiraux + +#include <dynamic-graph/factory.h> +#include <dynamic-graph/signal-caster.h> +#include <dynamic-graph/signal-cast-helper.h> + +#include "ros_time.hh" +#include "converter.hh" + +namespace dynamicgraph { + + DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(RosTime, "RosTime"); + + using namespace boost::posix_time; + + RosTime::RosTime (const std::string& name) : + Entity (name), + now_ ("RosTime("+name+")::output(boost::posix_time::ptime)::time") + { + signalRegistration (now_); + now_.setConstant (rosTimeToPtime (ros::Time::now())); + now_.setFunction (boost::bind (&RosTime::update, this, _1, _2)); + } + + ptime& + RosTime::update (ptime& time, const int& t) + { + time = rosTimeToPtime (ros::Time::now ()); + return time; + } + +} // namespace dynamicgraph diff --git a/src/ros_time.hh b/src/ros_time.hh new file mode 100644 index 0000000000000000000000000000000000000000..b715209f9edf8d2859e55c272da5e05a597afa45 --- /dev/null +++ b/src/ros_time.hh @@ -0,0 +1,29 @@ +/// +/// Copyright 2012 CNRS +/// +/// Author: Florent Lamiraux + +#ifndef DYNAMIC_GRAPH_ROS_TIME_HH +# define DYNAMIC_GRAPH_ROS_TIME_HH + +# include <ros/time.h> +# include <boost/date_time/posix_time/posix_time_types.hpp> +# include <dynamic-graph/signal.h> +# include <dynamic-graph/entity.h> + +namespace dynamicgraph { + + class RosTime : public dynamicgraph::Entity + { + DYNAMIC_GRAPH_ENTITY_DECL (); + public: + Signal <boost::posix_time::ptime, int> now_; + RosTime (const std::string& name); + protected: + boost::posix_time::ptime& + update (boost::posix_time::ptime& time, const int& t); + }; // class RosTime + +} // namespace dynamicgraph + +#endif // DYNAMIC_GRAPH_ROS_TIME_HH