coordConversion.cpp

Go to the documentation of this file.

#include <youbot_overhead_vision/coordConversion.h>

coordConversion::coordConversion()
{
  conversionService = n.advertiseService("coord_conversion", &coordConversion::convertCoordinates, this);

  n.getParam("/image_calibration/runFromSimulation", useSim);
}

bool coordConversion::convertCoordinates(youbot_overhead_vision::CoordConversion::Request &req,
                                         youbot_overhead_vision::CoordConversion::Response &res)
{
  float x = req.x;
  float y = req.y;
  int type = req.type;

  if (type == PIXEL_TO_METER)
  {
    if (useSim)
    {
      res.xConverted = x * SIM_M + SIM_X_B;
      res.yConverted = y * SIM_M + SIM_Y_B;
    }
    else
    {
      res.xConverted = x * REAL_M + REAL_X_B;
      res.yConverted = y * REAL_M + REAL_Y_B;
    }
  }
  else if (type == METER_TO_PIXEL)
  {
    if (useSim)
    {
      res.xConverted = (x - SIM_X_B) / SIM_M;
      res.yConverted = (y - SIM_Y_B) / SIM_M;
    }
    else
    {
      res.xConverted = (x - REAL_X_B) / REAL_M;
      res.yConverted = (y - REAL_Y_B) / REAL_M;
    }
  }
  else if (type == OCC_GRID_TO_METER)
  {
    if (useSim)
    {
      res.xConverted = x / OCC_GRID_REDUCTION * SIM_M + SIM_X_B;
      res.yConverted = y / OCC_GRID_REDUCTION * SIM_M + SIM_Y_B;
    }
    else
    {
      res.xConverted = x / OCC_GRID_REDUCTION * REAL_M + REAL_X_B;
      res.yConverted = y / OCC_GRID_REDUCTION * REAL_M + REAL_Y_B;
    }
  }
  else //invalid conversion type
  {
    res.xConverted = req.x;
    res.yConverted = req.y;
  }

  return true;
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "coordinate_converter");

  coordConversion converter;

  ros::spin();

  return 0;
}