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 parseParams(config);
75 if(m_namespace.empty()) {
77 } else {
79 }
80 if (m_node == nullptr) {
81 yCError(CONTROLBOARD_ROS2) << " opening " << m_node_name << " Node, check your yarp-ROS2 network configuration\n";
82 return false;
83 }
84 m_jointStateTopicName = m_topic_name;
85 m_publisherJointStates = m_node->create_publisher<sensor_msgs::msg::JointState>(m_jointStateTopicName, 10);
86 m_jointControlModeTopicName = m_topic_name + "/controlModes";
87 m_publisherControlModes = m_node->create_publisher<std_msgs::msg::Int8MultiArray>(m_jointControlModeTopicName, 10);
88
89 return true;
90}
91
92
93bool ControlBoard_nws_ros2::initRos2Control(const std::string& name){
94
95 m_posTopicName = name+"/position";
96 m_posDirTopicName = name+"/position_direct";
97 m_velTopicName = name+"/velocity";
98 m_getModesSrvName = name+"/get_modes";
99 m_setModesSrvName = name+"/set_modes";
100 m_getVelocitySrvName = name+"/get_velocity";
101 m_getPositionSrvName = name+"/get_position";
102 m_getAvailableModesSrvName = name+"/get_available_modes";
103 m_getJointsNamesSrvName = name+"/get_joints_names";
104
105 // Creating topics ------------------------------------------------------------------------------------------------- //
106
107 if(m_iPositionControl){
108 m_posSubscription = m_node->create_subscription<yarp_control_msgs::msg::Position>(m_posTopicName, 10,
109 std::bind(&ControlBoard_nws_ros2::positionTopic_callback,
110 this, std::placeholders::_1));
111 if(!m_posSubscription){
112 yCError(CONTROLBOARD_ROS2) << "Could not initialize the Position msg subscription";
113 RCLCPP_ERROR(m_node->get_logger(),"Could not initialize the Position msg subscription");
114
115 return false;
116 }
117 }
118 if(m_iPositionDirect){
119 m_posDirectSubscription = m_node->create_subscription<yarp_control_msgs::msg::PositionDirect>(m_posDirTopicName, 10,
120 std::bind(&ControlBoard_nws_ros2::positionDirectTopic_callback,
121 this, std::placeholders::_1));
122 if(!m_posDirectSubscription){
123 yCError(CONTROLBOARD_ROS2) << "Could not initialize the Position direct msg subscription";
124 RCLCPP_ERROR(m_node->get_logger(),"Could not initialize the Position direct msg subscription");
125
126 return false;
127 }
128 }
129 if(m_iVelocityControl){
130 m_velSubscription = m_node->create_subscription<yarp_control_msgs::msg::Velocity>(m_velTopicName, 10,
131 std::bind(&ControlBoard_nws_ros2::velocityTopic_callback,
132 this, std::placeholders::_1));
133 if(!m_velSubscription){
134 yCError(CONTROLBOARD_ROS2) << "Could not initialize the Velocity msg subscription";
135 RCLCPP_ERROR(m_node->get_logger(),"Could not initialize the Velocity msg subscription");
136
137 return false;
138 }
139 }
140
141 // Creating services ----------------------------------------------------------------------------------------------- //
142
143 if(!m_iControlMode){
144 return true;
145 }
146
149 qos.depth=10;
150 rmw_time_t time;
151 time.sec=10000;
152 time.nsec = 0;
153 qos.deadline= time;
154 qos.lifespan=time;
155 qos.liveliness_lease_duration=time;
159 qos.avoid_ros_namespace_conventions = true;
160 m_getControlModesSrv = m_node->create_service<yarp_control_msgs::srv::GetControlModes>(m_getModesSrvName,
161 std::bind(&ControlBoard_nws_ros2::getControlModesCallback,
162 this,std::placeholders::_1,std::placeholders::_2,
163 std::placeholders::_3));
164 if(!m_getControlModesSrv){
165 yCError(CONTROLBOARD_ROS2) << "Could not initialize the GetControlModes service";
166 RCLCPP_ERROR(m_node->get_logger(),"Could not initialize the GetControlModes service");
167
168 return false;
169 }
170 m_getPositionSrv = m_node->create_service<yarp_control_msgs::srv::GetPosition>(m_getPositionSrvName,
171 std::bind(&ControlBoard_nws_ros2::getPositionCallback,
172 this,std::placeholders::_1,std::placeholders::_2,
173 std::placeholders::_3));
174 if(!m_getPositionSrv){
175 yCError(CONTROLBOARD_ROS2) << "Could not initialize the GetPosition service";
176 RCLCPP_ERROR(m_node->get_logger(),"Could not initialize the GetPosition service");
177
178 return false;
179 }
180 m_getVelocitySrv = m_node->create_service<yarp_control_msgs::srv::GetVelocity>(m_getVelocitySrvName,
181 std::bind(&ControlBoard_nws_ros2::getVelocityCallback,
182 this,std::placeholders::_1,std::placeholders::_2,
183 std::placeholders::_3));
184 if(!m_getVelocitySrv){
185 yCError(CONTROLBOARD_ROS2) << "Could not initialize the GetVelocity service";
186 RCLCPP_ERROR(m_node->get_logger(),"Could not initialize the GetVelocity service");
187
188 return false;
189 }
190 m_setControlModesSrv = m_node->create_service<yarp_control_msgs::srv::SetControlModes>(m_setModesSrvName,
191 std::bind(&ControlBoard_nws_ros2::setControlModesCallback,
192 this,std::placeholders::_1,std::placeholders::_2,
193 std::placeholders::_3));
194 if(!m_setControlModesSrv){
195 yCError(CONTROLBOARD_ROS2) << "Could not initialize the SetControlModes service";
196 RCLCPP_ERROR(m_node->get_logger(),"Could not initialize the SetControlModes service");
197
198 return false;
199 }
200 m_getAvailableModesSrv = m_node->create_service<yarp_control_msgs::srv::GetAvailableControlModes>(m_getAvailableModesSrvName,
201 std::bind(&ControlBoard_nws_ros2::getAvailableModesCallback,
202 this,std::placeholders::_1,std::placeholders::_2,
203 std::placeholders::_3));
204 if(!m_getAvailableModesSrv){
205 yCError(CONTROLBOARD_ROS2) << "Could not initialize the GetAvailableModes service";
206 RCLCPP_ERROR(m_node->get_logger(),"Could not initialize the GetAvailableModes service");
207
208 return false;
209 }
210 m_getJointsNamesSrv = m_node->create_service<yarp_control_msgs::srv::GetJointsNames>(m_getJointsNamesSrvName,
211 std::bind(&ControlBoard_nws_ros2::getJointsNamesCallback,
212 this,std::placeholders::_1,std::placeholders::_2,
213 std::placeholders::_3));
214 if(!m_getJointsNamesSrv){
215 yCError(CONTROLBOARD_ROS2) << "Could not initialize the GetJointsNames service";
216 RCLCPP_ERROR(m_node->get_logger(),"Could not initialize the GetJointsNames service");
217
218 return false;
219 }
220
221 m_spinner = new Ros2Spinner(m_node);
222 if (!m_spinner){
223 yCError(CONTROLBOARD_ROS2) << "Could not initialize the GetJointsNames service";
224 RCLCPP_ERROR(m_node->get_logger(),"Could not initialize the GetJointsNames service");
225
226 return false;
227 }
228
229 std::string tmpName;
230 for(size_t i=0; i<m_subdevice_joints; i++){
231 if(!m_iAxisInfo->getAxisName(i,tmpName)){
232 yCError(CONTROLBOARD_ROS2) << "Error retrieving axis" << i << "name. For this device to work, every joint needs a name";
233
234 return false;
235 }
236 m_quickJointRef[tmpName] = i;
237 }
238
239 return true;
240}
241
242
243bool ControlBoard_nws_ros2::setDevice(yarp::dev::DeviceDriver* driver)
244{
246
247 // Save the pointer and subDeviceOwned
248 m_subdevice_ptr = driver;
249
250 m_subdevice_ptr->view(m_iPositionControl);
251 if (!m_iPositionControl) {
252 yCError(CONTROLBOARD_ROS2, "<%s - %s>: IPositionControl interface was not found in attached device.", m_node_name.c_str(), m_topic_name.c_str());
253 }
254
255 m_subdevice_ptr->view(m_iPositionDirect);
256 if (!m_iPositionDirect) {
257 yCError(CONTROLBOARD_ROS2, "<%s - %s>: IPositionDirect interface was not found in attached device.", m_node_name.c_str(), m_posTopicName.c_str());
258 }
259
260 m_subdevice_ptr->view(m_iVelocityControl);
261 if (!m_iVelocityControl) {
262 yCError(CONTROLBOARD_ROS2, "<%s - %s>: IVelocityControl interface was not found in attached device.", m_node_name.c_str(), m_posTopicName.c_str());
263 }
264
265 m_subdevice_ptr->view(m_iControlMode);
266 if (!m_iControlMode) {
267 yCError(CONTROLBOARD_ROS2, "<%s - %s>: IControlMode interface was not found in attached device.", m_node_name.c_str(), m_posTopicName.c_str());
268 }
269
270 m_subdevice_ptr->view(m_iEncodersTimed);
271 if (!m_iEncodersTimed) {
272 yCError(CONTROLBOARD_ROS2, "<%s - %s>: IEncodersTimed interface was not found in attached device. Quitting", m_node_name.c_str(), m_topic_name.c_str());
273 return false;
274 }
275
276 m_subdevice_ptr->view(m_iTorqueControl);
277 if (!m_iTorqueControl) {
278 yCWarning(CONTROLBOARD_ROS2, "<%s - %s>: ITorqueControl interface was not found in attached device.", m_node_name.c_str(), m_topic_name.c_str());
279 }
280
281 m_subdevice_ptr->view(m_iAxisInfo);
282 if (!m_iAxisInfo) {
283 yCError(CONTROLBOARD_ROS2, "<%s - %s>: IAxisInfo interface was not found in attached device. Quitting", m_node_name.c_str(), m_topic_name.c_str());
284 return false;
285 }
286
287 // Get the number of controlled joints
288 int tmp_axes;
289 if (!m_iEncodersTimed->getAxes(&tmp_axes)) {
290 yCError(CONTROLBOARD_ROS2, "<%s - %s>: Failed to get axes number for attached device ", m_node_name.c_str(), m_topic_name.c_str());
291 return false;
292 }
293 if (tmp_axes <= 0) {
294 yCError(CONTROLBOARD_ROS2, "<%s - %s>: attached device has an invalid number of joints (%d)", m_node_name.c_str(), m_topic_name.c_str(), tmp_axes);
295 return false;
296 }
297 m_subdevice_joints = static_cast<size_t>(tmp_axes);
298 m_times.resize(m_subdevice_joints);
299 m_ros_struct.name.resize(m_subdevice_joints);
300 m_ros_struct.position.resize(m_subdevice_joints);
301 m_ros_struct.velocity.resize(m_subdevice_joints);
302 m_ros_struct.effort.resize(m_subdevice_joints);
303
304 if (!updateAxisName()) {
305 return false;
306 }
307
308 return true;
309}
310
311
312void ControlBoard_nws_ros2::closeDevice()
313{
314 m_subdevice_ptr = nullptr;
315 m_subdevice_joints = 0;
316
317 m_times.clear();
318
319 // Clear all interfaces
320 m_iPositionControl = nullptr;
321 m_iEncodersTimed = nullptr;
322 m_iTorqueControl = nullptr;
323 m_iAxisInfo = nullptr;
324}
325
327{
328 if (!setDevice(poly)) {
329 return false;
330 }
331
333 if (!start()) {
334 yCError(CONTROLBOARD_ROS2) << "Error starting thread";
335 return false;
336 }
337
338 if(!m_msgs_name.empty())
339 {
340 if (m_msgs_name[0] != '/') {
342 }
343 if(!initRos2Control(m_msgs_name)){
344 yCError(CONTROLBOARD_ROS2) << "Error initializing the ROS2 control related topics and services";
345 RCLCPP_ERROR(m_node->get_logger(),"Error initializing the ROS2 control related topics and services");
346 return false;
347 }
348 }
349
350 if(m_spinner){
351 if(!m_spinner->start()){
352 yCError(CONTROLBOARD_ROS2) << "Error starting the spinner";
353 }
354 }
355
356 return true;
357}
358
360{
361 // Ensure that the device is not running
362 if (isRunning()) {
363 stop();
364 }
365
366 closeDevice();
367
368 return true;
369}
370
371bool ControlBoard_nws_ros2::updateAxisName()
372{
373 // IMPORTANT!! This function has to be called BEFORE the thread starts,
374 // the name has to be correct right from the first message!!
375
376 yCAssert(CONTROLBOARD_ROS2, m_iAxisInfo);
377
378 std::vector<std::string> tmpVect;
379 for (size_t i = 0; i < m_subdevice_joints; i++) {
380 std::string tmp;
381 bool ret = m_iAxisInfo->getAxisName(i, tmp);
382 if (!ret) {
383 yCError(CONTROLBOARD_ROS2, "Joint name for axis %zu not found!", i);
384 return false;
385 }
386 tmpVect.emplace_back(tmp);
387 }
388
389 yCAssert(CONTROLBOARD_ROS2, tmpVect.size() == m_subdevice_joints);
390
391 m_jointNames = tmpVect;
392
393 return true;
394}
395
397{
398 yCAssert(CONTROLBOARD_ROS2, m_iEncodersTimed);
399 yCAssert(CONTROLBOARD_ROS2, m_iAxisInfo);
400
401 bool positionsOk = m_iEncodersTimed->getEncodersTimed(m_ros_struct.position.data(), m_times.data());
403
404 bool speedsOk = m_iEncodersTimed->getEncoderSpeeds(m_ros_struct.velocity.data());
406
407 if (m_iTorqueControl) {
408 bool torqueOk = m_iTorqueControl->getTorques(m_ros_struct.effort.data());
410 }
411
412 // Update the port envelope time by averaging all timestamps
413 m_time.update(std::accumulate(m_times.begin(), m_times.end(), 0.0) / m_subdevice_joints);
415
416 // Data from HW have been gathered few lines before
418 for (size_t i = 0; i < m_subdevice_joints; i++) {
419 m_iAxisInfo->getJointType(i, jType);
421 m_ros_struct.position[i] = convertDegreesToRadians(m_ros_struct.position[i]);
422 m_ros_struct.velocity[i] = convertDegreesToRadians(m_ros_struct.velocity[i]);
423 }
424 }
425
426 m_ros_struct.name = m_jointNames;
427 m_ros_struct.header.stamp.sec = int(averageTime.getTime()); // FIXME
428 m_ros_struct.header.stamp.nanosec = static_cast<int>(1000000000 * (averageTime.getTime() - int(averageTime.getTime()))); // FIXME
429
430// m_ros_struct.header.stamp = m_node->get_clock()->now(); //@@@@@@@@@@@ FIXME: averageTime.getTime();
431// m_ros_struct.header.frame_id = m_frame_id; // FIXME
432
433
434 // FIXME
435 ++m_counter;
436// m_ros_struct.header.seq = m_counter++;
437
438 if(m_publisherJointStates->get_subscription_count()>0)
439 m_publisherJointStates->publish(m_ros_struct);
440
441
442 //get and publish Control Modes
443 std::vector<int> modes_array(m_subdevice_joints);
444 m_iControlMode->getControlModes(modes_array.data());
445
446 auto modes = std_msgs::msg::Int8MultiArray();
447 modes.data.resize(m_subdevice_joints);
448 for (size_t i = 0; i < m_subdevice_joints; i++) {
449 modes.data[i]=modes_array[i];
450 }
451 if(m_publisherControlModes->get_subscription_count()>0)
452 m_publisherControlModes->publish(modes);
453}
double convertDegreesToRadians(double degrees)
#define M_PI
bool parseParams(const yarp::os::Searchable &config) override
Parse the DeviceDriver parameters.
bool open(yarp::os::Searchable &prop) override
Open the DeviceDriver.
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
Close the DeviceDriver.
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 yarp::dev::ReturnValue getJointType(int axis, yarp::dev::JointTypeEnum &type)
Definition IAxisInfo.h:64
virtual yarp::dev::ReturnValue getAxisName(int axis, std::string &name)=0
virtual yarp::dev::ReturnValue getControlModes(std::vector< yarp::dev::ControlModeEnum > &mode)=0
Get the current control mode (multiple joints).
virtual yarp::dev::ReturnValue getEncodersTimed(double *encs, double *time)=0
Read the instantaneous acceleration of all axes.
virtual yarp::dev::ReturnValue getEncoderSpeeds(double *spds)=0
Read the instantaneous speed of all axes.
virtual yarp::dev::ReturnValue getAxes(size_t &ax)=0
Get the number of controlled axes.
virtual yarp::dev::ReturnValue 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 base class for nested structures that can be searched.
Definition Searchable.h:31
An abstraction for a time stamp and/or sequence number.
Definition Stamp.h:18
void update()
Set the timestamp to the current time, and increment the sequence number (wrapping to 0 if the sequen...
Definition Stamp.cpp:49
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
Scalar * data()
Return a pointer to the first element of the vector.
Definition VectorBase.h:245
iterator begin() noexcept
Returns an iterator to the beginning of the Vector.
Definition VectorBase.h:305
iterator end() noexcept
Definition VectorBase.h:311
void resize(size_t size)
Resize the vector.
Definition VectorBase.h:106
void clear()
Clear (removes all elements) the vector.
Definition VectorBase.h:216
#define yCError(component,...)
#define yCAssert(component, x)
#define yCWarning(component,...)
#define YARP_LOG_COMPONENT(name,...)
For streams capable of holding different kinds of content, check what they actually have.
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