Nearest obstacle in Map

asked 2016-02-23 16:13:19 -0500

silgon
647 ●28 ●34 ●42

updated 2016-02-24 03:03:11 -0500

Let's say I have a robot in a Map. The robot it already knows the map and it knows its position (with amcl). Is there a way to extract from the information of the robot and the map the nearest obstacle in the map? To put it simple, let's see the figure below. The blue circle is the robot. I know all the map (black line). I want to know the distance and the angle of the orange dotted line that represents the closest obstacle. I should be able to know this information with the map and odometry topics. How can I do it?

image description

Note: I'm pretty sure amcl guys did it somehow. But, it's there any simple way I can extract this info? Note 2: I want to add multiple multiple objects in the scene and I would like to put some dynamics based on this distance. And of course this objects don't have lasers. As I said before, I should be able to do it only with the position of the object and the map.

edit retag flag offensive close merge delete

Comments

i suppose, starting at theta = 0, you can trace a ray from the robot's origin, to the map wall. continue for 360 degrees or whatever resolution you want. and when you're done, the smallest distance you saw is the closest object. you then have the distance and the angle w.r.t. the robot.

zastrix ( 2016-02-23 19:11:08 -0500 )edit

Actually what I thought to be the best solution is to get all the distances for all the points in the map, the problem is that getting all this information will take a lot of time, because you have to process the map data first. And such a common problem that should be already solved.

silgon ( 2016-02-24 03:15:26 -0500 )edit

You can project a ray as you said, the problem is that you still need to process this map data.

silgon ( 2016-02-24 03:17:06 -0500 )edit

AMCL already calculates the Distance Transform of the static map to speed up calculating scan match scores. see the map_update_cspace() function in map_cspace.cpp:

https://github.com/ros-planning/navig...

To get at that data, you could add a ROS service to AMCL to tell you that value given an (x,y) location.

hal9000 ( 2019-07-08 15:11:48 -0500 )edit

add a comment

2

answered 2016-02-27 04:54:24 -0500

silgon
647 ●28 ●34 ●42

updated 2016-03-01 10:54:58 -0500

So, finally I reinvented the wheel for ROS. You can find the whole code in the next link.

https://gist.github.com/silgon/762674...

I copy the important part of my code here:

// remapping information in info variable
map_inf map_info;
map_info.size_x = map.info.width;
map_info.size_y = map.info.height;
map_info.scale = map.info.resolution;
map_info.origin_x = map.info.origin.position.x + (map_info.size_x / 2) * map_info.scale;
map_info.origin_y = map.info.origin.position.y + (map_info.size_y / 2) * map_info.scale;

// transform in case the pose to evaluate and the point are not in the same
// reference frame
try{
    tf_l.waitForTransform(map.header.frame_id,
                          pt.header.frame_id, ros::Time(0), ros::Duration(1));
    tf_l.transformPose(map.header.frame_id, pt, new_pt);
}
catch (tf::TransformException ex){
    ROS_ERROR("%s",ex.what());
}

// getting minimum distance
for (std::size_t j=0; j < map_info.size_y; j++) {
    for (std::size_t i=0; i < map_info.size_x; i++) {
        if(map.data[MAP_INDEX(map_info, i, j)]==100){
            // convert to world position
            w_x = MAP_WXGX(map_info, i);
            w_y = MAP_WYGY(map_info, j);
            dist_sq = pow(w_x-pt_x,2)+pow(w_y-pt_y,2);
            if (dist_sq<min_dist){
                min_dist = dist_sq;
                d_x = w_x;
                d_y = w_y;
            }
        }
    }
}
min_dist = sqrt(min_dist);

// given the position of the obstacle, get the angle in the robot reference
double angle;
angle = std::atan2(d_y-pt_y, d_x-pt_x)-pt_th;

Hope it's useful to somebody. ;)

edit flag offensive delete link

Comments

You can drop the sqrt from the inner loop for a bit more speed, if sqrt(a) < sqrt(b) then a < b for positive a and b. (unless compilers are smart about that now)

lucasw ( 2016-03-01 10:19:01 -0500 )edit

Touché about sqrt =). You're right. Thanks. About the answer you gave, probably it's ok for generalization, but in my answer I already take into account transformations between different coordinate systems (in the given link). I will give the function a glance, because it could be faster. =)

silgon ( 2016-03-01 10:54:11 -0500 )edit

1

