17bool HapticDevice_nws_ros2::publisherConfigureRosHandlers()
21 m_stat = m_node->create_publisher<geometry_msgs::msg::Pose>(
22 prefix +
"/state/pose", 10);
24 m_buttons = m_node->create_publisher<std_msgs::msg::Int32MultiArray>(
25 prefix +
"/state/buttons", 10);
30bool HapticDevice_nws_ros2::subscriberConfigureRosHandlers()
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));
47bool HapticDevice_nws_ros2::configureRosParameters()
49 std::map<int, double> params;
50 constexpr const char* DEFAULT_MODE_FEEDBACK =
"cartesian";
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);
60 m_node->declare_parameter<std::string>(
"mode_feedback", DEFAULT_MODE_FEEDBACK, descriptor_msg);
61 m_node->get_parameter(
"mode_feedback", mode_feedback);
63 m_params = m_node->add_on_set_parameters_callback([
this](
const auto & parameters) {
64 return parameter_callback(parameters);
70bool HapticDevice_nws_ros2::servicesConfigureRosHandlers()
74 m_stopFeedbackService = m_node->create_service<std_srvs::srv::Trigger>(
75 prefix +
"/stop_feedback",
76 std::bind(&HapticDevice_nws_ros2::stopFeedbackCallback,
78 std::placeholders::_1,
79 std::placeholders::_2));
81 if (!m_stopFeedbackService)
87 m_maxForce = m_node->create_service<yarp_control_msgs::srv::GetMaxFeedback>(
88 prefix +
"/get_max_feedback",
89 std::bind(&HapticDevice_nws_ros2::maxFeedbackCallback,
91 std::placeholders::_1,
102 m_getTransformation = m_node->create_service<yarp_control_msgs::srv::GetTransformation>(
103 prefix +
"/get_transform",
104 std::bind(&HapticDevice_nws_ros2::getTransformationCallback,
106 std::placeholders::_1,
107 std::placeholders::_2
110 if (!m_getTransformation)
116 m_setTransformation = m_node->create_service<yarp_control_msgs::srv::SetTransformation>(
117 prefix +
"/set_transform",
118 std::bind(&HapticDevice_nws_ros2::setTransformationCallback,
120 std::placeholders::_1,
121 std::placeholders::_2
124 if (!m_setTransformation)
134void HapticDevice_nws_ros2::destroyRosHandlers()
144 m_stopFeedbackService.reset();
146 m_getTransformation.reset();
147 m_setTransformation.reset();
153void HapticDevice_nws_ros2::feedbackCallback(
const geometry_msgs::msg::Vector3::SharedPtr msg)
155 if (iHapticDevice !=
nullptr)
158 if (msg->x != 0.0 || msg->y != 0.0 || msg->z != 0.0)
164 iHapticDevice->setFeedback(force);
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)
180 response->success =
false;
181 response->response =
"IHapticDevice interface not available";
186 if (!iHapticDevice->getTransformation(trans) || trans.
rows() < 3 || trans.
cols() < 4)
189 response->success =
false;
190 response->response =
"Failed to get transformation";
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);
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();
207 response->transform = trans_msg;
208 response->success =
true;
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)
218 response->success =
false;
219 response->response =
"Received null request";
223 if (iHapticDevice ==
nullptr)
232 const auto& q = request->transform.rotation;
233 const auto& t = request->transform.translation;
246 if (!iHapticDevice->setTransformation(T))
249 response->response =
"setTransformation failed";
250 response->success =
false;
254 response->success =
true;
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)
264 response->success =
false;
265 response->message =
"IHapticDevice interface not available";
269 if (iHapticDevice->stopFeedback())
272 response->success =
true;
273 response->message =
"Feedback stopped";
278 response->success =
false;
279 response->message =
"Failed to stop feedback";
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)
290 response->success =
false;
296 if (!iHapticDevice->getMaxFeedback(force) || force.
size() < 3)
299 response->success =
false;
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];
308 response->max_feedback = force_msg;
309 response->success =
true;
313rcl_interfaces::msg::SetParametersResult HapticDevice_nws_ros2::parameter_callback(
314 const std::vector<rclcpp::Parameter> ¶meters)
316 rcl_interfaces::msg::SetParametersResult result;
317 result.successful =
true;
319 for (
const auto & param : parameters)
321 if (param.get_name() ==
"mode_feedback")
323 std::string new_mode = param.value_to_string();
324 if (mode_feedback != new_mode)
326 bool success = forceModeCallback(new_mode);
329 mode_feedback = new_mode;
333 result.successful =
false;
334 result.reason =
"Failed to set mode_feedback to " + new_mode;
348bool HapticDevice_nws_ros2::forceModeCallback(
const std::string& mode)
356 if (mode !=
"cartesian" && mode !=
"joint")
362 bool success = (mode ==
"cartesian")
363 ? iHapticDevice->setCartesianForceMode()
364 : iHapticDevice->setJointTorqueMode();
373 if (!iHapticDevice->isCartesianForceModeEnabled(is_force_mode))
379 bool expected_force_mode = (mode ==
"cartesian");
380 if (is_force_mode != expected_force_mode)
405 if (!poly->
view(iHapticDevice))
411 if (!publisherConfigureRosHandlers() || !subscriberConfigureRosHandlers()
412 || !servicesConfigureRosHandlers() || !configureRosParameters())
415 destroyRosHandlers();
428 if (!m_spinner->
start())
446 destroyRosHandlers();
447 iHapticDevice =
nullptr;
470 if (m_node ==
nullptr)
501 if (PeriodicThread::isRunning())
503 PeriodicThread::stop();
512 if (iHapticDevice !=
nullptr)
517 if (iHapticDevice->getPosition(pos) && pos.
size() >= 3 &&
518 iHapticDevice->getOrientation(rpy) && rpy.
size() >= 3)
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];
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);
537 if (iHapticDevice->getButtons(buttons) && buttons.
size() >= 1)
539 std_msgs::msg::Int32MultiArray btn_msg;
540 for (
size_t i = 0; i < buttons.
size(); ++i)
542 btn_msg.
data.push_back(
static_cast<int>(buttons[i]));
544 m_buttons->publish(btn_msg);
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)
bool view(T *&x)
Get an interface to the device driver.
A container for a device driver.
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.
bool stop()
Stop the thread.
bool isRunning()
Returns true if the thread is running (Thread::start has been called successfully and the thread has ...
bool start()
Start the new thread running.
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,...
size_t cols() const
Return number of columns.
size_t rows() const
Return number of rows.
T * data()
Return a pointer to the first element of the vector.
#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...
The main, catch-all namespace for YARP.