Skip to content

Commit f72915e

Browse files
randaz81r1 user
randaz81
authored and
r1 user
committed
update for YARP4 release
1 parent 19b44a1 commit f72915e

File tree

5 files changed

+112
-86
lines changed

5 files changed

+112
-86
lines changed

‎CMakeLists.txt‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -31,8 +31,8 @@ list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/cmake")
3131
set(YARP_FORCE_DYNAMIC_PLUGINS TRUE CACHE INTERNAL "yarp-devices-ros2 is always built with dynamic plugins")
3232
option(BUILD_SHARED_LIBS "Build libraries as shared as opposed to static" ON)
3333

34-
find_package(YARP 3.12.0 COMPONENTS os sig dev serversql OPTIONAL_COMPONENTS math REQUIRED)
35-
find_package(YARP 3.12.0 COMPONENTS catch2 dev_tests QUIET)
34+
find_package(YARP 4.0.0 COMPONENTS os sig dev serversql OPTIONAL_COMPONENTS math REQUIRED)
35+
find_package(YARP 4.0.0 COMPONENTS catch2 dev_tests QUIET)
3636

3737
if(YARP_catch2_FOUND AND YARP_dev_tests_FOUND)
3838
option(YARP_COMPILE_TESTS "Enable YARP tests" OFF)

‎src/devices/rgbdSensor_nwc_ros2/RgbdSensor_nwc_ros2.cpp‎

Lines changed: 76 additions & 55 deletions
Original file line numberDiff line numberDiff line change
@@ -23,6 +23,7 @@
2323

2424
using namespace std::chrono_literals;
2525
using namespace std::placeholders;
26+
using namespace yarp::dev;
2627

2728
YARP_LOG_COMPONENT(RGBDSENSOR_NWC_ROS2, "yarp.ros2.RgbdSensor_nwc_ros2", yarp::os::Log::TraceType);
2829

@@ -168,172 +169,185 @@ int RgbdSensor_nwc_ros2::getRgbWidth()
168169
return m_current_rgb_image.width();
169170
}
170171

171-
bool RgbdSensor_nwc_ros2::getRgbSupportedConfigurations(yarp::sig::VectorOf<yarp::dev::CameraConfig> &configurations)
172+
yarp::dev::ReturnValue RgbdSensor_nwc_ros2::getRgbSupportedConfigurations(std::vector<yarp::dev::CameraConfig> &configurations)
172173
{
173174
yCWarning(RGBDSENSOR_NWC_ROS2) << "getRgbSupportedConfigurations not implemented yet";
174-
return false;
175+
return ReturnValue::return_code::return_value_error_not_implemented_by_device;
175176
}
176177

177-
bool RgbdSensor_nwc_ros2::getRgbResolution(int &width, int &height)
178+
yarp::dev::ReturnValue RgbdSensor_nwc_ros2::getRgbResolution(int &width, int &height)
178179
{
179180
if (!m_rgb_image_valid)
180181
{
181182
width=0;
182183
height=0;
183-
return false;
184+
return ReturnValue::return_code::return_value_error_not_ready;
184185
}
185186
width = m_current_rgb_image.width();
186187
height = m_current_rgb_image.height();
187-
return true;
188+
return ReturnValue_ok;
189+
}
190+
191+
yarp::dev::ReturnValue RgbdSensor_nwc_ros2::getDepthResolution(int &width, int &height)
192+
{
193+
if (!m_rgb_image_valid)
194+
{
195+
width=0;
196+
height=0;
197+
return ReturnValue::return_code::return_value_error_not_ready;
198+
}
199+
width = m_current_depth_image.width();
200+
height = m_current_depth_image.height();
201+
return ReturnValue_ok;
188202
}
189203

190-
bool RgbdSensor_nwc_ros2::setDepthResolution(int width, int height)
204+
yarp::dev::ReturnValue RgbdSensor_nwc_ros2::setDepthResolution(int width, int height)
191205
{
192206
yCWarning(RGBDSENSOR_NWC_ROS2) << "setDepthResolution not supported";
193-
return false;
207+
return ReturnValue::return_code::return_value_error_not_implemented_by_device;
194208
}
195209

