Software > Software

check which cells are covered by robot

**robvoi**:

Hi,

for an occupancy grid (resolution 1cm) I want to calculate which cells are covered by the robots body - so I can set them as "not occupied".

The robot has the form of a rectangle.

What’s the mathematical approach on this?

Sonar and IR sensor modeling I did with simple trigonometry. But with this one I am a bit stuck.

Thanks

Robert

**Roman505**:

Perhaps I have been enjoying myself too much this evening, but it is not clear to me what is your question :).

What data do you expect to have, on which we should calculate overlap or absence?

**robvoi**:

I have a rectangle with a given x/y pos and theta in an Cartesian coordinate system.

Given the position, length and width of the robot I want to know which coordinates are covered by the robots body so I can set them as known to be free of obstacles.

I have a solution now – but it’s not elegant:

Not the complete code but the approach should be visible:

--- Code: ---void setRobotPosAsFree() {

float[] LeftBottomCorner = new float[2];

float[] temp = new float[2];

for (float k = RobotWidth/2 ; k >= -RobotWidth/2 ; k=k-0.25) {

LeftBottomCorner = MapOffsetPosFloat(x_pos_MAP, y_pos_MAP, k, theta - radians(-90));

LeftBottomCorner = MapOffsetPosFloat(LeftBottomCorner[0], LeftBottomCorner[1], RobotLength/2, theta - radians(-180));

for (int x = 0; x <= RobotLength; x++) {

temp = MapOffsetPosFloat(LeftBottomCorner[0], LeftBottomCorner[1], x, theta);

map[int(temp[0])][int(temp[1])] = -127;

}

}

}

float[] MapOffsetPosFloat(float x, float y, float dist, float theta) {

float[] temp = new float[2];

float Xoff = dist * cos(theta); //x

float Yoff = dist * sin(theta); //y

temp[0] = x + Xoff;

temp[1] = y - Yoff;

return temp;

}

--- End code ---

**Roman505**:

Deleted, because I missed the case of a robot wholly covering a cell rather than merely intersecting with it.

I will stick with the first part of my original post which started "yes". :)

**jwatte**:

Your pose can be expressed as two orthonormal basis vectors Vx and Vy and two half-extents ex and ey.

You also need a center position PC.

In vector algebra, you want to set to 1 the cells where cell coordinate CC conforms to:

fabs(dot(CC-PC, Vx)) <= ex && fabs(dot(CC-PC, Vy)) <= ey

You can then scan the possible extent of the square around the center PC for intersection, and mark the appropriate cells.

The minimum X coordinate to test is PC.x - fabs(Vx.x) - fabs(Vy.x); the maximum X coordinate is PC.x + fabs(Vx.x) + fabs(Vy.x).

The minimum Y coordinate to test is PC.y - fabs(Vx.y) - fabs(Vy.y); the maximum Y coordinate is PC.y + fabs(Vx.y) + fabs(Vy.y).

Trying to rasterize in body space, like you're trying to do, is problematic because you will miss certain cells, and count others twice. You solve this by over-sampling by a factor of 4, which is quite wasteful.

Separately, the implementation you have has some inefficiencies, such as using "new" to create arrays of floats, which would be more efficient on the stack, and calling sin(theta) and cos(theta) once per cell instead of once per invocation. You also leak the things allocated with new[] at the end of your function.

Navigation

[0] Message Index

[#] Next page

Go to full version