Skip to content

Instantly share code, notes, and snippets.

@silgon
Last active March 21, 2024 02:59
Show Gist options
  • Save silgon/762674d94ce32b940bda to your computer and use it in GitHub Desktop.
Save silgon/762674d94ce32b940bda to your computer and use it in GitHub Desktop.
Get distance and angle of the closest obstacle from pose and map
#include <ros/ros.h>
#include <nav_msgs/OccupancyGrid.h>
#include <geometry_msgs/PoseStamped.h>
#include <tf/tf.h>
#include <tf/transform_listener.h>
#include <iterator>
#include <vector>
// some definitions of functions
#define MAP_INDEX(map, i, j) ((i) + (j) * map.size_x)
#define MAP_WXGX(map, i) (map.origin_x + (i - map.size_x / 2) * map.scale)
#define MAP_WYGY(map, j) (map.origin_y + (j - map.size_y / 2) * map.scale)
// information of the map
struct map_inf {
double size_x;
double size_y;
double scale;
double origin_x;
double origin_y;
};
std::pair<double, double> poseToMap(geometry_msgs::PoseStamped & pt,
nav_msgs::OccupancyGrid & map){
std::pair<double, double> result(0,0);
// 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;
// map (world reference)
double gt_x = map.info.origin.position.x;
double gt_y = map.info.origin.position.y;
tf::TransformListener tf_l(ros::Duration(10));
geometry_msgs::PoseStamped new_pt;
bool success = false;
// next line to avoid tf extrapolation into the past (not good code)
map.header.stamp = pt.header.stamp = ros::Time(0);
// 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());
}
// robot in map frame reference
double pt_x = new_pt.pose.position.x;
double pt_y = new_pt.pose.position.y;
double pt_th = tf::getYaw(new_pt.pose.orientation);
// variables to keep the distance
double dist_sq, min_dist = 100000;
double w_x, w_y; // world cordinates
double d_x, d_y; // saved world cordinates
ros::Time start = ros::Time::now();
// more efficient first with heigh and then width because map information
// is in row-major order
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;
// debug information
ROS_DEBUG_STREAM("Finding in "<< ros::Time::now()-start<<"s");
ROS_DEBUG_STREAM("Min. Distance and angle: "<< min_dist << "," <<angle*180/M_PI);
ROS_DEBUG_STREAM("Robot is at position "<< pt_x <<","<<pt_y<<","<<pt_th*180/M_PI);
ROS_DEBUG_STREAM("Obstacle is at position "<< d_x <<","<<d_y);
result = std::make_pair(min_dist, angle);
return result;
}
@arkinrc
Copy link

arkinrc commented May 21, 2019

hello mr. silgon, its really nice work. however, i got an error while compiling this code. 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&))' . please see the error message picture below. looking forward to your guidance. thanks ..

error

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment