Implement plugin for laser sensor

[traversaro]

Gazebo Modern equivalent https://github.com/robotology/gazebo-yarp-plugins/tree/master/plugins/lasersensor .

[lucapa17]

As usual, at first we try to launch a simple .sdf model containing the sensor (in this case a lidar sensor) to visualize its values using the command "gz topic". However in this case we don't understand which System Plugin is suitable for the lidar sensor.

For example in forcetorque sdf file we had:

    <plugin
      filename="gz-sim-forcetorque-system"
      name="gz::sim::systems::ForceTorque">
    </plugin>

and for IMU we had:

    <plugin
      filename="gz-sim-imu-system"
      name="gz::sim::systems::Imu">
    </plugin>

[lucapa17]

We should have solved this problem using gpu_lidar sensor instead of lidar sensor and using Sensor System Plugin. Now we proceed with the implementation of the plugin.

[GiacomoBisio]

We added a new pull request for the Laser sensor plugin (#19). The output of the command yarp read /read /laser is the following:

0.0 360.0 0.100000000000000005551 5.0 (1.11125278472900390625 1.10634744167327880859 1.10667657852172851562 1.11030185222625732422 1.10864961147308349609 1.12173914909362792969 1.11585438251495361328 1.11526703834533691406 1.12902033329010009766 1.12489259243011474609 1.12953507900238037109 1.13095843791961669922 1.13082158565521240234 1.13813734054565429688 1.15377604961395263672 1.14405727386474609375 1.15732824802398681641 1.15743625164031982422 1.16749060153961181641 1.19045126438140869141 1.18509876728057861328 1.19290518760681152344 1.19264888763427734375 1.21191620826721191406 1.21245372295379638672 inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf inf 1.21789622306823730469 1.19324958324432373047 1.19849312305450439453 1.18799138069152832031 1.178630828857421875 1.16958940029144287109 1.16730511188507080078 1.15926289558410644531 1.14865171909332275391 1.14918661117553710938 1.13976693153381347656 1.13293814659118652344 1.13386213779449462891 1.13239908218383789062 1.12463426589965820312 1.11819624900817871094 1.11554396152496337891 1.11691248416900634766 1.1094226837158203125 1.11155080795288085938 1.11591613292694091797 1.11988699436187744141 1.11187469959259033203 1.12193655967712402344) 2

In particular, we have different values:

• angle_min: 0.0
• angle_max: 360.0
• range_min: 0.100000000000000005551
• range_max: 5.0
• ranges values (360 values between round brackets)
• device status: 2

It seems to work properly, but we noticed that the value of "device_status" is set to 2. We saw that this value means DEVICE_GENERAL_ERROR. We don't know if it's something relevant or not, because also the output of gz sim model.sdf seems okay:

[INFO] GazeboYarpLaser: configuration of sensor  laser_sensor  loaded from yarpConfigurationFile :  laser.ini 
[DEBUG] |yarp.dev.PolyDriver|gazebo_laser| Parameters are (SENSOR (max_distance 5.0) (min_distance 0.100000000000000005551) (max_angle 360) (min_angle 0) (resolution 1) (allow_infinity 1)) (device gazebo_laser) (parentLinkName link_1) (sensorName laser_sensor) (sensorScopedName "model/laser_model/link/link_1/sensor/laser_sensor") (sensor_name laser_sensor) (yarpDeviceName laser_plugin_device)
[INFO] |yarp.devices.Lidar2DDeviceBase| Using the following parameters:
[INFO] |yarp.devices.Lidar2DDeviceBase| max_dist: 5  min_dist: 0.1
[INFO] |yarp.devices.Lidar2DDeviceBase| max_angle: 360  min_angle: 0
[INFO] |yarp.devices.Lidar2DDeviceBase| resolution: 1
[INFO] |yarp.devices.Lidar2DDeviceBase| sensors: 360
[INFO] |yarp.devices.Lidar2DDeviceBase| allow_infinity: true
[INFO] |yarp.dev.PolyDriver|gazebo_laser| Created device <gazebo_laser>. See C++ class GazeboYarpIMUDriver for documentation.
[INFO] Registered YARP device with instance name: model/laser_model/link/link_1/sensor/laser_sensor/laser_plugin_device
[DEBUG] Reading file laser_nws.xml
[DEBUG] yarprobotinterface: using xml parser for DTD v3.x
[DEBUG] Reading file laser_nws.xml
[DEBUG] Preprocessor complete in:  9.05991e-06 s
[INFO] startup phase starting...
[INFO] Opening device laser_nws_yarp with parameters [("robotName" = "laser"), ("name" = "/laser"), ("period" = "0.033333")]
[DEBUG] |yarp.dev.PolyDriver|laser_nws_yarp| Parameters are (device rangefinder2D_nws_yarp) (id laser_nws_yarp) (name "/laser") (period 0.033333000000000001517) (robotName laser)
[INFO] |yarp.os.Port|/laser| Port /laser active at tcp://192.168.235.180:10002/
[INFO] |yarp.os.Port|/laser/rpc:i| Port /laser/rpc:i active at tcp://192.168.235.180:10003/
[INFO] |yarp.dev.PolyDriver|laser_nws_yarp| Created wrapper <rangefinder2D_nws_yarp>. See C++ class Rangefinder2D_nws_yarp for documentation.
[INFO] Entering action level 5 of phase startup
[INFO] Executing attach action, level 5 on device laser_nws_yarp with parameters [("device" = "laser_plugin_device")]
[INFO] All actions for action level 5 of startup phase started. Waiting for unfinished actions.
[INFO] All actions for action level 5 of startup phase finished.
[INFO] startup phase finished.

[traversaro]

It seems to work properly, but we noticed that the value of "device_status" is set to 2. We saw that this value means DEVICE_GENERAL_ERROR.

That value is returned by the getDeviceStatus method: https://github.com/robotology/yarp/blob/0f9c9e63655107e26b3ce1a122c98b65612b4cc3/src/devices/Rangefinder2D_nws_yarp/Rangefinder2D_nws_yarp.cpp#L383 , and it is implemented in the Lidar2DDeviceBase base class: https://github.com/robotology/yarp/blob/0f9c9e63655107e26b3ce1a122c98b65612b4cc3/src/libYARP_dev/src/yarp/dev/Lidar2DDeviceBase.cpp#L101 . Perhaps we should just set the m_device_status attribute to the correct value?

[lucapa17]

Perhaps we should just set the m_device_status attribute to the correct value?

Okay thank you! We should have solved (1ec327fbf9761ad0e4b2e6294b458af56bb7ae86).

[lucapa17]

3969e6e9e29aa17139922069c9dade2ec024a05b