YARP
Yet Another Robot Platform
 
Loading...
Searching...
No Matches
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>
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
27YARP_LOG_COMPONENT(LASER_FROM_DEPTH, "yarp.devices.laserFromDepth")
28
29//-------------------------------------------------------------------------------------
30
32{
34 m_info = "LaserFromDepth device";
35 m_device_status = DEVICE_OK_STANDBY;
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 m_min_angle = 0;
44 m_max_angle = 60;
45 m_resolution = 1;
46
47 if (this->parseConfiguration(config) == false)
48 {
49 yCError(LASER_FROM_DEPTH) << "error parsing parameters";
50 return false;
51 }
52
53 Property prop;
54 if(!config.check("RGBD_SENSOR_CLIENT"))
55 {
56 yCError(LASER_FROM_DEPTH) << "missing RGBD_SENSOR_CLIENT section in configuration file!";
57 return false;
58 }
59 prop.fromString(config.findGroup("RGBD_SENSOR_CLIENT").toString());
60 prop.put("device", "RGBDSensor_nwc_yarp");
61
62 driver.open(prop);
63 if (!driver.isValid())
64 {
65 yCError(LASER_FROM_DEPTH) << "Error opening PolyDriver check parameters";
66 return false;
67 }
68 driver.view(iRGBD);
69 if (!iRGBD)
70 {
71 yCError(LASER_FROM_DEPTH) << "Error opening iRGBD interface. Device not available";
72 return false;
73 }
74
75 m_depth_width = iRGBD->getDepthWidth();
76 m_depth_height = iRGBD->getDepthHeight();
77 double hfov = 0;
78 double vfov = 0;
79 if (!iRGBD->getDepthFOV(hfov, vfov))
80 {
81 yCError(LASER_FROM_DEPTH) << "Error getting depth FOV";
82 return false;
83 }
84 m_sensorsNum = m_depth_width;
85 m_resolution = hfov / m_depth_width;
86 m_laser_data.resize(m_sensorsNum, 0.0);
87 m_max_angle = +hfov / 2;
88 m_min_angle = -hfov / 2;
90
91 yCInfo(LASER_FROM_DEPTH) << "Sensor ready";
92 return true;
93}
94
96{
98
99 if (driver.isValid()) {
100 driver.close();
101 }
102
103 yCInfo(LASER_FROM_DEPTH) << "closed";
104 return true;
105}
106
108{
109 std::lock_guard<std::mutex> guard(m_mutex);
110 m_min_distance = min;
111 m_max_distance = max;
112 return ReturnValue_ok;
113}
114
116{
117 std::lock_guard<std::mutex> guard(m_mutex);
118 yCWarning(LASER_FROM_DEPTH) << "setScanLimits not yet implemented";
119 return ReturnValue_ok;
120}
121
123{
124 std::lock_guard<std::mutex> guard(m_mutex);
125 yCWarning(LASER_FROM_DEPTH) << "setHorizontalResolution not yet implemented";
126 return ReturnValue_ok;
127}
128
130{
131 std::lock_guard<std::mutex> guard(m_mutex);
132 yCWarning(LASER_FROM_DEPTH) << "setScanRate not yet implemented";
133 return ReturnValue::return_code::return_value_error_not_implemented_by_device;
134}
135
137{
138#ifdef LASER_DEBUG
139 yCDebug(LASER_FROM_DEPTH) << "thread initialising...\n";
140 yCDebug(LASER_FROM_DEPTH) << "... done!\n";
141#endif
142
143 return true;
144}
145
147{
148#ifdef DEBUG_TIMING
149 double t1 = yarp::os::Time::now();
150#endif
151
153 if (!success || m_depth_image.getRawImage() == nullptr)
154 {
155 yCError(LASER_FROM_DEPTH) << "invalid image received";
156 return false;
157 }
158
161 {
162 yCError(LASER_FROM_DEPTH) << "invalid image size:" <<
164 "vs." <<
166 return false;
167 }
168
169
170 auto* pointer = (float*)m_depth_image.getPixelAddress(0, m_depth_height / 2);
171 double angle, distance, angleShift;
172
173 // angleshift is used to center the laser scan around 0 degrees,
174 // so that the laser scan goes from -deg to +deg.
175 // Example: if resolution is 1 deg and sensorsNum is 60,
176 // the laser scan will go from -30 deg to +30 deg.
178
179 for (size_t elem = 0; elem < m_sensorsNum; elem++)
180 {
181 angle = elem * m_resolution; //deg
182 //the 1 / cos(blabla) distortion simulate the way RGBD devices calculate the distance..
183 distance = *(pointer + elem);
184 distance /= cos((angle - angleShift) * DEG2RAD); //m
186 }
187
188 return true;
189}
190
192{
193 bool b = updateLidarData();
194 YARP_UNUSED(b);
195
196 return;
197}
198
200{
201#ifdef LASER_DEBUG
202 yCDebug(LASER_FROM_DEPTH)<<"LaserFromDepth Thread releasing...";
203 yCDebug(LASER_FROM_DEPTH) <<"... done.";
204#endif
205
206 return;
207}
#define DEG2RAD
#define ReturnValue_ok
Definition ReturnValue.h:82
laserFromDepth: This devices connects to an RGBDSensor_nws_yarp to receive depth data.
PolyDriver driver
yarp::dev::ReturnValue setScanRate(double rate) override
set the scan rate (scans per seconds)
void run() override
Loop function.
IRGBDSensor * iRGBD
bool acquireDataFromHW() override final
This method should be implemented by the user, and contain the logic to grab data from the hardware.
yarp::dev::ReturnValue setHorizontalResolution(double step) override
get the angular step between two measurements (if available)
bool close() override
Close the DeviceDriver.
yarp::dev::ReturnValue setScanLimits(double min, double max) override
set the scan angular range.
void threadRelease() override
Release method.
yarp::dev::ReturnValue setDistanceRange(double min, double max) override
set the device detection range.
bool threadInit() override
Initialization method.
yarp::sig::ImageOf< float > m_depth_image
virtual yarp::dev::ReturnValue 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.
bool isValid() const
Check if device is valid.
A mini-server for performing network communication in the background.
bool open(const std::string &name) override
Start port operation, with a specific name, with automatically-chosen network parameters.
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...
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.
void put(const std::string &key, const std::string &value)
Associate the given key with the given string.
Definition Property.cpp:987
A base class for nested structures that can be searched.
Definition Searchable.h:31
size_t width() const
Gets width of image in pixels.
Definition Image.h:171
unsigned char * getRawImage() const
Access to the internal image buffer.
Definition Image.cpp:462
size_t height() const
Gets height of image in pixels.
Definition Image.h:177
unsigned char * getPixelAddress(size_t x, size_t y) const
Get address of a pixel in memory.
Definition Image.h:245
const yarp::os::LogComponent & LASER_FROM_DEPTH()
#define yCInfo(component,...)
#define yCError(component,...)
#define yCWarning(component,...)
#define yCDebug(component,...)
#define YARP_LOG_COMPONENT(name,...)
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
#define YARP_UNUSED(var)
Definition api.h:162