void laserRequest_and_odom(ArServerClient *client, ArNetPacket *packet) { robot.lock(); ArNetPacket sending; sending.empty(); ArLaser* laser = robot.findLaser(1); if(!laser){ printf("Could not connect to Laser... exiting\n"); Aria::exit(1);} laser->lockDevice(); const std::list<ArSensorReading*> *sensorReadings = laser->getRawReadings(); // see ArRangeDevice interface doc sending.byte4ToBuf((ArTypes::Byte4)(sensorReadings->size())); for (std::list<ArSensorReading*>::const_iterator it2= sensorReadings->begin(); it2 != sensorReadings->end(); ++it2){ ArSensorReading* laserRead =*it2; sending.byte4ToBuf((ArTypes::Byte4)(laserRead->getRange())); //printf("%i,%i:",laserRead->getRange(),laserRead->getIgnoreThisReading()); } sending.byte4ToBuf((ArTypes::Byte4)(robot.getX())); sending.byte4ToBuf((ArTypes::Byte4)(robot.getY())); sending.byte4ToBuf((ArTypes::Byte4)(robot.getTh())); sending.byte4ToBuf((ArTypes::Byte4)(robot.getVel())); sending.byte4ToBuf((ArTypes::Byte4)(robot.getRotVel())); //printf("%1f,%1f,%1f\n",robot.getX(),robot.getY(),robot.getTh()); laser->unlockDevice(); robot.unlock(); sending.finalizePacket(); //sending.printHex(); client->sendPacketTcp(&sending); }
void sonar_stop(ArRobot* robot) { int numSonar; //Number of sonar on the robot int i; //Counter for looping //int j; numSonar = robot->getNumSonar(); //Get number of sonar ArSensorReading* sonarReading; //To hold each reading //for (j = 1; j < 6; j++) //{ robot->setVel(200); //Set translational velocity to 200 mm/s for (;;) { for (i = 0; i < numSonar; i++) //Loop through sonar { sonarReading = robot->getSonarReading(i); //Get each sonar reading cout << "Sonar reading " << i << " = " << sonarReading->getRange() << " Angle " << i << " = " << sonarReading->getSensorTh() << "\n"; //getchar(); if (sonarReading->getSensorTh() > -90 && sonarReading->getSensorTh() < 90 && sonarReading->getRange() < 500) robot->setVel(0); } } //<< " Angle " << i << " = " << //printf("Sonar Reading", i, "=",sonarReading) //robot->unlock(); //Lock robot during set up 18 //robot->comInt(ArCommands::ENABLE, 1); //Turn on the motors 19 //robot->setVel(200); //Set translational velocity to 200 mm/s //if (sonarReading[1] < 500) //{ //robot->setRotVel(0); //} }
void readSonars(ArRobot& robot, int numSonar){ char angle[64], value[64]; G_id += 1; ArSensorReading* sonarReading; string res; sprintf(value,"id=%d;",G_id); res += value; for (int i=0; i < numSonar; i++){ sonarReading = robot.getSonarReading(i); sprintf(value,"v%d=%05d;", i, sonarReading->getRange()); res += value; //cout << "Sonar reading " << i << " = " << sonarReading->getRange() << " Angle " << i << " = " << sonarReading->getSensorTh() << "\n"; } res += "\n"; fseek(G_SONAR_FD, SEEK_SET, 0); fwrite(res.c_str(), sizeof(char), res.size(), G_SONAR_FD); }
void ArLaserFilter::processReadings(void) { myLaser->lockDevice(); selfLockDevice(); const std::list<ArSensorReading *> *rdRawReadings; std::list<ArSensorReading *>::const_iterator rdIt; if ((rdRawReadings = myLaser->getRawReadings()) == NULL) { selfUnlockDevice(); myLaser->unlockDevice(); return; } size_t rawSize = myRawReadings->size(); size_t rdRawSize = myLaser->getRawReadings()->size(); while (rawSize < rdRawSize) { myRawReadings->push_back(new ArSensorReading); rawSize++; } // set where the pose was taken myCurrentBuffer.setPoseTaken( myLaser->getCurrentRangeBuffer()->getPoseTaken()); myCurrentBuffer.setEncoderPoseTaken( myLaser->getCurrentRangeBuffer()->getEncoderPoseTaken()); std::list<ArSensorReading *>::iterator it; ArSensorReading *rdReading; ArSensorReading *reading; #ifdef DEBUGRANGEFILTER FILE *file = NULL; file = ArUtil::fopen("/mnt/rdsys/tmp/filter", "w"); #endif std::map<int, ArSensorReading *> readingMap; int numReadings = 0; // first pass to copy the readings and put them into a map for (rdIt = rdRawReadings->begin(), it = myRawReadings->begin(); rdIt != rdRawReadings->end() && it != myRawReadings->end(); rdIt++, it++) { rdReading = (*rdIt); reading = (*it); *reading = *rdReading; readingMap[numReadings] = reading; numReadings++; } char buf[1024]; int i; int j; ArSensorReading *lastAddedReading = NULL; // now walk through the readings to filter them for (i = 0; i < numReadings; i++) { reading = readingMap[i]; // if we're ignoring this reading then just get on with life if (reading->getIgnoreThisReading()) continue; if (myMaxRange >= 0 && reading->getRange() > myMaxRange) { reading->setIgnoreThisReading(true); continue; } if (lastAddedReading != NULL) { if (lastAddedReading->getPose().findDistanceTo(reading->getPose()) < 50) { #ifdef DEBUGRANGEFILTER if (file != NULL) fprintf(file, "%.1f too close from last %6.0f\n", reading->getSensorTh(), lastAddedReading->getPose().findDistanceTo( reading->getPose())); #endif reading->setIgnoreThisReading(true); continue; } #ifdef DEBUGRANGEFILTER else if (file != NULL) fprintf(file, "%.1f from last %6.0f\n", reading->getSensorTh(), lastAddedReading->getPose().findDistanceTo( reading->getPose())); #endif } buf[0] = '\0'; bool goodAll = true; bool goodAny = false; if (myAnyFactor <= 0) goodAny = true; for (j = i - 1; (j >= 0 && //good && fabs(ArMath::subAngle(readingMap[j]->getSensorTh(), reading->getSensorTh())) <= myAngleToCheck); j--) { if (readingMap[j]->getIgnoreThisReading()) { #ifdef DEBUGRANGEFILTER sprintf(buf, "%s %6s", buf, "i"); #endif continue; } #ifdef DEBUGRANGEFILTER sprintf(buf, "%s %6d", buf, readingMap[j]->getRange()); #endif if (myAllFactor > 0 && !checkRanges(reading->getRange(), readingMap[j]->getRange(), myAllFactor)) goodAll = false; if (myAnyFactor > 0 && checkRanges(reading->getRange(), readingMap[j]->getRange(), myAnyFactor)) goodAny = true; } #ifdef DEBUGRANGEFILTER sprintf(buf, "%s %6d*", buf, reading->getRange()); #endif for (j = i + 1; (j < numReadings && //good && fabs(ArMath::subAngle(readingMap[j]->getSensorTh(), reading->getSensorTh())) <= myAngleToCheck); j++) { if (readingMap[j]->getIgnoreThisReading()) { #ifdef DEBUGRANGEFILTER sprintf(buf, "%s %6s", buf, "i"); #endif continue; } #ifdef DEBUGRANGEFILTER sprintf(buf, "%s %6d", buf, readingMap[j]->getRange()); #endif if (myAllFactor > 0 && !checkRanges(reading->getRange(), readingMap[j]->getRange(), myAllFactor)) goodAll = false; if (myAnyFactor > 0 && checkRanges(reading->getRange(), readingMap[j]->getRange(), myAnyFactor)) goodAny = true; } if (!goodAll || !goodAny) reading->setIgnoreThisReading(true); else lastAddedReading = reading; #ifdef DEBUGRANGEFILTER if (file != NULL) fprintf(file, "%5.1f %6d %c\t%s\n", reading->getSensorTh(), reading->getRange(), good ? 'g' : 'b', buf); #endif } #ifdef DEBUGRANGEFILTER if (file != NULL) fclose(file); #endif laserProcessReadings(); selfUnlockDevice(); myLaser->unlockDevice(); }
void ArLaser::laserProcessReadings(void) { // if we have no readings... don't do anything if (myRawReadings == NULL || myRawReadings->begin() == myRawReadings->end()) return; std::list<ArSensorReading *>::iterator sensIt; ArSensorReading *sReading; double x, y; double lastX = 0.0, lastY = 0.0; //unsigned int i = 0; ArTime len; len.setToNow(); bool clean; if (myCumulativeCleanInterval <= 0 || (myCumulativeLastClean.mSecSince() > myCumulativeCleanInterval)) { myCumulativeLastClean.setToNow(); clean = true; } else { clean = false; } myCurrentBuffer.setPoseTaken(myRawReadings->front()->getPoseTaken()); myCurrentBuffer.setEncoderPoseTaken( myRawReadings->front()->getEncoderPoseTaken()); myCurrentBuffer.beginRedoBuffer(); // walk the buffer of all the readings and see if we want to add them for (sensIt = myRawReadings->begin(); sensIt != myRawReadings->end(); ++sensIt) { sReading = (*sensIt); // if we have ignore readings then check them here if (!myIgnoreReadings.empty() && (myIgnoreReadings.find( (int) ceil(sReading->getSensorTh())) != myIgnoreReadings.end()) || myIgnoreReadings.find( (int) floor(sReading->getSensorTh())) != myIgnoreReadings.end()) sReading->setIgnoreThisReading(true); // see if the reading is valid if (sReading->getIgnoreThisReading()) continue; // if we have a max range then check it here... if (myMaxRange != 0 && sReading->getRange() > myMaxRange) { sReading->setIgnoreThisReading(true); } // see if the reading is valid... this is set up this way so that // max range readings can cancel out other readings, but will // still be ignored other than that... ones ignored for other // reasons were skipped above if (sReading->getIgnoreThisReading()) { internalProcessReading(sReading->getX(), sReading->getY(), sReading->getRange(), clean, true); continue; } // get our coords x = sReading->getX(); y = sReading->getY(); // see if we're checking on the filter near dist... if we are // and the reading is a good one we'll check the cumulative // buffer if (myMinDistBetweenCurrentSquared > 0.0000001) { // see where the last reading was //squaredDist = (x-lastX)*(x-lastX) + (y-lastY)*(y-lastY); // see if the reading is far enough from the last reading if (ArMath::squaredDistanceBetween(x, y, lastX, lastY) > myMinDistBetweenCurrentSquared) { lastX = x; lastY = y; // since it was a good reading, see if we should toss it in // the cumulative buffer... internalProcessReading(x, y, sReading->getRange(), clean, false); /* we don't do this part anymore since it wound up leaving // too many things not really tehre... if its outside of our // sensor angle to use to filter then don't let this one // clean (ArMath::fabs(sReading->getSensorTh()) > 50) // filterAddAndCleanCumulative(x, y, false); else*/ } // it wasn't far enough, skip this one and go to the next one else { continue; } } // we weren't filtering the readings, but see if it goes in the // cumulative buffer anyways else { internalProcessReading(x, y, sReading->getRange(), clean, false); } // now drop the reading into the current buffer myCurrentBuffer.redoReading(x, y); //i++; } myCurrentBuffer.endRedoBuffer(); /* Put this in to see how long the cumulative filtering is taking if (clean) printf("### %ld %d\n", len.mSecSince(), myCumulativeBuffer.getBuffer()->size()); */ internalGotReading(); }
void ArSickLogger::internalTakeReading(void) { const std::list<ArSensorReading *> *readings; std::list<ArSensorReading *>::const_iterator it; std::list<ArSensorReading *>::const_reverse_iterator rit; ArPose poseTaken; time_t msec; ArSensorReading *reading; bool usingAdjustedReadings; // we take readings in any of the following cases if we haven't // taken one yet or if we've been explicitly told to take one or if // we've gone further than myDistDiff if we've turned more than // myDegDiff if we've switched sign on velocity and gone more than // 50 mm (so it doesn't oscilate and cause us to trigger) if (myRobot->isConnected() && (!myFirstTaken || myTakeReadingExplicit || myLast.findDistanceTo(myRobot->getEncoderPose()) > myDistDiff || fabs(ArMath::subAngle(myLast.getTh(), myRobot->getEncoderPose().getTh())) > myDegDiff || (( (myLastVel < 0 && myRobot->getVel() > 0) || (myLastVel > 0 && myRobot->getVel() < 0)) && myLast.findDistanceTo(myRobot->getEncoderPose()) > 50))) { myWrote = true; mySick->lockDevice(); /// use the adjusted raw readings if we can, otherwise just use /// the raw readings like before if ((readings = mySick->getAdjustedRawReadings()) != NULL) { usingAdjustedReadings = true; } else { usingAdjustedReadings = false; readings = mySick->getRawReadings(); } if (readings == NULL || (it = readings->begin()) == readings->end() || myFile == NULL) { mySick->unlockDevice(); return; } myTakeReadingExplicit = false; myScanNumber++; if (usingAdjustedReadings) ArLog::log(ArLog::Normal, "Taking adjusted readings from the %d laser values", readings->size()); else ArLog::log(ArLog::Normal, "Taking readings from the %d laser values", readings->size()); myFirstTaken = true; myLast = myRobot->getEncoderPose(); poseTaken = (*readings->begin())->getEncoderPoseTaken(); myLastVel = myRobot->getVel(); msec = myStartTime.mSecSince(); fprintf(myFile, "scan1Id: %d\n", myScanNumber); fprintf(myFile, "time: %ld.%ld\n", msec / 1000, msec % 1000); /* ScanStudio isn't using these yet so don't print them fprintf(myFile, "velocities: %.2f %.2f\n", myRobot->getRotVel(), myRobot->getVel());*/ internalPrintPos(poseTaken); if (myUseReflectorValues) { fprintf(myFile, "reflector1: "); if (!mySick->isLaserFlipped()) { // make sure that the list is in increasing order for (it = readings->begin(); it != readings->end(); it++) { reading = (*it); if (!reading->getIgnoreThisReading()) fprintf(myFile, "%d ", reading->getExtraInt()); else fprintf(myFile, "0 "); } } else { for (rit = readings->rbegin(); rit != readings->rend(); rit++) { reading = (*rit); if (!reading->getIgnoreThisReading()) fprintf(myFile, "%d ", reading->getExtraInt()); else fprintf(myFile, "0 "); } } fprintf(myFile, "\n"); } /** Note that the the sick1: or scan1: must be the last thing in that timestamp, ie that you should put any other data before it. **/ if (myOldReadings) { fprintf(myFile, "sick1: "); if (!mySick->isLaserFlipped()) { // make sure that the list is in increasing order for (it = readings->begin(); it != readings->end(); it++) { reading = (*it); fprintf(myFile, "%d ", reading->getRange()); } } else { for (rit = readings->rbegin(); rit != readings->rend(); rit++) { reading = (*rit); fprintf(myFile, "%d ", reading->getRange()); } } fprintf(myFile, "\n"); } if (myNewReadings) { fprintf(myFile, "scan1: "); if (!mySick->isLaserFlipped()) { // make sure that the list is in increasing order for (it = readings->begin(); it != readings->end(); it++) { reading = (*it); if (!reading->getIgnoreThisReading()) fprintf(myFile, "%.0f %.0f ", reading->getLocalX() - mySick->getSensorPositionX(), reading->getLocalY() - mySick->getSensorPositionY()); else fprintf(myFile, "0 0 "); } } else { for (rit = readings->rbegin(); rit != readings->rend(); rit++) { reading = (*rit); if (!reading->getIgnoreThisReading()) fprintf(myFile, "%.0f %.0f ", reading->getLocalX() - mySick->getSensorPositionX(), reading->getLocalY() - mySick->getSensorPositionY()); else fprintf(myFile, "0 0 "); } } fprintf(myFile, "\n"); } mySick->unlockDevice(); } }
AREXPORT void ArLineFinder::fillPointsFromLaser(void) { const std::list<ArSensorReading *> *readings; std::list<ArSensorReading *>::const_iterator it; std::list<ArSensorReading *>::const_reverse_iterator rit; ArSensorReading *reading; int pointCount = 0; if (myPoints != NULL) delete myPoints; myPoints = new std::map<int, ArPose>; myRangeDevice->lockDevice(); readings = myRangeDevice->getRawReadings(); if (!myFlippedFound) { if (readings->begin() != readings->end()) { int size; size = readings->size(); it = readings->begin(); // advance along 10 readings for (int i = 0; i < 10 && i < size / 2; i++) it++; // see if we're flipped if (ArMath::subAngle((*(readings->begin()))->getSensorTh(), (*it)->getSensorTh()) > 0) myFlipped = true; else myFlipped = false; myFlippedFound = true; //printf("@@@ LINE %d %.0f\n", myFlipped, ArMath::subAngle((*(readings->begin()))->getSensorTh(), (*it)->getSensorTh())); } } if (readings->begin() == readings->end()) { myRangeDevice->unlockDevice(); return; } myPoseTaken = (*readings->begin())->getPoseTaken(); if (myFlipped) { for (rit = readings->rbegin(); rit != readings->rend(); rit++) { reading = (*rit); if (reading->getRange() > 5000 || reading->getIgnoreThisReading()) continue; (*myPoints)[pointCount] = reading->getPose(); pointCount++; } } else { for (it = readings->begin(); it != readings->end(); it++) { reading = (*it); if (reading->getRange() > 5000 || reading->getIgnoreThisReading()) continue; (*myPoints)[pointCount] = reading->getPose(); pointCount++; } } myRangeDevice->unlockDevice(); }
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); }
void RosAriaNode::publish() { // Note, this is called via SensorInterpTask callback (myPublishCB, named "ROSPublishingTask"). ArRobot object 'robot' sholud not be locked or unlocked. pos = robot->getPose(); tf::poseTFToMsg(tf::Transform(tf::createQuaternionFromYaw(pos.getTh()*M_PI/180), tf::Vector3(pos.getX()/1000, pos.getY()/1000, 0)), position.pose.pose); //Aria returns pose in mm. position.twist.twist.linear.x = robot->getVel()/1000; //Aria returns velocity in mm/s. position.twist.twist.linear.y = robot->getLatVel()/1000.0; position.twist.twist.angular.z = robot->getRotVel()*M_PI/180; position.header.frame_id = frame_id_odom; position.child_frame_id = frame_id_base_link; position.header.stamp = ros::Time::now(); pose_pub.publish(position); ROS_DEBUG("RosAria: publish: (time %f) pose x: %f, y: %f, angle: %f; linear vel x: %f, y: %f; angular vel z: %f", position.header.stamp.toSec(), (double)position.pose.pose.position.x, (double)position.pose.pose.position.y, (double)position.pose.pose.orientation.w, (double) position.twist.twist.linear.x, (double) position.twist.twist.linear.y, (double) position.twist.twist.angular.z ); // publishing transform odom->base_link odom_trans.header.stamp = ros::Time::now(); odom_trans.header.frame_id = frame_id_odom; odom_trans.child_frame_id = frame_id_base_link; odom_trans.transform.translation.x = pos.getX()/1000; odom_trans.transform.translation.y = pos.getY()/1000; odom_trans.transform.translation.z = 0.0; odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(pos.getTh()*M_PI/180); odom_broadcaster.sendTransform(odom_trans); // getStallValue returns 2 bytes with stall bit and bumper bits, packed as (00 00 FrontBumpers RearBumpers) int stall = robot->getStallValue(); unsigned char front_bumpers = (unsigned char)(stall >> 8); unsigned char rear_bumpers = (unsigned char)(stall); bumpers.header.frame_id = frame_id_bumper; bumpers.header.stamp = ros::Time::now(); std::stringstream bumper_info(std::stringstream::out); // Bit 0 is for stall, next bits are for bumpers (leftmost is LSB) for (unsigned int i=0; i<robot->getNumFrontBumpers(); i++) { bumpers.front_bumpers[i] = (front_bumpers & (1 << (i+1))) == 0 ? 0 : 1; bumper_info << " " << (front_bumpers & (1 << (i+1))); } ROS_DEBUG("RosAria: Front bumpers:%s", bumper_info.str().c_str()); bumper_info.str(""); // Rear bumpers have reverse order (rightmost is LSB) unsigned int numRearBumpers = robot->getNumRearBumpers(); for (unsigned int i=0; i<numRearBumpers; i++) { bumpers.rear_bumpers[i] = (rear_bumpers & (1 << (numRearBumpers-i))) == 0 ? 0 : 1; bumper_info << " " << (rear_bumpers & (1 << (numRearBumpers-i))); } ROS_DEBUG("RosAria: Rear bumpers:%s", bumper_info.str().c_str()); bumpers_pub.publish(bumpers); //Publish battery information // TODO: Decide if BatteryVoltageNow (normalized to (0,12)V) is a better option std_msgs::Float64 batteryVoltage; batteryVoltage.data = robot->getRealBatteryVoltageNow(); voltage_pub.publish(batteryVoltage); if(robot->haveStateOfCharge()) { std_msgs::Float32 soc; soc.data = robot->getStateOfCharge()/100.0; state_of_charge_pub.publish(soc); } // publish recharge state if changed char s = robot->getChargeState(); if(s != recharge_state.data) { ROS_INFO("RosAria: publishing new recharge state %d.", s); recharge_state.data = s; recharge_state_pub.publish(recharge_state); } // publish motors state if changed bool e = robot->areMotorsEnabled(); if(e != motors_state.data || !published_motors_state) { ROS_INFO("RosAria: publishing new motors state %d.", e); motors_state.data = e; motors_state_pub.publish(motors_state); published_motors_state = true; } // Publish sonar information, if enabled. if (use_sonar) { sensor_msgs::PointCloud cloud; //sonar readings. cloud.header.stamp = position.header.stamp; //copy time. // sonar sensors relative to base_link cloud.header.frame_id = frame_id_sonar; // Log debugging info std::stringstream sonar_debug_info; sonar_debug_info << "Sonar readings: "; for (int i = 0; i < robot->getNumSonar(); i++) { ArSensorReading* reading = NULL; reading = robot->getSonarReading(i); if(!reading) { ROS_WARN("RosAria: Did not receive a sonar reading."); continue; } // getRange() will return an integer between 0 and 5000 (5m) sonar_debug_info << reading->getRange() << " "; // local (x,y). Appears to be from the centre of the robot, since values may // exceed 5000. This is good, since it means we only need 1 transform. // x & y seem to be swapped though, i.e. if the robot is driving north // x is north/south and y is east/west. // //ArPose sensor = reading->getSensorPosition(); //position of sensor. // sonar_debug_info << "(" << reading->getLocalX() // << ", " << reading->getLocalY() // << ") from (" << sensor.getX() << ", " // << sensor.getY() << ") ;; " ; //add sonar readings (robot-local coordinate frame) to cloud geometry_msgs::Point32 p; p.x = reading->getLocalX() / 1000.0; p.y = reading->getLocalY() / 1000.0; p.z = 0.0; cloud.points.push_back(p); } ROS_DEBUG_STREAM(sonar_debug_info.str()); sonar_pub.publish(cloud); } }
void ArLaserFilter::processReadings(void) { myLaser->lockDevice(); selfLockDevice(); const std::list<ArSensorReading *> *rdRawReadings; std::list<ArSensorReading *>::const_iterator rdIt; if ((rdRawReadings = myLaser->getRawReadings()) == NULL) { selfUnlockDevice(); myLaser->unlockDevice(); return; } size_t rawSize = myRawReadings->size(); size_t rdRawSize = myLaser->getRawReadings()->size(); while (rawSize < rdRawSize) { myRawReadings->push_back(new ArSensorReading); rawSize++; } // set where the pose was taken myCurrentBuffer.setPoseTaken( myLaser->getCurrentRangeBuffer()->getPoseTaken()); myCurrentBuffer.setEncoderPoseTaken( myLaser->getCurrentRangeBuffer()->getEncoderPoseTaken()); std::list<ArSensorReading *>::iterator it; ArSensorReading *rdReading; ArSensorReading *reading; #ifdef DEBUGRANGEFILTER FILE *file = NULL; //file = ArUtil::fopen("/mnt/rdsys/tmp/filter", "w"); file = ArUtil::fopen("/tmp/filter", "w"); #endif std::map<int, ArSensorReading *> readingMap; int numReadings = 0; // first pass to copy the readings and put them into a map for (rdIt = rdRawReadings->begin(), it = myRawReadings->begin(); rdIt != rdRawReadings->end() && it != myRawReadings->end(); rdIt++, it++) { rdReading = (*rdIt); reading = (*it); *reading = *rdReading; readingMap[numReadings] = reading; numReadings++; } // if we're not doing any filtering, just short circuit out now if (myAllFactor <= 0 && myAnyFactor <= 0 && myAnyMinRange <= 0) { laserProcessReadings(); copyReadingCount(myLaser); selfUnlockDevice(); myLaser->unlockDevice(); #ifdef DEBUGRANGEFILTER if (file != NULL) fclose(file); #endif return; } char buf[1024]; int i; int j; //ArSensorReading *lastAddedReading = NULL; // now walk through the readings to filter them for (i = 0; i < numReadings; i++) { reading = readingMap[i]; // if we're ignoring this reading then just get on with life if (reading->getIgnoreThisReading()) continue; /* Taking this check out since the base class does it now and if * it gets marked ignore now it won't get used for clearing * cumulative readings if (myMaxRange >= 0 && reading->getRange() > myMaxRange) { #ifdef DEBUGRANGEFILTER if (file != NULL) fprintf(file, "%.1f beyond max range at %d\n", reading->getSensorTh(), reading->getRange()); #endif reading->setIgnoreThisReading(true); continue; } */ if (myAnyMinRange >= 0 && reading->getRange() < myAnyMinRange && (reading->getSensorTh() < myAnyMinRangeLessThanAngle || reading->getSensorTh() > myAnyMinRangeGreaterThanAngle)) { #ifdef DEBUGRANGEFILTER if (file != NULL) fprintf(file, "%.1f within min range at %d\n", reading->getSensorTh(), reading->getRange()); #endif reading->setIgnoreThisReading(true); continue; } /* if (lastAddedReading != NULL) { if (lastAddedReading->getPose().findDistanceTo(reading->getPose()) < 50) { #ifdef DEBUGRANGEFILTER if (file != NULL) fprintf(file, "%.1f too close from last %6.0f\n", reading->getSensorTh(), lastAddedReading->getPose().findDistanceTo( reading->getPose())); #endif reading->setIgnoreThisReading(true); continue; } #ifdef DEBUGRANGEFILTER else if (file != NULL) fprintf(file, "%.1f from last %6.0f\n", reading->getSensorTh(), lastAddedReading->getPose().findDistanceTo( reading->getPose())); #endif } */ buf[0] = '\0'; bool goodAll = true; bool goodAny = false; bool goodMinRange = true; if (myAnyFactor <= 0) goodAny = true; for (j = i - 1; (j >= 0 && //good && fabs(ArMath::subAngle(readingMap[j]->getSensorTh(), reading->getSensorTh())) <= myAngleToCheck); j--) { /* You can't skip these, or you get onesided filtering if (readingMap[j]->getIgnoreThisReading()) { #ifdef DEBUGRANGEFILTER sprintf(buf, "%s %6s", buf, "i"); #endif continue; } */ #ifdef DEBUGRANGEFILTER sprintf(buf, "%s %6d", buf, readingMap[j]->getRange()); #endif if (myAllFactor > 0 && !checkRanges(reading->getRange(), readingMap[j]->getRange(), myAllFactor)) goodAll = false; if (myAnyFactor > 0 && checkRanges(reading->getRange(), readingMap[j]->getRange(), myAnyFactor)) goodAny = true; if (myAnyMinRange > 0 && (reading->getSensorTh() < myAnyMinRangeLessThanAngle || reading->getSensorTh() > myAnyMinRangeGreaterThanAngle) && readingMap[j]->getRange() <= myAnyMinRange) goodMinRange = false; } #ifdef DEBUGRANGEFILTER sprintf(buf, "%s %6d*", buf, reading->getRange()); #endif for (j = i + 1; (j < numReadings && //good && fabs(ArMath::subAngle(readingMap[j]->getSensorTh(), reading->getSensorTh())) <= myAngleToCheck); j++) { // you can't ignore these or you get one sided filtering /* if (readingMap[j]->getIgnoreThisReading()) { #ifdef DEBUGRANGEFILTER sprintf(buf, "%s %6s", buf, "i"); #endif continue; } */ #ifdef DEBUGRANGEFILTER sprintf(buf, "%s %6d", buf, readingMap[j]->getRange()); #endif if (myAllFactor > 0 && !checkRanges(reading->getRange(), readingMap[j]->getRange(), myAllFactor)) goodAll = false; if (myAnyFactor > 0 && checkRanges(reading->getRange(), readingMap[j]->getRange(), myAnyFactor)) goodAny = true; if (myAnyMinRange > 0 && (reading->getSensorTh() < myAnyMinRangeLessThanAngle || reading->getSensorTh() > myAnyMinRangeGreaterThanAngle) && readingMap[j]->getRange() <= myAnyMinRange) goodMinRange = false; } if (!goodAll || !goodAny || !goodMinRange) reading->setIgnoreThisReading(true); /* else lastAddedReading = reading; */ #ifdef DEBUGRANGEFILTER if (file != NULL) fprintf(file, "%5.1f %6d %c\t%s\n", reading->getSensorTh(), reading->getRange(), goodAll && goodAny && goodMinRange ? 'g' : 'b', buf); #endif } #ifdef DEBUGRANGEFILTER if (file != NULL) fclose(file); #endif laserProcessReadings(); copyReadingCount(myLaser); selfUnlockDevice(); myLaser->unlockDevice(); }