API For Collision Detection

[nocholasrift]

Hi there, thank you for sharing this fantastic simulator. I was curious if there was any direct way using the C++ API to determine if, for example, a Jackal crashes into an obstacle in the environment. Or perhaps if there is some way to have a ROS message be published to some topic if such an event occurs.

[jlblancoc]

Hi!

C++ API

You asked for C++ API, so bear in mind that's useful only if you are using mvsim as a C++ library in your project (or modifying the sources in a fork, etc.). Yes, there is this:

   // Simulable.h

	/** Whether is is in collision right now. \sa  */
	bool isInCollision() const { return isInCollision_; }

	/** Whether a collision occurred since the last time this flag was manually
	 * reset.
	 * \sa isInCollision(), resetCollisionFlag()  */
	bool hadCollision() const { return hadCollisionFlag_; }

	/** Resets the condition reported by hadCollision() to false */
	void resetCollisionFlag() { hadCollisionFlag_ = false; }

and you can request a mvsim::World for the list of simulable objects at any moment via getListOfSimulableObjects(), then get the robot by name and check its collision flags.

ZMQ-based Python API

Using MVSim's direct Python API, you can use the get_pose() service, just like in this example and in the answer SrvGetPoseAnswer.proto there is a field objectIsInCollision.

Direct ROS message

Right now, nope, there is not! But as you can see from above, it shouldn't be hard to implement.

If there is interest in the feature I could add it. Any ideas on what would be an ideal ROS interface for collisions? A topic per robot, of type std_msgs::Bool or alike?

PS: There is so, so much yet to be documented on this project (!!) 😄

[nocholasrift]

Thanks! It would be great if it became a ROS feature, but even if not, you seem to already have a nice interface for it :). I think your suggestion makes sense, an std_msgs::Bool would make sense for each robot

[jlblancoc]

This is now a ROS topic too. A new ROS1/ROS2 topic is now published for each robot with the name "/collision" (or "/${vehicleName}/collision" if there are many) with a bool saying whether there is a collision or not.

Note that collisions with the underlying box2d library were trickier to detect than I recalled... they were not always detected so I added some additional checks with a new minimum threshold distance parameter that might need tuning in case of having problems.

The feature will exist in v0.9.4, when released by the OSRF team, or just clone and build locally if want to try right now.

Cheers!

[nocholasrift]

Hi there @jlblancoc. Thank you for adding this feature! It seems to work quite well for the most part, however, I do notice that in some instances during my simulations, the collisions are not detected. I've been adjusting the minimum threshold distance parameter as linked above up to a value of .1m (which feels quite high), and still sometimes collisions are not detected. This happens even when the Jackal has a head on collision with the obstacle. Are there any other parameters I could tweak in the code or box2d library to improve this behavior? I've attached a world file I've been using for testing below (sorry for the formatting, xmls dont seem to be supported by git...):

test.zip

[jlblancoc]

Yes, I also noticed that!
It must be either, a bug upstream in box2d, or that we are not using their API right (?).

The problem seems to happen mostly when two edges are face to face (parallel). Corners against edges seem to be detected as in collision almost always (right?).

If you have time to investigate it, it would be great! Also, it might help to open an issue upstream https://github.com/erincatto/box2d/issues explaining the issue to see if we have some help. If you do that, please mention that we are using these two techniques to detect collisions:

1. b2ContactEdge->contact->IsTouching() => it returns false
2. and also b2Distance() with the fixtures (including their transformations, etc.) => It works great sometimes, others it returns a large distance, even if the two bodies are face to face with two edges colliding.

In all cases, box2d internally correctly detects the collision, since the bodies don't penetrate each other, but what we need is to detect that situation!

Cheers,

[nocholasrift]

Certainly, I can take a look at this. I will play with the code some more and open an upstream issue in box2d accordingly. I'll reference this issue there and see what all is going on.

[nocholasrift]

@jlblancoc I'm just now getting around to grabbing some data from the debugger for the box2d issue I've opened. I'm building the project in the Debug configuration and launching the mvsim node with the lldb launch-prefix, however, when I try to set a breakpoint at Simulable::simul_post_timestep, no address can be resolved. Even looking through the image dump symtab, there are no mentions at all of the function, but other Simulable functions are present (like getVelocityLocal, for example). Any suggestions on the best way to go about this? I need to provide the data in b2DistanceInput di right before the b2Distance() call when a collision is not being detected. I have a world file with a configuration that will immediately cause this issue, just need to actually dump the data now. Thanks!

[jlblancoc]

Hi @nocholasrift

Maybe that's because of problems with the virtual table? (just guessing).
To be honest, 99% of times I debug using QtCreator: import that Debug build directory, place the breakpoint from the GUI and it works like a charm. Also, much easier to browse local symbols, etc. than from the CLI of gdb (haven't used lldb). Please, try that method to see if it works (?).

I have a world file with a configuration that will immediately cause this issue, just need to actually dump the data now.

Great idea to save debug time iterating 👍

[jlblancoc]

I just noticed there's a new total rewrite version of box2d, v3.0, upstream. Hopefully porting mvsim to it will fix this issue... 🤞