Add 2D/3D Lidar Launch Arg in iris_with_lidar.launch.py

[tejalbarnwal]

Description

This PR addresses #63 and implements the following:

• Adds two new models: iris_with_2d_lidar and iris_with_3d_lidar, which only include the lidar link and sensor.
• Updates iris_with_lidar.launch.py and iris_maze.launch.py to select the appropriate model (2D or 3D) based on a launch argument.

Testing

Modify the lidar_dim argument passed to iris_with_lidar.launch.py in iris_maze.launch.py to either 2 or 3.
Any other value prevents the robot_state_publisher node from launching, leading to:

[rviz2-7] [INFO] [1732732583.933054374] [rviz]: Message Filter dropping message: frame 'base_scan' at time 2.773 for reason 'discarding message because the queue is full'

This occurs because no TF data is published.

Work in Progress

I am working on raising a ValueError if an invalid lidar_dim (other than 2 or 3) is passed. However, a challenge remains: Gazebo launches the iris model even if a ValueError is raised, and the launch process exits in the terminal. Manually killing the Gazebo process (ruby PID) is currently required to stop it. I am actively working on resolving this issue.

Potential Improvements

One concern is ensuring consistency between the model selected via the launch argument and the model referenced in the world files (e.g., iris_maze.sdf, iris_runway.sdf). For instance, iris_maze.sdf references iris_with_lidar and defines its pose in the environment, requiring manual verification.
A possible improvement could involve enabling the independent spawning of the iris model in the world, decoupling it from other Gazebo components, to simplify integration.

[srmainwaring]

Thanks for the PR @tejalbarnwal. I'm wondering whether we can save some duplication and future maintenance overhead by using an <include> element similar to how we do for the iris_with_gimbal model in the ardupilot_gazebo repo.

We would need to create additional models for the 2D and 3D Lidar, each would need to have a canonical link called base_scan to be consistent with the expected TF tree.

    <include merge="true">
      <uri>model://lidar_3d</uri>
      <name>lidar</name>
      <pose degrees="true">0 0 0 0 0 0</pose>    
    </include>

The remainder of the common SDF can be factored out.

We may only need a model expressed for either 2d or 3d, because in the launch file we can use standard python string replacement to substitute the desired model at launch time (using replace on <uri>model://lidar_3d</uri>).

To resolve the consistency concern we should use the gz launch spawn model capability and load the model into the maze at the desired location. There is an example of how to do this for multiple robots here: https://github.com/srmainwaring/ardupilot_gz/blob/prs/pr-multi-vehicle/ardupilot_gz_bringup/launch/robots/iris.launch.py

If you like I can help you rework this to implement the suggestions above.

[Ryanf55]

This is off to a great start! Thanks for spending the time to submit your changes for a 3D Lidar Iris model! This is a nice capability addition that will go well with your bonxai demo.

I agree with Rhys on the requested changes, and can also help if you get stuck.

[tejalbarnwal]

Hi,
I have implemented the requested changes to dynamically replace the <uri>model://lidar_3d</uri> string. Additionally, the launch files now use the ros_gz_sim create executable to spawn the iris_with_lidar model. Please let me know your feedback on the updates.

[tejalbarnwal]

Hi @srmainwaring and @Ryanf55 ,
I would greatly appreciate any feedback on the latest commit I pushed, so we can refine it further for merging the PR at your convenience.

[tejalbarnwal]

Hi @srmainwaring and @Ryanf55 ,
Just checking in, did you get a chance to look at the PR? Let me know if there is anything specific you would like me to address.

[Ryanf55]

Sorry for the very long delay, here's some initial feedback. Once I'm back at my desktop in a few days, I can provide more feedback by testing the changes. Nice improvements!

[tejalbarnwal]

Hi @srmainwaring and @Ryanf55 , I have incorporated the changes you suggested into the code. Could you please review them?

[TannerGilbert]

Any update on this? I would like to build another PR on this but would prefer if this is merged first

[Ryanf55]

I tested this today with 2D and 3D lidar. RVIZ only shows a 2D line of data. Is this expected from only publishing /scan?

To reproduce:

colcon build --packages-up-to ardupilot_gz_bringup
source install/setup.bash
ros2 launch ardupilot_gz_bringup iris_maze.launch.py rviz:=true use_gz_tf:=true lidar_dim:=3

I was hoping with 3D data, we would have a point cloud to vizualize in RVIZ. Perhaps you will be adding this later?

I see that gazebo is publishing both a LaserScan and PointCloudPacked topic:

$ gz topic -i -t /lidar
Publishers [Address, Message Type]:
  tcp://100.93.95.86:41977, gz.msgs.LaserScan
Subscribers [Address, Message Type]:
  tcp://100.93.95.86:34803, gz.msgs.LaserScan
$ gz topic -i -t /lidar/points
Publishers [Address, Message Type]:
  tcp://100.93.95.86:41977, gz.msgs.PointCloudPacked
Subscribers [Address, Message Type]:
  tcp://100.93.95.86:34803, gz.msgs.PointCloudPacked

And the cloud data:

header:
  stamp:
    sec: 273
    nanosec: 300000000
  frame_id: base_scan
height: 1
width: 640
fields: '<sequence type: sensor_msgs/msg/PointField, length: 5>'
is_bigendian: false
point_step: 32
row_step: 20480
data: '<sequence type: uint8, length: 20480>'
is_dense: false

What's interesting is in the SDF, you have the vertical samples set to 60, not 1. But, Gazebo also shows just 2D plane data.

[tejalbarnwal]

I will look into this today and get back to you by tonight

[tejalbarnwal]

Hi,
I've fixed it. I realized I was inadvertently overriding the terminal argument provided by the user with the lidar config specified in iris_maze.launch.py.

[tejalbarnwal]

• Additionally, for the user's convenience, I implemented the option to use a separate config file specifically for the lidar configuration (2D or 3D) rather than loading both ROS and Gazebo topic configurations together in the ros_gz_bridge as it was previously done. I believe the current implementation could be further optimized by using placeholder replacement for the lidar config, but I was concerned that it might reduce the readability of the launch file. I would appreciate your thoughts on this.
• Furthermore, I also added the point cloud representation in the RViz config for better visualization.

[shubham-shahh]

I have tried this PR with the latest changes with KissICP and so far it's working perfectly!

[Ryanf55]

RVIZ works good now, but did you try the Gazebo Point Cloud display plugin? I don't see any points.

[tejalbarnwal]

Hi @Ryanf55 ,
Yes, I get the following warning:

[GUI] [Wrn] [PointCloud.cc:397] Float message and pointcloud are not of the same size, visualization may not be accurate

when I try the Gazebo Point Cloud display.

I noticed that when I try to run gz sim gpu_lidar_sensor.sdf --verbose, I encounter the same warning. I'll take a closer look into this and get back to you soon