YARP
Yet Another Robot Platform
 
Loading...
Searching...
No Matches
RGBDSensor_nws_yarp.cpp
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
7
9#include <yarp/os/LogStream.h>
10
11#include <cstdio>
12#include <cstring>
13#include <sstream>
14
15using namespace RGBDImpl;
16using namespace yarp::sig;
17using namespace yarp::dev;
18using namespace yarp::os;
19
20YARP_LOG_COMPONENT(RGBDSENSORNWSYARP, "yarp.devices.RgbdSensor_nws_yarp")
21
24 m_sensorStatus(IRGBDSensor::RGBD_SENSOR_NOT_READY),
25 verbose(4)
26{}
27
29{
30 close();
31 m_rgbdsensor = nullptr;
32 m_fgCtrl = nullptr;
33}
34
38{
39 if (!parseParams(config)) { return false; }
40
41 //port names
42 std::string rootName = m_name;
43 std::string rpcPort_Name = rootName + "/rpc:i";
44 colorFrame_StreamingPort_Name = rootName + "/rgbImage:o";
45 depthFrame_StreamingPort_Name = rootName + "/depthImage:o";
46
47 // Open ports
48 bool bRet;
49 bRet = true;
50 if (!rpcPort.open(rpcPort_Name))
51 {
52 yCError(RGBDSENSORNWSYARP) << "Unable to open rpc Port" << rpcPort_Name.c_str();
53 bRet = false;
54 }
55 rpcPort.setReader(*this);
56
57 if (!colorFrame_StreamingPort.open(colorFrame_StreamingPort_Name))
58 {
59 yCError(RGBDSENSORNWSYARP) << "Unable to open color streaming Port" << colorFrame_StreamingPort_Name.c_str();
60 bRet = false;
61 }
62 if (!depthFrame_StreamingPort.open(depthFrame_StreamingPort_Name))
63 {
64 yCError(RGBDSENSORNWSYARP) << "Unable to open depth streaming Port" << depthFrame_StreamingPort_Name.c_str();
65 bRet = false;
66 }
67
68 if (!bRet) {return false;}
69 return true;
70}
71
73{
74 yCTrace(RGBDSENSORNWSYARP, "Close");
75 detach();
76
77 // Closing port
78 rpcPort.interrupt();
79 colorFrame_StreamingPort.interrupt();
80 depthFrame_StreamingPort.interrupt();
81
82 rpcPort.close();
83 colorFrame_StreamingPort.close();
84 depthFrame_StreamingPort.close();
85
86 return true;
87}
88
94{
95 std::lock_guard lock (m_mutex);
96
99 }
100
101 m_rgbdsensor = nullptr;
102 m_fgCtrl = nullptr;
103 return true;
104}
105
107{
108 std::lock_guard lock (m_mutex);
109
110 if(poly)
111 {
112 poly->view(m_rgbdsensor);
113 poly->view(m_fgCtrl);
114 }
115
116 // m_rgbdsensor is mandatory
117 if(m_rgbdsensor == nullptr)
118 {
119 yCError(RGBDSENSORNWSYARP) << "Attached device has no valid IRGBDSensor interface.";
120 return false;
121 }
122
123 if(m_fgCtrl == nullptr)
124 {
125 yCWarning(RGBDSENSORNWSYARP) << "Attached device has no valid IFrameGrabberControls interface.";
126 }
127
128 // m_fgCtrl is optional and might be null
129 m_rgbd_RPC = std::make_unique<RGBDSensorMsgsImpl>(m_rgbdsensor, m_fgCtrl);
130
132 return PeriodicThread::start();
133}
134
135/* IRateThread interface */
136
138{
139 // Get interface from attached device if any.
140 return true;
141}
142
144{
145 // Detach() calls stop() which in turns calls this functions, therefore no calls to detach here!
146}
147
148bool RGBDSensor_nws_yarp::setCamInfo(const std::string& frame_id, const UInt& seq, const SensorType& sensorType)
149{
150 std::string distModel, currentSensor;
151 UInt i;
153 bool ok;
154
155 currentSensor = sensorType == COLOR_SENSOR ? "Rgb" : "Depth";
156 ok = sensorType == COLOR_SENSOR ? m_rgbdsensor->getRgbIntrinsicParam(camData) : m_rgbdsensor->getDepthIntrinsicParam(camData);
157
158 if (!ok)
159 {
160 yCError(RGBDSENSORNWSYARP) << "Unable to get intrinsic param from" << currentSensor << "sensor!";
161 return false;
162 }
163
164 if (camData.distortionModel.type != CameraDistortionType::YARP_PLUMB_BOB)
165 {
166 yCError(RGBDSENSORNWSYARP) << "Distortion model not supported";
167 return false;
168 }
169
170 return true;
171}
172
173bool RGBDSensor_nws_yarp::writeData()
174{
175 if (!m_rgbdsensor->getImages(colorImage, depthImage, &colorStamp, &depthStamp))
176 {
177 return false;
178 }
179
180 static Stamp oldColorStamp = Stamp(0, 0);
181 static Stamp oldDepthStamp = Stamp(0, 0);
182 bool rgb_data_ok = true;
183 bool depth_data_ok = true;
184
185 if (((colorStamp.getTime() - oldColorStamp.getTime()) > 0) == false)
186 {
187 rgb_data_ok=false;
188 //return true;
189 }
190 else { oldColorStamp = colorStamp; }
191
192 if (((depthStamp.getTime() - oldDepthStamp.getTime()) > 0) == false)
193 {
194 depth_data_ok=false;
195 //return true;
196 }
197 else { oldDepthStamp = depthStamp; }
198
199 // TBD: We should check here somehow if the timestamp was correctly updated and, if not, update it ourselves.
200 // In the following piece of code we are uing a copy instead of setExternal() because it is safer in a multithreaded environment.
201 // Indeed colorImage and depthImage can be modified by the attached device running in a different thread.
202 if (rgb_data_ok && colorFrame_StreamingPort.getOutputCount() > 0)
203 {
204 FlexImage& yColorImage = colorFrame_StreamingPort.prepare();
205 yColorImage = colorImage;
206 colorFrame_StreamingPort.setEnvelope(colorStamp);
207 colorFrame_StreamingPort.write();
208 }
209 if (depth_data_ok && depthFrame_StreamingPort.getOutputCount() > 0)
210 {
211 ImageOf<PixelFloat>& yDepthImage = depthFrame_StreamingPort.prepare();
213 depthFrame_StreamingPort.setEnvelope(depthStamp);
214 depthFrame_StreamingPort.write();
215 }
216
217 return true;
218}
219
221{
222 std::lock_guard lock (m_mutex);
223
224 if (m_rgbdsensor!=nullptr)
225 {
226 static int i = 0;
227 ReturnValue v = m_rgbdsensor->getSensorStatus(m_sensorStatus);
228 switch (m_sensorStatus)
229 {
231 {
232 if (!writeData()) {
233 yCError(RGBDSENSORNWSYARP, "Image not captured.. check hardware configuration");
234 }
235 i = 0;
236 }
237 break;
239 {
240 if(i < 1000) {
241 if((i % 30) == 0) {
242 yCInfo(RGBDSENSORNWSYARP) << "Device not ready, waiting...";
243 }
244 } else {
245 yCWarning(RGBDSENSORNWSYARP) << "Device is taking too long to start..";
246 }
247 i++;
248 }
249 break;
250 default:
251 {
252 if (verbose >= 1) { // better not to print it every cycle anyway, too noisy
253 yCError(RGBDSENSORNWSYARP, "%s: Sensor returned error", m_name.c_str());
254 }
255 }
256 }
257 }
258 else
259 {
260 if(verbose >= 6) {
261 yCError(RGBDSENSORNWSYARP, "%s: Sensor interface is not valid", m_name.c_str());
262 }
263 }
264}
265
267{
268 if (!connection.isValid()) { return false;}
269
270 std::lock_guard<std::mutex> lock(m_mutex);
271
272 if (m_rgbd_RPC)
273 {
274 if (m_rgbd_RPC->read(connection))
275 {
276 return true;
277 }
278 }
279 else
280 {
281 yCError(RGBDSENSORNWSYARP) << "m_rgbd_RPC interface is not valid";
282 return false;
283 }
284
285 yCError(RGBDSENSORNWSYARP) << "read() Command failed";
286 return false;
287}
#define DEFAULT_THREAD_PERIOD
const yarp::os::LogComponent & RGBDSENSORNWSYARP()
bool parseParams(const yarp::os::Searchable &config) override
Parse the DeviceDriver parameters.
RGBDSensor_nws_yarp: A Network grabber for kinect-like devices. This device will produce two streams ...
bool attach(yarp::dev::PolyDriver *poly) override
Attach to another object.
void run() override
Loop function.
bool close() override
Close the DeviceDriver.
void threadRelease() override
Release method.
bool threadInit() override
Initialization method.
bool read(yarp::os::ConnectionReader &connection) override
Read this object from a network connection.
bool detach() override
WrapperSingle interface.
bool open(yarp::os::Searchable &params) override
Device driver interface.
bool view(T *&x)
Get an interface to the device driver.
virtual yarp::dev::ReturnValue getDepthIntrinsicParam(yarp::sig::IntrinsicParams &intrinsic)=0
Get the intrinsic parameters of the depth camera.
A generic interface for cameras that have both color camera as well as depth camera sensor,...
Definition IRGBDSensor.h:39
virtual yarp::dev::ReturnValue getImages(yarp::sig::FlexImage &colorFrame, yarp::sig::ImageOf< yarp::sig::PixelFloat > &depthFrame, yarp::os::Stamp *colorStamp=nullptr, yarp::os::Stamp *depthStamp=nullptr)=0
Get the both the color and depth frame in a single call.
virtual yarp::dev::ReturnValue getSensorStatus(RGBDSensor_status &status)=0
Get the current status of the sensor, using enum type.
virtual yarp::dev::ReturnValue getRgbIntrinsicParam(yarp::sig::IntrinsicParams &intrinsic)=0
Get the intrinsic parameters of the rgb camera.
A container for a device driver.
Definition PolyDriver.h:23
A mini-server for performing network communication in the background.
void close() override
Stop port activity.
bool setEnvelope(PortWriter &envelope) override
Set an envelope (e.g., a timestamp) to the next message which will be sent.
bool open(const std::string &name) override
Start port operation, with a specific name, with automatically-chosen network parameters.
void interrupt() override
Interrupt any current reads or writes attached to the port.
int getOutputCount() override
Determine how many output connections this port has.
void write(bool forceStrict=false)
Write the current object being returned by BufferedPort::prepare.
T & prepare()
Access the object which will be transmitted by the next call to yarp::os::BufferedPort::write.
An interface for reading from a network connection.
virtual bool isValid() const =0
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...
void setReader(PortReader &reader) override
Set an external reader for port data.
Definition Port.cpp:470
void interrupt() override
Interrupt any current reads or writes attached to the port.
Definition Port.cpp:347
void close() override
Stop port activity.
Definition Port.cpp:330
bool open(const std::string &name) override
Start port operation, with a specific name, with automatically-chosen network parameters.
Definition Port.cpp:79
A base class for nested structures that can be searched.
Definition Searchable.h:31
An abstraction for a time stamp and/or sequence number.
Definition Stamp.h:18
double getTime() const
Get the time stamp.
Definition Stamp.cpp:34
Image class with user control of representation details.
Definition Image.h:361
#define yCInfo(component,...)
#define yCError(component,...)
#define yCTrace(component,...)
#define yCWarning(component,...)
#define YARP_LOG_COMPONENT(name,...)
For streams capable of holding different kinds of content, check what they actually have.
An interface to the operating system, including Port based communication.
The IntrinsicParams struct to handle the intrinsic parameter of cameras(RGB and RGBD either).