YARP
Yet Another Robot Platform
ControlBoard_nws_ros.h
Go to the documentation of this file.
1 /*
2  * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT)
3  * SPDX-License-Identifier: BSD-3-Clause
4  */
5 
6 #ifndef YARP_DEV_CONTROLBOARD_NWS_ROS_H
7 #define YARP_DEV_CONTROLBOARD_NWS_ROS_H
8 
10 #include <yarp/dev/WrapperSingle.h>
11 #include <yarp/os/PeriodicThread.h>
12 
16 #include <yarp/dev/IAxisInfo.h>
17 
18 #include <yarp/os/Stamp.h>
19 #include <yarp/sig/Vector.h>
20 
21 #include <string>
22 #include <vector>
23 
24 #include <yarp/os/Node.h>
25 #include <yarp/os/Publisher.h>
27 
28 
51 {
52 private:
54 
55  yarp::sig::Vector times; // time for each joint
56 
57  std::vector<std::string> jointNames; // name of the joints
58  std::string nodeName; // name of the rosNode
59  std::string topicName; // name of the rosTopic
60 
61  yarp::os::Node* node; // ROS node
62  std::uint32_t counter {0}; // incremental counter in the ROS message
63 
64  yarp::os::PortWriterBuffer<yarp::rosmsg::sensor_msgs::JointState> outputState_buffer; // Buffer associated to the ROS topic
65  yarp::os::Publisher<yarp::rosmsg::sensor_msgs::JointState> publisherPort; // Dedicated ROS topic publisher
66 
67  static constexpr double default_period = 0.02; // s
68  double period {default_period};
69 
70  yarp::os::Stamp time; // envelope to attach to the state port
71 
72  yarp::dev::DeviceDriver* subdevice_ptr{nullptr};
73  bool subdevice_owned {true};
74  size_t subdevice_joints {0};
75  bool subdevice_ready = false;
76 
77  yarp::dev::IPositionControl* iPositionControl{nullptr};
78  yarp::dev::IEncodersTimed* iEncodersTimed{nullptr};
79  yarp::dev::ITorqueControl* iTorqueControl{nullptr};
80  yarp::dev::IAxisInfo* iAxisInfo{nullptr};
81 
82  bool setDevice(yarp::dev::DeviceDriver* device, bool owned);
83  bool openAndAttachSubDevice(yarp::os::Property& prop);
84 
85  void closeDevice();
86  void closePorts();
87  bool updateAxisName();
88 
89 public:
95  ~ControlBoard_nws_ros() override = default;
96 
97  // yarp::dev::DeviceDriver
98  bool close() override;
99  bool open(yarp::os::Searchable& prop) override;
100 
101  // yarp::dev::WrapperSingle
102  bool attach(yarp::dev::PolyDriver* poly) override;
103  bool detach() override;
104 
105  // yarp::os::PeriodicThread
106  void run() override;
107 };
108 
109 #endif // YARP_DEV_CONTROLBOARD_NWS_ROS_H
define control board standard interfaces
contains the definition of a Vector type
controlBoard_nws_ros: A controlBoard network wrapper server for ROS.
bool attach(yarp::dev::PolyDriver *poly) override
Attach to another object.
void run() override
Loop function.
bool close() override
Close the DeviceDriver.
bool open(yarp::os::Searchable &prop) override
Open the DeviceDriver.
ControlBoard_nws_ros(ControlBoard_nws_ros &&)=delete
bool detach() override
Detach the object (you must have first called attach).
ControlBoard_nws_ros(const ControlBoard_nws_ros &)=delete
~ControlBoard_nws_ros() override=default
ControlBoard_nws_ros & operator=(const ControlBoard_nws_ros &)=delete
ControlBoard_nws_ros & operator=(ControlBoard_nws_ros &&)=delete
Interface implemented by all device drivers.
Definition: DeviceDriver.h:35
Interface for getting information about specific axes, if available.
Definition: IAxisInfo.h:40
Control board, extend encoder interface with timestamps.
Interface for a generic control board device implementing position control.
Interface for control boards implementing torque control.
A container for a device driver.
Definition: PolyDriver.h:24
Helper interface for an object that can wrap/or "attach" to a single other device.
Definition: WrapperSingle.h:32
The Node class.
Definition: Node.h:24
An abstraction for a periodic thread.
A class for storing options and configuration information.
Definition: Property.h:34
A base class for nested structures that can be searched.
Definition: Searchable.h:66
An abstraction for a time stamp and/or sequence number.
Definition: Stamp.h:22