It would certainly be faster if the robot is moving but the map isn't changing, because the output is the distance of every free space pixel to nearest black pixel, not just the current position. So the distanceTransform results could be cached and after movement the cached distance map queried.

lucasw ( 2016-03-01 12:21:09 -0500 )edit

@silgon, its very nice code to implement get the obstacle distance from map. i tried to use this code into my project but unfortunately got some errors. tried to solve the errors but still the same. could you please help me to findout the solution. the error is about no matching function for call to 'ros::NodeHandle::subscribe(const char [14], int, std::pair<double, double=""> (&)(geometry_msgs::PoseStamped&, nav_msgs::OccupancyGrid&))' . looking forward to your reply. thanks .

arkin8858 ( 2019-05-20 22:39:26 -0500 )edit

add a comment

1

answered 2016-03-01 10:15:24 -0500

lucasw

8729 ●137 ●239 ●256 https://github.com/lucasw

Take a look at opencv cv::distanceTransformhttp://docs.opencv.org/2.4/modules/im...

You would convert your map into a binary image making the obstacles 0/black, open space white. The label functionality will give you the location of the nearest obstacle.

edit flag offensive delete link

Comments

Hello @lucasw I've been working with the solution that you proposed. I'm a little bit lost, maybe you can help me here. If you could clone the this project then run make && ./distancetransform map.pgm you'll see that the distance part works.

silgon ( 2016-09-05 18:16:33 -0500 )edit

The problem is in the following lines. I'm pretty sure that for L46 the result should be 1 because that should be the label of the next zero pixel. I would like to have something generic. Any ideas?

silgon ( 2016-09-05 18:18:13 -0500 )edit

1

I just found your post. Tomorrow I'll try to do something, but I'm still a bit lost. I think the method your proposed can be great and as you said, way faster. Any input or help is welcome =)

silgon ( 2016-09-05 18:21:26 -0500 )edit

add a comment

0

answered 2016-02-24 08:52:51 -0500

ros_geller
92 ●6 ●8 ●15

updated 2016-02-24 11:49:14 -0500

If the map is stored as xy vector points, and your position in this map is stored as that too, then it's just matter of finding the angle between two vectors for the angle and subtracting the one vector with the other and calculating its norm to get the distance. So by assuming the map has a horisontal x-axis and vertical y-axis, and the angle is then the angle from x-axis to y-axis, you can do the following:

// your vector: u = [x1, y1]
// closest point vector: p = [x2, y2]
// L = length between you and the closest point
// Lx = length on the x-axis between you and the closest point
L =  sqrt((x2-x1)^2 + (y2-y1)^2); //
Lx = sqrt((x2-x1)^2);
angle = acos( La / L );

edit: If you want to actually find the point that is closest to you, then a simple shortest path like search will do the trick, or simply iterating over all points and calculating the absolute distance between you and that point for each point untill you find the shortest one.

Of course you have to offset this calculated angle with the angle of the robot with respect to the x-axis, or whatever frame of reference you are using.

edit flag offensive delete link

Comments

I know the solution is to find argmin_{x_p,y_p} sqrt((x_r-x_p)^2+(y_r-y_p)^2). Then given the found x_p and y_p the angle is arctan2(y_p-y_r, x_p-x_r) being _p the representation of the evaluation point and _r the representation of the robot.

silgon ( 2016-02-26 03:19:30 -0500 )edit

My question is about the map, because you have to do the transformation of the map, and you have to calculate all the points with the metaparameters that the map topic has, which can be cumbersome and tedious, nonetheless I think I'll have to do that.

silgon ( 2016-02-26 03:20:43 -0500 )edit

I'm sorry, seems like i misunderstood your question. I haven't used amcl before, but seeing as you get the map resolution, width, height and origin along with the occupancy grid, it seems like you have to perform these calculations idd.

ros_geller ( 2016-02-26 04:15:03 -0500 )edit

Easy to confuse though, as in your question you do say that you know all the map (black line) and your position in it, so my answer is in fact accurate with regards to that example. Anyhow, good luck.

ros_geller ( 2016-02-26 04:17:22 -0500 )edit

is it possible to get the distance for dynamic obstacles?

SH_Developer ( 2017-07-28 07:59:11 -0500 )edit

add a comment

Nearest obstacle in Map

Comments

3 Answers

Comments

Comments

Comments

Question Tools

Stats

Related questions

Nearest obstacle in Map edit

Comments

3 Answers

Comments

Comments

Comments

Question Tools

Stats

Related questions

Nearest obstacle in Map