YARP
Yet Another Robot Platform
RGBDSensorWrapper.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_RGBDSENSORWRAPPER_RGBDSENSORWRAPPER_H
7 #define YARP_DEV_RGBDSENSORWRAPPER_RGBDSENSORWRAPPER_H
8 
9 #include <vector>
10 #include <iostream>
11 #include <string>
12 #include <sstream>
13 
14 #include <yarp/os/Port.h>
15 #include <yarp/os/Time.h>
16 #include <yarp/os/Stamp.h>
17 #include <yarp/os/Bottle.h>
18 #include <yarp/os/Network.h>
19 #include <yarp/os/Property.h>
20 #include <yarp/os/PeriodicThread.h>
21 #include <yarp/os/BufferedPort.h>
22 
23 
24 #include <yarp/sig/Vector.h>
25 
26 #include <yarp/dev/IWrapper.h>
28 #include <yarp/dev/PolyDriver.h>
29 #include <yarp/dev/IRGBDSensor.h>
30 
34 
35 // ROS stuff
36 #include <yarp/os/Node.h>
37 #include <yarp/os/Publisher.h>
38 #include <yarp/os/Subscriber.h>
39 #include <yarp/rosmsg/TickTime.h>
42 
43 
44 namespace RGBDImpl
45 {
46  const std::string frameId_param = "ROS_frame_Id";
47  const std::string nodeName_param = "ROS_nodeName";
48  const std::string colorTopicName_param = "ROS_colorTopicName";
49  const std::string depthTopicName_param = "ROS_depthTopicName";
50  const std::string depthInfoTopicName_param = "ROS_depthInfoTopicName";
51  const std::string colorInfoTopicName_param = "ROS_colorInfoTopicName";
52  class RGBDSensorParser;
53 }
54 
55 #define DEFAULT_THREAD_PERIOD 0.03 // s
56 
57 // Following three definitions would fit better in a header file
58 // shared between client and server ... where to place it?
60 #define RGBD_WRAPPER_PROTOCOL_VERSION_MAJOR 1
61 #define RGBD_WRAPPER_PROTOCOL_VERSION_MINOR 0
62 
63 
64 
67 {
68 private:
69  yarp::dev::IRGBDSensor *iRGBDSensor;
73 
74 public:
76  ~RGBDSensorParser() override = default;
80  bool respond(const yarp::os::Bottle& cmd, yarp::os::Bottle& response) override;
81 };
82 
83 
128  public yarp::dev::IWrapper,
131 {
132 private:
138  typedef unsigned int UInt;
139 
140  enum SensorType{COLOR_SENSOR, DEPTH_SENSOR};
141 
142  template <class T>
143  struct param
144  {
145  param(T& inVar, std::string inName)
146  {
147  var = &inVar;
148  parname = inName;
149  }
150  T* var;
151  std::string parname;
152  };
153 
154  std::string colorFrame_StreamingPort_Name;
155  std::string depthFrame_StreamingPort_Name;
156  ImagePortType colorFrame_StreamingPort;
157  DepthPortType depthFrame_StreamingPort;
158 
159  // One RPC port should be enough for the wrapper in all cases
160  yarp::os::Port rpcPort;
161  std::string rpcPort_Name;
162  ImageTopicType rosPublisherPort_color;
163  ImageTopicType rosPublisherPort_depth;
164  DepthTopicType rosPublisherPort_colorCaminfo;
165  DepthTopicType rosPublisherPort_depthCaminfo;
166  yarp::os::Node* rosNode;
167  std::string nodeName;
168  std::string depthTopicName;
169  std::string colorTopicName;
170  std::string dInfoTopicName;
171  std::string cInfoTopicName;
172  std::string rosFrameId;
173  yarp::sig::FlexImage colorImage;
175  UInt nodeSeq;
176 
177  // It should be possible to attach this guy to more than one port, try to see what
178  // will happen when receiving 2 calls at the same time (receive one calls while serving
179  // another one, it will result in concurrent thread most probably) and buffering issues.
180 // sensor::depth::RGBDSensor_RPCMgsParser RPC_parser;
181 
182  //Helper class for RPCs
183  RGBDImpl::RGBDSensorParser rgbdParser;
184 
185  // Image data specs
186  // int hDim, vDim;
187  double period;
188  std::string sensorId;
189  yarp::dev::IRGBDSensor* sensor_p;
192  int verbose;
193  bool use_YARP;
194  bool use_ROS;
195  bool forceInfoSync;
196  bool initialize_YARP(yarp::os::Searchable& config);
197  bool initialize_ROS(yarp::os::Searchable& config);
198  bool read(yarp::os::ConnectionReader& connection);
199 
200  // Open the wrapper only, the attach method needs to be called before using it
201  // Typical usage: yarprobotinterface
202  bool openDeferredAttach(yarp::os::Searchable& prop);
203 
204  // If a subdevice parameter is given, the wrapper will open it and attach to immediately.
205  // Typical usage: simulator or command line
206  bool isSubdeviceOwned;
207  yarp::dev::PolyDriver* subDeviceOwned;
208  bool openAndAttachSubDevice(yarp::os::Searchable& prop);
209 
210  // Synch
211  yarp::os::Stamp colorStamp;
212  yarp::os::Stamp depthStamp;
213  yarp::os::Property m_conf;
214 
215  bool writeData();
216  bool setCamInfo(yarp::rosmsg::sensor_msgs::CameraInfo& cameraInfo,
217  const std::string& frame_id,
218  const UInt& seq,
219  const SensorType& sensorType);
220 
221  static std::string yarp2RosPixelCode(int code);
222 
223 public:
229  ~RGBDSensorWrapper() override;
230 
231  bool open(yarp::os::Searchable &params) override;
232  bool fromConfig(yarp::os::Searchable &params);
233  bool close() override;
234 
235  void setId(const std::string &id);
236  std::string getId();
237 
241  bool attachAll(const yarp::dev::PolyDriverList &p) override;
242  bool detachAll() override;
243 
244  bool attach(yarp::dev::PolyDriver *poly) override;
246  bool detach() override;
247 
248  bool threadInit() override;
249  void threadRelease() override;
250  void run() override;
251 };
252 
253 #endif // YARP_DEV_RGBDSENSORWRAPPER_RGBDSENSORWRAPPER_H
constexpr yarp::conf::vocab32_t VOCAB_PROTOCOL_VERSION
contains the definition of a Vector type
bool configure(yarp::dev::IRGBDSensor *interface)
~RGBDSensorParser() override=default
bool configure(yarp::dev::IFrameGrabberControls *_fgCtrl)
bool configure(yarp::dev::IRgbVisualParams *rgbInterface, yarp::dev::IDepthVisualParams *depthInterface)
bool respond(const yarp::os::Bottle &cmd, yarp::os::Bottle &response) override
Respond to a message.
RGBDSensorWrapper deprecated: A Network grabber for kinect-like devices.
bool detach() override
Detach the object (you must have first called attach).
bool attachAll(const yarp::dev::PolyDriverList &p) override
Specify which sensor this thread has to read from.
RGBDSensorWrapper & operator=(const RGBDSensorWrapper &)=delete
bool close() override
Close the DeviceDriver.
RGBDSensorWrapper & operator=(RGBDSensorWrapper &&)=delete
bool fromConfig(yarp::os::Searchable &params)
void run() override
Loop function.
RGBDSensorWrapper(RGBDSensorWrapper &&)=delete
RGBDSensorWrapper(const RGBDSensorWrapper &)=delete
bool threadInit() override
Initialization method.
bool open(yarp::os::Searchable &params) override
Device driver interface.
void setId(const std::string &id)
void threadRelease() override
Release method.
bool detachAll() override
Detach the object (you must have first called attach).
bool attach(yarp::dev::PolyDriver *poly) override
Attach to another object.
Interface implemented by all device drivers.
Definition: DeviceDriver.h:35
A cheap and cheerful framework for human readable/writable forms of messages to devices.
Definition: DeviceDriver.h:131
An interface for retrieving intrinsic parameter from a depth camera.
Control interface for frame grabber devices.
Interface for an object that can wrap/attach to to another.
A generic interface for cameras that have both color camera as well as depth camera sensor,...
Definition: IRGBDSensor.h:40
An interface for retrieving intrinsic parameter from a rgb camera.
Interface for an object that can wrap/or "attach" to another.
Definition: IWrapper.h:26
A container for a device driver.
Definition: PolyDriver.h:24
A simple collection of objects that can be described and transmitted in a portable way.
Definition: Bottle.h:74
An interface for reading from a network connection.
The Node class.
Definition: Node.h:24
An abstraction for a periodic thread.
A mini-server for network communication.
Definition: Port.h:47
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
Image class with user control of representation details.
Definition: Image.h:414
const std::string depthInfoTopicName_param
const std::string depthTopicName_param
const std::string colorTopicName_param
const std::string nodeName_param
const std::string frameId_param
const std::string colorInfoTopicName_param
std::int32_t vocab32_t
Definition: numeric.h:78
constexpr yarp::conf::vocab32_t createVocab32(char a, char b=0, char c=0, char d=0)
Create a vocab from chars.
Definition: Vocab.h:28