ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange |
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. ;)
2 | No.2 Revision |
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. ;)
3 | No.3 Revision |
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. ;)