Skip to content

Instantly share code, notes, and snippets.

Embed
What would you like to do?
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