void SensorDetectPopup::sensorTask(void) { // Basic obstacle detection if (myPopupDisplayed) return; double detectAngle, detectRange; detectRange = myRobot->checkRangeDevicesCurrentPolar(-90, 90, &detectAngle); if (detectRange > 0 && detectRange <= 500) { if(myPrevObstacleAngleValid && fabs(detectAngle - myPrevObstacleAngle) < 0.0001) return; ArLog::log(ArLog::Normal, "popupExample: New obstacle detected at range %f, angle %f. Displaying popup dialog on client...", detectRange, detectAngle); ArServerHandlerPopupInfo info("popupExample", // ID "Object Detected", // Title "A range sensor detected a reading within 0.5 meters of the robot.", // Message ArServerHandlerPopup::INFORMATION, // Type 0, // Default button 0, // Cancel/escape button 5, // Timeout (sec.) NULL, // Timeout String "OK", "Acknowleged.", // Button 0 Label/Acknowlegement "Turn Around", "Requested rotate...", // Button 1 Label/Acknowlegement "Shut Down", "Shutting down server..." // Button 2 Label/Acknowlegement ); int id = myPopupServer->createPopup(&info, myPopupClosedCB); ArLog::log(ArLog::Normal, "\t...Created a popup with ID=%d", id); myPopupDisplayed = true; myPrevObstacleAngle = detectAngle; myPrevObstacleAngleValid = true; } }
int main(int argc, char **argv) { Aria::init(); ArArgumentParser argParser(&argc, argv); argParser.loadDefaultArguments(); ArRobot robot; ArSick laser; std::ofstream stream; // for loggin time_t timer; char buffer[120]; //for loggin ArRobotConnector robotConnector(&argParser, &robot); ArLaserConnector laserConnector(&argParser, &robot, &robotConnector); int32_t count = 0; readyLog(stream); if (!robotConnector.connectRobot()) { ArLog::log(ArLog::Terse, "Could not connect to the robot."); if (argParser.checkHelpAndWarnUnparsed()) { // -help not given, just exit. Aria::logOptions(); Aria::exit(1); return 1; } } // Trigger argument parsing if (!Aria::parseArgs() || !argParser.checkHelpAndWarnUnparsed()) { Aria::logOptions(); Aria::exit(1); return 1; } ArKeyHandler keyHandler; Aria::setKeyHandler(&keyHandler); robot.attachKeyHandler(&keyHandler); puts("This program will make the robot wander around. It uses some avoidance\n" "actions if obstacles are detected, otherwise it just has a\n" "constant forward velocity.\n\nPress CTRL-C or Escape to exit."); //ArSonarDevice sonar; //robot.addRangeDevice(&sonar); robot.addRangeDevice(&laser); robot.runAsync(true); // try to connect to laser. if fail, warn but continue, using sonar only if (!laserConnector.connectLasers()) { ArLog::log(ArLog::Normal, "Warning: unable to connect to requested lasers, will wander using robot sonar only."); } double sampleAngle, dist; auto sampleRange = laser.currentReadingPolar(-20, 20, &sampleAngle); auto degreeStr = laser.getDegreesChoicesString(); std::cout << "auto sampleRange = laser.currentReadingPolar(-20, 20, &sampleAngle);" << sampleRange << std::endl; std::cout << "auto degreeStr = laser.getDegreesChoicesString(); : " << degreeStr << std::endl; // turn on the motors, turn off amigobot sounds robot.enableMotors(); //robot.getLaserMap() robot.comInt(ArCommands::SOUNDTOG, 0); // add a set of actions that combine together to effect the wander behavior ArActionStallRecover recover; ArActionBumpers bumpers; ArActionAvoidFront avoidFrontNear("Avoid Front Near", 100, 0); ArActionAvoidFront avoidFrontFar; ArActionConstantVelocity constantVelocity("Constant Velocity", 400); robot.addAction(&recover, 100); robot.addAction(&bumpers, 75); robot.addAction(&avoidFrontNear, 50); robot.addAction(&avoidFrontFar, 49); robot.addAction(&constantVelocity, 25); stream << "IMPORTANT !! == robot.getRobotRadius() : " << robot.getRobotRadius << std::endl; std::string timestamp; while (1) { // // time(&timer); strftime(buffer, 80, " - timestamp : %I:%M:%S", localtime(&timer)); timestamp = buffer; dist = robot.checkRangeDevicesCurrentPolar(-70, 70, &sampleAngle) - robot.getRobotRadius(); stream << count << timestamp << "checkRangeDevicesCurrentPolar(-70, 70, &angle) : " << dist << std::endl; Sleep(500); count++; } // wwill add processor here // wait for robot task loop to end before exiting the program robot.waitForRunExit(); Aria::exit(0); return 0; }