/*------------------------------------------------------------- getSonarsReadings -------------------------------------------------------------*/ void CActivMediaRobotBase::getSonarsReadings( bool &thereIsObservation, CObservationRange &obs ) { #if MRPT_HAS_ARIA ASSERTMSG_(THE_ROBOT!=NULL, "Robot is not connected") THE_ROBOT->lock(); obs.minSensorDistance = 0; obs.maxSensorDistance = 30; int i,N =THE_ROBOT->getNumSonar(); obs.sensorLabel = "BASE_SONARS"; obs.sensorConeApperture = DEG2RAD( 30 ); obs.timestamp = system::now(); obs.sensedData.clear(); unsigned int time_cnt = THE_ROBOT->getCounter(); if (m_lastTimeSonars == time_cnt) { thereIsObservation = false; THE_ROBOT->unlock(); return; } for (i=0;i<N;i++) { ArSensorReading *sr = THE_ROBOT->getSonarReading(i); if (sr->getIgnoreThisReading()) continue; // if (!sr->isNew(time_cnt)) // { //thereIsObservation = false; //break; // } obs.sensedData.push_back( CObservationRange::TMeasurement() ); CObservationRange::TMeasurement & newObs = obs.sensedData.back(); newObs.sensorID = i; newObs.sensorPose.x = 0.001*sr->getSensorX(); newObs.sensorPose.y = 0.001*sr->getSensorY(); newObs.sensorPose.z = 0; //0.001*sr->getSensorZ(); newObs.sensorPose.yaw = DEG2RAD( sr->getSensorTh() ); newObs.sensorPose.pitch = 0; newObs.sensorPose.roll = 0; newObs.sensedDistance = 0.001*THE_ROBOT->getSonarRange(i); } THE_ROBOT->unlock(); thereIsObservation = !obs.sensedData.empty(); // keep the last time: if (thereIsObservation) m_lastTimeSonars = time_cnt; #else MRPT_UNUSED_PARAM(thereIsObservation); MRPT_UNUSED_PARAM(obs); THROW_EXCEPTION("MRPT has been compiled with 'MRPT_BUILD_ARIA'=OFF, so this class cannot be used."); #endif }
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); }