196-
bool RgbdSensor_nwc_ros2::setRgbResolution(int width, int height)
210+
yarp::dev::ReturnValue RgbdSensor_nwc_ros2::setRgbResolution(int width, int height)
197211
{
198212
yCWarning(RGBDSENSOR_NWC_ROS2) << "setRgbResolution not supported";
199-
return false;
213+
return ReturnValue::return_code::return_value_error_not_implemented_by_device;
200214
}
201215

202-
bool RgbdSensor_nwc_ros2::setRgbFOV(double horizontalFov, double verticalFov)
216+
yarp::dev::ReturnValue RgbdSensor_nwc_ros2::setRgbFOV(double horizontalFov, double verticalFov)
203217
{
204218
yCWarning(RGBDSENSOR_NWC_ROS2) << "setRgbFOV not supported";
205-
return false;
219+
return ReturnValue::return_code::return_value_error_not_implemented_by_device;
206220
}
207221

208-
bool RgbdSensor_nwc_ros2::setDepthFOV(double horizontalFov, double verticalFov)
222+
yarp::dev::ReturnValue RgbdSensor_nwc_ros2::setDepthFOV(double horizontalFov, double verticalFov)
209223
{
210224
yCWarning(RGBDSENSOR_NWC_ROS2) << "setDepthFOV not supported";
211-
return false;
225+
return ReturnValue::return_code::return_value_error_not_implemented_by_device;
212226
}
213227

214-
bool RgbdSensor_nwc_ros2::setDepthAccuracy(double accuracy)
228+
yarp::dev::ReturnValue RgbdSensor_nwc_ros2::setDepthAccuracy(double accuracy)
215229
{
216230
yCWarning(RGBDSENSOR_NWC_ROS2) << "setDepthAccuracy not supported";
217-
return false;
231+
return ReturnValue::return_code::return_value_error_not_implemented_by_device;
218232
}
219233

220234
// not sure that it is correct, maybe I should save the full image dimension
221-
bool RgbdSensor_nwc_ros2::getRgbFOV(double &horizontalFov, double &verticalFov)
235+
yarp::dev::ReturnValue RgbdSensor_nwc_ros2::getRgbFOV(double &horizontalFov, double &verticalFov)
222236
{
223237
if (!m_rgb_image_valid && ! m_rgb_stamp_valid)
224238
{
225239
horizontalFov=0;
226240
verticalFov=0;
227-
return false;
241+
return ReturnValue::return_code::return_value_error_not_ready;
228242
yCError(RGBDSENSOR_NWC_ROS2) << "get rgb IntrinsicParam missing information";
229243
}
230244
horizontalFov = 2 * atan(m_max_rgb_width / (2 * m_rgb_params.focalLengthX)) * 180.0 / M_PI;
231245
verticalFov = 2 * atan(m_max_rgb_height / (2 * m_rgb_params.focalLengthY)) * 180.0 / M_PI;
232-
return true;
246+
return ReturnValue_ok;
233247
}
234248

235249

236-
bool RgbdSensor_nwc_ros2::getRgbMirroring(bool& mirror)
250+
yarp::dev::ReturnValue RgbdSensor_nwc_ros2::getRgbMirroring(bool& mirror)
237251
{
238252
yCWarning(RGBDSENSOR_NWC_ROS2) << "Mirroring not supported";
239-
return false;
253+
return ReturnValue::return_code::return_value_error_not_implemented_by_device;
240254
}
241255

242-
bool RgbdSensor_nwc_ros2::setRgbMirroring(bool mirror)
256+
yarp::dev::ReturnValue RgbdSensor_nwc_ros2::setRgbMirroring(bool mirror)
243257
{
244258
yCWarning(RGBDSENSOR_NWC_ROS2) << "Mirroring not supported";
245-
return false;
259+
return ReturnValue::return_code::return_value_error_not_implemented_by_device;
246260
}
247261

