YARP
Yet Another Robot Platform
laserFromDepth.cpp
Go to the documentation of this file.
1 /*
2  * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT)
3  * SPDX-License-Identifier: LGPL-2.1-or-later
4  */
5 
6 #define _USE_MATH_DEFINES
7 
8 #include "laserFromDepth.h"
9 
10 #include <yarp/os/Time.h>
11 #include <yarp/os/Log.h>
12 #include <yarp/os/LogStream.h>
13 #include <yarp/os/ResourceFinder.h>
14 
15 #include <cmath>
16 #include <cstring>
17 #include <cstdlib>
18 #include <iostream>
19 #include <limits>
20 #include <mutex>
21 
22 
23 #ifndef DEG2RAD
24 #define DEG2RAD M_PI/180.0
25 #endif
26 
27 YARP_LOG_COMPONENT(LASER_FROM_DEPTH, "yarp.devices.laserFromDepth")
28 
29 //-------------------------------------------------------------------------------------
30 
31 bool LaserFromDepth::open(yarp::os::Searchable& config)
32 {
33  Property subConfig;
34  m_info = "LaserFromDepth device";
35  m_device_status = DEVICE_OK_STANBY;
36 
37 #ifdef LASER_DEBUG
38  yCDebug(LASER_FROM_DEPTH) << "%s\n", config.toString().c_str();
39 #endif
40 
41  m_min_distance = 0.1; //m
42  m_max_distance = 2.5; //m
43 
44  if (this->parseConfiguration(config) == false)
45  {
46  yCError(LASER_FROM_DEPTH) << "error parsing parameters";
47  return false;
48  }
49 
50  Property prop;
51  if(!config.check("RGBD_SENSOR_CLIENT"))
52  {
53  yCError(LASER_FROM_DEPTH) << "missing RGBD_SENSOR_CLIENT section in configuration file!";
54  return false;
55  }
56  prop.fromString(config.findGroup("RGBD_SENSOR_CLIENT").toString());
57  prop.put("device", "RGBDSensorClient");
58 
59  driver.open(prop);
60  if (!driver.isValid())
61  {
62  yCError(LASER_FROM_DEPTH) << "Error opening PolyDriver check parameters";
63  return false;
64  }
65  driver.view(iRGBD);
66  if (!iRGBD)
67  {
68  yCError(LASER_FROM_DEPTH) << "Error opening iRGBD interface. Device not available";
69  return false;
70  }
71 
72  m_depth_width = iRGBD->getDepthWidth();
73  m_depth_height = iRGBD->getDepthHeight();
74  double hfov = 0;
75  double vfov = 0;
76  iRGBD->getDepthFOV(hfov, vfov);
77  m_sensorsNum = m_depth_width;
78  m_resolution = hfov / m_depth_width;
79  m_laser_data.resize(m_sensorsNum, 0.0);
80  m_max_angle = +hfov / 2;
81  m_min_angle = -hfov / 2;
82  PeriodicThread::start();
83 
84  yCInfo(LASER_FROM_DEPTH) << "Sensor ready";
85  return true;
86 }
87 
89 {
90  PeriodicThread::stop();
91 
92  if (driver.isValid()) {
93  driver.close();
94  }
95 
96  yCInfo(LASER_FROM_DEPTH) << "closed";
97  return true;
98 }
99 
100 bool LaserFromDepth::setDistanceRange(double min, double max)
101 {
102  std::lock_guard<std::mutex> guard(m_mutex);
103  m_min_distance = min;
104  m_max_distance = max;
105  return true;
106 }
107 
108 bool LaserFromDepth::setScanLimits(double min, double max)
109 {
110  std::lock_guard<std::mutex> guard(m_mutex);
111  yCWarning(LASER_FROM_DEPTH) << "setScanLimits not yet implemented";
112  return true;
113 }
114 
116 {
117  std::lock_guard<std::mutex> guard(m_mutex);
118  yCWarning(LASER_FROM_DEPTH) << "setHorizontalResolution not yet implemented";
119  return true;
120 }
121 
123 {
124  std::lock_guard<std::mutex> guard(m_mutex);
125  yCWarning(LASER_FROM_DEPTH) << "setScanRate not yet implemented";
126  return false;
127 }
128 
130 {
131 #ifdef LASER_DEBUG
132  yCDebug(LASER_FROM_DEPTH) << "thread initialising...\n";
133  yCDebug(LASER_FROM_DEPTH) << "... done!\n";
134 #endif
135 
136  return true;
137 }
138 
140 {
141 #ifdef DEBUG_TIMING
142  double t1 = yarp::os::Time::now();
143 #endif
144 
146  if (m_depth_image.getRawImage() == nullptr)
147  {
148  yCError(LASER_FROM_DEPTH) << "invalid image received";
149  return false;
150  }
151 
152  if (m_depth_image.width() != m_depth_width ||
154  {
155  yCError(LASER_FROM_DEPTH) << "invalid image size";
156  return false;
157  }
158 
159 
160  auto* pointer = (float*)m_depth_image.getPixelAddress(0, m_depth_height / 2);
161  double angle, distance, angleShift;
162 
163  angleShift = m_sensorsNum * m_resolution / 2;
164 
165  for (size_t elem = 0; elem < m_sensorsNum; elem++)
166  {
167  angle = elem * m_resolution; //deg
168  //the 1 / cos(blabla) distortion simulate the way RGBD devices calculate the distance..
169  distance = *(pointer + elem);
170  distance /= cos((angle - angleShift) * DEG2RAD); //m
171  m_laser_data[m_sensorsNum - 1 - elem] = distance;
172  }
173 
174  return true;
175 }
176 
178 {
179  m_mutex.lock();
180  updateLidarData();
181  m_mutex.unlock();
182  return;
183 }
184 
186 {
187 #ifdef LASER_DEBUG
188  yCDebug(LASER_FROM_DEPTH)<<"LaserFromDepth Thread releasing...";
189  yCDebug(LASER_FROM_DEPTH) <<"... done.";
190 #endif
191 
192  return;
193 }
laserFromDepth: Documentation to be added
PolyDriver driver
void run() override
Loop function.
size_t m_depth_height
IRGBDSensor * iRGBD
bool acquireDataFromHW() override final
This method should be implemented by the user, and contain the logic to grab data from the hardware.
size_t m_depth_width
bool close() override
Close the DeviceDriver.
bool setScanLimits(double min, double max) override
set the scan angular range.
bool setScanRate(double rate) override
set the scan rate (scans per seconds)
void threadRelease() override
Release method.
bool threadInit() override
Initialization method.
bool setDistanceRange(double min, double max) override
set the device detection range.
bool setHorizontalResolution(double step) override
get the angular step between two measurments (if available)
yarp::sig::ImageOf< float > m_depth_image
virtual bool getDepthImage(yarp::sig::ImageOf< yarp::sig::PixelFloat > &depthImage, yarp::os::Stamp *timeStamp=nullptr)=0
Get the depth frame from the device.
virtual bool updateLidarData()
This utility method calls in sequence: grabDataFromHW(), updateTimestamp and applyLimitsOnLaserData()...
bool close() override
Close the DeviceDriver.
Definition: PolyDriver.cpp:173
bool isValid() const
Check if device is valid.
Definition: PolyDriver.cpp:196
A class for storing options and configuration information.
Definition: Property.h:33
void fromString(const std::string &txt, bool wipe=true)
Interprets a string as a list of properties.
Definition: Property.cpp:1063
void put(const std::string &key, const std::string &value)
Associate the given key with the given string.
Definition: Property.cpp:1015
A base class for nested structures that can be searched.
Definition: Searchable.h:63
unsigned char * getPixelAddress(size_t x, size_t y) const
Get address of a pixel in memory.
Definition: Image.h:237
size_t width() const
Gets width of image in pixels.
Definition: Image.h:163
unsigned char * getRawImage() const
Access to the internal image buffer.
Definition: Image.cpp:541
size_t height() const
Gets height of image in pixels.
Definition: Image.h:169
const yarp::os::LogComponent & LASER_FROM_DEPTH()
#define DEG2RAD
#define yCInfo(component,...)
Definition: LogComponent.h:171
#define yCError(component,...)
Definition: LogComponent.h:213
#define yCWarning(component,...)
Definition: LogComponent.h:192
#define yCDebug(component,...)
Definition: LogComponent.h:128
#define YARP_LOG_COMPONENT(name,...)
Definition: LogComponent.h:76
double now()
Return the current time in seconds, relative to an arbitrary starting point.
Definition: Time.cpp:121
The main, catch-all namespace for YARP.
Definition: dirs.h:16