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;
}
