int main( int argc, char **argv ){ // parse our args and make sure they were all accounted for ArSimpleConnector connector(&argc, argv); ArRobot robot; ArSick sick; double dist, angle = 0; // Allow for esc to release robot ArKeyHandler keyHandler; Aria::setKeyHandler(&keyHandler); robot.attachKeyHandler(&keyHandler); printf("You may press escape to exit\n"); if( !connector.parseArgs() || argc > 1 ){ connector.logOptions(); exit(1); } // add the laser to the robot robot.addRangeDevice(&sick); // try to connect, if we fail exit if( !connector.connectRobot(&robot) ){ printf("Could not connect to robot... exiting\n"); Aria::shutdown(); return 1; } // start the robot running, true so that if we lose connection the run stops robot.runAsync(true); // now set up the laser connector.setupLaser(&sick); sick.runAsync(); if( !sick.blockingConnect() ){ printf("Could not connect to SICK laser... exiting\n"); Aria::shutdown(); return 1; } robot.comInt(ArCommands::ENABLE, 1); ArPose pose(0, -1000, 0); robot.moveTo(pose); ArPose prev_pose = robot.getPose(); double total_distance = 0; printf("Connected\n"); ArUtil::sleep(1000); PathLog log("../Data/reactive.dat"); int iterations_wo_movement = 0; while( iterations_wo_movement < CONSECUTIVE_NON_MOTIONS ){ // Get updated measurement sick.lockDevice(); dist = sick.currentReadingPolar(-90, 90, &angle); sick.unlockDevice(); dist = (dist > 30000) ? 0 : dist - IDEAL_DISTANCE; trackRobot(&robot, dist, angle); ArUtil::sleep(500); pose = robot.getPose(); log.write(pose); total_distance += getDistance(prev_pose, pose); prev_pose = pose; // Determine if the robot is done tracking isRobotTracking(&iterations_wo_movement, dist, angle); } ArUtil::sleep(1000); log.close(); ofstream output; output.open("../Data/reactive_dist.dat", ios::out | ios::trunc); output << "Reactive 1 " << total_distance << endl; output.close(); Aria::exit(0); return 0; }
void S_TargetApproach( ArServerClient *serverclient, ArNetPacket *socket) { //Important: to halt current movement of camera, making the reading curret. G_PTZHandler->haltPanTilt(); cout << "The last step: TargetApproach!" <<endl; //G_PathPlanning->setCollisionRange(1000); //G_PathPlanning->setFrontClearance(40); //G_PathPlanning->setGoalDistanceTolerance(2500); double camAngle = -1 * G_PTZHandler->getPan(); double robotHeading = robot.getPose().getTh(); double robotCurrentX = robot.getPose().getX() ; double robotCurrentY = robot.getPose().getY(); double angle = 0.0; double distance =0.0; double targetX, targetY; int disThreshold = 500; //test mode // G_PTZHandler->panRel(-30); //G_PathPlanning->setFrontClearance(40); //G_PathPlanning->setObsThreshold(1); cout << "--------------Path Planning Information--------------------" <<endl; cout << "SafeCollisionRange = " << G_PathPlanning->getSafeCollisionRange() << endl; cout << "FrontClearance = " << G_PathPlanning->getFrontClearance() << endl << "GoalDistanceTolerance = " << G_PathPlanning->getGoalDistanceTolerance() << endl << "setGoalOccupiedFailDistance = " << G_PathPlanning->getGoalOccupiedFailDistance() << endl << "getObsThreshold = " << G_PathPlanning->getObsThreshold() <<endl << "getLocalPathFailDistance = " << G_PathPlanning->getLocalPathFailDistance() << endl; //getCurrentGoal //--------------- Set up the laser range device to read the distance from the target ----------------------------- // for(int i =0; i< 20;i++) //sick.currentReadingPolar(robotHeading+ camAngle -2.5, robotHeading+ camAngle +2.5, &angle); //Begin: sick.lockDevice(); //if (distance == 0 || distance>disThreshold) distance = sick.currentReadingPolar(/*robotHeading+*/ camAngle -2.5, /*robotHeading+*/ camAngle +2.5, &angle)-disThreshold; //double distance = sick.currentReadingPolar(89, 90, &angle); cout << "The closest reading is " << distance << " at " << angle << " degree , " << "disThreshold : " <<disThreshold << " " << robotHeading << " " << camAngle<< endl; sick.unlockDevice(); //---------------------------------------------------------------------------------------------------------------- //basic_turn(camAngle); cout << "Camera Angle is " << camAngle << endl; cout << "before calculation, check originalX, originalY " << robotCurrentX << " " << robotCurrentY << " robotHeading is " << robotHeading << endl<<endl; coordinateCalculation(robotCurrentX,robotCurrentY,&targetX,&targetY,camAngle,robotHeading,distance); // //cout << "before movement, check targetX, target Y" << targetX << " " << targetY << "robotHeading is "<< robotHeading << endl << endl; cout << "targetX : " <<targetX << " targetY" <<targetY; G_PathPlanning->pathPlanToPose(ArPose(targetX,targetY,camAngle),true,true); cout << "RobotMotion is processing..." <<endl; G_PTZHandler->reset(); ArUtil::sleep(200); G_PTZHandler->tiltRel(-10); while(G_PathPlanning->getState() != ArPathPlanningTask::REACHED_GOAL ) { if (G_PathPlanning->getState() == ArPathPlanningTask::FAILED_MOVE) { G_PathPlanning->cancelPathPlan();cout << "x " << robot.getPose().getX()<< " y " <<robot.getPose().getY() <<endl; break; } else if(G_PathPlanning->getState() == ArPathPlanningTask::FAILED_PLAN) { // getchar(); //getchar(); //getchar(); // getchar(); // disThreshold+=50; // // goto Begin; } } //serverclient->sendPacketTcp(socket); ArUtil::sleep(3000); cout << "RobotMotion is heading home..." <<endl; G_PathPlanning->pathPlanToPose(ArPose(0,0,0),true,true); while(G_PathPlanning->getState() != ArPathPlanningTask::REACHED_GOAL ) { //cout << G_PathPlanning->getState() <<endl; if (G_PathPlanning->getState() == ArPathPlanningTask::FAILED_MOVE) { G_PathPlanning->cancelPathPlan(); break; } // //else if(G_PathPlanning->getState() == ArPathPlanningTask::FAILED_PLAN) // // {G_PathPlanning->pathPlanToPose(ArPose(-300,30,0),true,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; }