Merge pull request #3132 from randaz81/Rangefinder_refactor

Rangefinder2D devices refactored

doc/release/master.md

@@ -75,6 +75,15 @@ New Features
 * Added followPath functionality
 * Added paths as possible arguments in gotoLocation in Navigation2D_nwc
 
+#### Rangefinder2DTransformer
+
+* Removed deprecated device Rangerfinder2DClient
+* Added device Rangerfinder2DTransformer, with similar functionalities to Rangerfinder2DClient.
+
+#### Rangefinder2D_nwc_yarp, Rangefinder2D_nws_yarp
+
+* Network protocol now uses IDL thrift
+
 ### GUIs
 
 #### yarpopencvdisplay

src/devices/CMakeLists.txt

@@ -39,6 +39,7 @@ yarp_begin_plugin_library(yarpmod
   add_subdirectory(SDLJoypad)
   add_subdirectory(upowerBattery)
   add_subdirectory(multipleanalogsensorsremapper)
+  add_subdirectory(Rangefinder2DTransformer)
   add_subdirectory(map2DStorage)
   add_subdirectory(usbCamera)
   add_subdirectory(controlBoardCouplingHandler)

src/devices/networkWrappers/Rangefinder2DClient/CMakeLists.txt → src/devices/Rangefinder2DTransformer/CMakeLists.txt

@@ -1,24 +1,27 @@
 # SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT)
 # SPDX-License-Identifier: BSD-3-Clause
 
-yarp_prepare_plugin(Rangefinder2DClient
+yarp_prepare_plugin(rangefinder2DTransformer
   CATEGORY device
-  TYPE Rangefinder2DClient
-  INCLUDE Rangefinder2DClient.h
+  TYPE Rangefinder2DTransformer
+  INCLUDE Rangefinder2DTransformer.h
   DEPENDS "TARGET YARP::YARP_math"
+  GENERATE_PARSER
   DEFAULT ON
 )
 
-if(NOT SKIP_Rangefinder2DClient)
-  yarp_add_plugin(yarp_Rangefinder2DClient)
+if(NOT SKIP_rangefinder2DTransformer)
+  yarp_add_plugin(yarp_rangefinder2DTransformer)
 
-  target_sources(yarp_Rangefinder2DClient
+  target_sources(yarp_rangefinder2DTransformer
     PRIVATE
-      Rangefinder2DClient.cpp
-      Rangefinder2DClient.h
+      Rangefinder2DTransformer.cpp
+      Rangefinder2DTransformer.h
+      Rangefinder2DTransformer_ParamsParser.cpp
+      Rangefinder2DTransformer_ParamsParser.h
   )
 
-  target_link_libraries(yarp_Rangefinder2DClient
+  target_link_libraries(yarp_rangefinder2DTransformer
     PRIVATE
       YARP::YARP_os
       YARP::YARP_sig
@@ -33,7 +36,7 @@ if(NOT SKIP_Rangefinder2DClient)
   )
 
   yarp_install(
-    TARGETS yarp_Rangefinder2DClient
+    TARGETS yarp_rangefinder2DTransformer
     EXPORT YARP_${YARP_PLUGIN_MASTER}
     COMPONENT ${YARP_PLUGIN_MASTER}
     LIBRARY DESTINATION ${YARP_DYNAMIC_PLUGINS_INSTALL_DIR}
@@ -43,7 +46,7 @@ if(NOT SKIP_Rangefinder2DClient)
 
   set(YARP_${YARP_PLUGIN_MASTER}_PRIVATE_DEPS ${YARP_${YARP_PLUGIN_MASTER}_PRIVATE_DEPS} PARENT_SCOPE)
 
-  set_property(TARGET yarp_Rangefinder2DClient PROPERTY FOLDER "Plugins/Device")
+  set_property(TARGET yarp_rangefinder2DTransformer PROPERTY FOLDER "Plugins/Device")
 
   if(YARP_COMPILE_TESTS)
     add_subdirectory(tests)

src/devices/Rangefinder2DTransformer/Rangefinder2DTransformer.cpp

@@ -0,0 +1,218 @@
+/*
+ * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT)
+ * SPDX-License-Identifier: LGPL-2.1-or-later
+ */
+
+#define _USE_MATH_DEFINES
+
+#include "Rangefinder2DTransformer.h"
+
+#include <yarp/os/Log.h>
+#include <yarp/os/LogStream.h>
+#include <yarp/dev/IFrameTransform.h>
+#include <yarp/math/Math.h>
+#include <yarp/math/FrameTransform.h>
+
+#include <limits>
+#include <cmath>
+
+using namespace yarp::dev;
+using namespace yarp::os;
+using namespace yarp::sig;
+
+#ifndef DEG2RAD
+#define DEG2RAD M_PI/180.0
+#endif
+
+namespace {
+YARP_LOG_COMPONENT(RANGEFINDER2DTRANSFORMER, "yarp.device.Rangefinder2DTransformer")
+}
+
+bool Rangefinder2DTransformer::open(yarp::os::Searchable& config)
+{
+    if (!parseParams(config))
+    {
+        return false;
+    }
+
+    if (!m_laser_frame_name.empty() && !m_robot_frame_name.empty())
+    {
+        // get the position of the device, if it is available
+        auto* drv = new yarp::dev::PolyDriver;
+        Property TransformClientOptions;
+        TransformClientOptions.put("device", "frameTransformClient");
+        TransformClientOptions.put("local", "/rangefinder2DTransformClient");
+        TransformClientOptions.put("remote", "/transformServer");
+        TransformClientOptions.put("period", 0.010);
+        bool b_canOpenTransformClient = drv->open(TransformClientOptions);
+        if (b_canOpenTransformClient)
+        {
+            yarp::dev::IFrameTransform* iTrf = nullptr;
+            drv->view(iTrf);
+            if (!iTrf)
+            {
+                yCError(RANGEFINDER2DTRANSFORMER) << "A Problem occurred while trying to view the IFrameTransform interface";
+                drv->close();
+                delete drv;
+                return false;
+            }
+
+            yarp::sig::Matrix mat;
+            iTrf->getTransform(m_laser_frame_name, m_robot_frame_name, mat);
+            yarp::sig::Vector v = yarp::math::dcm2rpy(mat);
+            m_device_position_x = mat[0][3];
+            m_device_position_y = mat[1][3];
+            m_device_position_theta = v[2];
+            if (fabs(v[0]) < 1e-6 && fabs(v[1]) < 1e-6)
+            {
+                yCError(RANGEFINDER2DTRANSFORMER) << "Laser device is not planar";
+                return false;
+            }
+            yCInfo(RANGEFINDER2DTRANSFORMER) << "Position information obtained from frametransform server (x,y,t):" << m_device_position_x << " " << m_device_position_y << " " << m_device_position_theta;
+            drv->close();
+            delete drv;
+        }
+    }
+    else
+    {
+        yCInfo(RANGEFINDER2DTRANSFORMER) << "Position information obtained from configuration parameters (x,y,t):" << m_device_position_x << " " << m_device_position_y << " " << m_device_position_theta;
+    }
+
+    return true;
+}
+
+bool Rangefinder2DTransformer::close()
+{
+    return true;
+}
+
+bool Rangefinder2DTransformer::getRawData(yarp::sig::Vector& data, double* timestamp)
+{
+    std::vector<LaserMeasurementData> scans;
+    double lastTs;
+
+    bool ret = sens_p->getLaserMeasurement(scans, &lastTs);
+    if (!ret) {
+        return false;
+    }
+
+    if (timestamp != nullptr)
+    {
+        *timestamp = lastTs;
+    }
+    return true;
+}
+
+bool Rangefinder2DTransformer::getLaserMeasurement(std::vector<LaserMeasurementData>& data, double* timestamp)
+{
+    std::vector<LaserMeasurementData> scans;
+    double lastTs;
+
+    bool ret = sens_p->getLaserMeasurement(scans,&lastTs);
+    if (!ret)
+    {
+        return false;
+    }
+
+    size_t size = scans.size();
+    data.resize(size);
+    if (m_scan_angle_max < m_scan_angle_min)
+    {
+        yCError(RANGEFINDER2DTRANSFORMER) << "getLaserMeasurement failed";
+        return false;
+    }
+    double laser_angle_of_view = m_scan_angle_max - m_scan_angle_min;
+    for (size_t i = 0; i < size; i++)
+    {
+        double angle = (i / double(size) * laser_angle_of_view + m_device_position_theta + m_scan_angle_min) * DEG2RAD;
+        double rho;
+        double theta;
+        scans[i].get_polar(rho, theta);
+#if 1 //cartesian version is preferable, even if more computationally expensive, since it takes in account device_position
+        data[i].set_cartesian(rho * cos(angle) + m_device_position_x, rho * sin(angle) + m_device_position_y);
+#else
+        data[i].set_polar(rho, theta);
+#endif
+    }
+    if (timestamp!=nullptr)
+    {
+        *timestamp = lastTs;
+    }
+    return true;
+}
+
+bool Rangefinder2DTransformer::getDistanceRange(double& min, double& max)
+{
+    return sens_p->getDistanceRange(min, max);
+}
+
+bool Rangefinder2DTransformer::setDistanceRange(double min, double max)
+{
+    return sens_p->setDistanceRange(min, max);
+}
+
+bool Rangefinder2DTransformer::getScanLimits(double& min, double& max)
+{
+    return sens_p->getScanLimits(min, max);
+}
+
+bool Rangefinder2DTransformer::setScanLimits(double min, double max)
+{
+    return sens_p->setScanLimits(min, max);
+}
+
+bool Rangefinder2DTransformer::getHorizontalResolution(double& step)
+{
+    return sens_p->getHorizontalResolution(step);
+}
+
+bool Rangefinder2DTransformer::setHorizontalResolution(double step)
+{
+    return sens_p->setHorizontalResolution(step);
+}
+
+bool Rangefinder2DTransformer::getScanRate(double& rate)
+{
+    return sens_p->getScanRate(rate);
+}
+
+bool Rangefinder2DTransformer::setScanRate(double rate)
+{
+    return sens_p->setScanRate(rate);
+}
+
+bool Rangefinder2DTransformer::getDeviceStatus(Device_status& status)
+{
+    return sens_p->getDeviceStatus(status);
+}
+
+bool Rangefinder2DTransformer::getDeviceInfo(std::string& device_info)
+{
+    return sens_p->getDeviceInfo(device_info);
+}
+
+bool Rangefinder2DTransformer::attach(yarp::dev::PolyDriver* driver)
+{
+    if (driver->isValid())
+    {
+        driver->view(sens_p);
+        if (!sens_p->getScanLimits(m_scan_angle_min, m_scan_angle_max))
+        {
+            yCError(RANGEFINDER2DTRANSFORMER) << "getScanLimits failed";
+            return false;
+        }
+    }
+
+    if (nullptr == sens_p)
+    {
+        yCError(RANGEFINDER2DTRANSFORMER, "subdevice passed to attach method is invalid");
+        return false;
+    }
+    return true;
+}
+
+bool Rangefinder2DTransformer::detach()
+{
+    sens_p = nullptr;
+    return true;
+}

src/devices/Rangefinder2DTransformer/Rangefinder2DTransformer.h

@@ -0,0 +1,78 @@
+/*
+ * SPDX-FileCopyrightText: 2024-2024 Istituto Italiano di Tecnologia (IIT)
+ * SPDX-License-Identifier: LGPL-2.1-or-later
+ */
+
+#ifndef YARP_DEV_RANGEFINDER2DTRANSFORMER
+#define YARP_DEV_RANGEFINDER2DTRANSFORMER
+
+
+#include <yarp/os/Network.h>
+#include <yarp/os/Stamp.h>
+#include <yarp/os/BufferedPort.h>
+#include <yarp/dev/IRangefinder2D.h>
+#include <yarp/dev/ControlBoardInterfaces.h>
+#include <yarp/dev/ControlBoardHelpers.h>
+#include <yarp/sig/LaserScan2D.h>
+#include <yarp/sig/Vector.h>
+#include <yarp/os/Time.h>
+#include <yarp/dev/PolyDriver.h>
+#include <yarp/dev/WrapperSingle.h>
+
+#include "Rangefinder2DTransformer_ParamsParser.h"
+#include <mutex>
+
+
+#define DEFAULT_THREAD_PERIOD 20 //ms
+const int LASER_TIMEOUT=100; //ms
+
+/**
+* @ingroup dev_impl_wrapper dev_impl_network_lidar
+*
+* \brief `Rangefinder2DTransformer`: A device which acts a virtual laser, it attaches to another lidar,
+* which provides a stream a measurements, and reclocates them in the space. The new origin can be specified
+* both manually, as a 2D point in space, or though a tranform.
+*
+* Parameters required by this device are shown in class: Rangefinder2DTransformer_ParamsParser
+*
+*/
+class Rangefinder2DTransformer:
+        public yarp::dev::DeviceDriver,
+        public yarp::dev::IRangefinder2D,
+        public yarp::dev::WrapperSingle,
+        public Rangefinder2DTransformer_ParamsParser
+{
+protected:
+    // interfaces
+    yarp::dev::IRangefinder2D* sens_p=nullptr;
+
+    //data
+    double m_scan_angle_min = std::nan("1");
+    double m_scan_angle_max = std::nan("1");
+
+public:
+
+    /* DevideDriver methods */
+    bool open(yarp::os::Searchable& config) override;
+    bool close() override;
+
+    /* IRangefinder2D methods */
+    bool getLaserMeasurement(std::vector<yarp::sig::LaserMeasurementData> &data, double* timestamp = nullptr) override;
+    bool getRawData(yarp::sig::Vector &data, double* timestamp = nullptr) override;
+    bool getDeviceStatus(Device_status &status) override;
+    bool getDistanceRange(double& min, double& max) override;
+    bool setDistanceRange(double min, double max) override;
+    bool getScanLimits(double& min, double& max) override;
+    bool setScanLimits(double min, double max) override;
+    bool getHorizontalResolution(double& step) override;
+    bool setHorizontalResolution(double step) override;
+    bool getScanRate(double& rate) override;
+    bool setScanRate(double rate) override;
+    bool getDeviceInfo(std::string &device_info) override;
+
+    /* WrapperSingle  methods */
+    bool attach(yarp::dev::PolyDriver* driver) override;
+    bool detach() override;
+};
+
+#endif // YARP_DEV_RANGEFINDER2DCLIENT_RANGEFINDER2DCLIENT_H