248-
bool RgbdSensor_nwc_ros2::getRgbIntrinsicParam(yarp::os::Property& intrinsic)
262+
yarp::dev::ReturnValue RgbdSensor_nwc_ros2::getRgbIntrinsicParam(yarp::os::Property& intrinsic)
249263
{
250264
if (m_rgb_stamp_valid)
251265
{
252266
intrinsic.clear();
253267
m_rgb_params.toProperty(intrinsic);
254-
return true;
268+
return ReturnValue_ok;
255269
}
256-
return false;
270+
return ReturnValue::return_code::return_value_error_not_ready;
257271
}
258272

259-
int RgbdSensor_nwc_ros2::getDepthHeight()
273+
int RgbdSensor_nwc_ros2::getDepthHeight()
260274
{
261275
if (!m_depth_image_valid) return 0;
262276
return m_current_depth_image.height();
263277
}
264278

265-
int RgbdSensor_nwc_ros2::getDepthWidth()
279+
int RgbdSensor_nwc_ros2::getDepthWidth()
266280
{
267281
if (!m_depth_image_valid) return 0;
268282
return m_current_depth_image.width();
269283
}
270284

271-
bool RgbdSensor_nwc_ros2::getDepthFOV(double& horizontalFov, double& verticalFov)
285+
yarp::dev::ReturnValue RgbdSensor_nwc_ros2::getDepthFOV(double& horizontalFov, double& verticalFov)
272286
{
273287
if (!m_depth_image_valid && !m_depth_stamp_valid)
274288
{
275289
horizontalFov=0;
276290
verticalFov=0;
277-
return false;
291+
return ReturnValue::return_code::return_value_error_not_ready;
278292
yCError(RGBDSENSOR_NWC_ROS2) << "get depth IntrinsicParam missing information";
279293
}
280294
horizontalFov = 2 * atan(m_max_rgb_width / (2 * m_depth_params.focalLengthX)) * 180.0 / M_PI;
281295
verticalFov = 2 * atan(m_max_rgb_height / (2 * m_depth_params.focalLengthY)) * 180.0 / M_PI;
282-
return true;
296+
return ReturnValue_ok;
283297
}
284298

285-
bool RgbdSensor_nwc_ros2::getDepthIntrinsicParam(yarp::os::Property& intrinsic)
299+
yarp::dev::ReturnValue RgbdSensor_nwc_ros2::getDepthIntrinsicParam(yarp::os::Property& intrinsic)
286300
{
287301
if(m_depth_stamp_valid)
288302
{
289303
intrinsic.clear();
290304
m_depth_params.toProperty(intrinsic);
291-
return true;
305+
return ReturnValue_ok;
292306
}
293-
return false;
307+
return ReturnValue::return_code::return_value_error_not_ready;
294308
}
295309

296-
double RgbdSensor_nwc_ros2::getDepthAccuracy()
310+
yarp::dev::ReturnValue RgbdSensor_nwc_ros2::getDepthAccuracy(double& accuracy)
297311
{
298312
yCWarning(RGBDSENSOR_NWC_ROS2) << "getDepthAccuracy not supported";
299-
return 0;
313+
return ReturnValue::return_code::return_value_error_not_implemented_by_device;
300314
}
301315

302-
bool RgbdSensor_nwc_ros2::getDepthClipPlanes(double& nearPlane, double& farPlane)
316+
yarp::dev::ReturnValue RgbdSensor_nwc_ros2::getDepthClipPlanes(double& nearPlane, double& farPlane)
303317
{
304318
yCWarning(RGBDSENSOR_NWC_ROS2) << "getDepthClipPlanes not supported";
305-
return false;
319+
return ReturnValue::return_code::return_value_error_not_implemented_by_device;
306320
}
307321

308-
bool RgbdSensor_nwc_ros2::setDepthClipPlanes(double nearPlane, double farPlane)
322+
yarp::dev::ReturnValue RgbdSensor_nwc_ros2::setDepthClipPlanes(double nearPlane, double farPlane)
309323
{
310324
yCWarning(RGBDSENSOR_NWC_ROS2) << "setDepthClipPlanes not supported";
311-
return false;
325+
return ReturnValue::return_code::return_value_error_not_implemented_by_device;
312326
}
313327

