YARP
Yet Another Robot Platform
 
Loading...
Searching...
No Matches
ControlBoard_nws_ros2.cpp
Go to the documentation of this file.
1/*
2 * SPDX-FileCopyrightText: 2023 Istituto Italiano di Tecnologia (IIT)
3 * SPDX-License-Identifier: BSD-3-Clause
4 */
5
6#ifndef _USE_MATH_DEFINES
7#define _USE_MATH_DEFINES
8#endif
9
11
12#include <chrono>
13#include <functional>
14#include <memory>
15#include <string>
16#include <cmath>
17
19#include <yarp/os/LogStream.h>
20#include <Ros2Utils.h>
21#include <rcutils/logging_macros.h>
22
23using namespace std::chrono_literals;
24using namespace yarp::os;
25using namespace yarp::dev;
26using namespace yarp::sig;
27
28namespace {
29YARP_LOG_COMPONENT(CONTROLBOARD_ROS2, "yarp.ros2.controlBoard_nws_ros2", yarp::os::Log::TraceType);
30
31
33inline double convertDegreesToRadians(double degrees)
34{
35 return degrees / 180.0 * M_PI;
36}
37
38}
39
40
45
46void ControlBoard_nws_ros2::closePorts()
47{
48 // FIXME
49 yCWarning(CONTROLBOARD_ROS2, "FIXME: closePorts() is not implemented yet!");
50 YARP_UNUSED(this);
51}
52
54{
55 if(m_spinner){
56 if(m_spinner->isRunning()){
57 m_spinner->stop();
58 }
59 }
60 // Ensure that the device is not running
61 if (isRunning()) {
62 stop();
63 }
64
65 closeDevice();
66 closePorts();
67
68 return true;
69}
70
71
73{
74 Property prop;
75 prop.fromString(config.toString());
76
77 // Check parameter, so if both are present we use the correct one
78 if (prop.check("period")) {
79 if (!prop.find("period").isFloat64()) {
80 yCError(CONTROLBOARD_ROS2) << "'period' parameter is not a double value";
81 return false;
82 }
83 m_period = prop.find("period").asFloat64();
84 if (m_period <= 0) {
85 yCError(CONTROLBOARD_ROS2) << "'period' parameter is not valid, read value is" << m_period;
86 return false;
87 }
88 } else {
89 yCDebug(CONTROLBOARD_ROS2) << "'period' parameter missing, using default thread period = 0.02s";
90 m_period = m_default_period;
91 }
92
93 // check for node_name parameter
94 if (!config.check("node_name")) {
95 yCError(CONTROLBOARD_ROS2) << " cannot find node_name parameter";
96 return false;
97 }
98 m_nodeName = config.find("node_name").asString();
99 if(m_nodeName[0] == '/'){
100 yCError(CONTROLBOARD_ROS2) << "node_name cannot have an initial /";
101 return false;
102 }
103 // check for topic_name parameter
104 if (!config.check("topic_name")) {
105 yCError(CONTROLBOARD_ROS2) << " cannot find topic_name parameter";
106 return false;
107 }
108 m_jointStateTopicName = config.find("topic_name").asString();
109 if(m_jointStateTopicName[0] != '/'){
110 yCError(CONTROLBOARD_ROS2) << "topic_name must begin with an initial /";
111 return false;
112 }
113 yCInfo(CONTROLBOARD_ROS2) << "topic_name is " << m_jointStateTopicName;
114
115 m_node = NodeCreator::createNode(m_nodeName);
116 m_publisher = m_node->create_publisher<sensor_msgs::msg::JointState>(m_jointStateTopicName, 10);
117
118 if (config.check("msgs_name")) {
119 m_msgs_name = config.find("msgs_name").asString();
120 }
121
122 return true;
123}
124
125
126bool ControlBoard_nws_ros2::initRos2Control(const std::string& name){
127
128 m_posTopicName = name+"/position";
129 m_posDirTopicName = name+"/position_direct";
130 m_velTopicName = name+"/velocity";
131 m_getModesSrvName = name+"/get_modes";
132 m_setModesSrvName = name+"/set_modes";
133 m_getVelocitySrvName = name+"/get_velocity";
134 m_getPositionSrvName = name+"/get_position";
135 m_getAvailableModesSrvName = name+"/get_available_modes";
136 m_getJointsNamesSrvName = name+"/get_joints_names";
137
138 // Creating topics ------------------------------------------------------------------------------------------------- //
139
140 if(m_iPositionControl){
141 m_posSubscription = m_node->create_subscription<yarp_control_msgs::msg::Position>(m_posTopicName, 10,
142 std::bind(&ControlBoard_nws_ros2::positionTopic_callback,
143 this, std::placeholders::_1));
144 if(!m_posSubscription){
145 yCError(CONTROLBOARD_ROS2) << "Could not initialize the Position msg subscription";
146 RCLCPP_ERROR(m_node->get_logger(),"Could not initialize the Position msg subscription");
147
148 return false;
149 }
150 }
151 if(m_iPositionDirect){
152 m_posDirectSubscription = m_node->create_subscription<yarp_control_msgs::msg::PositionDirect>(m_posDirTopicName, 10,
153 std::bind(&ControlBoard_nws_ros2::positionDirectTopic_callback,
154 this, std::placeholders::_1));
155 if(!m_posDirectSubscription){
156 yCError(CONTROLBOARD_ROS2) << "Could not initialize the Position direct msg subscription";
157 RCLCPP_ERROR(m_node->get_logger(),"Could not initialize the Position direct msg subscription");
158
159 return false;
160 }
161 }
162 if(m_iVelocityControl){
163 m_velSubscription = m_node->create_subscription<yarp_control_msgs::msg::Velocity>(m_velTopicName, 10,
164 std::bind(&ControlBoard_nws_ros2::velocityTopic_callback,
165 this, std::placeholders::_1));
166 if(!m_velSubscription){
167 yCError(CONTROLBOARD_ROS2) << "Could not initialize the Velocity msg subscription";
168 RCLCPP_ERROR(m_node->get_logger(),"Could not initialize the Velocity msg subscription");
169
170 return false;
171 }
172 }
173
174 // Creating services ----------------------------------------------------------------------------------------------- //
175
176 if(!m_iControlMode){
177 return true;
178 }
179
182 qos.depth=10;
183 rmw_time_t time;
184 time.sec=10000;
185 time.nsec = 0;
186 qos.deadline= time;
187 qos.lifespan=time;
188 qos.liveliness_lease_duration=time;
192 qos.avoid_ros_namespace_conventions = true;
193 m_getControlModesSrv = m_node->create_service<yarp_control_msgs::srv::GetControlModes>(m_getModesSrvName,
194 std::bind(&ControlBoard_nws_ros2::getControlModesCallback,
195 this,std::placeholders::_1,std::placeholders::_2,
196 std::placeholders::_3));
197 if(!m_getControlModesSrv){
198 yCError(CONTROLBOARD_ROS2) << "Could not initialize the GetControlModes service";
199 RCLCPP_ERROR(m_node->get_logger(),"Could not initialize the GetControlModes service");
200
201 return false;
202 }
203 m_getPositionSrv = m_node->create_service<yarp_control_msgs::srv::GetPosition>(m_getPositionSrvName,
204 std::bind(&ControlBoard_nws_ros2::getPositionCallback,
205 this,std::placeholders::_1,std::placeholders::_2,
206 std::placeholders::_3));
207 if(!m_getPositionSrv){
208 yCError(CONTROLBOARD_ROS2) << "Could not initialize the GetPosition service";
209 RCLCPP_ERROR(m_node->get_logger(),"Could not initialize the GetPosition service");
210
211 return false;
212 }
213 m_getVelocitySrv = m_node->create_service<yarp_control_msgs::srv::GetVelocity>(m_getVelocitySrvName,
214 std::bind(&ControlBoard_nws_ros2::getVelocityCallback,
215 this,std::placeholders::_1,std::placeholders::_2,
216 std::placeholders::_3));
217 if(!m_getVelocitySrv){
218 yCError(CONTROLBOARD_ROS2) << "Could not initialize the GetVelocity service";
219 RCLCPP_ERROR(m_node->get_logger(),"Could not initialize the GetVelocity service");
220
221 return false;
222 }
223 m_setControlModesSrv = m_node->create_service<yarp_control_msgs::srv::SetControlModes>(m_setModesSrvName,
224 std::bind(&ControlBoard_nws_ros2::setControlModesCallback,
225 this,std::placeholders::_1,std::placeholders::_2,
226 std::placeholders::_3));
227 if(!m_setControlModesSrv){
228 yCError(CONTROLBOARD_ROS2) << "Could not initialize the SetControlModes service";
229 RCLCPP_ERROR(m_node->get_logger(),"Could not initialize the SetControlModes service");
230
231 return false;
232 }
233 m_getAvailableModesSrv = m_node->create_service<yarp_control_msgs::srv::GetAvailableControlModes>(m_getAvailableModesSrvName,
234 std::bind(&ControlBoard_nws_ros2::getAvailableModesCallback,
235 this,std::placeholders::_1,std::placeholders::_2,
236 std::placeholders::_3));
237 if(!m_getAvailableModesSrv){
238 yCError(CONTROLBOARD_ROS2) << "Could not initialize the GetAvailableModes service";
239 RCLCPP_ERROR(m_node->get_logger(),"Could not initialize the GetAvailableModes service");
240
241 return false;
242 }
243 m_getJointsNamesSrv = m_node->create_service<yarp_control_msgs::srv::GetJointsNames>(m_getJointsNamesSrvName,
244 std::bind(&ControlBoard_nws_ros2::getJointsNamesCallback,
245 this,std::placeholders::_1,std::placeholders::_2,
246 std::placeholders::_3));
247 if(!m_getJointsNamesSrv){
248 yCError(CONTROLBOARD_ROS2) << "Could not initialize the GetJointsNames service";
249 RCLCPP_ERROR(m_node->get_logger(),"Could not initialize the GetJointsNames service");
250
251 return false;
252 }
253
254 m_spinner = new Ros2Spinner(m_node);
255 if (!m_spinner){
256 yCError(CONTROLBOARD_ROS2) << "Could not initialize the GetJointsNames service";
257 RCLCPP_ERROR(m_node->get_logger(),"Could not initialize the GetJointsNames service");
258
259 return false;
260 }
261
262 std::string tmpName;
263 for(size_t i=0; i<m_subdevice_joints; i++){
264 if(!m_iAxisInfo->getAxisName(i,tmpName)){
265 yCError(CONTROLBOARD_ROS2) << "Error retrieving axis" << i << "name. For this device to work, every joint needs a name";
266
267 return false;
268 }
269 m_quickJointRef[tmpName] = i;
270 }
271
272 return true;
273}
274
275
276bool ControlBoard_nws_ros2::setDevice(yarp::dev::DeviceDriver* driver)
277{
279
280 // Save the pointer and subDeviceOwned
281 m_subdevice_ptr = driver;
282
283 m_subdevice_ptr->view(m_iPositionControl);
284 if (!m_iPositionControl) {
285 yCError(CONTROLBOARD_ROS2, "<%s - %s>: IPositionControl interface was not found in attached device.", m_nodeName.c_str(), m_jointStateTopicName.c_str());
286 }
287
288 m_subdevice_ptr->view(m_iPositionDirect);
289 if (!m_iPositionDirect) {
290 yCError(CONTROLBOARD_ROS2, "<%s - %s>: IPositionDirect interface was not found in attached device.", m_nodeName.c_str(), m_posTopicName.c_str());
291 }
292
293 m_subdevice_ptr->view(m_iVelocityControl);
294 if (!m_iVelocityControl) {
295 yCError(CONTROLBOARD_ROS2, "<%s - %s>: IVelocityControl interface was not found in attached device.", m_nodeName.c_str(), m_posTopicName.c_str());
296 }
297
298 m_subdevice_ptr->view(m_iControlMode);
299 if (!m_iControlMode) {
300 yCError(CONTROLBOARD_ROS2, "<%s - %s>: IControlMode interface was not found in attached device.", m_nodeName.c_str(), m_posTopicName.c_str());
301 }
302
303 m_subdevice_ptr->view(m_iEncodersTimed);
304 if (!m_iEncodersTimed) {
305 yCError(CONTROLBOARD_ROS2, "<%s - %s>: IEncodersTimed interface was not found in attached device. Quitting", m_nodeName.c_str(), m_jointStateTopicName.c_str());
306 return false;
307 }
308
309 m_subdevice_ptr->view(m_iTorqueControl);
310 if (!m_iTorqueControl) {
311 yCWarning(CONTROLBOARD_ROS2, "<%s - %s>: ITorqueControl interface was not found in attached device.", m_nodeName.c_str(), m_jointStateTopicName.c_str());
312 }
313
314 m_subdevice_ptr->view(m_iAxisInfo);
315 if (!m_iAxisInfo) {
316 yCError(CONTROLBOARD_ROS2, "<%s - %s>: IAxisInfo interface was not found in attached device. Quitting", m_nodeName.c_str(), m_jointStateTopicName.c_str());
317 return false;
318 }
319
320 // Get the number of controlled joints
321 int tmp_axes;
322 if (!m_iEncodersTimed->getAxes(&tmp_axes)) {
323 yCError(CONTROLBOARD_ROS2, "<%s - %s>: Failed to get axes number for attached device ", m_nodeName.c_str(), m_jointStateTopicName.c_str());
324 return false;
325 }
326 if (tmp_axes <= 0) {
327 yCError(CONTROLBOARD_ROS2, "<%s - %s>: attached device has an invalid number of joints (%d)", m_nodeName.c_str(), m_jointStateTopicName.c_str(), tmp_axes);
328 return false;
329 }
330 m_subdevice_joints = static_cast<size_t>(tmp_axes);
331 m_times.resize(m_subdevice_joints);
332 m_ros_struct.name.resize(m_subdevice_joints);
333 m_ros_struct.position.resize(m_subdevice_joints);
334 m_ros_struct.velocity.resize(m_subdevice_joints);
335 m_ros_struct.effort.resize(m_subdevice_joints);
336
337 if (!updateAxisName()) {
338 return false;
339 }
340
341 return true;
342}
343
344
345void ControlBoard_nws_ros2::closeDevice()
346{
347 m_subdevice_ptr = nullptr;
348 m_subdevice_joints = 0;
349
350 m_times.clear();
351
352 // Clear all interfaces
353 m_iPositionControl = nullptr;
354 m_iEncodersTimed = nullptr;
355 m_iTorqueControl = nullptr;
356 m_iAxisInfo = nullptr;
357}
358
360{
361 if (!setDevice(poly)) {
362 return false;
363 }
364
365 setPeriod(m_period);
366 if (!start()) {
367 yCError(CONTROLBOARD_ROS2) << "Error starting thread";
368 return false;
369 }
370
371 if(!m_msgs_name.empty())
372 {
373 if (m_msgs_name[0] != '/') {
374 m_msgs_name = "/"+m_msgs_name;
375 }
376 if(!initRos2Control(m_msgs_name)){
377 yCError(CONTROLBOARD_ROS2) << "Error initializing the ROS2 control related topics and services";
378 RCLCPP_ERROR(m_node->get_logger(),"Error initializing the ROS2 control related topics and services");
379 return false;
380 }
381 }
382
383 if(m_spinner){
384 if(!m_spinner->start()){
385 yCError(CONTROLBOARD_ROS2) << "Error starting the spinner";
386 }
387 }
388
389 return true;
390}
391
393{
394 // Ensure that the device is not running
395 if (isRunning()) {
396 stop();
397 }
398
399 closeDevice();
400
401 return true;
402}
403
404bool ControlBoard_nws_ros2::updateAxisName()
405{
406 // IMPORTANT!! This function has to be called BEFORE the thread starts,
407 // the name has to be correct right from the first message!!
408
409 yCAssert(CONTROLBOARD_ROS2, m_iAxisInfo);
410
411 std::vector<std::string> tmpVect;
412 for (size_t i = 0; i < m_subdevice_joints; i++) {
413 std::string tmp;
414 bool ret = m_iAxisInfo->getAxisName(i, tmp);
415 if (!ret) {
416 yCError(CONTROLBOARD_ROS2, "Joint name for axis %zu not found!", i);
417 return false;
418 }
419 tmpVect.emplace_back(tmp);
420 }
421
422 yCAssert(CONTROLBOARD_ROS2, tmpVect.size() == m_subdevice_joints);
423
424 m_jointNames = tmpVect;
425
426 return true;
427}
428
430{
431 yCAssert(CONTROLBOARD_ROS2, m_iEncodersTimed);
432 yCAssert(CONTROLBOARD_ROS2, m_iAxisInfo);
433
434 bool positionsOk = m_iEncodersTimed->getEncodersTimed(m_ros_struct.position.data(), m_times.data());
436
437 bool speedsOk = m_iEncodersTimed->getEncoderSpeeds(m_ros_struct.velocity.data());
439
440 if (m_iTorqueControl) {
441 bool torqueOk = m_iTorqueControl->getTorques(m_ros_struct.effort.data());
443 }
444
445 // Update the port envelope time by averaging all timestamps
446 m_time.update(std::accumulate(m_times.begin(), m_times.end(), 0.0) / m_subdevice_joints);
448
449 // Data from HW have been gathered few lines before
451 for (size_t i = 0; i < m_subdevice_joints; i++) {
452 m_iAxisInfo->getJointType(i, jType);
454 m_ros_struct.position[i] = convertDegreesToRadians(m_ros_struct.position[i]);
455 m_ros_struct.velocity[i] = convertDegreesToRadians(m_ros_struct.velocity[i]);
456 }
457 }
458
459 m_ros_struct.name = m_jointNames;
460 m_ros_struct.header.stamp.sec = int(averageTime.getTime()); // FIXME
461 m_ros_struct.header.stamp.nanosec = static_cast<int>(1000000000 * (averageTime.getTime() - int(averageTime.getTime()))); // FIXME
462
463// m_ros_struct.header.stamp = m_node->get_clock()->now(); //@@@@@@@@@@@ FIXME: averageTime.getTime();
464// m_ros_struct.header.frame_id = m_frame_id; // FIXME
465
466
467 // FIXME
468 ++m_counter;
469// m_ros_struct.header.seq = m_counter++;
470
471 m_publisher->publish(m_ros_struct);
472}
bool ret
bool open(yarp::os::Searchable &prop) override
Initialize the object.
void run() override
Loop function.
bool attach(yarp::dev::PolyDriver *poly) override
Attach to another object.
bool detach() override
Detach the object (you must have first called attach).
bool close() override
Shut the object down.
static rclcpp::Node::SharedPtr createNode(std::string name)
Definition Ros2Utils.cpp:9
Interface implemented by all device drivers.
bool view(T *&x)
Get an interface to the device driver.
virtual bool getAxisName(int axis, std::string &name)=0
virtual bool getJointType(int axis, yarp::dev::JointTypeEnum &type)
Definition IAxisInfo.h:63
virtual bool getEncodersTimed(double *encs, double *time)=0
Read the instantaneous acceleration of all axes.
virtual bool getAxes(int *ax)=0
Get the number of controlled axes.
virtual bool getEncoderSpeeds(double *spds)=0
Read the instantaneous speed of all axes.
virtual bool getTorques(double *t)=0
Get the value of the torque for all joints (this is the feedback if you have torque sensors).
A container for a device driver.
Definition PolyDriver.h:23
A mini-server for performing network communication in the background.
@ TraceType
Definition Log.h:92
An abstraction for a periodic thread.
bool setPeriod(double period)
Set the (new) period of the thread.
bool isRunning() const
Returns true when the thread is started, false otherwise.
bool start()
Call this to start the thread.
void stop()
Call this to stop the thread, this call blocks until the thread is terminated (and releaseThread() ca...
A class for storing options and configuration information.
Definition Property.h:33
Value & find(const std::string &key) const override
Gets a value corresponding to a given keyword.
void fromString(const std::string &txt, bool wipe=true)
Interprets a string as a list of properties.
bool check(const std::string &key) const override
Check if there exists a property of the given name.
A base class for nested structures that can be searched.
Definition Searchable.h:56
virtual bool check(const std::string &key) const =0
Check if there exists a property of the given name.
virtual std::string toString() const =0
Return a standard text representation of the content of the object.
virtual Value & find(const std::string &key) const =0
Gets a value corresponding to a given keyword.
An abstraction for a time stamp and/or sequence number.
Definition Stamp.h:21
void update()
Set the timestamp to the current time, and increment the sequence number (wrapping to 0 if the sequen...
Definition Stamp.cpp:124
bool stop()
Stop the thread.
Definition Thread.cpp:81
bool isRunning()
Returns true if the thread is running (Thread::start has been called successfully and the thread has ...
Definition Thread.cpp:105
bool start()
Start the new thread running.
Definition Thread.cpp:93
virtual yarp::conf::float64_t asFloat64() const
Get 64-bit floating point value.
Definition Value.cpp:222
virtual bool isFloat64() const
Checks if value is a 64-bit floating point number.
Definition Value.cpp:150
void resize(size_t size) override
Resize the vector.
Definition Vector.h:221
iterator begin() noexcept
Returns an iterator to the beginning of the VectorOf.
Definition Vector.h:454
T * data()
Return a pointer to the first element of the vector.
Definition Vector.h:206
iterator end() noexcept
Returns an iterator to the end of the VectorOf.
Definition Vector.h:461
#define M_PI
#define yCInfo(component,...)
#define yCError(component,...)
#define yCAssert(component, x)
#define yCWarning(component,...)
#define yCDebug(component,...)
#define YARP_LOG_COMPONENT(name,...)
For streams capable of holding different kinds of content, check what they actually have.
@ VOCAB_JOINTTYPE_REVOLUTE
Definition IAxisInfo.h:24
An interface to the operating system, including Port based communication.
The main, catch-all namespace for YARP.
Definition dirs.h:16
#define YARP_UNUSED(var)
Definition api.h:162
double convertDegreesToRadians(double degrees)
convert degrees to radiants for ROS messages