1 |
|
|
/// |
2 |
|
|
/// Copyright 2012 CNRS |
3 |
|
|
/// |
4 |
|
|
/// Author: Florent Lamiraux |
5 |
|
|
|
6 |
|
|
#include "ros_time.hh" |
7 |
|
|
|
8 |
|
|
#include <dynamic-graph/factory.h> |
9 |
|
|
#include <dynamic-graph/signal-cast-helper.h> |
10 |
|
|
#include <dynamic-graph/signal-caster.h> |
11 |
|
|
|
12 |
|
|
#include "converter.hh" |
13 |
|
|
|
14 |
|
|
namespace dynamicgraph { |
15 |
|
|
|
16 |
|
|
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(RosTime, "RosTime"); |
17 |
|
|
|
18 |
|
|
using namespace boost::posix_time; |
19 |
|
|
|
20 |
|
|
const std::string RosTime::docstring_( |
21 |
|
|
"Export ROS time into dynamic-graph.\n" |
22 |
|
|
"\n" |
23 |
|
|
" Signal \"time\" provides time as given by ros::time as\n" |
24 |
|
|
" boost::posix_time::ptime type.\n"); |
25 |
|
|
|
26 |
|
|
RosTime::RosTime(const std::string& _name) |
27 |
|
|
: Entity(_name), |
28 |
|
|
now_("RosTime(" + _name + ")::output(boost::posix_time::ptime)::time") { |
29 |
|
|
signalRegistration(now_); |
30 |
|
|
now_.setConstant(rosTimeToPtime(ros::Time::now())); |
31 |
|
|
now_.setFunction(boost::bind(&RosTime::update, this, _1, _2)); |
32 |
|
|
} |
33 |
|
|
|
34 |
|
|
ptime& RosTime::update(ptime& time, const int&) { |
35 |
|
|
time = rosTimeToPtime(ros::Time::now()); |
36 |
|
|
return time; |
37 |
|
|
} |
38 |
|
|
|
39 |
|
|
std::string RosTime::getDocString() const { return docstring_; } |
40 |
|
|
} // namespace dynamicgraph |