314-
bool RgbdSensor_nwc_ros2::getDepthMirroring(bool& mirror)
328+
yarp::dev::ReturnValue RgbdSensor_nwc_ros2::getDepthMirroring(bool& mirror)
315329
{
316330
yCWarning(RGBDSENSOR_NWC_ROS2) << "getDepthMirroring not supported";
317-
return false;
331+
return ReturnValue::return_code::return_value_error_not_implemented_by_device;
318332
}
319333

320-
bool RgbdSensor_nwc_ros2::setDepthMirroring(bool mirror)
334+
yarp::dev::ReturnValue RgbdSensor_nwc_ros2::setDepthMirroring(bool mirror)
321335
{
322336
yCWarning(RGBDSENSOR_NWC_ROS2) << "setDepthMirroring not supported";
323-
return false;
337+
return ReturnValue::return_code::return_value_error_not_implemented_by_device;
324338
}
325339

326-
bool RgbdSensor_nwc_ros2::getExtrinsicParam(yarp::sig::Matrix& extrinsic)
340+
yarp::dev::ReturnValue RgbdSensor_nwc_ros2::getExtrinsicParam(yarp::sig::Matrix& extrinsic)
327341
{
328342
yCWarning(RGBDSENSOR_NWC_ROS2) << "getExtrinsicParam not supported";
329-
return false;
343+
return ReturnValue::return_code::return_value_error_not_implemented_by_device;
330344
}
331345

332346

