YARP
Yet Another Robot Platform
 
Loading...
Searching...
No Matches
ControlBoard_nws_ros2.h
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 YARP_DEV_CONTROLBOARD_NWS_ROS2_H
7#define YARP_DEV_CONTROLBOARD_NWS_ROS2_H
8
12
19#include <yarp/dev/IAxisInfo.h>
20#include <Ros2Spinner.h>
21
22#include <yarp/os/Stamp.h>
23#include <yarp/sig/Vector.h>
24
25#include <string>
26#include <vector>
27
28#include <rclcpp/rclcpp.hpp>
29#include <sensor_msgs/msg/joint_state.hpp>
30#include <std_msgs/msg/header.hpp>
31#include <std_msgs/msg/float64_multi_array.hpp>
32#include <map>
33
34//Custom ros2 interfaces
35#include <yarp_control_msgs/srv/get_control_modes.hpp>
36#include <yarp_control_msgs/srv/get_position.hpp>
37#include <yarp_control_msgs/srv/get_velocity.hpp>
38#include <yarp_control_msgs/srv/set_control_modes.hpp>
39#include <yarp_control_msgs/srv/get_available_control_modes.hpp>
40#include <yarp_control_msgs/srv/get_joints_names.hpp>
41#include <yarp_control_msgs/msg/position.hpp>
42#include <yarp_control_msgs/msg/velocity.hpp>
43#include <yarp_control_msgs/msg/position_direct.hpp>
44
45#include <mutex>
46
47
48
71{
72private:
73 sensor_msgs::msg::JointState m_ros_struct;
74
75 yarp::sig::Vector m_times; // time for each joint
76
77 std::vector<std::string> m_jointNames; // name of the joints
78 std::string m_nodeName; // name of the rosNode
79 std::string m_jointStateTopicName; // name of the rosTopic
80 std::string m_msgs_name;
81 std::string m_posTopicName;
82 std::string m_posDirTopicName;
83 std::string m_velTopicName;
84 std::string m_getModesSrvName;
85 std::string m_getPositionSrvName;
86 std::string m_getVelocitySrvName;
87 std::string m_setModesSrvName;
88 std::string m_getJointsNamesSrvName;
89 std::string m_getAvailableModesSrvName;
90 std::map<std::string,size_t> m_quickJointRef;
91 mutable std::mutex m_cmdMutex;
92
93// yarp::os::Node* node; // ROS node
94 std::uint32_t m_counter {0}; // incremental counter in the ROS message
95
96// yarp::os::PortWriterBuffer<yarp::rosmsg::sensor_msgs::JointState> rosOutputState_buffer; // Buffer associated to the ROS topic
97// yarp::os::Publisher<yarp::rosmsg::sensor_msgs::JointState> rosPublisherPort; // Dedicated ROS topic publisher
98
99 rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr m_publisher;
100 rclcpp::Node::SharedPtr m_node;
101
102 static constexpr double m_default_period = 0.02; // s
103 double m_period {m_default_period};
104
105 yarp::os::Stamp m_time; // envelope to attach to the state port
106
107 size_t m_subdevice_joints {0};
108
109 // Devices
110 yarp::dev::DeviceDriver* m_subdevice_ptr{nullptr};
111 yarp::dev::IAxisInfo* m_iAxisInfo{nullptr};
112 yarp::dev::IEncodersTimed* m_iEncodersTimed{nullptr};
113 yarp::dev::ITorqueControl* m_iTorqueControl{nullptr};
114 yarp::dev::IPositionDirect* m_iPositionDirect{nullptr};
115 yarp::dev::IVelocityControl* m_iVelocityControl{nullptr};
116 yarp::dev::IControlMode* m_iControlMode{nullptr};
117 yarp::dev::IPositionControl* m_iPositionControl{nullptr};
118
119 // Ros2 related attributes
120 Ros2Spinner* m_spinner{nullptr};
121 rclcpp::Subscription<yarp_control_msgs::msg::Position>::SharedPtr m_posSubscription;
122 rclcpp::Subscription<yarp_control_msgs::msg::PositionDirect>::SharedPtr m_posDirectSubscription;
123 rclcpp::Subscription<yarp_control_msgs::msg::Velocity>::SharedPtr m_velSubscription;
124 rclcpp::Service<yarp_control_msgs::srv::GetJointsNames>::SharedPtr m_getJointsNamesSrv;
125 rclcpp::Service<yarp_control_msgs::srv::GetControlModes>::SharedPtr m_getControlModesSrv;
126 rclcpp::Service<yarp_control_msgs::srv::GetPosition>::SharedPtr m_getPositionSrv;
127 rclcpp::Service<yarp_control_msgs::srv::GetVelocity>::SharedPtr m_getVelocitySrv;
128 rclcpp::Service<yarp_control_msgs::srv::SetControlModes>::SharedPtr m_setControlModesSrv;
129 rclcpp::Service<yarp_control_msgs::srv::GetAvailableControlModes>::SharedPtr m_getAvailableModesSrv;
130
131 bool setDevice(yarp::dev::DeviceDriver* device);
132 bool initRos2Control(const std::string& name);
133
134 void closeDevice();
135 void closePorts();
136 bool updateAxisName();
137
138 // Utilities
139 bool messageVectorsCheck(const std::string &valueName, const std::vector<std::string> &names, const std::vector<double> &ref_values, const std::vector<double> &derivative);
140 bool messageVectorsCheck(const std::string &valueName, const std::vector<std::string> &names, const std::vector<double> &ref_values);
141 bool messageVectorsCheck(const std::string &valueName, const std::vector<std::string> &names, const std::vector<std::string> &ref_values);
142 bool namesCheck(const std::vector<std::string> &names);
143
144 // Service callbacks
145 void getControlModesCallback(const std::shared_ptr<rmw_request_id_t> request_header,
146 const std::shared_ptr<yarp_control_msgs::srv::GetControlModes::Request> request,
147 std::shared_ptr<yarp_control_msgs::srv::GetControlModes::Response> response);
148 void getPositionCallback(const std::shared_ptr<rmw_request_id_t> request_header,
149 const std::shared_ptr<yarp_control_msgs::srv::GetPosition::Request> request,
150 std::shared_ptr<yarp_control_msgs::srv::GetPosition::Response> response);
151 void getVelocityCallback(const std::shared_ptr<rmw_request_id_t> request_header,
152 const std::shared_ptr<yarp_control_msgs::srv::GetVelocity::Request> request,
153 std::shared_ptr<yarp_control_msgs::srv::GetVelocity::Response> response);
154 void setControlModesCallback(const std::shared_ptr<rmw_request_id_t> request_header,
155 const std::shared_ptr<yarp_control_msgs::srv::SetControlModes::Request> request,
156 std::shared_ptr<yarp_control_msgs::srv::SetControlModes::Response> response);
157 void getJointsNamesCallback(const std::shared_ptr<rmw_request_id_t> request_header,
158 const std::shared_ptr<yarp_control_msgs::srv::GetJointsNames::Request> request,
159 std::shared_ptr<yarp_control_msgs::srv::GetJointsNames::Response> response);
160 void getAvailableModesCallback(const std::shared_ptr<rmw_request_id_t> request_header,
161 const std::shared_ptr<yarp_control_msgs::srv::GetAvailableControlModes::Request> request,
162 std::shared_ptr<yarp_control_msgs::srv::GetAvailableControlModes::Response> response);
163
164 //Subscription callbacks
165 void positionTopic_callback(const yarp_control_msgs::msg::Position::SharedPtr msg);
166 void positionDirectTopic_callback(const yarp_control_msgs::msg::PositionDirect::SharedPtr msg);
167 void velocityTopic_callback(const yarp_control_msgs::msg::Velocity::SharedPtr msg);
168
169public:
175 ~ControlBoard_nws_ros2() override = default;
176
177 // yarp::dev::DeviceDriver
178 bool close() override;
179 bool open(yarp::os::Searchable& prop) override;
180
181 // yarp::dev::IWrapper
182 bool attach(yarp::dev::PolyDriver* poly) override;
183 bool detach() override;
184
185 // yarp::os::PeriodicThread
186 void run() override;
187};
188
189#endif // YARP_DEV_CONTROLBOARD_NWS_ROS2_H
define control board standard interfaces
contains the definition of a Vector type
controlBoard_nws_ros: A controlBoard network wrapper server for ROS2.
bool open(yarp::os::Searchable &prop) override
Initialize the object.
ControlBoard_nws_ros2(const ControlBoard_nws_ros2 &)=delete
void run() override
Loop function.
bool attach(yarp::dev::PolyDriver *poly) override
Attach to another object.
ControlBoard_nws_ros2(ControlBoard_nws_ros2 &&)=delete
~ControlBoard_nws_ros2() override=default
bool detach() override
Detach the object (you must have first called attach).
ControlBoard_nws_ros2 & operator=(const ControlBoard_nws_ros2 &)=delete
bool close() override
Shut the object down.
ControlBoard_nws_ros2 & operator=(ControlBoard_nws_ros2 &&)=delete
Interface implemented by all device drivers.
Interface for getting information about specific axes, if available.
Definition IAxisInfo.h:36
Interface for setting control mode in control board.
Control board, extend encoder interface with timestamps.
Interface for a generic control board device implementing position control.
Interface for a generic control board device implementing position control.
Interface for control boards implementing torque control.
Interface for control boards implementing velocity control.
A container for a device driver.
Definition PolyDriver.h:23
Helper interface for an object that can wrap/or "attach" to a single other device.
An abstraction for a periodic thread.
A base class for nested structures that can be searched.
Definition Searchable.h:56
An abstraction for a time stamp and/or sequence number.
Definition Stamp.h:21