Skip to content

Instantly share code, notes, and snippets.

Created March 18, 2015 14:51
  • Star 0 You must be signed in to star a gist
  • Fork 0 You must be signed in to fork a gist
Star You must be signed in to star a gist
Save soulslicer/d28c15c918b361ec33be to your computer and use it in GitHub Desktop.
Sonar RTheta
bool sonar_pixel(sonar::sonar_pixel::Request &req, sonar::sonar_pixel::Response &rsp)
int row = ((double)(req.y) / HEIGHT) * BVTMagImage_GetHeight(rangeImage);
int col = ((double)(req.x) / WIDTH) * BVTMagImage_GetWidth(rangeImage);
rsp.range = BVTMagImage_GetPixelRange(rangeImage, (int)row, (int)col);
rsp.bearing = BVTMagImage_GetPixelRelativeBearing(rangeImage, (int)row, (int)col);
ROS_INFO("range bearing for pixel [%d %d] = [%f %f]", req.x, req.y, rsp.range, rsp.bearing);
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment