Merge pull request #488 from robotology/lidar_base_refactor

laser sensors plugins refactored
robotology · Jun 3, 2020 · 54c5022 · 54c5022
2 parents 81f7dc1 + 392c3b5
commit 54c5022
Show file tree

Hide file tree

Showing 3 changed files with 56 additions and 281 deletions.
diff --git a/plugins/doublelaser/src/DoubleLaser.cc b/plugins/doublelaser/src/DoubleLaser.cc
@@ -173,7 +173,6 @@ GZ_REGISTER_MODEL_PLUGIN(GazeboYarpDoubleLaser)
             yError() << "GazeboYarpDoubleLaser: LASERFRONT-CFG group is missing in configuration file";
             return;
         }
-
         doublelaser_dev_parameters.addGroup("LASERFRONT-CFG").fromString(m_parameters.findGroup("LASERFRONT-CFG").toString());
 
         if(!m_parameters.check("LASERBACK-CFG"))
@@ -182,7 +181,10 @@ GZ_REGISTER_MODEL_PLUGIN(GazeboYarpDoubleLaser)
             return;
         }
         doublelaser_dev_parameters.addGroup("LASERBACK-CFG").fromString(m_parameters.findGroup("LASERBACK-CFG").toString());
-
+
+        if(m_parameters.check("SENSOR")) {doublelaser_dev_parameters.addGroup("SENSOR").fromString(m_parameters.findGroup("SENSOR").toString());}
+        if(m_parameters.check("SKIP"))   {doublelaser_dev_parameters.addGroup("SKIP").fromString(m_parameters.findGroup("SKIP").toString());}
+
         if(!m_driver_doublelaser.open(doublelaser_dev_parameters) )
         {
             yError()<<"GazeboYarpDoubleLaser: error opening DoubleLaser yarp device ";

diff --git a/plugins/lasersensor/include/yarp/dev/LaserSensorDriver.h b/plugins/lasersensor/include/yarp/dev/LaserSensorDriver.h
@@ -10,6 +10,7 @@
 #include <yarp/dev/DeviceDriver.h>
 #include <yarp/dev/IRangefinder2D.h>
 #include <yarp/dev/LaserMeasurementData.h>
+#include <yarp/dev/Lidar2DDeviceBase.h>
 #include <yarp/os/Stamp.h>
 #include <yarp/dev/IPreciselyTimed.h>
 #include <boost/shared_ptr.hpp>
@@ -37,16 +38,10 @@ namespace gazebo {
     }
 }
 
-struct Range_t
-{
-    double min;
-    double max;
-};
-
 extern const std::string YarpLaserSensorScopedName;
 
 class yarp::dev::GazeboYarpLaserSensorDriver: 
-    public yarp::dev::IRangefinder2D,
+    public yarp::dev::Lidar2DDeviceBase,
     public yarp::dev::IPreciselyTimed,
     public yarp::dev::DeviceDriver
 {
@@ -61,49 +56,30 @@ class yarp::dev::GazeboYarpLaserSensorDriver:
      */
 
     //DEVICE DRIVER
-    virtual bool open(yarp::os::Searchable& config);
-    virtual bool close();
-
-    //ANALOG SENSOR
-    virtual bool getLaserMeasurement (std::vector<yarp::dev::LaserMeasurementData> &data);
-    virtual bool getRawData (yarp::sig::Vector &data);
-    virtual bool getDeviceStatus (Device_status &status);
-    virtual bool getDistanceRange (double &min, double &max);
-    virtual bool setDistanceRange (double min, double max);
-    virtual bool getScanLimits (double &min, double &max);
-    virtual bool setScanLimits (double min, double max);
-    virtual bool getHorizontalResolution (double &step);
-    virtual bool setHorizontalResolution (double step);
-    virtual bool getScanRate (double &rate);
-    virtual bool setScanRate (double rate);
-    virtual bool getDeviceInfo (std::string &device_info);
+    virtual bool open(yarp::os::Searchable& config) override;
+    virtual bool close() override;
+
+    //IRangefinder2D
+    virtual bool setDistanceRange (double min, double max) override;
+    virtual bool setScanLimits (double min, double max) override;
+    virtual bool setHorizontalResolution (double step) override;
+    virtual bool setScanRate (double rate) override;
 
     //PRECISELY TIMED
-    virtual yarp::os::Stamp getLastInputStamp();
+    virtual yarp::os::Stamp getLastInputStamp() override;
 
 
 private:
-    double m_max_angle; 
-    double m_min_angle; 
-    double m_max_clip_range; 
-    double m_min_clip_range; 
-    double m_max_discard_range; 
-    double m_min_discard_range;
-    double m_max_gazebo_range; 
-    double m_min_gazebo_range;
-    double m_samples;  
-    double m_resolution;
-    double m_rate; 
-    bool   m_enable_clip_range;
-    bool   m_enable_discard_range;
+    double m_gazebo_max_angle;
+    double m_gazebo_min_angle;
+    double m_gazebo_max_range;
+    double m_gazebo_min_range;
+    double m_gazebo_resolution;
+    size_t m_gazebo_samples;
+    double m_gazebo_scan_rate;
     bool   m_first_run;
 
-    Device_status m_device_status;
-    std::vector <Range_t> range_skip_vector;
-
-    std::vector<double> m_sensorData; //buffer for laser data
     yarp::os::Stamp m_lastTimestamp; //buffer for last timestamp data
-    std::mutex m_mutex; //mutex for accessing the data
     gazebo::sensors::RaySensor* m_parentSensor;
     gazebo::event::ConnectionPtr m_updateConnection;