mapping LaserScan to obstacle_distance mavlink message

[rmackay9]

I'm attempting to use the obstacle_distance plugin to send distances from a 360degree RPLidar range finder to ArduPilot. The OBSTACLE_DISTANCE mavlink messages are being sent to AP but only include the first 72 degrees of data from the range finder.

The messages from RPLidar are appear in /scan as LaserScan messages and below is a screen shot of the output of, "rostopic echo /scan".

I've enabled the obstacle_distance plugin by adding "obstacle_distance" to the apm_pluginlist.yaml file and then in the node.launch file, I've added a remap like this, "<remap from="/mavros/obstacle/send" to="/scan" />".

The problem is though that the LaserScan message's have 1degree increments and the OBSTACLE_DISTANCE message's distance array only has 72 slots. Ideally I would like to combine the LaserScan data down into 45degree buckets and then send to AP via the OBSTACLE_DISTANCE message. I know 45 degree buckets is coarse but it should be sufficient for the obstacle avoidance we want to use this data for.

[vooon]

You need to add filter node that will reduce number of points.
I think that node should be already written, just need to google.
And that is not specific to mavros, it is more generic ros comms.

[rmackay9]

OK, thanks for that. I'll close this issue then 'cuz sounds like we've got a path to a solution and it hopefully/probably won't require a change to mavros. Txs again.

[rmackay9]

@vooon, we're making progress on getting the 360 lidar data into AP (ArduPilot driver enhancement is here).

We've also hacked together a change to RPLidar's node.cpp that resizes the output (look for "Resize of data for mavros" in this link here).

My guess is that we should take this bit of code and instead put it into the obstacle_distance plugin somehow but I was wondering if you agreed before we work on a mavros PR to do that.

[vooon]

I think that adding scale math to plugin is ok (as new subscriber), but of course not lidar driver.

[rmackay9]

@vooon,

I'm thinking about fixing this in mavros's obstacle distance plugin by adding some code into ObstacleDistancePlugin::obstacle_cb(). The new lines will check if req->ranges.size() > obstacle.distances.size() and if "yes" it will:

• calculate a "compression_factor" integer which is round_up(req->ranges / obstacle.distances.size())
• if the compression_factor > 1 it will loop through the req->ranges, finding the minimum distance of each "compression_factor" slots and the placing this number into obstacle.distances[]. So for example, if "compression_factor" was "3" then
  • obstacle.distances[0] = min(req->ranges[0], req->ranges[1], req->ranges[2])
  • obstacle.distances[1] =min(req->ranges[3], req->ranges[4], req->ranges[5]), etc

What do you think about this possible solution?

[TSC21]

@rmackay9 any changes with respect to the obstacle distance plugin should be checked with the PX4 avoidance team first - @mrivi @baumanta @jkflying @nicovanduijn

[rmackay9]

@TSC21, sure thanks for the advice. I hope that this change is purely an improvement with no downside to existing users. I think the existing code doesn't correctly handle sources that produce more than 72 distances.

[TSC21]

All that is an improvement is well received. But we should always start by not breaking what already works. So I would bring this to discussion with the PX4 avoidance team first.

[vooon]

@rmackay9 i think it is ok, but maybe good to add switch parameter that will enable compression.

[razekmaiden]

Dear @rmackay9, First of all, thank you very much for your proposed solution to publish data from RPLidar (LaserScan) to mavlink. I have already modified node.cpp of RPLidar package, I have recompiled the package and data of length 72 are published in the topic "/mavros/obstacle/send". However, I receive a lot of error messages in the terminal where the mavros node is started (as you can see in the image). I have not been able to identify the problem.

The launcher used is apm.launch. In MissionPlanner it is possible to see the measurements in the "radar" window, however, every few seconds, it remains blank. Do you have any idea of what it might be? .PD: mavros and mavlink have been installed from binaries.

[bj-neilson]

Hi @rmackay9,
I've got my RPLidarA2 running on my TX2 with Ros Melodic. It's running a navigation stack so obstacle avoidance is catered for there. I'm really interested however in knowing if this issue re sending /scan data via mavros to ArduRover has been solved in an agreed way? Would be good to have ArduRover collision avoidance working in this way! Thanks, Ben

[Ebaker310]

@razekmaiden

However, I receive a lot of error messages in the terminal where the mavros node is started (as you can see in the image). I have not been able to identify the problem.

Did you resolve this issue?