void sonarPrinter(void)
{
  double d;
  double th;
	

  d = sonar.currentReadingPolar(-45, 45, &th);
  printf("front: %5.0fmm ", d);
  if (d != sonar.getMaxRange())
    printf("%3.0fdeg ", th);
  else
    printf("???deg ");

  d = sonar.currentReadingPolar(-135, -45, &th);
  printf(" right: %5.0fmm ", d);
  if (d != sonar.getMaxRange())
    printf("%3.0fdeg ", th);
  else
    printf("???deg ");

  d = sonar.currentReadingPolar(45, 135, &th);
  printf(" left: %5.0fmm ", d);
  if (d != sonar.getMaxRange())
    printf("%3.0fdeg ", th);
  else
    printf("???deg ");

  d = sonar.currentReadingPolar(135, -135, &th);
  printf(" back: %5.0fmm ", d);
  if (d != sonar.getMaxRange())
    printf("%3.0fdeg ", th);
  else
    printf("???deg ");

  printf("\r");
  fflush(stdout);
}
Exemplo n.º 2
0
void sonarPrinter(void)
{
  fprintf(stdout, "in sonarPrinter()\n"); fflush(stdout);
  double scale = (double)half_size / (double)sonar.getMaxRange();

  /*
  ArSonarDevice *sd;

  std::list<ArPoseWithTime *>::iterator it;
  std::list<ArPoseWithTime *> *readings;
  ArPose *pose;

  sd = (ArSonarDevice *)robot->findRangeDevice("sonar");
  if (sd != NULL)
  {
    sd->lockDevice();
    readings = sd->getCurrentBuffer();
    if (readings != NULL)
    {
      for (it = readings->begin(); it != readings->end(); ++it)
      {
        pose = (*it);
        //pose->log();
      }
    }
    sd->unlockDevice();
  }
*/
  double range;
  double angle;

  /*
   * example to show how to find closest readings within polar sections
   */
  printf("Closest readings within polar sections:\n");

  double start_angle = -45;
  double end_angle = 45;
  range = sonar.currentReadingPolar(start_angle, end_angle, &angle);
  printf(" front quadrant: %5.0f  ", range);
  //if (range != sonar.getMaxRange())
  if (std::fabs(range - sonar.getMaxRange()) > std::numeric_limits<double>::epsilon())
    printf("%3.0f ", angle);
  printf("\n");
#if defined(VISP_HAVE_X11) || defined (VISP_HAVE_GDI)
  //if (isInitialized && range != sonar.getMaxRange())
  if (isInitialized && std::fabs(range - sonar.getMaxRange()) > std::numeric_limits<double>::epsilon())
  {
    double x = range * cos(vpMath::rad(angle)); // position of the obstacle in the sensor frame
    double y = range * sin(vpMath::rad(angle));

    // Conversion in pixels so that the robot frame is in the middle of the image
    double j = -y * scale + half_size; // obstacle position
    double i = -x * scale + half_size;

    vpDisplay::display(I);
    vpDisplay::displayLine(I, half_size, half_size, 0, 0, vpColor::red, 5);
    vpDisplay::displayLine(I, half_size, half_size, 0, 2*half_size-1, vpColor::red, 5);
    vpDisplay::displayLine(I, half_size, half_size, i, j, vpColor::green, 3);
    vpDisplay::displayCross(I, i, j, 7, vpColor::blue);
  }
#endif

  range = sonar.currentReadingPolar(-135, -45, &angle);
  printf(" right quadrant: %5.0f ", range);
  //if (range != sonar.getMaxRange())
  if (std::fabs(range - sonar.getMaxRange()) > std::numeric_limits<double>::epsilon())
    printf("%3.0f ", angle);
  printf("\n");

  range = sonar.currentReadingPolar(45, 135, &angle);
  printf(" left quadrant: %5.0f ", range);
  //if (range != sonar.getMaxRange())
  if (std::fabs(range - sonar.getMaxRange()) > std::numeric_limits<double>::epsilon())
    printf("%3.0f ", angle);
  printf("\n");

  range = sonar.currentReadingPolar(-135, 135, &angle);
  printf(" back quadrant: %5.0f ", range);
  //if (range != sonar.getMaxRange())
  if (std::fabs(range - sonar.getMaxRange()) > std::numeric_limits<double>::epsilon())
    printf("%3.0f ", angle);
  printf("\n");

  /*
   * example to show how get all sonar sensor data
   */
  ArSensorReading *reading;
  for (int sensor = 0; sensor < robot->getNumSonar(); sensor++)
  {
    reading = robot->getSonarReading(sensor);
    if (reading != NULL)
    {
      angle = reading->getSensorTh();
      range = reading->getRange();
      double sx = reading->getSensorX(); // position of the sensor in the robot frame
      double sy = reading->getSensorY();
      double ox = range * cos(vpMath::rad(angle)); // position of the obstacle in the sensor frame
      double oy = range * sin(vpMath::rad(angle));
      double x = sx + ox; // position of the obstacle in the robot frame
      double y = sy + oy;

      // Conversion in pixels so that the robot frame is in the middle of the image
      double sj = -sy * scale + half_size; // sensor position
      double si = -sx * scale + half_size;
      double j = -y * scale + half_size; // obstacle position
      double i = -x * scale + half_size;

      //      printf("%d x: %.1f y: %.1f th: %.1f d: %d\n", sensor, reading->getSensorX(),
      //             reading->getSensorY(), reading->getSensorTh(), reading->getRange());

#if defined(VISP_HAVE_X11) || defined (VISP_HAVE_GDI)
      //if (isInitialized && range != sonar.getMaxRange())
      if (isInitialized && std::fabs(range - sonar.getMaxRange()) > std::numeric_limits<double>::epsilon())
      {
        vpDisplay::displayLine(I, si, sj, i, j, vpColor::blue, 2);
        vpDisplay::displayCross(I, si, sj, 7, vpColor::blue);
        char legend[15];
        sprintf(legend, "%d: %1.2fm", sensor, float(range)/1000);
        vpDisplay::displayCharString(I, i-7, j+7, legend, vpColor::blue);
      }
#endif
    }

  }

#if defined(VISP_HAVE_X11) || defined (VISP_HAVE_GDI)
  if (isInitialized)
    vpDisplay::flush(I);
#endif

  fflush(stdout);
}