Sonar RTheta

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);
}