333347
/*
334348
* TODO FIXME to update with header instead of stamp
335349
*/
336-
bool RgbdSensor_nwc_ros2::getRgbImage(yarp::sig::FlexImage& rgb_image, yarp::os::Stamp* rgb_image_stamp)
350+
yarp::dev::ReturnValue RgbdSensor_nwc_ros2::getRgbImage(yarp::sig::FlexImage& rgb_image, yarp::os::Stamp* rgb_image_stamp)
337351
{
338352
std::lock_guard<std::mutex> guard_rgb_image(m_rgb_image_mutex);
339353
std::lock_guard<std::mutex> guard_rgb_camera_info(m_rgb_camera_info_mutex);
@@ -361,13 +375,14 @@ bool RgbdSensor_nwc_ros2::getRgbImage(yarp::sig::FlexImage& rgb_image, yarp::os:
361375
{
362376
yCError(RGBDSENSOR_NWC_ROS2, "missing rgb image");
363377
}
364-
return rgb_ok;
378+
if (rgb_ok) return ReturnValue_ok;
379+
return ReturnValue::return_code::return_value_error_not_ready;
365380
}
366381

367382
/*
368383
* TODO FIXME to update with header instead of stamp
369384
*/
370-
bool RgbdSensor_nwc_ros2::getDepthImage(depthImage& depth_image, yarp::os::Stamp* depth_image_stamp)
385+
yarp::dev::ReturnValue RgbdSensor_nwc_ros2::getDepthImage(depthImage& depth_image, yarp::os::Stamp* depth_image_stamp)
371386
{
372387
std::lock_guard<std::mutex> guard_depth_image(m_depth_image_mutex);
373388
std::lock_guard<std::mutex> guard_depth_camera_info(m_depth_camera_info_mutex);
@@ -396,28 +411,34 @@ bool RgbdSensor_nwc_ros2::getDepthImage(depthImage& depth_image, yarp::os::Stamp
396411
{
397412
yCError(RGBDSENSOR_NWC_ROS2, "missing depth image");
398413
}
399-
return depth_ok;
414+
415+
if (depth_ok) return ReturnValue_ok;
416+
return ReturnValue::return_code::return_value_error_not_ready;
400417
}
401418
/*
402419
* TODO FIXME to update with header instead of stamp
403420
*/
404-
bool RgbdSensor_nwc_ros2::getImages(yarp::sig::FlexImage& rgb_image, depthImage& depth_image, yarp::os::Stamp* rgb_image_stamp, yarp::os::Stamp* depth_image_stamp)
421+
yarp::dev::ReturnValue RgbdSensor_nwc_ros2::getImages(yarp::sig::FlexImage& rgb_image, depthImage& depth_image, yarp::os::Stamp* rgb_image_stamp, yarp::os::Stamp* depth_image_stamp)
405422
{
406423
bool rgb_ok, depth_ok;
407424
rgb_ok = getRgbImage(rgb_image, rgb_image_stamp);
408425
depth_ok = getDepthImage(depth_image, depth_image_stamp);
409-
return (rgb_ok && depth_ok);
426+
427+
if (rgb_ok && depth_ok) return ReturnValue_ok;
428+
return ReturnValue::return_code::return_value_error_not_ready;
410429
}
411430

412431

413-
RgbdSensor_nwc_ros2::RGBDSensor_status RgbdSensor_nwc_ros2::getSensorStatus()
432+
yarp::dev::ReturnValue RgbdSensor_nwc_ros2::getSensorStatus(yarp::dev::IRGBDSensor::RGBDSensor_status& status)
414433
{
415-
return RGBD_SENSOR_OK_IN_USE;
434+
status=RGBD_SENSOR_OK_IN_USE;
435+
return ReturnValue_ok;
416436
}
417437

418438

419-
std::string RgbdSensor_nwc_ros2::getLastErrorMsg(yarp::os::Stamp* timeStamp)
439+
yarp::dev::ReturnValue RgbdSensor_nwc_ros2::getLastErrorMsg(std::string& message, yarp::os::Stamp* timeStamp)
420440
{
421441
yCError(RGBDSENSOR_NWC_ROS2) << "get last error not yet implemented";
422-
return "get last error not yet implemented";
442+
message = "get last error not yet implemented";
443+
return ReturnValue::return_code::return_value_error_not_implemented_by_device;
423444
}

‎src/devices/rgbdSensor_nwc_ros2/RgbdSensor_nwc_ros2.h‎

Lines changed: 28 additions & 27 deletions
Original file line numberDiff line numberDiff line change
@@ -131,33 +131,34 @@ class RgbdSensor_nwc_ros2:
131131
// IRGBDSensor
132132
int getRgbHeight() override;
133133
int getRgbWidth() override;
134-
bool getRgbSupportedConfigurations(yarp::sig::VectorOf<yarp::dev::CameraConfig> &configurations) override;
135-
bool getRgbResolution(int &width, int &height) override;
136-
bool setRgbResolution(int width, int height) override;
137-
bool getRgbFOV(double& horizontalFov, double& verticalFov) override;
138-
bool setRgbFOV(double horizontalFov, double verticalFov) override;
139-
bool getRgbMirroring(bool& mirror) override;
140-
bool setRgbMirroring(bool mirror) override;
141-
142-
bool getRgbIntrinsicParam(yarp::os::Property& intrinsic) override;
134+
yarp::dev::ReturnValue getRgbSupportedConfigurations(std::vector<yarp::dev::CameraConfig> &configurations) override;
135+
yarp::dev::ReturnValue getRgbResolution(int &width, int &height) override;
136+
yarp::dev::ReturnValue setRgbResolution(int width, int height) override;
137+
yarp::dev::ReturnValue getRgbFOV(double& horizontalFov, double& verticalFov) override;
138+
yarp::dev::ReturnValue setRgbFOV(double horizontalFov, double verticalFov) override;
139+
yarp::dev::ReturnValue getRgbMirroring(bool& mirror) override;
140+
yarp::dev::ReturnValue setRgbMirroring(bool mirror) override;
141+
142+
yarp::dev::ReturnValue getRgbIntrinsicParam(yarp::os::Property& intrinsic) override;
143143
int getDepthHeight() override;
144144
int getDepthWidth() override;
145-
bool setDepthResolution(int width, int height) override;
146-
bool getDepthFOV(double& horizontalFov, double& verticalFov) override;
147-
bool setDepthFOV(double horizontalFov, double verticalFov) override;
148-
bool getDepthIntrinsicParam(yarp::os::Property& intrinsic) override;
149-
double getDepthAccuracy() override;
150-
bool setDepthAccuracy(double accuracy) override;
151-
bool getDepthClipPlanes(double& nearPlane, double& farPlane) override;
152-
bool setDepthClipPlanes(double nearPlane, double farPlane) override;
153-
bool getDepthMirroring(bool& mirror) override;
154-
bool setDepthMirroring(bool mirror) override;
155-
156-
bool getExtrinsicParam(yarp::sig::Matrix &extrinsic) override;
157-
158-
bool getRgbImage(yarp::sig::FlexImage& rgb_image, yarp::os::Stamp* rgb_image_stamp = NULL) override;
159-
bool getDepthImage(depthImage& depth_image, yarp::os::Stamp* depth_image_stamp = nullptr) override;
160-
bool getImages(yarp::sig::FlexImage& rgb_image, depthImage& depth_image, yarp::os::Stamp* rgb_image_stamp=nullptr, yarp::os::Stamp* depth_image_stamp=nullptr) override;
145+
yarp::dev::ReturnValue getDepthResolution(int& width, int& height) override;
146+
yarp::dev::ReturnValue setDepthResolution(int width, int height) override;
147+
yarp::dev::ReturnValue getDepthFOV(double& horizontalFov, double& verticalFov) override;
148+
yarp::dev::ReturnValue setDepthFOV(double horizontalFov, double verticalFov) override;
149+
yarp::dev::ReturnValue getDepthIntrinsicParam(yarp::os::Property& intrinsic) override;
150+
yarp::dev::ReturnValue getDepthAccuracy(double& accuracy) override;
151+
yarp::dev::ReturnValue setDepthAccuracy(double accuracy) override;
152+
yarp::dev::ReturnValue getDepthClipPlanes(double& nearPlane, double& farPlane) override;
153+
yarp::dev::ReturnValue setDepthClipPlanes(double nearPlane, double farPlane) override;
154+
yarp::dev::ReturnValue getDepthMirroring(bool& mirror) override;
155+
yarp::dev::ReturnValue setDepthMirroring(bool mirror) override;
156+
157+
yarp::dev::ReturnValue getExtrinsicParam(yarp::sig::Matrix &extrinsic) override;
158+
159+
yarp::dev::ReturnValue getRgbImage(yarp::sig::FlexImage& rgb_image, yarp::os::Stamp* rgb_image_stamp = NULL) override;
160+
yarp::dev::ReturnValue getDepthImage(depthImage& depth_image, yarp::os::Stamp* depth_image_stamp = nullptr) override;
161+
yarp::dev::ReturnValue getImages(yarp::sig::FlexImage& rgb_image, depthImage& depth_image, yarp::os::Stamp* rgb_image_stamp=nullptr, yarp::os::Stamp* depth_image_stamp=nullptr) override;
161162

162163
void callback(sensor_msgs::msg::CameraInfo::SharedPtr msg, std::string topic);
163164
void callback(sensor_msgs::msg::Image::SharedPtr msg, std::string topic);
@@ -167,8 +168,8 @@ class RgbdSensor_nwc_ros2:
167168
void color_raw_callback(const sensor_msgs::msg::Image::SharedPtr msg);
168169
void color_info_callback(const sensor_msgs::msg::CameraInfo::SharedPtr msg);
169170

170-
yarp::dev::IRGBDSensor::RGBDSensor_status getSensorStatus() override;
171-
std::string getLastErrorMsg(yarp::os::Stamp* timeStamp = NULL) override;
171+
yarp::dev::ReturnValue getSensorStatus(yarp::dev::IRGBDSensor::RGBDSensor_status& status) override;
172+
yarp::dev::ReturnValue getLastErrorMsg(std::string& message, yarp::os::Stamp* timeStamp = NULL) override;
172173
};
173174

174175

‎src/devices/rgbdSensor_nws_ros2/RgbdSensor_nws_ros2.cpp‎

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -155,7 +155,9 @@ void RgbdSensor_nws_ros2::run()
155155
{
156156
if (sensor_p!=nullptr) {
157157
static int i = 0;
158-
switch (sensor_p->getSensorStatus()) {
158+
yarp::dev::IRGBDSensor::RGBDSensor_status status;
159+
yarp::dev::ReturnValue ret = sensor_p->getSensorStatus(status);
160+
switch (status) {
159161
case(yarp::dev::IRGBDSensor::RGBD_SENSOR_OK_IN_USE) :
160162
if (!writeData()) {
161163
yCError(RGBDSENSOR_NWS_ROS2, "Image not captured.. check hardware configuration");

0 commit comments

Comments
 (0)