Add Laserscan support

[ajsampathk]

Issue link: https://github.com/RobotecAI/RGLGazeboPlugin/issues/26

Testing

Tested on Ubuntu 22.04.

• Follow normal build and install instructions
• Modify the test_world/rgl_playground.sdf.
• Add <publish_laserscan>true</publish_laserscan> in the plugin element here
• Replace the pattern type of the lidar in rgl_playground to the following

  <pattern_lidar2d>
  <horizontal>
    <samples>1800</samples>
    <min_angle>-3.14159</min_angle>
    <max_angle>3.14159</max_angle>
  </horizontal>
  </pattern_lidar2d>

• Run the demo and try ign topic -i -t /rgl_lidar You should see the message is ignition.msgs.LaserScan
• Optionally you can run the ros2_gz_bridge to verify in RViz as well.

Additions

• LoadLaserScanParams function to verify and parse field info for laserscan msg
• LoadPatternFromLidar2d function for creating a 2D lidar
• CreateLaserScanMsg funciton to create the laserscan message
• <publish_laserscan> param check logic in LoadConfiguration and publishLaserScan flag to decide which type of message to advertise and publish

• Documentation in Readme

  Modifications

• [MAJOR] Changed API result node from rglNodeCompact to rglNodeYield to be able to use rgl_node_points_yield instead of rgl_node_points_compact. This allows to get the range values by adding RGL_FIELD_DISTANCE_F32 to the result fields alongside RGL_FIELD_XYZ_F32 for the pointcloud

  Notes

• The LaserScan messages need to have an intensity or the ros2_gz_bridge will not be happy and seg faults. I have set a static 100.0 value to all the intensity value here but we can easily get that from the API as well with the rgl_node_points_yield API call. However, this relates to https://github.com/RobotecAI/RGLGazeboPlugin/issues/15, So I have excluded adding the RGL_FILED_INTENSITY_F32 to the yieldFields to obtain the intensity value here. (will be happy to come up with a PR for this as well if there is nothing planned for it yet)
• I have restricted the usage of the publish_laserscan param to only unifrom 2D patterns, but can extend to pattern_custom if requested (not very familiar with that option)

[msz-rai]

@ajsampathk

The LaserScan messages need to have an intensity or the ros2_gz_bridge will not be happy and seg faults. I have set a static 100.0 value to all the intensity value here but we can easily get that from the API as well with the rgl_node_points_yield API call.

Thank you for noticing it. This is fine for now :+1:

However, this relates to Request to add "intensity" field to the PointCloud2 message #15, So I have excluded adding the RGL_FILED_INTENSITY_F32 to the yieldFields to obtain the intensity value here. (will be happy to come up with a PR for this as well if there is nothing planned for it yet)

We haven't planned this one shortly. We will be always happy for the contribution!

[ajsampathk]

@msz-rai Sorry about the delay, but I think I managed to apply most of the suggestions.

One thing regarding:

These values could be also retrieved from the lidarPattern container:

• scanHMin, scanHMax - the yaw angle of the first and the last ray in the container (it is needed to convert rgl_mat3x4f to angles, see AnglesToRglMat3x4f for reference).
• scanHSamples - the size of the container.

It was a cool idea which I tried out, which you can check in commit ddd926c, and ultimately decided to only do this for scanHSamples, My reasoning is this:

• Extracting the min, max angles from the pattern is a computationally heavy process (and also feels a bit hacky for me).
• Which means doing it at the time of creating the message is inefficient.
• But if we decide to do this ONCE during configuration, we might as well avoid the calculations altogether and get it from the SDF.

The number of samples however is a straightforward and is a good substitute to extracting it from SDF.

Let me know what you think! Ready for review once again.

Thanks

[msz-rai]

@ajsampathk Would you like to add something more to this PR or should I merge it now?

[ajsampathk]

@msz-rai Just working on the publishMessageType to remove the sdf->HasElement inside the configuration function. But it can honestly go in through a different PR.

I'm okay merging this if you are!