Detect obstacles via sonar

[jim-v]

Needs to handle input from sonar on Magni and Loki.

Done when Loki and Magni pause for obstacles.

[rohbotics]

@jim-v How do you want to approach this?

I was thinking that we could check if the sonars cones are intersecting with the motion plan and slow down/stop if they are.

We could also go for the occupancy grid approach, filling it with sonar data, and then checking the plan against that.

Both of these would require some configuration to specify the robot footprint relative to base link.

[jim-v]

@rohbotics I was thinking we could do it with 2D geometry, similar to how we handle LIDAR. We should be able to look up the pose of the sonar S relative to base_link. The vector SA is normal to the sonar, and it's length is calculated from the ping time. Vectors SB and SC are the sides of the triangle (2D projection of cone), which are SA rotated by +/- half the angle of the cone. The distance to the object is the highest x component of B or C that is between the inflation lines. This is for going forward. A more complicated version is necessary for rotating in place.

[davecrawley]

What's out there already that solves these problems? Might save us from re-inventing the wheel. I seem to have a vague recollection that someone released a package that attempted to make Sonar data "LIDAR like". In short a package that solves these very problems.

Rohan I think you told me about this - the author was a guy from google.

David

On Wed, Dec 27, 2017 at 6:15 PM, Rohan Agrawal notifications@github.com wrote:

@jim-v https://github.com/jim-v How do you want to approach this?

I was thinking that we could check if the sonars cones are intersecting with the motion plan and slow down/stop if they are.

We could also go for the occupancy grid approach, filling it with sonar data, and then checking the plan against that.

Both of these would require some configuration to specify the robot footprint relative to base link.

— You are receiving this because you are subscribed to this thread. Reply to this email directly, view it on GitHub https://github.com/UbiquityRobotics/move_basic/issues/26#issuecomment-354215076, or mute the thread https://github.com/notifications/unsubscribe-auth/ALDeVqSjYWefAcIyq0eusX0vwkLRcFayks5tEvmqgaJpZM4QSKti .

[jim-v]

@davecrawley there are packages that create costmaps from sonar data, but that is unnecessarily complex for such a simple planner.

[jim-v]

@rohbotics I did a proof of concept implementation in my personal repo: https://github.com/jim-v/move_basic/blob/sonar/src/sonar_ranger.cpp It seems to work quite well, though I have only implemented obstacle checking in forward direction.

[waynegramlich]

Are you doing this using Loki?

[jim-v]

Yes - it's the only robot with sonars that I have access to.

[waynegramlich]

I looked at the Sonar firmware in the bus_loki repo at rev_f/bus_common/Sonar.cpp. It is pretty involved what I do when it comes to scheduling sonars for triggering. It starts with Sonars_Controller::poll(). I do two things, I prioritize sonars that are in the direction of travel (forward or reverse) and I prioritize sonars that previously returned that they were "close". Thus, as the robot gets closer to objects, the sonars get fired off more. This is just for your information.

[rohbotics]

@jim-v Do you want to make a PR for at least the forward detection, so I can review it?

We can decide later whether to merge it, but doing a review in the earlier stages would probably be better than doing a massive one at the end of development.

[jim-v]

Closed via #27