Closed traversaro closed 1 year ago
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>
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.
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:
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.
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?
Perhaps we should just set the m_device_status attribute to the correct value?
Okay thank you! We should have solved (1ec327fbf9761ad0e4b2e6294b458af56bb7ae86).
3969e6e9e29aa17139922069c9dade2ec024a05b
Gazebo Modern equivalent https://github.com/robotology/gazebo-yarp-plugins/tree/master/plugins/lasersensor .