ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question

Revision history [back]

click to hide/show revision 1
initial version

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

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

I copy the important part of the 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 = sqrt(pow(w_x-pt_x,2)+pow(w_y-pt_y,2));
            if (dist<min_dist){
                min_dist = dist;
                d_x = w_x;
                d_y = w_y;
            }
        }
    }
}
// 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. ;)

So, finally I reinvented the wheel. 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 the 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 = sqrt(pow(w_x-pt_x,2)+pow(w_y-pt_y,2));
            if (dist<min_dist){
                min_dist = dist;
                d_x = w_x;
                d_y = w_y;
            }
        }
    }
}
// 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. ;)

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{
    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 = sqrt(pow(w_x-pt_x,2)+pow(w_y-pt_y,2));
dist_sq = pow(w_x-pt_x,2)+pow(w_y-pt_y,2);
            if (dist<min_dist){
(dist_sq<min_dist){
                min_dist = 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. ;)