# Nearest obstacle in Map

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?

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.

Originally posted by silgon on ROS Answers with karma: 649 on 2016-02-23

Post score: 0

Comment by zastrix on 2016-02-23:
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.

Comment by silgon on 2016-02-24:
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.

Comment by silgon on 2016-02-24:
You can project a ray as you said, the problem is that you still need to process this map data.

Comment by hal9000 on 2019-07-08:
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:

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

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

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

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{
}
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. ;)

Originally posted by silgon with karma: 649 on 2016-02-27

This answer was ACCEPTED on the original site

Post score: 2

Comment by lucasw on 2016-03-01:
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)

Comment by silgon on 2016-03-01:
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. =)

Comment by lucasw on 2016-03-01:
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.

Comment by arkin8858 on 2019-05-20:
@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 .

Take a look at opencv cv::distanceTransform http://docs.opencv.org/2.4/modules/imgproc/doc/miscellaneous_transformations.html#distancetransform

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.

Originally posted by lucasw with karma: 8729 on 2016-03-01

This answer was NOT ACCEPTED on the original site

Post score: 1

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.
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?