1 |
|
|
#include "ros_publish.hh" |
2 |
|
|
|
3 |
|
|
#include <dynamic-graph/command.h> |
4 |
|
|
#include <dynamic-graph/factory.h> |
5 |
|
|
#include <dynamic-graph/linear-algebra.h> |
6 |
|
|
#include <ros/ros.h> |
7 |
|
|
#include <std_msgs/Float64.h> |
8 |
|
|
#include <std_msgs/UInt32.h> |
9 |
|
|
|
10 |
|
|
#include <boost/assign.hpp> |
11 |
|
|
#include <boost/bind.hpp> |
12 |
|
|
#include <boost/foreach.hpp> |
13 |
|
|
#include <boost/format.hpp> |
14 |
|
|
#include <boost/function.hpp> |
15 |
|
|
#include <boost/make_shared.hpp> |
16 |
|
|
#include <stdexcept> |
17 |
|
|
|
18 |
|
|
#include "dynamic_graph_bridge/ros_init.hh" |
19 |
|
|
|
20 |
|
|
#define ENABLE_RT_LOG |
21 |
|
|
#include <dynamic-graph/real-time-logger.h> |
22 |
|
|
|
23 |
|
|
namespace dynamicgraph { |
24 |
|
|
|
25 |
|
|
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(RosPublish, "RosPublish"); |
26 |
|
|
const double RosPublish::ROS_JOINT_STATE_PUBLISHER_RATE = 0.01; |
27 |
|
|
|
28 |
|
|
namespace command { |
29 |
|
|
namespace rosPublish { |
30 |
|
|
|
31 |
|
|
Add::Add(RosPublish& entity, const std::string& docstring) |
32 |
|
|
: Command( |
33 |
|
|
entity, |
34 |
|
|
boost::assign::list_of(Value::STRING)(Value::STRING)(Value::STRING), |
35 |
|
|
docstring) {} |
36 |
|
|
|
37 |
|
|
Value Add::doExecute() { |
38 |
|
|
RosPublish& entity = static_cast<RosPublish&>(owner()); |
39 |
|
|
std::vector<Value> values = getParameterValues(); |
40 |
|
|
|
41 |
|
|
const std::string& type = values[0].value(); |
42 |
|
|
const std::string& signal = values[1].value(); |
43 |
|
|
const std::string& topic = values[2].value(); |
44 |
|
|
|
45 |
|
|
if (type == "boolean") |
46 |
|
|
entity.add<bool>(signal, topic); |
47 |
|
|
else if (type == "double") |
48 |
|
|
entity.add<double>(signal, topic); |
49 |
|
|
else if (type == "unsigned") |
50 |
|
|
entity.add<unsigned int>(signal, topic); |
51 |
|
|
else if (type == "int") |
52 |
|
|
entity.add<int>(signal, topic); |
53 |
|
|
else if (type == "matrix") |
54 |
|
|
entity.add<Matrix>(signal, topic); |
55 |
|
|
else if (type == "vector") |
56 |
|
|
entity.add<Vector>(signal, topic); |
57 |
|
|
else if (type == "vector3") |
58 |
|
|
entity.add<specific::Vector3>(signal, topic); |
59 |
|
|
else if (type == "vector3Stamped") |
60 |
|
|
entity.add<std::pair<specific::Vector3, Vector> >(signal, topic); |
61 |
|
|
else if (type == "matrixHomo") |
62 |
|
|
entity.add<sot::MatrixHomogeneous>(signal, topic); |
63 |
|
|
else if (type == "matrixHomoStamped") |
64 |
|
|
entity.add<std::pair<sot::MatrixHomogeneous, Vector> >(signal, topic); |
65 |
|
|
else if (type == "twist") |
66 |
|
|
entity.add<specific::Twist>(signal, topic); |
67 |
|
|
else if (type == "twistStamped") |
68 |
|
|
entity.add<std::pair<specific::Twist, Vector> >(signal, topic); |
69 |
|
|
else if (type == "string") |
70 |
|
|
entity.add<std::string>(signal, topic); |
71 |
|
|
else |
72 |
|
|
throw std::runtime_error("bad type"); |
73 |
|
|
return Value(); |
74 |
|
|
} |
75 |
|
|
|
76 |
|
|
} // namespace rosPublish |
77 |
|
|
} // end of namespace command. |
78 |
|
|
|
79 |
|
|
const std::string RosPublish::docstring_( |
80 |
|
|
"Publish dynamic-graph signals as ROS topics.\n" |
81 |
|
|
"\n" |
82 |
|
|
" Use command \"add\" to publish a new ROS topic.\n"); |
83 |
|
|
|
84 |
|
|
RosPublish::RosPublish(const std::string& n) |
85 |
|
|
: dynamicgraph::Entity(n), |
86 |
|
|
// RosPublish do not use callback so do not create a useless spinner. |
87 |
|
|
nh_(rosInit(false)), |
88 |
|
|
bindedSignal_(), |
89 |
|
|
trigger_(boost::bind(&RosPublish::trigger, this, _1, _2), sotNOSIGNAL, |
90 |
|
|
MAKE_SIGNAL_STRING(name, true, "int", "trigger")), |
91 |
|
|
rate_(0, 10000000), |
92 |
|
|
nextPublication_() { |
93 |
|
|
aofs_.open("/tmp/ros_publish.txt"); |
94 |
|
|
dgADD_OSTREAM_TO_RTLOG(aofs_); |
95 |
|
|
|
96 |
|
|
try { |
97 |
|
|
if (ros::Time::isSimTime()) |
98 |
|
|
nextPublication_ = ros::Time::now(); |
99 |
|
|
else { |
100 |
|
|
clock_gettime(CLOCK_REALTIME, &nextPublicationRT_); |
101 |
|
|
} |
102 |
|
|
} catch (const std::exception& exc) { |
103 |
|
|
throw std::runtime_error("Failed to call ros::Time::now ():" + |
104 |
|
|
std::string(exc.what())); |
105 |
|
|
} |
106 |
|
|
signalRegistration(trigger_); |
107 |
|
|
trigger_.setNeedUpdateFromAllChildren(true); |
108 |
|
|
|
109 |
|
|
std::string docstring = |
110 |
|
|
"\n" |
111 |
|
|
" Add a signal writing data to a ROS topic\n" |
112 |
|
|
"\n" |
113 |
|
|
" Input:\n" |
114 |
|
|
" - type: string among ['double', 'matrix', 'vector', 'vector3',\n" |
115 |
|
|
" 'vector3Stamped', 'matrixHomo', " |
116 |
|
|
"'matrixHomoStamped',\n" |
117 |
|
|
" 'twist', 'twistStamped'],\n" |
118 |
|
|
" - signal: the signal name in dynamic-graph,\n" |
119 |
|
|
" - topic: the topic name in ROS.\n" |
120 |
|
|
"\n"; |
121 |
|
|
addCommand("add", new command::rosPublish::Add(*this, docstring)); |
122 |
|
|
} |
123 |
|
|
|
124 |
|
|
RosPublish::~RosPublish() { aofs_.close(); } |
125 |
|
|
|
126 |
|
|
void RosPublish::display(std::ostream& os) const { |
127 |
|
|
os << CLASS_NAME << std::endl; |
128 |
|
|
} |
129 |
|
|
|
130 |
|
|
void RosPublish::rm(const std::string& signal) { |
131 |
|
|
if (bindedSignal_.find(signal) == bindedSignal_.end()) return; |
132 |
|
|
|
133 |
|
|
if (signal == "trigger") { |
134 |
|
|
std::cerr << "The trigger signal should not be removed. Aborting." |
135 |
|
|
<< std::endl; |
136 |
|
|
return; |
137 |
|
|
} |
138 |
|
|
|
139 |
|
|
// lock the mutex to avoid deleting the signal during a call to trigger |
140 |
|
|
boost::mutex::scoped_lock lock(mutex_); |
141 |
|
|
|
142 |
|
|
signalDeregistration(signal); |
143 |
|
|
bindedSignal_.erase(signal); |
144 |
|
|
} |
145 |
|
|
|
146 |
|
|
std::vector<std::string> RosPublish::list() const { |
147 |
|
|
std::vector<std::string> result(bindedSignal_.size()); |
148 |
|
|
std::transform(bindedSignal_.begin(), bindedSignal_.end(), result.begin(), |
149 |
|
|
[](const auto& pair) { return pair.first; }); |
150 |
|
|
return result; |
151 |
|
|
} |
152 |
|
|
|
153 |
|
|
void RosPublish::clear() { |
154 |
|
|
std::map<std::string, bindedSignal_t>::iterator it = bindedSignal_.begin(); |
155 |
|
|
for (; it != bindedSignal_.end();) { |
156 |
|
|
if (it->first != "trigger") { |
157 |
|
|
rm(it->first); |
158 |
|
|
it = bindedSignal_.begin(); |
159 |
|
|
} else { |
160 |
|
|
++it; |
161 |
|
|
} |
162 |
|
|
} |
163 |
|
|
} |
164 |
|
|
|
165 |
|
|
int& RosPublish::trigger(int& dummy, int t) { |
166 |
|
|
typedef std::map<std::string, bindedSignal_t>::iterator iterator_t; |
167 |
|
|
ros::Time aTime; |
168 |
|
|
if (ros::Time::isSimTime()) { |
169 |
|
|
aTime = ros::Time::now(); |
170 |
|
|
if (aTime <= nextPublication_) return dummy; |
171 |
|
|
|
172 |
|
|
nextPublication_ = aTime + rate_; |
173 |
|
|
} else { |
174 |
|
|
struct timespec aTimeRT; |
175 |
|
|
clock_gettime(CLOCK_REALTIME, &aTimeRT); |
176 |
|
|
nextPublicationRT_.tv_sec = aTimeRT.tv_sec + rate_.sec; |
177 |
|
|
nextPublicationRT_.tv_nsec = aTimeRT.tv_nsec + rate_.nsec; |
178 |
|
|
if (nextPublicationRT_.tv_nsec > 1000000000) { |
179 |
|
|
nextPublicationRT_.tv_nsec -= 1000000000; |
180 |
|
|
nextPublicationRT_.tv_sec += 1; |
181 |
|
|
} |
182 |
|
|
} |
183 |
|
|
|
184 |
|
|
boost::mutex::scoped_lock lock(mutex_); |
185 |
|
|
|
186 |
|
|
for (iterator_t it = bindedSignal_.begin(); it != bindedSignal_.end(); ++it) { |
187 |
|
|
boost::get<1>(it->second)(t); |
188 |
|
|
} |
189 |
|
|
return dummy; |
190 |
|
|
} |
191 |
|
|
|
192 |
|
|
std::string RosPublish::getDocString() const { return docstring_; } |
193 |
|
|
|
194 |
|
|
} // end of namespace dynamicgraph. |