Fixed RLidar

Index

• Searching Chassis
• Using RLidar

Searching Chassis

SMARTS uses in all its examples a chassis called BoxChassis, which I didn’t think much of at first. Investigating the guts I found the location of all the urdfs of each vehicle that they have, I was looking for in which files they are referenced and …. none.

It turns out that BoxChassis basically creates a square of the dimensions you ask for and on top of that you get a .glb that looks like a car/truck etc. Well, testing with that class (BoxChassis) it turns out that the rays don’t detect obstacles, it seems that the way they create the box has broken collisions. Searching further I found another chassis called AckermannChassis which does use the urdfs of the vehicles, and with this chassis the rays DO WORK.

Here an example of BoxChassis:

Here an example of AckermannChassis (how it should look):

Looking for information on pybullet I found the problem within BoxChassis, the collision assignment is done erroneously.

bullet_client.setCollisionFilterGroupMask(
    self._bullet_body._bullet_id, -1, 0x0, 0x0
)

Pybullet assigns collisions by dividing it into groups, so objects that do not belong to a group (0x0, 0x0) do not have collisions. After modifying this and assigning the vehicles to group 1 (0x1, 0x1 randomly) everything works perfectly.

Fixed:

bullet_client.setCollisionFilterGroupMask(
    self._bullet_body._bullet_id, -1, 0x1, 0x1
)

BoxChassis in pybullet after fix:

Using RLidar

Once the collisions have been fixed, it is time to look into the behaviour of the RLidar.

After analysing its output, I found that it returns collisions in absolute coordinates, so I had to make some modifications to handle the data easily.

The operation is simple:

• All measurements are extracted from the point cloud (with and without detected collisions).
• A first screening is done to eliminate all null measurements.
• They are converted to relative coordinates by subtracting the absolute coordinates (ray origin).
• Finally we calculate the distance to (0,0,0) and by using atan2 we calculate the angle. For simplicity I have been working with the closest measurement.

def closest_obstacle_warning(self, measurements, aux_measures):

    measurements = measurements[~np.all(measurements == [0, 0, 0], axis=1)] - aux_measures[0]
    distances = np.linalg.norm(measurements, axis=1)
    
    min_index = np.argmin(distances)
    closest_point = measurements[min_index]
    angle = np.degrees(np.arctan2(closest_point[1], closest_point[0]))  # atan2(y, x)
    
    print(f"El obstáculo más cercano está a {angle:.2f} grados, con una distancia de {distances[min_index]:.2f} unidades.")

Here is an example:

Car position:

RLidar exit:

Point Cloud Detection plot: