[gz-sim] x500_lidar for testing CollisionPrevention

[dakejahl]

Added another x500 model with a lidar attached.
https://app.gazebosim.org/OpenRobotics/fuel/models/Lidar%202d%20v2

Subscribes to gz::msgs::LaserScan and publishes to ObstacleDistance.

[MaEtUgR]

Nice, I definitely want to use this to fix CollisionPrevention. Thanks for looking into the simulation part 🙏

[dirksavage88]

gz_collision_prevention_test.tar.gz

Run this command:
PX4_GZ_WORLD=hall make px4_sitl gz_x500_lidar

[dirksavage88]

Something doesn't look right with the obstacle distance array. Is it supposed to report the max distance if close up to an obstacle? I thought the array bins started at 0 and wrapped around the vehicle CW with bins w/o sensor coverage filled with UINT16_MAX.

Maybe something is wrong with my gz sim collisions...I saw this on depth cam too where it would report the max distance of '1911' (cm) even though it was facing an obstacle that should have taken up the entire 70 degree FOV.

[dakejahl]

@dirksavage88

A value of 0 means that the obstacle is right in front of the sensor. A value of max_distance +1 means no obstacle is present. A value of UINT16_MAX for unknown/not used.

Index 0 in the array corresponds to the first distance measurement which starts at the angle_offset which is 135* in this case, so those up close measurements appear in the middle of the array, they are the small values ~40cm

edit: check obstacle_distance_fused if you want the CollisionPrevention remapped representation. Also check out the QGC overlay to validate the results. You will see that it's correct! That 90* issue with the depth cam model is because it's not converting FLU to FRD -- the array gets reversed

[dirksavage88]

If it is mapped correctly the something else is wrong but with the gz sim collision model itself? Collision prevention did not stop me from hitting the box that was shown in the screenshot and params are matching with what I had for the depth cam model. The depth cam model did work to stop from collision but the array showed some strange range bin values. I’ll check the fused array.

[dakejahl]

The depth cam model did work to stop from collision

Since we know now that the depth cam model is publishing the distances array in reverse order and CP still works it's probably an issue with the CP code itself.

[dirksavage88]

The depth cam model did work to stop from collision

Since we know now that the depth cam model is publishing the distances array in reverse order and CP still works it's probably an issue with the CP code itself.

I’m copying your files into my local branch. I tried building the PR branch on a cloned local of your fork and had issues, so I can’t rule out build issues on my end. I did notice depth cam obstacle map was flipped but thought it was QGC visual only, that makes sense that the array itself was flipped and why I’m seeing large bin values where it should be shorter distances….

CP would still reliably reduce velocity in the forward direction even if the array is flipped.

[dakejahl]

@dirksavage88 I just tested it and it works for me but it's just really shitty. The velocity contraints will kick in too slowly and you'll hit the obstacle if you're moving towards it too fast. Try increasing CP_DIST and reduce the max horizontal speed and tilt angle. The tilt angle and tilt rates needs to be reduced or the lidar will be looking at the ground

[dirksavage88]

@dirksavage88 I just tested it and it works for me but it's just really shitty. The velocity contraints will kick in too slowly and you'll hit the obstacle if you're moving towards it too fast. Try increasing CP_DIST and reduce the max horizontal speed and tilt angle. The tilt angle and tilt rates needs to be reduced or the lidar will be looking at the ground

I did play with the settings and it's a bit better. My concern is that if I turn the CP go_no_go parameter on I'm not able to roll to the right, which means something may be off with the obstacle distance map message. I pulled the lidar specs and it looks like there's a 90 degree gap, so that may explain why I'm seeing a gap in coverage but it should be directly aft of the vehicle and not the sectors to the right.

[dirksavage88]

Okay I think this large angle offset is the reason for the right side lacking coverage, it shifts the entire array counter clockwise so the obstacle map leaves the right side exposed. Recommend changing it to -5. I’ve used -2.5 here on another rotating LiDAR: https://github.com/PX4/PX4-Autopilot/blob/6a34b63b60251aa15703a66ad9f4479943868f16/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp#L78C11-L78C11

[dakejahl]

Ahh the issue is that this value is already negative

so it should be

Suggested change

	obs.angle_offset = static_cast<float>(-angle_min_deg);
	obs.angle_offset = static_cast<float>(angle_min_deg);

this gives me the correct internal map array representation

TOPIC: obstacle_distance_fused
 obstacle_distance_fused
    timestamp: 101680000 (0.040000 seconds ago)
    increment: 10.00000
    angle_offset: 0.00000
    distances: [149, 156, 169, 184, 221, 296, 378, 412, 404, 409, 425, 460, 517, 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, 470, 438, 428, 424, 437, 405, 287, 228, 190, 166, 152, 145, 151, 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535]
    min_distance: 10
    max_distance: 3000
    frame: 12
    sensor_type: 0

Although this now (correctly?) produces the 90 degree issue in QGC 😅 So I think QGC needs a fix.

[OgnjenX]

Hello everyone, I have one simple question...
How did you manage to visualize 2d lidar data in QGroundControl? @dakejahl
Thank you in advance!

[dakejahl]

@OgnjenX With a vehicle connected go to the Safety page and check the "show obstacle distance overlay" box

[OgnjenX]

@OgnjenX With a vehicle connected go to the Safety page and check the "show obstacle distance overlay" box

For some reason, it does not show data on the map with this configuration:

But when I run 'listener obstacle_distance' in MAVLink Console, I get:

[DronecodeBot]

This pull request has been mentioned on Discussion Forum for PX4, Pixhawk, QGroundControl, MAVSDK, MAVLink. There might be relevant details there:

https://discuss.px4.io/t/using-obstacle-distance-uorb-msgs-for-collision-prevention/37524/8