6#ifndef _USE_MATH_DEFINES
7#define _USE_MATH_DEFINES
21#include <rcutils/logging_macros.h>
23using namespace std::chrono_literals;
46void ControlBoard_nws_ros2::closePorts()
80 if (m_node ==
nullptr) {
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);
93bool ControlBoard_nws_ros2::initRos2Control(
const std::string& name){
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";
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){
113 RCLCPP_ERROR(m_node->get_logger(),
"Could not initialize the Position msg subscription");
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){
124 RCLCPP_ERROR(m_node->get_logger(),
"Could not initialize the Position direct msg subscription");
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){
135 RCLCPP_ERROR(m_node->get_logger(),
"Could not initialize the Velocity msg subscription");
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){
166 RCLCPP_ERROR(m_node->get_logger(),
"Could not initialize the GetControlModes service");
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){
176 RCLCPP_ERROR(m_node->get_logger(),
"Could not initialize the GetPosition service");
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){
186 RCLCPP_ERROR(m_node->get_logger(),
"Could not initialize the GetVelocity service");
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){
196 RCLCPP_ERROR(m_node->get_logger(),
"Could not initialize the SetControlModes service");
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){
206 RCLCPP_ERROR(m_node->get_logger(),
"Could not initialize the GetAvailableModes service");
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){
216 RCLCPP_ERROR(m_node->get_logger(),
"Could not initialize the GetJointsNames service");
224 RCLCPP_ERROR(m_node->get_logger(),
"Could not initialize the GetJointsNames service");
230 for(
size_t i=0; i<m_subdevice_joints; i++){
232 yCError(
CONTROLBOARD_ROS2) <<
"Error retrieving axis" << i <<
"name. For this device to work, every joint needs a name";
248 m_subdevice_ptr = driver;
250 m_subdevice_ptr->
view(m_iPositionControl);
251 if (!m_iPositionControl) {
255 m_subdevice_ptr->
view(m_iPositionDirect);
256 if (!m_iPositionDirect) {
260 m_subdevice_ptr->
view(m_iVelocityControl);
261 if (!m_iVelocityControl) {
265 m_subdevice_ptr->
view(m_iControlMode);
266 if (!m_iControlMode) {
270 m_subdevice_ptr->
view(m_iEncodersTimed);
271 if (!m_iEncodersTimed) {
276 m_subdevice_ptr->
view(m_iTorqueControl);
277 if (!m_iTorqueControl) {
281 m_subdevice_ptr->
view(m_iAxisInfo);
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);
304 if (!updateAxisName()) {
312void ControlBoard_nws_ros2::closeDevice()
314 m_subdevice_ptr =
nullptr;
315 m_subdevice_joints = 0;
320 m_iPositionControl =
nullptr;
321 m_iEncodersTimed =
nullptr;
322 m_iTorqueControl =
nullptr;
323 m_iAxisInfo =
nullptr;
328 if (!setDevice(poly)) {
345 RCLCPP_ERROR(m_node->get_logger(),
"Error initializing the ROS2 control related topics and services");
351 if(!m_spinner->
start()){
371bool ControlBoard_nws_ros2::updateAxisName()
378 std::vector<std::string>
tmpVect;
379 for (
size_t i = 0; i < m_subdevice_joints; i++) {
407 if (m_iTorqueControl) {
413 m_time.
update(std::accumulate(m_times.
begin(), m_times.
end(), 0.0) / m_subdevice_joints);
418 for (
size_t i = 0; i < m_subdevice_joints; i++) {
426 m_ros_struct.name = m_jointNames;
438 if(m_publisherJointStates->get_subscription_count()>0)
439 m_publisherJointStates->publish(m_ros_struct);
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++) {
451 if(m_publisherControlModes->get_subscription_count()>0)
452 m_publisherControlModes->publish(modes);
double convertDegreesToRadians(double degrees)
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)
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)
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.
A mini-server for performing network communication in the background.
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.
An abstraction for a time stamp and/or sequence number.
void update()
Set the timestamp to the current time, and increment the sequence number (wrapping to 0 if the sequen...
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.
void resize(size_t size) override
Resize the vector.
iterator begin() noexcept
Returns an iterator to the beginning of the VectorOf.
T * data()
Return a pointer to the first element of the vector.
iterator end() noexcept
Returns an iterator to the end of the VectorOf.
#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.
@ VOCAB_JOINTTYPE_REVOLUTE
An interface to the operating system, including Port based communication.
The main, catch-all namespace for YARP.