How to get the distance between two objects (meshes)

Hello all

I am interesting in wright a plugin that calculates the distance between my robot and the objects around it. My intention is to use this measure in order to grade the performance of obstacle avoidance algorithms. Therefore, i am looking for some kind of unambiguous world state data, and not a sensor simulation.

My question: is there a function in the Gazebo API that performs these kind of calculations, that i can use it in my plugin ?

thanks.

Asked by dmeltz on 2014-04-28 01:39:54 UTC

Comments

Answers

One way of doing is writing a plugin similar to p3d (LINK) and iterating through all the models in the simulation world and getting the relative position between your robot and all those models.

Hope it helps!

Asked by peshala on 2014-05-08 23:02:47 UTC

Comments

I understand that i need to iterate between all objects in the scenario but the thing that i looking for is a function that will sit inside my iteration loop and be able to calculate minimal distance once it is given two objects positions and meshes. I sure that some variation of this function is used to calculate collisions between objects.

Asked by dmeltz on 2014-07-14 10:09:04 UTC

Maybe FCL is what you're looking for?

Asked by scpeters on 2014-10-28 23:45:13 UTC