YARP
Yet Another Robot Platform
 
Loading...
Searching...
No Matches
HapticDevice_nws_ros2.cpp
Go to the documentation of this file.
1// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
2
4
5#include <cmath>
6#include <yarp/math/Math.h>
7#include <yarp/os/LogStream.h>
8#include <Ros2Utils.h>
9
10YARP_LOG_COMPONENT(HAPTICDEVICE_NWS_ROS2, "yarp.devices.HapticDevice_nws_ros2")
11
12HapticDevice_nws_ros2::HapticDevice_nws_ros2() : yarp::os::PeriodicThread(m_period)
13{
14}
15
16// -----------------------------------------------------------------------------
17bool HapticDevice_nws_ros2::publisherConfigureRosHandlers()
18{
19 const auto prefix = m_topic_name.empty() ? "" : ("/" + m_topic_name);
20
21 m_stat = m_node->create_publisher<geometry_msgs::msg::Pose>(
22 prefix + "/state/pose", 10);
23
24 m_buttons = m_node->create_publisher<std_msgs::msg::Int32MultiArray>(
25 prefix + "/state/buttons", 10);
26
27 return true;
28}
29
30bool HapticDevice_nws_ros2::subscriberConfigureRosHandlers()
31{
32 const auto prefix = m_topic_name.empty() ? "" : ("/" + m_topic_name);
33
34 m_feedback = m_node->create_subscription<geometry_msgs::msg::Vector3>(
35 prefix + "/feedback", 10,
36 std::bind(&HapticDevice_nws_ros2::feedbackCallback, this, std::placeholders::_1));
37
38 if (!m_feedback)
39 {
40 yCError(HAPTICDEVICE_NWS_ROS2) << "Could not initialize the feedback subscription";
41 return false;
42 }
43
44 return true;
45}
46
47bool HapticDevice_nws_ros2::configureRosParameters()
48{
49 std::map<int, double> params;
50 constexpr const char* DEFAULT_MODE_FEEDBACK = "cartesian";
51
52 // Declare node Parameters
53 rcl_interfaces::msg::ParameterDescriptor descriptor_msg;
54 descriptor_msg.name = "mode_feedback";
55 descriptor_msg.description = "Defines the type of the feedback, cartesian or joint.";
56 descriptor_msg.read_only = false;
57 descriptor_msg.additional_constraints = "Only 'cartesian' or 'joint' are allowed.";
58 descriptor_msg.set__type(rcl_interfaces::msg::ParameterType::PARAMETER_STRING);
59
60 m_node->declare_parameter<std::string>("mode_feedback", DEFAULT_MODE_FEEDBACK, descriptor_msg);
61 m_node->get_parameter("mode_feedback", mode_feedback);
62
63 m_params = m_node->add_on_set_parameters_callback([this](const auto & parameters) {
64 return parameter_callback(parameters);
65 });
66
67 return true;
68}
69
70bool HapticDevice_nws_ros2::servicesConfigureRosHandlers()
71{
72 const auto prefix = m_topic_name.empty() ? "" : ("/" + m_topic_name);
73
74 m_stopFeedbackService = m_node->create_service<std_srvs::srv::Trigger>(
75 prefix + "/stop_feedback",
76 std::bind(&HapticDevice_nws_ros2::stopFeedbackCallback,
77 this,
78 std::placeholders::_1,
79 std::placeholders::_2));
80
81 if (!m_stopFeedbackService)
82 {
83 yCError(HAPTICDEVICE_NWS_ROS2) << "Could not initialize the StopFeedback service";
84 return false;
85 }
86
87 m_maxForce = m_node->create_service<yarp_control_msgs::srv::GetMaxFeedback>(
88 prefix + "/get_max_feedback",
89 std::bind(&HapticDevice_nws_ros2::maxFeedbackCallback,
90 this,
91 std::placeholders::_1,
92 std::placeholders::_2
93 )
94 );
95
96 if (!m_maxForce)
97 {
98 yCError(HAPTICDEVICE_NWS_ROS2) << "Could not initialize the MaxForceFeedback service";
99 return false;
100 }
101
102 m_getTransformation = m_node->create_service<yarp_control_msgs::srv::GetTransformation>(
103 prefix + "/get_transform",
104 std::bind(&HapticDevice_nws_ros2::getTransformationCallback,
105 this,
106 std::placeholders::_1,
107 std::placeholders::_2
108 ));
109
110 if (!m_getTransformation)
111 {
112 yCError(HAPTICDEVICE_NWS_ROS2) << "Could not initialize the get_transformation service";
113 return false;
114 }
115
116 m_setTransformation = m_node->create_service<yarp_control_msgs::srv::SetTransformation>(
117 prefix + "/set_transform",
118 std::bind(&HapticDevice_nws_ros2::setTransformationCallback,
119 this,
120 std::placeholders::_1,
121 std::placeholders::_2
122 ));
123
124 if (!m_setTransformation)
125 {
126 yCError(HAPTICDEVICE_NWS_ROS2) << "Could not initialize the set_transformation service";
127 return false;
128 }
129
130 return true;
131}
132// -----------------------------------------------------------------------------
133
134void HapticDevice_nws_ros2::destroyRosHandlers()
135{
136 // Publishers
137 m_stat.reset();
138 m_buttons.reset();
139
140 // Subscribers
141 m_feedback.reset();
142
143 // Services
144 m_stopFeedbackService.reset();
145 m_maxForce.reset();
146 m_getTransformation.reset();
147 m_setTransformation.reset();
148
149 // Parameters
150 m_params.reset();
151}
152// -----------------------------------------------------------------------------
153void HapticDevice_nws_ros2::feedbackCallback(const geometry_msgs::msg::Vector3::SharedPtr msg)
154{
155 if (iHapticDevice != nullptr)
156 {
157 yarp::sig::Vector force(3, 0.0);
158 if (msg->x != 0.0 || msg->y != 0.0 || msg->z != 0.0)
159 {
160 force[0] = msg->x;
161 force[1] = msg->y;
162 force[2] = msg->z;
163 }
164 iHapticDevice->setFeedback(force);
165 }
166 else
167 {
168 yCError(HAPTICDEVICE_NWS_ROS2) << "IHapticDevice interface not available in feedback callback";
169 }
170}
171
172// -----------------------------------------------------------------------------
173void HapticDevice_nws_ros2::getTransformationCallback(
174 const std::shared_ptr<yarp_control_msgs::srv::GetTransformation::Request> request,
175 std::shared_ptr<yarp_control_msgs::srv::GetTransformation::Response> response)
176{
177 if (!iHapticDevice)
178 {
179 yCError(HAPTICDEVICE_NWS_ROS2) << "IHapticDevice interface not available";
180 response->success = false;
181 response->response = "IHapticDevice interface not available";
182 return;
183 }
184 // Transform
185 yarp::sig::Matrix trans;
186 if (!iHapticDevice->getTransformation(trans) || trans.rows() < 3 || trans.cols() < 4)
187 {
188 yCError(HAPTICDEVICE_NWS_ROS2) << "Failed to get transformation";
189 response->success = false;
190 response->response = "Failed to get transformation";
191 return;
192 }
193
194 geometry_msgs::msg::Transform trans_msg;
195 trans_msg.translation.x = trans(0, 3);
196 trans_msg.translation.y = trans(1, 3);
197 trans_msg.translation.z = trans(2, 3);
198
200 yarp::sig::Matrix R = trans.submatrix(0,2,0,2);
201 trans_q.fromRotationMatrix(R);
202 trans_msg.rotation.x = trans_q.x();
203 trans_msg.rotation.y = trans_q.y();
204 trans_msg.rotation.z = trans_q.z();
205 trans_msg.rotation.w = trans_q.w();
206
207 response->transform = trans_msg;
208 response->success = true;
209}
210
211void HapticDevice_nws_ros2::setTransformationCallback(
212 const yarp_control_msgs::srv::SetTransformation::Request::SharedPtr request,
213 std::shared_ptr<yarp_control_msgs::srv::SetTransformation::Response> response)
214{
215 if (!request)
216 {
217 yCError(HAPTICDEVICE_NWS_ROS2) << "Received null request in set_transformation callback";
218 response->success = false;
219 response->response = "Received null request";
220 return;
221 }
222
223 if (iHapticDevice == nullptr)
224 {
225 yCError(HAPTICDEVICE_NWS_ROS2) << "IHapticDevice interface not available in set_transformation callback";
226 return;
227 }
228
229 yarp::sig::Matrix T(4, 4);
230 T.eye();
231
232 const auto& q = request->transform.rotation;
233 const auto& t = request->transform.translation;
234
235 // Quaternion to rotation matrix
237 yarp_q.x() = q.x;
238 yarp_q.y() = q.y;
239 yarp_q.z() = q.z;
240 yarp_q.w() = q.w;
241 T.setSubmatrix(yarp_q.toRotationMatrix3x3(), 0, 0);
242 T(0, 3) = t.x;
243 T(1, 3) = t.y;
244 T(2, 3) = t.z;
245
246 if (!iHapticDevice->setTransformation(T))
247 {
248 yCError(HAPTICDEVICE_NWS_ROS2) << "setTransformation() failed";
249 response->response = "setTransformation failed";
250 response->success = false;
251 return;
252 }
253
254 response->success = true;
255}
256
257void HapticDevice_nws_ros2::stopFeedbackCallback(
258 const std::shared_ptr<std_srvs::srv::Trigger::Request> request,
259 std::shared_ptr<std_srvs::srv::Trigger::Response> response)
260{
261 if (!iHapticDevice)
262 {
263 yCError(HAPTICDEVICE_NWS_ROS2) << "IHapticDevice interface not available";
264 response->success = false;
265 response->message = "IHapticDevice interface not available";
266 return;
267 }
268
269 if (iHapticDevice->stopFeedback())
270 {
271 yCInfo(HAPTICDEVICE_NWS_ROS2) << "Feedback stopped";
272 response->success = true;
273 response->message = "Feedback stopped";
274 }
275 else
276 {
277 yCError(HAPTICDEVICE_NWS_ROS2) << "Failed to stop feedback";
278 response->success = false;
279 response->message = "Failed to stop feedback";
280 }
281}
282
283void HapticDevice_nws_ros2::maxFeedbackCallback(
284 const std::shared_ptr<yarp_control_msgs::srv::GetMaxFeedback::Request> request,
285 std::shared_ptr<yarp_control_msgs::srv::GetMaxFeedback::Response> response)
286{
287 if (!iHapticDevice)
288 {
289 yCError(HAPTICDEVICE_NWS_ROS2) << "IHapticDevice interface not available";
290 response->success = false;
291 return;
292 }
293
294 // Force Feedback
295 yarp::sig::Vector force;
296 if (!iHapticDevice->getMaxFeedback(force) || force.size() < 3)
297 {
298 yCError(HAPTICDEVICE_NWS_ROS2) << "Failed to get max feedback";
299 response->success = false;
300 return;
301 }
302
303 geometry_msgs::msg::Vector3 force_msg;
304 force_msg.x = force[0];
305 force_msg.y = force[1];
306 force_msg.z = force[2];
307
308 response->max_feedback = force_msg;
309 response->success = true;
310}
311
312// -----------------------------------------------------------------------------
313rcl_interfaces::msg::SetParametersResult HapticDevice_nws_ros2::parameter_callback(
314 const std::vector<rclcpp::Parameter> &parameters)
315{
316 rcl_interfaces::msg::SetParametersResult result;
317 result.successful = true;
318
319 for (const auto & param : parameters)
320 {
321 if (param.get_name() == "mode_feedback")
322 {
323 std::string new_mode = param.value_to_string();
324 if (mode_feedback != new_mode)
325 {
326 bool success = forceModeCallback(new_mode);
327 if (success)
328 {
329 mode_feedback = new_mode;
330 }
331 else
332 {
333 result.successful = false;
334 result.reason = "Failed to set mode_feedback to " + new_mode;
335 yCError(HAPTICDEVICE_NWS_ROS2) << result.reason;
336 }
337 }
338 }
339 else
340 {
341 yCWarning(HAPTICDEVICE_NWS_ROS2, "Parameter '%s' is not recognized. Ignoring.", param.get_name().c_str());
342 }
343 }
344
345 return result;
346}
347
348bool HapticDevice_nws_ros2::forceModeCallback(const std::string& mode)
349{
350 if (!iHapticDevice)
351 {
352 yCError(HAPTICDEVICE_NWS_ROS2) << "IHapticDevice interface not available";
353 return false;
354 }
355
356 if (mode != "cartesian" && mode != "joint")
357 {
358 yCError(HAPTICDEVICE_NWS_ROS2, "Invalid value for mode_feedback: %s. Allowed values are 'cartesian' or 'joint'", mode.c_str());
359 return false;
360 }
361
362 bool success = (mode == "cartesian")
363 ? iHapticDevice->setCartesianForceMode()
364 : iHapticDevice->setJointTorqueMode();
365
366 if (!success)
367 {
368 yCError(HAPTICDEVICE_NWS_ROS2) << "Failed to set haptic mode";
369 return false;
370 }
371
372 bool is_force_mode;
373 if (!iHapticDevice->isCartesianForceModeEnabled(is_force_mode))
374 {
375 yCError(HAPTICDEVICE_NWS_ROS2) << "Failed to get haptic mode";
376 return false;
377 }
378
379 bool expected_force_mode = (mode == "cartesian");
380 if (is_force_mode != expected_force_mode)
381 {
382 yCError(HAPTICDEVICE_NWS_ROS2) << "Switched to" << mode << "mode failed";
383 return false;
384 }
385
386 yCInfo(HAPTICDEVICE_NWS_ROS2) << "Switched to" << mode << "mode successfully";
387 return true;
388}
389
390// -----------------------------------------------------------------------------
392{
393 if (poly == nullptr)
394 {
395 yCError(HAPTICDEVICE_NWS_ROS2) << "attach() received nullptr";
396 return false;
397 }
398
399 if (!poly->isValid())
400 {
401 yCError(HAPTICDEVICE_NWS_ROS2) << "attach() received invalid PolyDriver";
402 return false;
403 }
404
405 if (!poly->view(iHapticDevice))
406 {
407 yCError(HAPTICDEVICE_NWS_ROS2) << "attach() failed to obtain iHapticDevice interface";
408 return false;
409 }
410
411 if (!publisherConfigureRosHandlers() || !subscriberConfigureRosHandlers()
412 || !servicesConfigureRosHandlers() || !configureRosParameters())
413 {
414 yCError(HAPTICDEVICE_NWS_ROS2) << "Failed to configure ROS handlers";
415 destroyRosHandlers();
416 return false;
417 }
418
421 {
422 yCError(HAPTICDEVICE_NWS_ROS2) << "Error starting thread";
423 return false;
424 }
425
426 if (m_spinner)
427 {
428 if (!m_spinner->start())
429 {
430 yCError(HAPTICDEVICE_NWS_ROS2) << "Error starting the spinner";
431 return false;
432 }
433 }
434
435 return true;
436}
437
438// -----------------------------------------------------------------------------
439
441{
443 {
445 }
446 destroyRosHandlers();
447 iHapticDevice = nullptr;
448 return true;
449}
450
451// ------------------- DeviceDriver Related ------------------------------------
452
454{
455 if (!parseParams(config))
456 {
457 yCError(HAPTICDEVICE_NWS_ROS2) << "Failed to parse parameters";
458 return false;
459 }
460
461 if (m_namespace.empty())
462 {
464 }
465 else
466 {
468 }
469
470 if (m_node == nullptr)
471 {
472 yCError(HAPTICDEVICE_NWS_ROS2) << "Opening" << m_node_name << "node failed, check your YARP-ROS2 network configuration";
473 return false;
474 }
475
476 m_spinner = new Ros2Spinner(m_node);
477 if (!m_spinner)
478 {
479 yCError(HAPTICDEVICE_NWS_ROS2) << "Could not initialize the Ros2Spinner";
480 return false;
481 }
482
483 yCInfo(HAPTICDEVICE_NWS_ROS2) << "Node" << m_node_name << "launched successfully";
484 return true;
485}
486
487// -----------------------------------------------------------------------------
488
490{
491 if (m_spinner)
492 {
493 if (m_spinner->isRunning())
494 {
495 m_spinner->stop();
496 }
497 delete m_spinner;
498 m_spinner = nullptr;
499 }
500
501 if (PeriodicThread::isRunning())
502 {
503 PeriodicThread::stop();
504 }
505
506 return detach();
507}
508
509// -----------------------------------------------------------------------------
511{
512 if (iHapticDevice != nullptr)
513 {
514 // Pose
517 if (iHapticDevice->getPosition(pos) && pos.size() >= 3 &&
518 iHapticDevice->getOrientation(rpy) && rpy.size() >= 3)
519 {
520 geometry_msgs::msg::Pose pose_msg;
521 pose_msg.position.x = pos[0];
522 pose_msg.position.y = pos[1];
523 pose_msg.position.z = pos[2];
524
527 pose_q.fromRotationMatrix(dcm);
528 pose_msg.orientation.x = pose_q.x();
529 pose_msg.orientation.y = pose_q.y();
530 pose_msg.orientation.z = pose_q.z();
531 pose_msg.orientation.w = pose_q.w();
532 m_stat->publish(pose_msg);
533 }
534
535 // Buttons
536 yarp::sig::Vector buttons;
537 if (iHapticDevice->getButtons(buttons) && buttons.size() >= 1)
538 {
539 std_msgs::msg::Int32MultiArray btn_msg;
540 for (size_t i = 0; i < buttons.size(); ++i)
541 {
542 btn_msg.data.push_back(static_cast<int>(buttons[i]));
543 }
544 m_buttons->publish(btn_msg);
545 }
546 }
547}
FeatureMode mode
const yarp::os::LogComponent & HAPTICDEVICE_NWS_ROS2()
bool parseParams(const yarp::os::Searchable &config) override
Parse the DeviceDriver parameters.
HapticDevice_nws_ros2: A HapticDevice network wrapper server for ROS2.
bool close() override
Close the DeviceDriver.
bool attach(yarp::dev::PolyDriver *poly) override
Attach to another object.
void run() override
Loop function.
bool detach() override
Detach the object (you must have first called attach).
bool open(yarp::os::Searchable &config) override
Open the DeviceDriver.
static rclcpp::Node::SharedPtr createNode(std::string name)
Definition Ros2Utils.cpp:9
bool view(T *&x)
Get an interface to the device driver.
A container for a device driver.
Definition PolyDriver.h:23
bool isValid() const
Check if device is valid.
void fromRotationMatrix(const yarp::sig::Matrix &R)
Converts a rotation matrix to a quaternion.
yarp::sig::Matrix toRotationMatrix3x3() const
Converts a quaternion to a rotation matrix.
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
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
A class for a Matrix.
Definition Matrix.h:40
Matrix submatrix(size_t r1, size_t r2, size_t c1, size_t c2) const
Extract a submatrix from (r1,c1) to (r2,c2) (extremes included), as in Matlab B=A(r1:r2,...
Definition Matrix.h:186
size_t cols() const
Return number of columns.
Definition Matrix.h:91
size_t rows() const
Return number of rows.
Definition Matrix.h:85
Scalar * data()
Return a pointer to the first element of the vector.
Definition VectorBase.h:245
size_t size() const
Returns the number of elements in the vector.
Definition VectorBase.h:167
#define yCInfo(component,...)
#define yCError(component,...)
#define yCWarning(component,...)
#define YARP_LOG_COMPONENT(name,...)
yarp::sig::Matrix rpy2dcm(const yarp::sig::Vector &rpy)
Converts roll-pitch-yaw angles in the corresponding dcm (direction cosine matrix) rotation matrix (de...
Definition math.cpp:847
The main, catch-all namespace for YARP.
Definition dirs.h:16