Exemplo n.º 1
0
int main(int argc, char **argv)
{
  Aria::init();

  ArSimpleConnector connector(&argc, argv);
  ArRobot robot;
  ArSick sick;

  if (!Aria::parseArgs() || argc > 1)
  {
    Aria::logOptions();
    Aria::exit(1); // exit program with error code 1
    return 1;
  }
  
  ArKeyHandler keyHandler;
  Aria::setKeyHandler(&keyHandler);
  robot.attachKeyHandler(&keyHandler);


  robot.addRangeDevice(&sick);

  // Create the ArLineFinder object. Set it to log lots of information about its
  // processing.
  ArLineFinder lineFinder(&sick);
  lineFinder.setVerbose(true);

  // Add key callbacks that simply call the ArLineFinder::getLinesAndSaveThem()
  // function, which searches for lines in the current set of laser sensor
  // readings, and saves them in files with the names 'points' and 'lines'.
  ArFunctorC<ArLineFinder> findLineCB(&lineFinder, 
				  &ArLineFinder::getLinesAndSaveThem);
  keyHandler.addKeyHandler('f', &findLineCB);
  keyHandler.addKeyHandler('F', &findLineCB);

  
  ArLog::log(ArLog::Normal, "lineFinderExample: connecting to robot...");
  if (!connector.connectRobot(&robot))
  {
    printf("Could not connect to robot... exiting\n");
    Aria::exit(1);  // exit program with error code 1
    return 1;
  }
  robot.runAsync(true);

  // now set up the laser
  ArLog::log(ArLog::Normal, "lineFinderExample: connecting to SICK laser...");
  connector.setupLaser(&sick);
  sick.runAsync();
  if (!sick.blockingConnect())
  {
    printf("Could not connect to SICK laser... exiting\n");
    Aria::exit(1);
    return 1;
  }

  printf("If you press the 'f' key the points and lines found will be saved\n");
  printf("Into the 'points' and 'lines' file in the current working directory\n");

  robot.waitForRunExit();
  Aria::exit(0);
  return 0;
}
Exemplo n.º 2
0
int main(int argc, char **argv)
{
    int ret; //Don't know what this variable is for
    
    ArRobot robot;// Robot object
    
    ArSick sick; // Laser scanner
    ArSerialConnection laserCon; // Scanner connection
    
    ArSerialConnection con; // Robot connection
    
    std::string str; // Standard output
    
    
    // sonar, must be added to the robot
    ArSonarDevice sonar;
    
    // the actions we'll use to wander
    // recover from stalls
    ArActionStallRecover recover;
    // react to bumpers
    ArActionBumpers bumpers;
    // limiter for close obstacles
    ArActionLimiterForwards limiter("speed limiter near", 300, 600, 250, 1.1);
    // limiter for far away obstacles
    ArActionLimiterForwards limiterFar("speed limiter far", 300, 1100, 600, 1.1);
    // limiter for the table sensors
    ArActionLimiterTableSensor tableLimiter;
    // actually move the robot
    ArActionConstantVelocity constantVelocity("Constant Velocity", 400);
    // turn the orbot if its slowed down
    ArActionTurn turn;
    
    
    
    
    // mandatory init
    Aria::init();
    
    // Parse all our args
    ArSimpleConnector connector(&argc, argv);
    connector.parseArgs();
    
    if (argc > 1)
    {
        connector.logOptions();
        exit(1);
    }
    
    // add the sonar to the robot
    robot.addRangeDevice(&sonar);
    // add the laser to the robot
    robot.addRangeDevice(&sick);
    
    // NOTE: HARDCODED USB PORT!
    // Attempt to open hard-coded USB to robot
    if ((ret = con.open("/dev/ttyUSB2")) != 0){
        // If connection fails, exit
        str = con.getOpenMessage(ret);
        printf("Open failed: %s\n", str.c_str());
        Aria::shutdown();
        return 1;
    }
    
    // set the robot to use the given connection
    robot.setDeviceConnection(&con);
    
    // do a blocking connect, if it fails exit
    if (!robot.blockingConnect())
    {
        printf("Could not connect to robot... exiting\n");
        Aria::shutdown();
        return 1;
    }
    
    
    
    
    // turn on the motors, turn off amigobot sounds
    //robot.comInt(ArCommands::SONAR, 0);
    robot.comInt(ArCommands::SOUNDTOG, 0);
    
    
    
    // start the robot running, true so that if we lose connection the run stops
    robot.runAsync(true);
    
    
    // Attempt to connect to SICK using another hard-coded USB connection
    sick.setDeviceConnection(&laserCon);
    if((ret=laserCon.open("/dev/ttyUSB3")) !=0) {
        //If connection fails, shutdown
        Aria::shutdown();
        return 1;
    }
    
    //Configure the SICK
    sick.configureShort(false,/*not using sim*/ArSick::BAUD38400,ArSick::DEGREES180,ArSick::INCREMENT_HALF);
    
    //Run the sick
    sick.runAsync();
    
    // Presumably test to make sure that the connection is good
    if(!sick.blockingConnect()){
        printf("Could not get sick...exiting\n");
        Aria::shutdown();
        return 1;
    }
    printf("We are connected to the laser!");
    
    /*
     robot.lock();
     robot.comInt(ArCommands::ENABLE, 1);
     robot.unlock();
     */
    int range [361] = {0};
    int drange [360] = {0};
    int i = 0;
    int obj_range [2];
    int old_range [360]={0};
    clock_t now, prev;
    while(1){
        range [361] = {0};
        drange [360] = {0};
        i = 0;
        obj_range[2];
        
        std::list<ArSensorReading *> *readings;
        std::list<ArSensorReading *>::iterator it;
        sick.lockDevice();
        readings=(list<ArSensorReading *,allocator<ArSensorReading *> > *)sick.getRawReadings();
        
        if(NULL!=readings){
            if ((readings->end() != readings->begin())){
                for (it = readings->begin(); it!= readings->end(); it++){
                    //	      std::cout << (*it)->getRange()<<" ";
                    range[i] = ((*it)->getRange());
                    if(i){
                        drange[i-1] = range[i] - range[i-1];
                        printf("%f %i %i\r\n", (float)i/2.0, range[i], drange[i-1]);
                    }
                    i++;
                }
                int i = 0;
                //detect the object range
                while (i < 360) {
                    if (range[i]>Default_Distance + alpha) {
                        ;
                    } else {
                        if (obj_range[0]=0)
                            obj_range[0]=i;
                        else
                            obj_range[1]=i;
                    }
                }
                if (!now)
                    prev=now;
                now=clock();
                duration=now-prev;
                /******moving straight*******/
                float speed = avg_speed(obj_range,old_range,range,(float)duration)
                
                /*while(i < 360){
                 int r_edge = 0;
                 int l_edge = 0;
                 float obsticle_degree = 0;
                 if(drange[i] > D_DISTANCE){
                 r_edge = i;
                 while(drange[i] > -(D_DISTANCE)){
                 i++;
                 }
                 l_edge = i;
                 obsticle_degree = (r_edge + (l_edge - r_edge)/2.0)/2.0;
                 printf("\r\n object detected at %f\r\n", obsticle_degree);
                 }
                 std::cout<<std::endl;
                 }*/
            }
            else{
                std::cout << "(readings->end() == readings -> begin())" << std::endl;
            }
        }
        else{
// when we connect turn off the sonar, turn on the motors, and disable amigobot
// sound
void ConnHandler::connected(void)
{
  myRobot->comInt(ArCommands::SONAR, 0);
  myRobot->comInt(ArCommands::ENABLE, 1);
  myRobot->comInt(ArCommands::SOUNDTOG, 0);
}
Exemplo n.º 4
0
int main(void)
{
  ArSerialConnection con;
  ArRobot robot;
  int ret;
  std::string str;
  ArActionLimiterForwards limiter("speed limiter near", 300, 600, 250);
  ArActionLimiterForwards limiterFar("speed limiter far", 300, 1100, 400);
  ArActionLimiterBackwards backwardsLimiter;
  ArActionConstantVelocity stop("stop", 0);
  ArActionConstantVelocity backup("backup", -200);
  ArSonarDevice sonar;
  ArACTS_1_2 acts;
  ArSonyPTZ sony(&robot);
  ArGripper gripper(&robot, ArGripper::GENIO);
  
  Acquire acq(&acts, &gripper);
  DriveTo driveTo(&acts, &gripper, &sony);
  PickUp pickUp(&acts, &gripper, &sony);

  TakeBlockToWall takeBlock(&robot, &gripper, &sony, &acq, &driveTo, &pickUp,
			    &backup);

  Aria::init();

   if (!acts.openPort(&robot))
   {
     printf("Could not connect to acts\n");
     exit(1);
   }
  
  robot.addRangeDevice(&sonar);
  //con.setBaud(38400);
  if ((ret = con.open()) != 0)
  {
    str = con.getOpenMessage(ret);
    printf("Open failed: %s\n", str.c_str());
    Aria::shutdown();
    return 1;
  }

  robot.setDeviceConnection(&con);
  if (!robot.blockingConnect())
  {
    printf("Could not connect to robot... exiting\n");
    Aria::shutdown();
    return 1;
  }

  sony.init();
  ArUtil::sleep(1000);
  //robot.setAbsoluteMaxTransVel(400);

  robot.setStateReflectionRefreshTime(250);
  robot.comInt(ArCommands::ENABLE, 1);
  robot.comInt(ArCommands::SOUNDTOG, 0);

  ArUtil::sleep(200);
  robot.addAction(&limiter, 100);
  robot.addAction(&limiterFar, 99);
  robot.addAction(&backwardsLimiter, 98);
  robot.addAction(&acq, 77);
  robot.addAction(&driveTo, 76);
  robot.addAction(&pickUp, 75);
  robot.addAction(&backup, 50);
  robot.addAction(&stop, 30);

  robot.run(true);
  
  Aria::shutdown();
  return 0;
}
Exemplo n.º 5
0
int main(int argc, char **argv) 
{
  std::string str;
  int ret;
  int successes = 0, failures = 0;
  int action;
  bool exitOnFailure = true;
  
  ArSerialConnection con;
  ArRobot robot;

  srand(time(NULL));
// if (!exitOnFailure)
//    ArLog::init(ArLog::None, ArLog::Terse);
  //else
  //ArLog::init(ArLog::None);
  while (1)
  {
    if (con.getStatus() != ArDeviceConnection::STATUS_OPEN &&
	(ret = con.open(ArUtil::COM2)) != 0)
    {
      str = con.getOpenMessage(ret);
      printf("Open failed: %s\n", str.c_str());
      ++failures;
      if (exitOnFailure)
      {
	printf("Failed\n");
	exit(0);
      }
      else
      {
	ArUtil::sleep(200);
	continue;
      }
    }
    robot.setDeviceConnection(&con);
    ArUtil::sleep((rand() % 5) * 100);
    if (robot.blockingConnect())
    {
      robot.comInt(ArCommands::SONAR, 0);
      robot.comInt(ArCommands::SOUNDTOG, 0);
      robot.comInt(ArCommands::PLAYLIST, 0);
      robot.comInt(ArCommands::ENCODER, 1);
      ArUtil::sleep((rand() % 20) * 100);
      ++successes;
      // okay, now try to leave it in a messed up state
      action = rand() % 8;
      switch (action) {
      case 0:
	printf("Discon  0 ");
	robot.disconnect();
	ArUtil::sleep(100);
	robot.com(0);
	break;
      case 1:
	printf("Discon  1 ");
	robot.disconnect();
	ArUtil::sleep(100);
	robot.com(0);
	ArUtil::sleep(100);
	robot.com(1);
	break;
      case 2:
	printf("Discon  2 ");
	robot.disconnect();
	ArUtil::sleep(100);
	robot.com(0);
	ArUtil::sleep(100);
	robot.com(1);
	ArUtil::sleep(100);
	robot.com(2);
	break;
      case 3:
	printf("Discon 10 ");
	robot.disconnect();
	ArUtil::sleep(100);
	robot.com(10);
	break;
      case 4:
	printf("Discon    ");
	robot.disconnect();
	break;
      default:
	printf("Leave     ");
	break;
      }
    }
    else
    {
      if (exitOnFailure)
      {
	printf("Failed after %d tries.\n", successes);
	exit(0);
      }
      printf("Failed ");
      ++failures;
    }
    if ((rand() % 2) == 0)
    {
      printf(" ! RadioDisconnect ! ");
      con.write("|||\15", strlen("!!!\15"));
      
      ArUtil::sleep(100);
      con.write("WMD\15", strlen("WMD\15"));
      ArUtil::sleep(200);
    }
    if ((rand() % 2) == 0)
    {
      printf(" ! ClosePort !\n");
      con.close();
    }
    else
      printf("\n");
    printf("#### %d successes %d failures, %% %.2f success\n", successes, failures,
	   (float)successes/(float)(successes+failures)*100);


    ArUtil::sleep((rand() % 2)* 1000);
  }
  return 0; 
}
Exemplo n.º 6
0
int main (int argc, char** argv) {
	
	Aria::init();
	Arnl::init();
	ArRobot robot;
	ArArgumentParser parser(&argc, argv);
	parser.loadDefaultArguments();
	ArRobotConnector robotConnector(&parser, &robot);
	if (!robotConnector.connectRobot()) {
		ArLog::log(ArLog::Terse, "Could not connect to the robot.");
		if(parser.checkHelpAndWarnUnparsed())
		{
			// -help not given, just exit.
			Aria::logOptions();
			Aria::exit(1);
			return 1;
		}
	}
	
	ArSonarDevice sonarDev;
	ArLog::log(ArLog::Normal, "OK");
	char fileDir[1024];
	ArUtil::addDirectories(fileDir, sizeof(fileDir), Aria::getDirectory(), "examples");
	ArLog::log(ArLog::Normal, "Installation directory is: %s\nMaps directory is: %s\n", Aria::getDirectory(), fileDir);
	//		strcpy(fileDir, "columbia.map");
	ArLog::log(ArLog::Normal, "file Maps directory is: %s\n",fileDir);
	ArMap map(fileDir);
	map.readFile("columbia.map");
	// set it up to ignore empty file names (otherwise the parseFile
	// on the config will fail)
	map.setIgnoreEmptyFileName(true);



	robot.addRangeDevice(&sonarDev);
	// set home pose
	robot.moveTo(ArPose(0,0,0));

	ArPathPlanningTask pathPlan(&robot, &sonarDev, &map);
	ArActionGoto gotoPoseAction("goto");
//	gotoPoseAction.activate();
//	pathPlan.getPathPlanActionGroup()->addAction(&gotoPoseAction, 50);
//	pathPlan.getPathPlanAction()->activate();
	ArForbiddenRangeDevice forbidden(&map);
	robot.addRangeDevice(&forbidden);
	pathPlan.addRangeDevice(&forbidden, ArPathPlanningTask::CURRENT);
//	pathPlan.planAndSetupAction(ArPose(0, 0, 0));
//	pathPlan.
/*
	if (pathPlan.pathPlanToPose(ArPose(1000, 1000, 0), true, true))
		ArLog::log(ArLog::Normal, "OK");
	else
		ArLog::log(ArLog::Normal, "FAILED");
*/
	// create functor
	ArGlobalFunctor1<ArPose> goalDone(&addGoalDone);// = new ArGlobalFunctor1(&addGoalDone);
	ArGlobalFunctor1<ArPose> goalFail(&addGoalFailed);
	ArGlobalFunctor1<ArPose> goalFinished(&addGoalFinished);
	ArGlobalFunctor1<ArPose> goalInterrupted(&addGoalInterrupted);

	// add functor
	pathPlan.addGoalDoneCB(&goalDone);
	pathPlan.addGoalFailedCB(&goalFail);
	pathPlan.addGoalFinishedCB(&goalFinished);
	pathPlan.addGoalInterruptedCB(&goalInterrupted);

	robot.runAsync(true);
	robot.enableMotors();
	pathPlan.runAsync();

//	ArPathPlanningTask::PathPlanningState state = pathPlan.getState();
//	while(!pathPlan.pathPlanToPose(ArPose(1000, 1000, 0), true, true));
	pathPlan.pathPlanToPose(ArPose(1000, 1000, 0), true, true);
	ArPathPlanningTask::PathPlanningState state = pathPlan.getState();
	char* s = "";
	switch(state)
	  {
	    case ArPathPlanningTask::NOT_INITIALIZED: {s = "NOT_INITIALIZED"; break;}
	    case ArPathPlanningTask::PLANNING_PATH: { s = "PLANNING_PATH";break;}
	    case ArPathPlanningTask::MOVING_TO_GOAL:{s = "MOVING_TO_GOAL";break;}
	    case ArPathPlanningTask::REACHED_GOAL: {s = "REACHED_GOAL";break;}
	    case ArPathPlanningTask::FAILED_PLAN: { s = "FAILED_PLAN";break;}
	    case ArPathPlanningTask::FAILED_MOVE: { s="FAILED_MOVE";break;}
	    case ArPathPlanningTask::ABORTED_PATHPLAN:{s = "ABORTED_PATHPLAN";break;}
//	    case ArPathPlanningTask::INVALID:
//	    default:
//	      return "UNKNOWN";
	  }
	ArLog::log(ArLog::Normal,s);
	robot.waitForRunExit();
//	pathPlan.
//	char* text = new char[512];
//	pathPlan.getStatusString(text, sizeof(text));
//	 printf("Planning status: %s.\n", text);
//	while(pathPlan.pathPlanToPose(ArPose(1000, 1000, 0), true, true));
	Aria::shutdown();
//	Arnl::s;
	Aria::exit(0);
}
int main(int argc, char **argv)
{
    Aria::init();
    ArArgumentParser parser(&argc, argv);
    parser.loadDefaultArguments();
    ArRobot robot;
    ArRobotConnector robotConnector(&parser, &robot);
    if(!robotConnector.connectRobot())
    {
      ArLog::log(ArLog::Terse, "teleopActionsExample: Could not connect to the robot.");
      if(parser.checkHelpAndWarnUnparsed())
      {
          Aria::logOptions();
          Aria::exit(1);
      }
    }
    if (!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed())
    {
      Aria::logOptions();
      Aria::exit(1);
    }
    ArLog::log(ArLog::Normal, "teleopActionsExample: Connected.");

  
  // limiter for close obstacles
  ArActionLimiterForwards limiter("speed limiter near", 300, 600, 250);
  // limiter for far away obstacles
  ArActionLimiterForwards limiterFar("speed limiter far", 300, 1100, 400);
  // limiter that checks IR sensors (like Peoplebot has)
  ArActionLimiterTableSensor tableLimiter;
  // limiter so we don't bump things backwards
  ArActionLimiterBackwards backwardsLimiter;
  // the joydrive action
  ArActionJoydrive joydriveAct;
  // the keydrive action
  ArActionKeydrive keydriveAct;
  
  // sonar device, used by the limiter actions.
  ArSonarDevice sonar;



  printf("This program will allow you to use a joystick or keyboard to control the robot.\nYou can use the arrow keys to drive, and the spacebar to stop.\nFor joystick control press the trigger button and then drive.\nPress escape to exit.\n");

  // if we don't have a joystick, let 'em know
  if (!joydriveAct.joystickInited())
    printf("Do not have a joystick, only the arrow keys on the keyboard will work.\n");
  
  // add the sonar to the robot
  robot.addRangeDevice(&sonar);


  // set the robots maximum velocity (sonar don't work at all well if you're
  // going faster)
  robot.setAbsoluteMaxTransVel(400);

  // enable the motor
  robot.enableMotors();

  // Add the actions, with the limiters as highest priority, then the teleop.
  // actions.  This will keep the teleop. actions from being able to drive too 
  // fast and hit something
  robot.addAction(&tableLimiter, 100);
  robot.addAction(&limiter, 95);
  robot.addAction(&limiterFar, 90);
  robot.addAction(&backwardsLimiter, 85);
  robot.addAction(&joydriveAct, 50);
  robot.addAction(&keydriveAct, 45);

  // Configure the joydrive action so it will let the lower priority actions
  // (i.e. keydriveAct) request motion if the joystick button is
  // not pressed.
  joydriveAct.setStopIfNoButtonPressed(false);

  
  // run the robot, true means that the run will exit if connection lost
  robot.run(true);
  
  Aria::exit(0);
}
int main(int argc, char **argv) 
{
  Aria::init();
  
  ArArgumentParser argParser(&argc, argv);

  ArSimpleConnector con(&argParser);

  ArRobot robot;

  // the connection handler from above
  ConnHandler ch(&robot);

  if(!Aria::parseArgs())
  {
    Aria::logOptions();
    Aria::shutdown();
    return 1;
  }

  if(!con.connectRobot(&robot))
  {
    ArLog::log(ArLog::Normal, "directMotionExample: Could not connect to the robot. Exiting.");
    return 1;
  }

  ArLog::log(ArLog::Normal, "directMotionExample: Connected.");

  // Run the robot processing cycle in its own thread. Note that after starting this
  // thread, we must lock and unlock the ArRobot object before and after
  // accessing it.
  robot.runAsync(false);


  // Send the robot a series of motion commands directly, sleeping for a 
  // few seconds afterwards to give the robot time to execute them.
  printf("directMotionExample: Setting rot velocity to 100 deg/sec then sleeping 3 seconds\n");
  robot.lock();
  robot.setRotVel(100);
  robot.unlock();
  ArUtil::sleep(3*1000);
  printf("Stopping\n");
  robot.lock();
  robot.setRotVel(0);
  robot.unlock();
  ArUtil::sleep(200);

  printf("directMotionExample: Telling the robot to go 300 mm on left wheel and 100 mm on right wheel for 5 seconds\n");
  robot.lock();
  robot.setVel2(300, 100);
  robot.unlock();
  ArTime start;
  start.setToNow();
  while (1)
  {
    robot.lock();
    if (start.mSecSince() > 5000)
    {
      robot.unlock();
      break;
    }   
    robot.unlock();
    ArUtil::sleep(50);
  }
  
  printf("directMotionExample: Telling the robot to move forwards one meter, then sleeping 5 seconds\n");
  robot.lock();
  robot.move(1000);
  robot.unlock();
  start.setToNow();
  while (1)
  {
    robot.lock();
    if (robot.isMoveDone())
    {
      printf("directMotionExample: Finished distance\n");
      robot.unlock();
      break;
    }
    if (start.mSecSince() > 5000)
    {
      printf("directMotionExample: Distance timed out\n");
      robot.unlock();
      break;
    }   
    robot.unlock();
    ArUtil::sleep(50);
  }
  printf("directMotionExample: Telling the robot to move backwards one meter, then sleeping 5 seconds\n");
  robot.lock();
  robot.move(-1000);
  robot.unlock();
  start.setToNow();
  while (1)
  {
    robot.lock();
    if (robot.isMoveDone())
    {
      printf("directMotionExample: Finished distance\n");
      robot.unlock();
      break;
    }
    if (start.mSecSince() > 10000)
    {
      printf("directMotionExample: Distance timed out\n");
      robot.unlock();
      break;
    }
    robot.unlock();
    ArUtil::sleep(50);
  }
  printf("directMotionExample: Telling the robot to turn to 180, then sleeping 4 seconds\n");
  robot.lock();
  robot.setHeading(180);
  robot.unlock();
  start.setToNow();
  while (1)
  {
    robot.lock();
    if (robot.isHeadingDone(5))
    {
      printf("directMotionExample: Finished turn\n");
      robot.unlock();
      break;
    }
    if (start.mSecSince() > 5000)
    {
      printf("directMotionExample: Turn timed out\n");
      robot.unlock();
      break;
    }
    robot.unlock();
    ArUtil::sleep(100);
  }
  printf("directMotionExample: Telling the robot to turn to 90, then sleeping 2 seconds\n");
  robot.lock();
  robot.setHeading(90);
  robot.unlock();
  start.setToNow();
  while (1)
  {
    robot.lock();
    if (robot.isHeadingDone(5))
    {
      printf("directMotionExample: Finished turn\n");
      robot.unlock();
      break;
    }
    if (start.mSecSince() > 5000)
    {
      printf("directMotionExample: turn timed out\n");
      robot.unlock();
      break;
    }
    robot.unlock();
    ArUtil::sleep(100);
  }
  printf("directMotionExample: Setting vel2 to 200 mm/sec on both wheels, then sleeping 3 seconds\n");
  robot.lock();
  robot.setVel2(200, 200);
  robot.unlock();
  ArUtil::sleep(3000);
  printf("directMotionExample: Stopping the robot, then sleeping for 2 seconds\n");
  robot.lock();
  robot.stop();
  robot.unlock();
  ArUtil::sleep(2000);
  printf("directMotionExample: Setting velocity to 200 mm/sec then sleeping 3 seconds\n");
  robot.lock();
  robot.setVel(200);
  robot.unlock();
  ArUtil::sleep(3000);
  printf("directMotionExample: Stopping the robot, then sleeping for 2 seconds\n");
  robot.lock();
  robot.stop();
  robot.unlock();
  ArUtil::sleep(2000);
  printf("directMotionExample: Setting vel2 with 0 on left wheel, 200 mm/sec on right, then sleeping 5 seconds\n");
  robot.lock();
  robot.setVel2(0, 200);
  robot.unlock();
  ArUtil::sleep(5000);
  printf("directMotionExample: Telling the robot to rotate at 50 deg/sec then sleeping 5 seconds\n");
  robot.lock();
  robot.setRotVel(50);
  robot.unlock();
  ArUtil::sleep(5000);
  printf("directMotionExample: Telling the robot to rotate at -50 deg/sec then sleeping 5 seconds\n");
  robot.lock();
  robot.setRotVel(-50);
  robot.unlock();
  ArUtil::sleep(5000);
  printf("directMotionExample: Setting vel2 with 0 on both wheels, then sleeping 3 seconds\n");
  robot.lock();
  robot.setVel2(0, 0);
  robot.unlock();
  ArUtil::sleep(3000);
  printf("directMotionExample: Now having the robot change heading by -125 degrees, then sleeping for 6 seconds\n");
  robot.lock();
  robot.setDeltaHeading(-125);
  robot.unlock();
  ArUtil::sleep(6000);
  printf("directMotionExample: Now having the robot change heading by 45 degrees, then sleeping for 6 seconds\n");
  robot.lock();
  robot.setDeltaHeading(45);
  robot.unlock();
  ArUtil::sleep(6000);
  printf("directMotionExample: Setting vel2 with 200 on left wheel, 0 on right wheel, then sleeping 5 seconds\n");
  robot.lock();
  robot.setVel2(200, 0);
  robot.unlock();
  ArUtil::sleep(5000);

  printf("directMotionExample: Done, shutting down Aria and exiting.\n");
  Aria::shutdown();
  return 0;
}
Exemplo n.º 9
0
int main(int argc, char **argv)
{
  // mandatory init
  Aria::init();

  //ArLog::init(ArLog::StdOut, ArLog::Verbose);

  // set up our parser
  ArArgumentParser parser(&argc, argv);

  // load the default arguments 
  parser.loadDefaultArguments();

  // robot
  ArRobot robot;
  // set up our simple connector
  ArRobotConnector robotConnector(&parser, &robot);


  // add a gyro, it'll see if it should attach to the robot or not
  ArAnalogGyro gyro(&robot);


  // set up the robot for connecting
  if (!robotConnector.connectRobot())
  {
    printf("Could not connect to robot... exiting\n");
    Aria::exit(1);
  }

  ArDataLogger dataLogger(&robot, "dataLog.txt");
  dataLogger.addToConfig(Aria::getConfig());
  
  // our base server object
  ArServerBase server;

  ArLaserConnector laserConnector(&parser, &robot, &robotConnector);
  ArServerSimpleOpener simpleOpener(&parser);


  ArClientSwitchManager clientSwitchManager(&server, &parser);

  // parse the command line... fail and print the help if the parsing fails
  // or if the help was requested
  if (!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed())
  {    
    Aria::logOptions();
    Aria::exit(1);
  }

  // Set up where we'll look for files such as user/password 
  char fileDir[1024];
  ArUtil::addDirectories(fileDir, sizeof(fileDir), Aria::getDirectory(), 
			 "ArNetworking/examples");

  // first open the server up
  if (!simpleOpener.open(&server, fileDir, 240))
  {
    if (simpleOpener.wasUserFileBad())
      printf("Bad user/password/permissions file\n");
    else
      printf("Could not open server port\n");
    exit(1);
  }

  // Range devices:
 
 
  ArSonarDevice sonarDev;
  robot.addRangeDevice(&sonarDev);

  ArIRs irs;
  robot.addRangeDevice(&irs);

  ArBumpers bumpers;
  robot.addRangeDevice(&bumpers);

  // attach services to the server
  ArServerInfoRobot serverInfoRobot(&server, &robot);
  ArServerInfoSensor serverInfoSensor(&server, &robot);
  ArServerInfoDrawings drawings(&server);

  // modes for controlling robot movement
  ArServerModeStop modeStop(&server, &robot);
  ArServerModeRatioDrive modeRatioDrive(&server, &robot);  
  ArServerModeWander modeWander(&server, &robot);
  modeStop.addAsDefaultMode();
  modeStop.activate();

  // set up the simple commands
  ArServerHandlerCommands commands(&server);
  ArServerSimpleComUC uCCommands(&commands, &robot);  // send commands directly to microcontroller
  ArServerSimpleComMovementLogging loggingCommands(&commands, &robot); // control debug logging
  ArServerSimpleComGyro gyroCommands(&commands, &robot, &gyro); // configure gyro
  ArServerSimpleComLogRobotConfig configCommands(&commands, &robot); // control more debug logging
  ArServerSimpleServerCommands serverCommands(&commands, &server); // control ArNetworking debug logging
  ArServerSimpleLogRobotDebugPackets logRobotDebugPackets(&commands, &robot, ".");  // debugging tool

  // ArServerModeDrive is an older drive mode. ArServerModeRatioDrive is newer and generally performs better,
  // but you can use this for old clients if neccesary.
  //ArServerModeDrive modeDrive(&server, &robot);
  //modeDrive.addControlCommands(&commands); // configure the drive modes (e.g. enable/disable safe drive)

  ArServerHandlerConfig serverHandlerConfig(&server, Aria::getConfig()); // make a config handler
  ArLog::addToConfig(Aria::getConfig()); // let people configure logging

  modeRatioDrive.addToConfig(Aria::getConfig(), "Teleop settings"); // able to configure teleop settings
  modeRatioDrive.addControlCommands(&commands);

  // Forward video if either ACTS or SAV server are running.
  // You can find out more about SAV and ACTS on our website
  // http://robots.activmedia.com. ACTS is for color tracking and is
  // a separate product. SAV just does software A/V transmitting and is
  // free to all our customers. Just run ACTS or SAV server before you
  // start this program and this class here will forward video from the
  // server to the client.
  ArHybridForwarderVideo videoForwarder(&server, "localhost", 7070);
  
  // Control a pan/tilt/zoom camera, if one is installed, and the video
  // forwarder was enabled above.
  ArPTZ *camera = NULL;
  ArServerHandlerCamera *handlerCamera = NULL;
  ArCameraCollection *cameraCollection = NULL;
  if (videoForwarder.isForwardingVideo())
  {
    bool invertedCamera = false;
    camera = new ArVCC4(&robot,	invertedCamera, 
			ArVCC4::COMM_UNKNOWN, true, true);
    camera->init();

    cameraCollection = new ArCameraCollection();
    cameraCollection->addCamera("Cam1", "VCC4", "Camera", "VCC4");
    handlerCamera = new ArServerHandlerCamera("Cam1", 
		                              &server, 
					      &robot,
					      camera, 
					      cameraCollection);
  }

  // You can use this class to send a set of arbitrary strings 
  // for MobileEyes to display, this is just a small example
  ArServerInfoStrings stringInfo(&server);
  Aria::getInfoGroup()->addAddStringCallback(stringInfo.getAddStringFunctor());
  Aria::getInfoGroup()->addStringInt(
	  "Motor Packet Count", 10, 
	  new ArConstRetFunctorC<int, ArRobot>(&robot, 
					       &ArRobot::getMotorPacCount));
  /*
  Aria::getInfoGroup()->addStringInt(
	  "Laser Packet Count", 10, 
	  new ArRetFunctorC<int, ArSick>(&sick, 
					 &ArSick::getSickPacCount));
  */
  
  // start the robot running, true means that if we lose connection the run thread stops
  robot.runAsync(true);


  // connect the laser(s) if it was requested
  if (!laserConnector.connectLasers())
  {
    printf("Could not connect to lasers... exiting\n");
    Aria::exit(2);
  }
  

  drawings.addRobotsRangeDevices(&robot);

  // log whatever we wanted to before the runAsync
  simpleOpener.checkAndLog();
  // now let it spin off in its own thread
  server.runAsync();

  printf("Server is now running...\n");

  // Add a key handler so that you can exit by pressing
  // escape. Note that a key handler prevents you from running
  // a program in the background on Linux, since it expects an 
  // active terminal to read keys from; remove this if you want
  // to run it in the background.
  ArKeyHandler *keyHandler;
  if ((keyHandler = Aria::getKeyHandler()) == NULL)
  {
    keyHandler = new ArKeyHandler;
    Aria::setKeyHandler(keyHandler);
    robot.lock();
    robot.attachKeyHandler(keyHandler);
    robot.unlock();
    printf("To exit, press escape.\n");
  }

  // Read in parameter files.
  std::string configFile = "serverDemoConfig.txt";
  Aria::getConfig()->setBaseDirectory("./");
  if (Aria::getConfig()->parseFile(configFile.c_str(), true, true))
  {
    ArLog::log(ArLog::Normal, "Loaded config file %s", configFile.c_str());
  }
  else
  {
    if (ArUtil::findFile(configFile.c_str()))
    {
      ArLog::log(ArLog::Normal, 
		 "Trouble loading configuration file %s, continuing",
		 configFile.c_str());
    }
    else
    {
      ArLog::log(ArLog::Normal, 
		 "No configuration file %s, will try to create if config used",
		 configFile.c_str());
    }
  }

  clientSwitchManager.runAsync();

  robot.lock();
  robot.enableMotors();
  robot.unlock();

  robot.waitForRunExit();
  Aria::exit(0);
}
Exemplo n.º 10
0
int main(int argc, char **argv) 
{
  std::string str;
  int ret;
  int dist;
  ArTime start;
  ArPose startPose;
  bool vel2 = false;

  // connection to the robot
  ArSerialConnection con;
  // the robot
  ArRobot robot;
  // the connection handler from above
  ConnHandler ch(&robot);
  
  // init area with a dedicated signal handling thread
  Aria::init(Aria::SIGHANDLE_THREAD);

  if (argc != 2 || (dist = atoi(argv[1])) == 0)
    {
      printf("Usage: %s <distInMM>\n", argv[0]);
      exit(0);
    }
  if (dist < 1000)
    {
      printf("You must go at least a meter\n");
      exit(0);
    }
  // open the connection with the defaults, exit if failed
  if ((ret = con.open()) != 0)
  {
    str = con.getOpenMessage(ret);
    printf("Open failed: %s\n", str.c_str());
    Aria::shutdown();
    return 1;
  }

  // set the robots connection
  robot.setDeviceConnection(&con);
  // try to connect, if we fail, the connection handler should bail
  if (!robot.blockingConnect())
  {
    // this should have been taken care of by the connection handler
    // but just in case
    printf(
    "asyncConnect failed because robot is not running in its own thread.\n");
    Aria::shutdown();
    return 1;
  }
  // run the robot in its own thread, so it gets and processes packets and such
  robot.runAsync(false);

  // just a big long set of printfs, direct motion commands and sleeps,
  // it should be self-explanatory

  robot.lock();

  /*
  robot.setAbsoluteMaxTransVel(2000);
  robot.setTransVelMax(2000);
  robot.setTransAccel(1000);
  robot.setTransDecel(1000);
  robot.comInt(82, 30); // rotkp
  robot.comInt(83, 200); // rotkv
  robot.comInt(84, 0); // rotki
  robot.comInt(85, 30); // transkp
  robot.comInt(86, 450); // transkv
  robot.comInt(87, 4); // transki

  */
  printf("Driving %d mm (going full speed for that far minus a meter then stopping)\n", dist);
  if (vel2)
    robot.setVel2(2200, 2200);
  else
    robot.setVel(2200);
  robot.unlock();
  start.setToNow();
  startPose = robot.getPose();
  while (1)
  {
    robot.lock();
    printf("\r vel: %.0f x: %.0f y: %.0f: dist: %.0f heading: %.2f",
	   robot.getVel(), robot.getX(), robot.getY(), 
	   startPose.findDistanceTo(robot.getPose()),
	   robot.getTh());
    if (startPose.findDistanceTo(robot.getPose()) > abs(dist) - 1000)
    {
      printf("\nFinished distance\n");
      robot.setVel(0);
      robot.unlock();
      break;
    }
    if (start.mSecSince() > 10000)
    {
      printf("\nDistance timed out\n");
      robot.setVel(0);
      robot.unlock();
      break;
    }   
    robot.unlock();
    ArUtil::sleep(50);
  }
  
  if (vel2)
    robot.setVel2(0, 0);
  else
    robot.setVel(0);
  start.setToNow();
  while (1)
    {
      robot.lock();
      if (vel2)
	robot.setVel2(0, 0);
      else
	robot.setVel(0);
      if (fabs(robot.getVel()) < 20)
	{
	  printf("Stopped\n");
	  robot.unlock();
	  break;
	}
      if (start.mSecSince() > 2000)
	{
	  printf("\nStop timed out\n");
	  robot.unlock();
	  break;
	}
      robot.unlock();
      ArUtil::sleep(50);
    }
  robot.lock();
  robot.disconnect();
  robot.unlock();
  // shutdown and ge tout
  Aria::shutdown();
  return 0;
}
Exemplo n.º 11
0
int main(int argc, char **argv)
{
  Aria::init();
  ArArgumentParser parser(&argc, argv);
  parser.loadDefaultArguments();

  ArRobot robot;
  ArRobotConnector robotConnector(&parser, &robot);
  ArLaserConnector laserConnector(&parser, &robot, &robotConnector);
  ArAnalogGyro analogGyro(&robot);

  // Always connect to the laser, and add half-degree increment and 180 degrees as default arguments for
  // laser
  parser.addDefaultArgument("-connectLaser -laserDegrees 180 -laserIncrement half");
  
  if(!robotConnector.connectRobot())
  {
    ArLog::log(ArLog::Terse, "sickLagger: Could not connect to the robot.");
    if(parser.checkHelpAndWarnUnparsed())
    {
      Aria::logOptions();
      Aria::exit(1);
    }
  }

  if(!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed())
  {
    Aria::logOptions();
    Aria::exit(1);
  }

  ArKeyHandler keyHandler;
  Aria::setKeyHandler(&keyHandler);
  robot.attachKeyHandler(&keyHandler);

#ifdef WIN32
  printf("Pausing 5 seconds so you can disconnect VNC if you are using it.\n");
  ArUtil::sleep(5000);
#endif


  std::string filename = "1scans.2d";
  if (argc > 1)
    filename = argv[1];
  printf("Logging to file %s\n", filename.c_str());
  
  
  

  ArActionGroupRatioDriveUnsafe group(&robot);
  group.activateExclusive();

  robot.runAsync(true);

  if(!laserConnector.connectLasers(false, false, true))
  {
    ArLog::log(ArLog::Terse, "sickLogger: Error: Could not connect to laser(s).  Use -help to list options.");
    Aria::exit(3);
  }


  // Allow some time for first set of laser reading to arrive
  ArUtil::sleep(500);
  

  // enable the motors, disable amigobot sounds
  robot.comInt(ArCommands::SONAR, 0);
  robot.comInt(ArCommands::ENABLE, 1);
  robot.comInt(ArCommands::SOUND, 32);
  robot.comInt(ArCommands::SOUNDTOG, 0);
  // enable the joystick driving from the one connected to the microcontroller
  robot.comInt(ArCommands::JOYDRIVE, 1);

  // Create the logging object
  // This must be created after the robot and laser are connected so it can get 
  // correct parameters from them.
  // The third argmument is how far the robot must travel before a new laser
  // scan is logged.
  // The fourth argument is how many degrees the robot must rotate before a new
  // laser scan is logged. The sixth boolean argument is whether to place a goal
  // when the g or G key is pressed (by adding a handler to the global
  // ArKeyHandler)  or when the robots joystick button is
  // pressed.
  ArLaser *laser = robot.findLaser(1);
  if(!laser)
  {
    ArLog::log(ArLog::Terse, "sickLogger: Error, not connected to any lasers.");
    Aria::exit(2);
  }
  ArLaserLogger logger(&robot, laser, 300, 25, filename.c_str(), true);

  // just hang out and wait for the end
  robot.waitForRunExit();

  Aria::exit(0);
}
Exemplo n.º 12
0
// the important function
void KeyPTU::drive(void)
{

  // if the PTU isn't initialized, initialize it here... it has to be 
  // done here instead of above because it needs to be done when the 
  // robot is connected
  if (!myPTUInited && myRobot->isConnected())
  {
    ArLog::log(ArLog::Normal, "Initializing ArDPPTU...");
    myPTU.init();
    ArLog::log(ArLog::Normal, "Resetting PTU and performing self-calibration...");
    myPTU.resetCalib();
    myPTU.awaitExec(); // DPPTU will wait for self-calibration to end before executing the following commands (though they will still be sent)
    mySlew = myPTU.getPanSlew(); //uses only pan slew rate
    myPTU.awaitExec();
    myPTUInited = true;
    myInit = false;
    myAbsolute = true;
  }

  if (myInit == true)  // User hit initialization key
  {
    ArLog::log(ArLog::Normal, "Initializing PTU...");
    myPTU.init();
    myInit = false;
    myDesiredPanPos = myPTU.getPan();
    myDesiredTiltPos = myPTU.getTilt();
    mySlew = myPTU.getPanSlew(); //uses only pan slew rate
    myReset = false;
  }

  if (myReset == true) // User hit reset key
  {
    ArLog::log(ArLog::Normal, "Resetting PTU and performing self-calibration...");
    myPTU.resetCalib();
    myPTU.awaitExec();
    myDesiredPanPos = myPTU.getPan();
    myDesiredTiltPos = myPTU.getTilt();
    myReset = false;
  }
  else   // User did nothing, or hit a key that changed myDesiredPanPos, myDesiredTiltPos, or mySlew (so request PTU to move if those changed since last request)
  {

    // Some PTUs can determine their current position (with encoders, etc) and return that.
    // canGetRealPanTilt() will return true in this case, and getPan() and
    // getTilt() will return those received values.  Otherwise, getPan() and
    // getTilt() return the last commanded values.  getLastPanRequest() and
    // getLastTiltRequest() will always return the last commanded values sent by
    // ArDPPTU (so in the case that canGetRealPanTilt() is false, getPan() and
    // getTilt() return the same pair of values as getLastPanRequest() and
    // getLastTiltRequest().  ArDPPTU::canGetRealPanTilt() is initialally false,
    // but once the first set of pan and tilt positions is read back from the
    // PTU device, it becomes true.  
    if(myPTU.canGetRealPanTilt())
      printf("Position (%.1f deg, %.1f deg)     [Incr. %d deg]     Press ? for help  \r", myPTU.getPan(), myPTU.getTilt(), myPosIncrement);
    else
      printf("Requested (%.1f deg, %.1f deg)     [Incr. %d deg]     Press ? for help  \r", myPTU.getPan(), myPTU.getTilt(), myPosIncrement);

    if (myDesiredPanPos != myPTU.getLastPanRequest())
    {
      if (myAbsolute)
      	myPTU.pan(myDesiredPanPos);
      else
        myPTU.panRel(myDesiredPanPos - myPTU.getPan());
    }

    if (myDesiredTiltPos != myPTU.getLastTiltRequest())
    {
      if (myAbsolute)
        myPTU.tilt(myDesiredTiltPos);
      else
        myPTU.tiltRel(myDesiredTiltPos - myPTU.getTilt());
    }

    if (mySlew != myPTU.getPanSlew())
    {
      myPTU.panSlew(mySlew);
      myPTU.tiltSlew(mySlew);
    }

  }

}
Exemplo n.º 13
0
int main(void)
{
  int ret;
  std::string str;
  CBTest cbTest;

  ArFunctorC<CBTest> connectCB(&cbTest, &CBTest::connected);
  ArFunctorC<CBTest> failedConnectCB(&cbTest, &CBTest::failedConnect);
  ArFunctorC<CBTest> disconnectCB(&cbTest, &CBTest::disconnected);
  ArFunctorC<CBTest> disconnectErrorCB(&cbTest, &CBTest::disconnectedError);

  ArSerialConnection con;
  ArRobot robot;

  printf("If a robot is attached to your port you should see:\n");
  printf("Failed connect, Connected, Disconnected Error, Connected, Disconnected\n");
  printf("If no robot is attached you should see:\n");
  printf("Failed connect, Failed connect, Failed connect\n");
  printf("-------------------------------------------------------\n");
  ArLog::init(ArLog::None, ArLog::Terse);

  srand(time(NULL));

  robot.setDeviceConnection(&con);
  robot.addConnectCB(&connectCB, ArListPos::FIRST);
  robot.addFailedConnectCB(&failedConnectCB, ArListPos::FIRST);
  robot.addDisconnectNormallyCB(&disconnectCB, ArListPos::FIRST);
  robot.addDisconnectOnErrorCB(&disconnectErrorCB, ArListPos::FIRST);
  
  // this should fail since there isn't an open port yet
  robot.blockingConnect();
  
  if ((ret = con.open()) != 0)
  {
    str = con.getOpenMessage(ret);
    printf("Open failed: %s\n", str.c_str());
    exit(0);
  }
  
  robot.blockingConnect();

  con.close();
  robot.loopOnce();
  

  if ((ret = con.open()) != 0)
  {
    str = con.getOpenMessage(ret);
    printf("Open failed: %s\n", str.c_str());
    exit(0);
  }  
  robot.blockingConnect();
  robot.disconnect();

  exit(0);

}
int main(int argc, char **argv)
{

  Aria::init();
  ArRobot robot;
  ArArgumentParser parser(&argc, argv);
  parser.loadDefaultArguments();

  ArLog::log(ArLog::Terse, "WARNING: this program does no sensing or avoiding of obstacles, the robot WILL collide with any objects in the way! Make sure the robot has approximately 3 meters of free space on all sides.");

  // ArRobotConnector connects to the robot, get some initial data from it such as type and name,
  // and then loads parameter files for this robot.
  ArRobotConnector robotConnector(&parser, &robot);
  if(!robotConnector.connectRobot())
  {
    ArLog::log(ArLog::Terse, "simpleMotionCommands: Could not connect to the robot.");
    if(parser.checkHelpAndWarnUnparsed())
    {
        Aria::logOptions();
        Aria::exit(1);
    }
  }
  if (!Aria::parseArgs())
  {
    Aria::logOptions();
    Aria::shutdown();
    return 1;
  }
  
  ArLog::log(ArLog::Normal, "simpleMotionCommands: Connected.");

  // Start the robot processing cycle running in the background.
  // True parameter means that if the connection is lost, then the 
  // run loop ends.
  robot.runAsync(true);

  // Print out some data from the SIP.  

  // We must "lock" the ArRobot object
  // before calling its methods, and "unlock" when done, to prevent conflicts
  // with the background thread started by the call to robot.runAsync() above.
  // See the section on threading in the manual for more about this.
  // Make sure you unlock before any sleep() call or any other code that will
  // take some time; if the robot remains locked during that time, then
  // ArRobot's background thread will be blocked and unable to communicate with
  // the robot, call tasks, etc.
  
  robot.lock();
  ArLog::log(ArLog::Normal, "simpleMotionCommands: Pose=(%.2f,%.2f,%.2f), Trans. Vel=%.2f, Rot. Vel=%.2f, Battery=%.2fV",
    robot.getX(), robot.getY(), robot.getTh(), robot.getVel(), robot.getRotVel(), robot.getBatteryVoltage());
  robot.unlock();

  // Sleep for 3 seconds.
  ArLog::log(ArLog::Normal, "simpleMotionCommands: Will start driving in 3 seconds...");
  ArUtil::sleep(3000);

  // Set forward velocity to 50 mm/s
  ArLog::log(ArLog::Normal, "simpleMotionCommands: Driving forward at 250 mm/s for 5 sec...");
  robot.lock();
  robot.enableMotors();
  robot.setVel(250);
  robot.unlock();
  ArUtil::sleep(5000);

  ArLog::log(ArLog::Normal, "simpleMotionCommands: Stopping.");
  robot.lock();
  robot.stop();
  robot.unlock();
  ArUtil::sleep(1000);

  ArLog::log(ArLog::Normal, "simpleMotionCommands: Rotating at 10 deg/s for 5 sec...");
  robot.lock();
  robot.setRotVel(10);
  robot.unlock();
  ArUtil::sleep(5000);

  ArLog::log(ArLog::Normal, "simpleMotionCommands: Rotating at -10 deg/s for 10 sec...");
  robot.lock();
  robot.setRotVel(-10);
  robot.unlock();
  ArUtil::sleep(10000);

  ArLog::log(ArLog::Normal, "simpleMotionCommands: Driving forward at 150 mm/s for 5 sec...");
  robot.lock();
  robot.setRotVel(0);
  robot.setVel(150);
  robot.unlock();
  ArUtil::sleep(5000);

  ArLog::log(ArLog::Normal, "simpleMotionCommands: Stopping.");
  robot.lock();
  robot.stop();
  robot.unlock();
  ArUtil::sleep(1000);


  // Other motion command functions include move(), setHeading(),
  // setDeltaHeading().  You can also adjust acceleration and deceleration
  // values used by the robot with setAccel(), setDecel(), setRotAccel(),
  // setRotDecel().  See the ArRobot class documentation for more.

  
  robot.lock();
  ArLog::log(ArLog::Normal, "simpleMotionCommands: Pose=(%.2f,%.2f,%.2f), Trans. Vel=%.2f, Rot. Vel=%.2f, Battery=%.2fV",
    robot.getX(), robot.getY(), robot.getTh(), robot.getVel(), robot.getRotVel(), robot.getBatteryVoltage());
  robot.unlock();

  
  ArLog::log(ArLog::Normal, "simpleMotionCommands: Ending robot thread...");
  robot.stopRunning();

  // wait for the thread to stop
  robot.waitForRunExit();

  // exit
  ArLog::log(ArLog::Normal, "simpleMotionCommands: Exiting.");
  return 0;
}
Exemplo n.º 15
0
int main(int argc, char **argv)
{
  Aria::init();
  ArSimpleConnector con(&argc, argv);
  ArRobot robot;
  ArP2Arm arm;


  if(!Aria::parseArgs()) 
  {
    Aria::logOptions();
    Aria::shutdown();
    return 1;
  }

  ArLog::log(ArLog::Normal, "armExample: Connecting to the robot.");
  if(!con.connectRobot(&robot))
  {
    ArLog::log(ArLog::Terse, "armExample: Could not connect to the robot. Exiting.");
    Aria::shutdown();
    return 1;
  }
  robot.runAsync(true);

  // turn off sonar
  robot.comInt(28, 0);

  // Set up and initialize the arm
  arm.setRobot(&robot);
  if (arm.init() != ArP2Arm::SUCCESS)
  {
    ArLog::log(ArLog::Terse, "armExample: Error initializing the P2 Arm!");
    return 1;
  }

  // Print out some of the settings
  P2ArmJoint *joint;
  printf("Current joint info:\nJoint   Vel  Home  Center\n");
  for (int i=1; i<=ArP2Arm::NumJoints; i++)
  {
    joint = arm.getJoint(i);
    printf("  %2i:  %5i %5i   %5i\n", i, joint->myVel, joint->myHome, joint->myCenter);
  }
  printf("\n");

  // Put the arm to work
  printf("Powering on  (takes a couple seconds to stabilize)\n");
  arm.powerOn();

  // Request one status packet and print out the arm's status
  printf("Current arm status:\n");
  arm.requestStatus(ArP2Arm::StatusSingle);
  ArUtil::sleep(200);  // Give time to get the packet
  printf("Arm Status: ");
  if (arm.getStatus() & ArP2Arm::ArmGood)
    printf("Good=1 ");
  else
    printf("Good=0 ");
  if (arm.getStatus() & ArP2Arm::ArmInited)
    printf("Inited=1 ");
  else
    printf("Inited=0 ");
  if (arm.getStatus() & ArP2Arm::ArmPower)
    printf("Power=1 ");
  else
    printf("Power=0 ");
  if (arm.getStatus() & ArP2Arm::ArmHoming)
    printf("Homing=1 ");
  else
    printf("Homing=0 ");
  printf("\n\n");

  // Move the arm joints to specific positions
  printf("Moving Arm...\n");
  int deploy_offset[] = {0, -100, 10, 40, 80, -55, 20};
  for (int i=1; i<=ArP2Arm::NumJoints; i++)
  {
    joint = arm.getJoint(i);
    arm.moveToTicks(i, joint->myCenter + deploy_offset[i]);
  }

  // Wait for arm to achieve new position, printing joint positions and M for
  // moving, NM for not moving.
  arm.requestStatus(ArP2Arm::StatusContinuous);
  ArUtil::sleep(300); // wait a moment for arm status packet update with joints moving
  bool moving = true;
  while (moving)
  {
      moving = false;
      printf("Joints: ");
      for (int i=1; i<=ArP2Arm::NumJoints; i++)
      {
        printf("[%d] %.0f, ", i, arm.getJointPos(i));
        if (arm.getMoving(i))
        {
	        printf("M;  ");
	        moving = true;
	      }
        else
        {
          printf("NM; ");
        }
	    }
      printf("\r");
  }
  printf("\n\n");

  // Return arm to park, wait, and disconnect. (Though the arm will automatically park
  // on client disconnect)
  printf("Parking arm.\n");
  arm.park();

  // Wait 5 seconds or until arm shuts off
  for(int i = 5; (i > 0) && (arm.getStatus() & ArP2Arm::ArmPower); i--)
  {
    ArUtil::sleep(1000);
  }

  // Disconnect from robot, etc., and exit.
  printf("Shutting down ARIA and exiting.\n");
  Aria::shutdown();

  return(0);
}
Exemplo n.º 16
0
int main(int argc, char** argv)
{
  // Initialize some global data
  Aria::init();

  // If you want ArLog to print "Verbose" level messages uncomment this:
  //ArLog::init(ArLog::StdOut, ArLog::Verbose);

  // This object parses program options from the command line
  ArArgumentParser parser(&argc, argv);

  // Load some default values for command line arguments from /etc/Aria.args
  // (Linux) or the ARIAARGS environment variable.
  parser.loadDefaultArguments();

  // Central object that is an interface to the robot and its integrated
  // devices, and which manages control of the robot by the rest of the program.
  ArRobot robot;

  // Object that connects to the robot or simulator using program options
  ArRobotConnector robotConnector(&parser, &robot);

  // If the robot has an Analog Gyro, this object will activate it, and 
  // if the robot does not automatically use the gyro to correct heading,
  // this object reads data from it and corrects the pose in ArRobot
  ArAnalogGyro gyro(&robot);

  // Connect to the robot, get some initial data from it such as type and name,
  // and then load parameter files for this robot.
  if (!robotConnector.connectRobot())
  {
    // Error connecting:
    // if the user gave the -help argumentp, then just print out what happened,
    // and continue so options can be displayed later.
    if (!parser.checkHelpAndWarnUnparsed())
    {
      ArLog::log(ArLog::Terse, "Could not connect to robot, will not have parameter file so options displayed later may not include everything");
    }
    // otherwise abort
    else
    {
      ArLog::log(ArLog::Terse, "Error, could not connect to robot.");
      Aria::logOptions();
      Aria::exit(1);
    }
  }

  if(!robot.isConnected())
  {
    ArLog::log(ArLog::Terse, "Internal error: robot connector succeeded but ArRobot::isConnected() is false!");
  }

  // Connector for laser rangefinders
  ArLaserConnector laserConnector(&parser, &robot, &robotConnector);

  // Connector for compasses
  ArCompassConnector compassConnector(&parser);

  // Parse the command line options. Fail and print the help message if the parsing fails
  // or if the help was requested with the -help option
  if (!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed())
  {    
    Aria::logOptions();
    Aria::exit(1);
    return 1;
  }

  // Used to access and process sonar range data
  ArSonarDevice sonarDev;
  
  // Used to perform actions when keyboard keys are pressed
  ArKeyHandler keyHandler;
  Aria::setKeyHandler(&keyHandler);

  // ArRobot contains an exit action for the Escape key. It also 
  // stores a pointer to the keyhandler so that other parts of the program can
  // use the same keyhandler.
  robot.attachKeyHandler(&keyHandler);
  printf("You may press escape to exit\n");

  // Attach sonarDev to the robot so it gets data from it.
  robot.addRangeDevice(&sonarDev);

  
  // Start the robot task loop running in a new background thread. The 'true' argument means if it loses
  // connection the task loop stops and the thread exits.
  robot.runAsync(true);

  // Connect to the laser(s) if lasers were configured in this robot's parameter
  // file or on the command line, and run laser processing thread if applicable
  // for that laser class.  For the purposes of this demo, add all
  // possible lasers to ArRobot's list rather than just the ones that were
  // connected by this call so when you enter laser mode, you
  // can then interactively choose which laser to use from that list of all
  // lasers mentioned in robot parameters and on command line. Normally,
  // only connected lasers are put in ArRobot's list.
  if (!laserConnector.connectLasers(
        false,  // continue after connection failures
        false,  // add only connected lasers to ArRobot
        true    // add all lasers to ArRobot
  ))
  {
    printf("Could not connect to lasers... exiting\n");
    Aria::exit(2);
  }

/* not needed, robot connector will do it by default
  if (!sonarConnector.connectSonars(
        false,  // continue after connection failures
        false,  // add only connected lasers to ArRobot
        true    // add all lasers to ArRobot
  ))
  {
    printf("Could not connect to sonars... exiting\n");
    Aria::exit(2);
  }
*/

  // Create and connect to the compass if the robot has one.
  ArTCM2 *compass = compassConnector.create(&robot);
  if(compass && !compass->blockingConnect()) {
    compass = NULL;
  }
  
  // Sleep for a second so some messages from the initial responses
  // from robots and cameras and such can catch up
  ArUtil::sleep(1000);

  // We need to lock the robot since we'll be setting up these modes
  // while the robot task loop thread is already running, and they 
  // need to access some shared data in ArRobot.
  robot.lock();

  // now add all the modes for this demo
  // these classes are defined in ArModes.cpp in ARIA's source code.
  
  if(robot.getOrigRobotConfig()->getHasGripper())
    new ArModeGripper(&robot, "gripper", 'g', 'G');
  else
    ArLog::log(ArLog::Normal, "Robot does not indicate that it has a gripper.");
  ArModeActs actsMode(&robot, "acts", 'a', 'A');
  ArModeTCM2 tcm2(&robot, "tcm2", 'm', 'M', compass);
  ArModeIO io(&robot, "io", 'i', 'I');
  ArModeConfig cfg(&robot, "report robot config", 'o' , 'O');
  ArModeCommand command(&robot, "command", 'd', 'D');
  ArModeCamera camera(&robot, "camera", 'c', 'C');
  ArModePosition position(&robot, "position", 'p', 'P', &gyro);
  ArModeSonar sonar(&robot, "sonar", 's', 'S');
  ArModeBumps bumps(&robot, "bumps", 'b', 'B');
  ArModeLaser laser(&robot, "laser", 'l', 'L');
  ArModeWander wander(&robot, "wander", 'w', 'W');
  ArModeUnguardedTeleop unguardedTeleop(&robot, "unguarded teleop", 'u', 'U');
  ArModeTeleop teleop(&robot, "teleop", 't', 'T');


  // activate the default mode
  teleop.activate();

  // turn on the motors
  robot.comInt(ArCommands::ENABLE, 1);

  robot.unlock();
  
  // Block execution of the main thread here and wait for the robot's task loop
  // thread to exit (e.g. by robot disconnecting, escape key pressed, or OS
  // signal)
  robot.waitForRunExit();

  Aria::exit(0);
  return 0;

}
Exemplo n.º 17
0
int main(int argc, char **argv)
{
  bool done;
  double distToTravel = 2300;

  // whether to use the sim for the laser or not, if you use the sim
  // for hte laser, you have to use the sim for the robot too
  bool useSim = false;
  // the laser
  ArSick sick;
  // connection
  ArDeviceConnection *con;
  // Laser connection
  ArSerialConnection laserCon;
  // robot
  ArRobot robot;

  // set a default filename
  //std::string filename = "c:\\log\\1scans.2d";
  std::string filename = "1scans.2d";
  // see if we want to use a different filename
  //if (argc > 1)
  //Lfilename = argv[1];
  printf("Logging to file %s\n", filename.c_str());
  // start the logger with good values
  sick.configureShort(useSim, ArSick::BAUD38400,
		      ArSick::DEGREES180, ArSick::INCREMENT_HALF);
  ArSickLogger logger(&robot, &sick, 300, 25, filename.c_str());
  
  // mandatory init
  Aria::init();

  // add it to the robot
  robot.addRangeDevice(&sick);

  //ArAnalogGyro gyro(&robot);


  // if we're not using the sim, make a serial connection and set it up
  if (!useSim)
  {
    ArSerialConnection *serCon;
    serCon = new ArSerialConnection;
    serCon->setPort();
    //serCon->setBaud(38400);
    con = serCon;
  }
  // if we are using the sim, set up a tcp connection
  else
  {
    ArTcpConnection *tcpCon;
    tcpCon = new ArTcpConnection;
    tcpCon->setPort();
    con = tcpCon;
  }

  // set the connection on the robot
  robot.setDeviceConnection(con);
  // try to connect, if we fail exit
  if (!robot.blockingConnect())
  {
    printf("Could not connect to robot... exiting\n");
    Aria::shutdown();
    return 1;
  }


  // set up a key handler so escape exits and attach to the robot
  ArKeyHandler keyHandler;
  robot.attachKeyHandler(&keyHandler);

  // run the robot, true here so that the run will exit if connection lost
  robot.runAsync(true);



  // if we're not using the sim, set up the port for the laser
  if (!useSim)
  {
    laserCon.setPort(ArUtil::COM3);
    sick.setDeviceConnection(&laserCon);
  }


  // now that we're connected to the robot, connect to the laser
  sick.runAsync();


  if (!sick.blockingConnect())
  {
    printf("Could not connect to SICK laser... exiting\n");
    robot.disconnect();
    Aria::shutdown();
    return 1;
  }

#ifdef WIN32
  // wait until someone pushes the motor button to go
  while (1)
  {
    robot.lock();
    if (!robot.isRunning())
      exit(0);
    if (robot.areMotorsEnabled())
    {
      robot.unlock();
      break;
    }
    robot.unlock();
    ArUtil::sleep(100);
  }
#endif

  // basically from here on down the robot just cruises around a bit

  robot.lock();
  // enable the motors, disable amigobot sounds
  robot.comInt(ArCommands::ENABLE, 1);

  // move a couple meters
  robot.setRotVel(3000);
  robot.unlock();

  ArUtil::sleep(15 * 1000);

  robot.lock();
  robot.disconnect();
  robot.unlock();
  keyHandler.restore();
  exit(1);

  // now exit
  return 0;
}
Exemplo n.º 18
0
int main()
{
  ArSyncTask *task;
  UserTaskTest test;
  ArRobot robot;

  ArFunctorC<UserTaskTest> userTaskOne(&test, &UserTaskTest::userTaskOne);
  ArFunctorC<UserTaskTest> userTaskTwo(&test, &UserTaskTest::userTaskTwo);
  ArFunctorC<UserTaskTest> userTaskThree(&test, &UserTaskTest::userTaskThree);
  ArFunctorC<UserTaskTest> userTaskFour(&test, &UserTaskTest::userTaskFour);


  printf("Before tasks added:\n");
  robot.logUserTasks();
  printf("---------------------------------------------------------------\n");

  // the order you add tasks doesn't matter, it goes off of the integer
  // you give the function call to add them 
  // Caveat: if you give the function call the same number it goes off of order
  robot.addUserTask("procTwo", 20, &userTaskTwo);
  robot.addUserTask("procFour", 40, &userTaskFour, &test.myTaskFourState);
  robot.addUserTask("procThree", 30, &userTaskThree);
  robot.addUserTask("procOne", 10, &userTaskOne);

  printf("After tasks added:\n");
  robot.logUserTasks();
  printf("---------------------------------------------------------------\n");
  printf("What happens when these are run:\n");
  robot.loopOnce();

  printf("---------------------------------------------------------------\n");

  printf("After tasks removed by name:\n");
  printf("---------------------------------------------------------------\n");
  robot.remUserTask("procOne");
  robot.remUserTask("procTwo");
  robot.remUserTask("procThree");
  robot.remUserTask("procFour");
  robot.logUserTasks();


  printf("\nAfter they are added again, procThree is found by name and set to SUSPEND and procFour is found by functor and set to FAILURE:\n");
  printf("---------------------------------------------------------------\n");

  printf("---------------------------------------------------------------\n");
  robot.addUserTask("procTwo", 20, &userTaskTwo);
  robot.addUserTask("procFour", 40, &userTaskFour, &test.myTaskFourState);
  robot.addUserTask("procThree", 30, &userTaskThree);
  robot.addUserTask("procOne", 10, &userTaskOne);
  task = robot.findUserTask("procThree");
  if (task != NULL)
    task->setState(ArTaskState::SUSPEND);

  task = robot.findUserTask(&userTaskFour);
  if (task != NULL) 
    task->setState(ArTaskState::FAILURE);

  robot.logUserTasks();

  task = robot.findUserTask(&userTaskFour);
  if (task != NULL)
  {
    printf("---------------------------------------------------------------\n");

    printf("Did the real state on procFourState get set:\n");
    printf("getState: %d realState: %d\n", task->getState(), 
	   test.myTaskFourState);
  }


  printf("---------------------------------------------------------------\n");
  printf("What happens when these are run:\n");
  robot.loopOnce();

  printf("---------------------------------------------------------------\n");


  printf("After tasks removed by functor:\n");
  printf("---------------------------------------------------------------\n");
  robot.remUserTask(&userTaskOne);
  robot.remUserTask(&userTaskTwo);
  robot.remUserTask(&userTaskThree);
  robot.remUserTask(&userTaskFour);
  robot.logUserTasks();
  
  
}
Exemplo n.º 19
0
int
main(int argc, char *argv[])
{
  // Initialize location of Aria, Arnl and their args.
  Aria::init();
  Arnl::init();

  // The robot object
  ArRobot robot;
#ifndef SONARNL
  // The laser object
  ArSick sick(181, 361);
#endif

  // Parse them command line arguments.
  ArArgumentParser parser(&argc, argv);

  // Set up our simpleConnector
  ArSimpleConnector simpleConnector(&parser);

  // Load default arguments for this computer
  parser.loadDefaultArguments();

  // Parse its arguments for the simple connector.
  simpleConnector.parseArgs();

  // sonar, must be added to the robot, for teleop and wander
  ArSonarDevice sonarDev;
  // add the sonar to the robot
  robot.addRangeDevice(&sonarDev);

  ArMap arMap;
#ifndef SONARNL
  // Initialize the localization
  ArLocalizationTask locTask(&robot, &sick, &arMap);
  // Make the path task
  ArPathPlanningTask pathTask(&robot, &sick, &sonarDev, &arMap);
#else
  // Initialize the localization
  ArSonarLocalizationTask locTask(&robot, &sonarDev, &arMap);
  // Make the path task
  ArPathPlanningTask pathTask(&robot, NULL, &sonarDev, &arMap);
#endif


  // Stop the robot as soon as localization fails.
  ArFunctor1C<ArPathPlanningTask, int>
  locaFailed(&pathTask, &ArPathPlanningTask::trackingFailed);
  locTask.addFailedLocalizationCB(&locaFailed);

  // Read in param files.
  Aria::getConfig()->useArgumentParser(&parser);
  if (!Aria::getConfig()->parseFile(Arnl::getTypicalParamFileName()))
  {
    printf("Trouble loading configuration file, exiting\n");
    exit(1);
  }
  // Warn about unknown params.
  if (!parser.checkHelpAndWarnUnparsed())
  {
    printf("\nUsage: %s -map mapfilename\n\n", argv[0]);
    simpleConnector.logOptions();
    exit(2);
  }

  // Our server
  ArServerBase server;

  // First open the server up
  if (!server.open(7272))
  {
    printf("Could not open server port\n");
    exit(1);
  }

  // Connect the robot
  if (!simpleConnector.connectRobot(&robot))
  {
    printf("Could not connect to robot... exiting\n");
    Aria::shutdown();
    return 1;
  }

  robot.com2Bytes(31, 14, 0);
  robot.com2Bytes(31, 15, 0);
  ArUtil::sleep(100);
  robot.com2Bytes(31, 14, 1);
  robot.com2Bytes(31, 15, 1);
  robot.enableMotors();
  robot.clearDirectMotion();

#ifndef SONARNL
  // Set up the laser before handing it to the laser mode
  simpleConnector.setupLaser(&sick);

  // Add the laser to the robot
  robot.addRangeDevice(&sick);
#endif

  // Start the robot thread.
  robot.runAsync(true);

#ifndef SONARNL
  // Start the laser thread.
  sick.runAsync();

  // Connect the laser
  if (!sick.blockingConnect())
  {
    printf("Couldn't connect to sick, exiting\n");
    Aria::shutdown();
    return 1;
  }
#endif

  ArUtil::sleep(300);

  // If you want to set the number of samples change the line below
  locTask.setNumSamples(2000);
  // Localize the robot to home
  if(locTask.localizeRobotAtHomeBlocking())
  {
    printf("Successfully localized at home.\n");
  }
  else
  {
    printf("WARNING: Unable to localize at home position!\n");
  }


  robot.lock();
  // attach stuff to the server
  ArServerInfoRobot serverInfoRobot(&server, &robot);
  ArServerInfoSensor serverInfoSensor(&server, &robot);
  ArServerInfoPath serverInfoPath(&server, &robot, &pathTask);
  ArServerInfoLocalization serverInfoLocalization(&server, &robot, &locTask);

#ifndef SONARNL
  // Set it up to handle maps.
  ArServerHandlerMap serverMap(&server, &arMap, ArServerHandlerMap::POINTS);
#else
  ArServerHandlerMap serverMap(&server, &arMap, ArServerHandlerMap::LINES);
#endif

  // Set up a service that allows the client to monitor the communication
  // between the robot and the client.
  //
  ArServerHandlerCommMonitor serverCommMonitor(&server);


  //ArServerModeGoto modeGoto(&server, &robot, &pathTask);
  //ArServerModeStop modeStop(&server, &robot);
  //ArServerModeDrive modeDrive(&server, &robot);

  SimpleTask simpleTask(&robot, &pathTask);

  robot.unlock();
  // Read in param files.
  Aria::getConfig()->parseFile(Arnl::getTypicalParamFileName());

  // Now let it spin off in its own thread
  server.run();
  exit(0);

}
Exemplo n.º 20
0
int main(int argc, char **argv)
{
  Aria::init();
  ArRobot robot;
  ArArgumentParser parser(&argc, argv);
  parser.loadDefaultArguments();

  ArRobotConnector robotConnector(&parser, &robot);
  ArLaserConnector laserConnector(&parser, &robot, &robotConnector);

  // Connect to the robot, get some initial data from it such as type and name,
  // and then load parameter files for this robot.
  if(!robotConnector.connectRobot())
  {
    ArLog::log(ArLog::Terse, "lasersExample: Could not connect to the robot.");
    if(parser.checkHelpAndWarnUnparsed())
    {
        // -help not given
        Aria::logOptions();
        Aria::exit(1);
    }
  }


  if (!Aria::parseArgs())
  {
    Aria::logOptions();
    Aria::exit(2);
    return 2;
  }
  
  ArLog::log(ArLog::Normal, "lasersExample: Connected to robot.");

  // Start the robot processing cycle running in the background.
  // True parameter means that if the connection is lost, then the 
  // run loop ends.
  robot.runAsync(true);


  // Connect to laser(s) as defined in parameter files.
  // (Some flags are available as arguments to connectLasers() to control error behavior and to control which lasers are put in the list of lasers stored by ArRobot. See docs for details.)
  if(!laserConnector.connectLasers())
  {
    ArLog::log(ArLog::Terse, "Could not connect to configured lasers. Exiting.");
    Aria::exit(3);
    return 3;
  }

  // Allow some time to read laser data
  ArUtil::sleep(500);

  ArLog::log(ArLog::Normal, "Connected to all lasers.");

  // Print out some data from each connected laser.
  while(robot.isConnected())
  {
	int numLasers = 0;

 	  // Get a pointer to ArRobot's list of connected lasers. We will lock the robot while using it to prevent changes by tasks in the robot's background task thread or any other threads. Each laser has an index. You can also store the laser's index or name (laser->getName()) and use that to get a reference (pointer) to the laser object using ArRobot::findLaser().
	  robot.lock();
	  std::map<int, ArLaser*> *lasers = robot.getLaserMap();

	  for(std::map<int, ArLaser*>::const_iterator i = lasers->begin(); i != lasers->end(); ++i)
	  {
		int laserIndex = (*i).first;
		ArLaser* laser = (*i).second;
		if(!laser)
			continue;
		++numLasers;
		laser->lockDevice();

		// The current readings are a set of obstacle readings (with X,Y positions as well as other attributes) that are the most recent set from teh laser.
		std::list<ArPoseWithTime*> *currentReadings = laser->getCurrentBuffer(); // see ArRangeDevice interface doc

		// The raw readings are just range or other data supplied by the sensor. It may also include some device-specific extra values associated with each reading as well. (e.g. Reflectance for LMS200)
		const std::list<ArSensorReading*> *rawReadings = laser->getRawReadings();
			

		// There is a utility to find the closest reading wthin a range of degrees around the laser, here we use this laser's full field of view (start to end)
		// If there are no valid closest readings within the given range, dist will be greater than laser->getMaxRange().
		double angle = 0;
		double dist = laser->currentReadingPolar(laser->getStartDegrees(), laser->getEndDegrees(), &angle);

		ArLog::log(ArLog::Normal, "Laser #%d (%s): %s.\n\tHave %d 'current' readings.\n\tHave %d 'raw' readings.\n\tClosest reading is at %3.0f degrees and is %2.4f meters away.", 
			laserIndex, laser->getName(), (laser->isConnected() ? "connected" : "NOT CONNECTED"), 
			currentReadings->size(), 
			rawReadings->size(),
			angle, dist/1000.0);
                laser->unlockDevice();
	    }
	if(numLasers == 0)
		ArLog::log(ArLog::Normal, "No lasers.");
	else
		ArLog::log(ArLog::Normal, "");

        // Unlock robot and sleep for 5 seconds before next loop.
	robot.unlock();
  	ArUtil::sleep(5000);
   }

  ArLog::log(ArLog::Normal, "lasersExample: exiting.");
  Aria::exit(0);
  return 0;
}
Exemplo n.º 21
0
void TakeBlockToWall::handler(void)
{
  Color tempColor;

  switch (myState) 
  {
  case STATE_START:
    setState(STATE_ACQUIRE_BLOCK);
    myDropWall = COLOR_FIRST_WALL;
    myLapWall = COLOR_SECOND_WALL;
    printf("!! Started state handling!\n");
    //handler();
    return;
    break;
  case STATE_ACQUIRE_BLOCK:
    if (myNewState)
    {
      printf("!! Acquire block\n");
      myNewState = false;
      mySony->panTilt(0, -10);
      myAcquire->activate();
      myAcquire->setChannel(COLOR_BLOCK);
      myPickUp->deactivate();
      myDriveTo->deactivate();
      myBackup->deactivate();
    }
    if (myGripper->getGripState() == 2 &&
	myGripper->getBreakBeamState() != 0)
    {
      printf("###### AcquireBlock: Successful (have cube?)\n");
      setState(STATE_ACQUIRE_DROP_WALL);
      //handler();
      return;
    } 
    else if (myGripper->getBreakBeamState() != 0)
    {
      printf("###### AcquireBlock: Successful (cube in gripper?)\n");
      setState(STATE_PICKUP_BLOCK);
      //handler();
      return;
    }
    if (myAcquire->getState() == Acquire::STATE_FAILED || 
	myStateStart.mSecSince() > 35000)
    {
      printf("###### AcqiureBlock: failed\n");
      setState(STATE_BACKUP);
      //handler();
      return;
    }
    else if (myAcquire->getState() == Acquire::STATE_SUCCEEDED)
    {
      printf("###### AcquireBlock: successful\n");
      setState(STATE_PICKUP_BLOCK);
      //handler();
      return;
    }
    break;
  case STATE_PICKUP_BLOCK:
    if (myNewState)
    {
      printf("!! Pickup block\n");
      myNewState = false;
      mySony->panTilt(0, -15);
      myAcquire->deactivate();
      myPickUp->activate();
      myPickUp->setChannel(COLOR_BLOCK);
      myDriveTo->deactivate();
      myBackup->deactivate();
    }
    if (myPickUp->getState() == PickUp::STATE_FAILED)
    {
      printf("###### PickUpBlock: failed\n");
      setState(STATE_BACKUP);
      //handler();
      return;
    }
    else if (myPickUp->getState() == PickUp::STATE_SUCCEEDED)
    {
      printf("###### PickUpBlock: successful\n");
      setState(STATE_ACQUIRE_DROP_WALL);
      //handler();
      return;
    }
    break;
  case STATE_BACKUP:
    if (myNewState)
    {
      printf("!! Backup\n");
      myAcquire->deactivate();
      myPickUp->deactivate();
      myDriveTo->deactivate();
      myBackup->activate();
      myNewState = false;
    }
    if (myStateStart.mSecSince() > 2000)
    {
      printf("###### Backup: done\n");
      setState(STATE_ACQUIRE_BLOCK2);
      //handler();
      return;
    }
    break;
  case STATE_ACQUIRE_BLOCK2:
    if (myNewState)
    {
      printf("!! Acquire block 2\n");
      myNewState = false;
      mySony->panTilt(0, -25);
      myAcquire->activate();
      myAcquire->setChannel(COLOR_BLOCK);
      myPickUp->deactivate();
      myDriveTo->deactivate();
      myBackup->deactivate();
    }
    if (myGripper->getGripState() == 2 &&
	myGripper->getBreakBeamState() != 0)
    {
      printf("###### AcquireBlock2: Successful (have cube?)\n");
      setState(STATE_ACQUIRE_DROP_WALL);
      //handler();
      return;
    }
    else if (myGripper->getBreakBeamState() != 0)
    {
      printf("###### AcquireBlock2: Successful (cube in gripper?)\n");
      setState(STATE_PICKUP_BLOCK2);
      //handler();
      return;
    }
    if (myAcquire->getState() == Acquire::STATE_FAILED ||
	myStateStart.mSecSince() > 35000)
    {
      printf("###### AcqiureBlock2: failed\n");
      setState(STATE_FAILED);
      //handler();
      return;
    }
    else if (myAcquire->getState() == Acquire::STATE_SUCCEEDED)
    {
      printf("###### AcquireBlock2: successful\n");
      setState(STATE_PICKUP_BLOCK2);
      //handler();
      return;
    }
    break;
  case STATE_PICKUP_BLOCK2:
    if (myNewState)
    {
      printf("!! Pickup block 2\n");
      myNewState = false;
      myAcquire->deactivate();
      myPickUp->activate();
      mySony->panTilt(0, -25);
      myPickUp->setChannel(COLOR_BLOCK);
      myDriveTo->deactivate();
      myBackup->deactivate();
    }
    if (myPickUp->getState() == PickUp::STATE_FAILED)
    {
      printf("###### PickUpBlock2: failed\n");
      setState(STATE_FAILED);
      //handler();
      return;
    }
    else if (myPickUp->getState() == PickUp::STATE_SUCCEEDED)
    {
      printf("###### PickUpBlock2: successful\n");
      setState(STATE_ACQUIRE_DROP_WALL);
      //handler();
      return;
    }
    break;
  case STATE_ACQUIRE_DROP_WALL:
    if (myNewState)
    {
      printf("!! Acquire Drop wall, channel %d\n", myDropWall);
      myNewState = false;
      mySony->panTilt(0, -5);
      myAcquire->activate();
      myAcquire->setChannel(myDropWall);
      myPickUp->deactivate();
      myDriveTo->deactivate();
      myBackup->deactivate();
    }
    if (myGripper->getGripState() != 2 || 
	myGripper->getBreakBeamState() == 0)
    {
      printf("###### AcquireDropWall:: failed (lost cube)\n");
      setState(STATE_BACKUP);	       
      //handler();
      return;
    }
    if (myAcquire->getState() == Acquire::STATE_FAILED ||
	myStateStart.mSecSince() > 35000)
    {
      printf("###### AcquireDropWall:: failed\n");
      setState(STATE_FAILED);
      //handler();
      return;
    }
    else if (myAcquire->getState() == Acquire::STATE_SUCCEEDED)
    {
      printf("###### AcquireDropWall: successful\n");
      setState(STATE_DRIVETO_DROP_WALL);
      //handler();
      return;
    }
    break;
  case STATE_DRIVETO_DROP_WALL:
    if (myNewState)
    {
      printf("!! Driveto Drop wall, channel %d\n", myDropWall);
      myNewState = false;
      myAcquire->deactivate();
      myPickUp->deactivate();
      myDriveTo->activate();
      myDriveTo->setChannel(myDropWall);
      myBackup->deactivate();
    }
    if (myGripper->getGripState() != 2 || 
	myGripper->getBreakBeamState() == 0)
    {
      printf("###### DriveToDropWall:: failed (lost cube)\n");
      setState(STATE_BACKUP);
      //handler();
      return;
    }
    if (myDriveTo->getState() == DriveTo::STATE_FAILED)
    {
      printf("###### DriveToDropWall: failed\n");
      setState(STATE_FAILED);
      //handler();
      return;
    }
    else if (myDriveTo->getState() == DriveTo::STATE_SUCCEEDED)
    {
      printf("###### DriveToDropWall: succesful\n");
      setState(STATE_DROP);
      //handler();
      return;
    }
    break;
  case STATE_DROP:
    if (myNewState)
    {
      printf("!! Drop\n");
      printf("@@@@@ Drop lowering lift\n");
      myGripper->liftDown(); 
      myNewState = false;
      myAcquire->deactivate();
      myPickUp->deactivate();
      myDriveTo->deactivate();
      myBackup->deactivate();
      myGripOpenSent = false;
    }
    
    myGripper->liftDown();
    if ((myStateStart.mSecSince() > 500 && 
	 myGripper->isLiftMaxed() && myStateStart.mSecSince() < 5000) ||
	myStateStart.mSecSince() > 5000)
    {
      myGripper->gripOpen();
      /*if (!myGripOpenSent)
      {
	ArUtil::sleep(3);
	myGripper->gripOpen();
      }
      myGripOpenSent = true;
      */
    }
    if (myGripper->getGripState() == 1 || myStateStart.mSecSince() > 6000)
    {
      printf("###### Drop: success\n");
      setState(STATE_DROP_BACKUP);
      //handler();
      return;
    }
    break;
  case STATE_DROP_BACKUP:
    if (myNewState)
    {
      printf("!! Drop backup\n");
      myAcquire->deactivate();
      myPickUp->deactivate();
      myDriveTo->deactivate();
      myBackup->activate();
      myNewState = false;
    }
    if (myStateStart.mSecSince() > 2000)
    {
      printf("###### Drop_Backup: done\n");
      setState(STATE_ACQUIRE_LAP_WALL);
      //handler();
      return;
    }
    break;
  case STATE_ACQUIRE_LAP_WALL:
    if (myNewState)
    {
      printf("!! Acquire Lap wall, channel %d\n", myLapWall);
      myNewState = false;
      mySony->panTilt(0, -5);
      myAcquire->activate();
      myAcquire->setChannel(myLapWall);
      myPickUp->deactivate();
      myDriveTo->deactivate();
      myBackup->deactivate();
    }
    if (myAcquire->getState() == Acquire::STATE_FAILED ||
	myStateStart.mSecSince() > 35000)
    {
      printf("###### AcquireLapWall:: failed\n");
      setState(STATE_SWITCH);
      //handler();
      return;
    }
    else if (myAcquire->getState() == Acquire::STATE_SUCCEEDED)
    {
      printf("###### AcquireLapWall: successful\n");
      setState(STATE_DRIVETO_LAP_WALL);
      //handler();
      return;
    }
    break;
  case STATE_DRIVETO_LAP_WALL:
    if (myNewState)
    {
      printf("!! Driveto Lap wall, channel %d\n", myLapWall);
      myNewState = false;
      myAcquire->deactivate();
      myPickUp->deactivate();
      myDriveTo->activate();
      myDriveTo->setChannel(myLapWall);
      myBackup->deactivate();
    }
    if (myDriveTo->getState() == DriveTo::STATE_FAILED)
    {
      printf("###### DriveToLapWall: failed\n");
      setState(STATE_SWITCH);
      //handler();
      return;
    }
    else if (myDriveTo->getState() == DriveTo::STATE_SUCCEEDED)
    {
      printf("###### DriveToLapWall: succesful\n");
      setState(STATE_SWITCH);
      //handler();
      return;
    }
    break;
  case STATE_SWITCH:
    printf("!! Switching walls around.\n");
    tempColor = myDropWall;
    myDropWall = myLapWall;
    myLapWall = tempColor;
    setState(STATE_ACQUIRE_BLOCK);
    //handler();
    return;
  case STATE_FAILED:
    printf("@@@@@ Failed to complete the task!\n");
    myRobot->comInt(ArCommands::SONAR, 0);
    ArUtil::sleep(50);
    myRobot->comStr(ArCommands::SAY, "\52\77\37\62\42\70");
    ArUtil::sleep(500);
    Aria::shutdown();
    myRobot->disconnect();
    myRobot->stopRunning();
    return;
  }

}
Exemplo n.º 22
0
int main(int argc, char **argv)
{
  Aria::init();
  ArRobot robot;
  ArSonarDevice sonar;
  ArSick sick;
  robot.addRangeDevice(&sonar);
  ArServerBase server;

  // Argument parser:
  ArArgumentParser parser(&argc, argv);
  parser.loadDefaultArguments();

  // Connector and server opener:
  ArRobotConnector robotConnector(&parser, &robot);
  if(!robotConnector.connectRobot())
  {
    ArLog::log(ArLog::Terse, "popupExample: Could not connect to the robot.");
    if(parser.checkHelpAndWarnUnparsed())
    {
      Aria::logOptions();
    }
    Aria::exit(1);
  }

  ArLaserConnector laserConnector(&parser, &robot, &robotConnector);

  ArServerSimpleOpener simpleOpener(&parser);

  // Get command-line and other parameters
  if(!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed())
  {
    Aria::logOptions();
    Aria::exit(1);
  }

  robot.runAsync(true);

  // connect to the laser
  if(!laserConnector.connectLasers())
  {
    ArLog::log(ArLog::Normal, "popupExample: Warning: Could not connect to lasers.");
  }


  // Open the server
  if(!simpleOpener.open(&server))
  {
    ArLog::log(ArLog::Terse, "popupExample: Error, could not open server.");
    return 1;
  }
  server.runAsync();
  ArLog::log(ArLog::Normal, "popupExample: Server running. Press control-C to exit.");
  ArLog::log(ArLog::Normal, "popupExample: Each time an obstacle is detected near the robot, a new popup message will be created. Connect with MobileEyes to see them.");

  // Sends robot position etc.
  ArServerInfoRobot robotInfoServer(&server, &robot);

  // This service sends drawings e.g. showing range device positions
  ArServerInfoDrawings drawingsServer(&server);
  drawingsServer.addRobotsRangeDevices(&robot);

  // This service can send messages to clients to display as popup dialogs:
  ArServerHandlerPopup popupServer(&server);

  // This object contains the robot sensor interpretation task and creates
  // popups:
  SensorDetectPopup(&robot, &popupServer);

  // modes for controlling robot movement
  ArServerModeStop modeStop(&server, &robot);
  ArServerModeRatioDrive modeRatioDrive(&server, &robot);  
  ArServerModeWander modeWander(&server, &robot);
  modeStop.addAsDefaultMode();
  modeStop.activate();

  // allow configuration of driving and other settings
  ArServerHandlerConfig serverHandlerConfig(&server, Aria::getConfig()); // make a config handler
  ArLog::addToConfig(Aria::getConfig()); // let people configure logging

  modeRatioDrive.addToConfig(Aria::getConfig(), "Teleop settings"); // able to configure teleop settings

  robot.enableMotors();
  robot.waitForRunExit();

  Aria::exit(0);
}
int _tmain(int argc, char** argv)
{
    //-------------- M A I N    D E L   P R O G R A M A   D E L    R O B O T------------//
    //----------------------------------------------------------------------------------//

    //inicializaion de variables
    Aria::init();
    ArArgumentParser parser(&argc, argv);
    parser.loadDefaultArguments();
    ArSimpleConnector simpleConnector(&parser);
    ArRobot robot;
    ArSonarDevice sonar;
    ArAnalogGyro gyro(&robot);
    robot.addRangeDevice(&sonar);
    ActionTurns turn(400, 55);
    robot.addAction(&turn, 49);
    ActionTurns turn2(400, 55);
    robot.addAction(&turn2, 49);
    turn.deactivate();
    turn2.deactivate();

    // presionar tecla escape para salir del programa
    ArKeyHandler keyHandler;
    Aria::setKeyHandler(&keyHandler);
    robot.attachKeyHandler(&keyHandler);
    printf("Presionar ESC para salir\n");

    // uso de sonares para evitar colisiones con las paredes u
    // obstaculos grandes, mayores a 8cm de alto
    ArActionLimiterForwards limiterAction("limitador velocidad cerca", 300, 600, 250);
    ArActionLimiterForwards limiterFarAction("limitador velocidad lejos", 300, 1100, 400);
    ArActionLimiterTableSensor tableLimiterAction;
    robot.addAction(&tableLimiterAction, 100);
    robot.addAction(&limiterAction, 95);
    robot.addAction(&limiterFarAction, 90);


    // Inicializon la funcion de goto
    ArActionGoto gotoPoseAction("goto");
    robot.addAction(&gotoPoseAction, 50);

    // Finaliza el goto si es que no hace nada
    ArActionStop stopAction("stop");
    robot.addAction(&stopAction, 40);

    // Parser del CLI
    if (!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed())
    {
        Aria::logOptions();
        exit(1);
    }

    // Conexion del robot
    if (!simpleConnector.connectRobot(&robot))
    {
        printf("Could not connect to robot... exiting\n");
        Aria::exit(1);
    }
    robot.runAsync(true);

    // enciende motores, apaga sonidos
    robot.enableMotors();
    robot.comInt(ArCommands::SOUNDTOG, 0);

    // Imprimo algunos datos del robot como posicion velocidad y bateria
    robot.lock();
    ArLog::log(ArLog::Normal, "Posicion=(%.2f,%.2f,%.2f), Trans. Vel=%.2f, Bateria=%.2fV",
               robot.getX(), robot.getY(), robot.getTh(), robot.getVel(), robot.getBatteryVoltage());
    robot.unlock();

    const int duration = 100000; //msec
    ArLog::log(ArLog::Normal, "Completados los puntos en %d segundos", duration/1000);

    bool first = true;
    int goalNum = 0;
    int color = 3;
    ArTime start;
    start.setToNow();
    while (Aria::getRunning())
    {
        robot.lock();

        // inicia el primer punto
        if (first || gotoPoseAction.haveAchievedGoal())
        {
            first = false;

            goalNum++; //cambia de 0 a 1 el contador
            printf("El contador esta en: --> %d <---\n",goalNum);
            if (goalNum > 20)
                goalNum = 1;

            //comienza la secuencia de puntos
            if (goalNum == 1)
            {
                gotoPoseAction.setGoal(ArPose(1150, 0));
                ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
                           gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
                // Imprimo algunos datos del robot como posicion velocidad y bateria
                robot.lock();
                ArLog::log(ArLog::Normal, "Posicion=(%.2f,%.2f,%.2f), Trans. Vel=%.2f, Bateria=%.2fV",
                           robot.getX(), robot.getY(), robot.getTh(), robot.getVel(), robot.getBatteryVoltage());
                robot.unlock();
            }
            else if (goalNum == 2)
            {
                printf("Gira 90 grados izquierda\n");
                robot.unlock();
                turn.myActivate = 1;
                turn.myDirection = 1;
                turn.activate();
                ArUtil::sleep(1000);
                turn.deactivate();
                turn.myActivate = 0;
                turn.myDirection = 0;
                robot.lock();
            }
            else if (goalNum == 3)
            {
                gotoPoseAction.setGoal(ArPose(1150, 2670));
                ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
                           gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
                // Imprimo algunos datos del robot como posicion velocidad y bateria
                robot.lock();
                ArLog::log(ArLog::Normal, "Posicion=(%.2f,%.2f,%.2f), Trans. Vel=%.2f, Bateria=%.2fV",
                           robot.getX(), robot.getY(), robot.getTh(), robot.getVel(), robot.getBatteryVoltage());
                robot.unlock();
            }
            else if (goalNum == 4)
            {
                printf("Gira 90 grados izquierda\n");
                robot.unlock();
                turn2.myActivate = 1;
                turn2.myDirection = 1;
                turn2.activate();
                ArUtil::sleep(1000);
                turn2.deactivate();
                turn2.myActivate = 0;
                turn2.myDirection = 0;
                robot.lock();
            }
            else if (goalNum == 5)
            {
                gotoPoseAction.setGoal(ArPose(650, 2670));
                ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
                           gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
            }
            else if (goalNum == 6)
            {
                printf("Gira 90 grados izquierda\n");
                robot.unlock();
                turn2.myActivate = 1;
                turn2.myDirection = 1;
                turn2.activate();
                ArUtil::sleep(1000);
                turn2.deactivate();
                turn2.myActivate = 0;
                turn2.myDirection = 0;
                robot.lock();
            }
            else if (goalNum == 7)
            {
                gotoPoseAction.setGoal(ArPose(650, 0));
                ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
                           gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
            }
            else if (goalNum == 8)
            {
                gotoPoseAction.setGoal(ArPose(1800,1199));
                ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
                           gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
            }
            else if (goalNum == 9)
            {
                gotoPoseAction.setGoal(ArPose(2600, 1199));
                ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
                           gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
            }
            else if (goalNum == 10)
            {
                if (color == 1)
                {
                    gotoPoseAction.setGoal(ArPose(2800, 850));
                    ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
                               gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
                }
                if (color == 2)
                {
                    gotoPoseAction.setGoal(ArPose(3500, 1199));
                    ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
                               gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
                }
                if (color == 3)
                {
                    gotoPoseAction.setGoal(ArPose(2800, 1550));
                    ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
                               gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
                }
            }
            else if (goalNum == 11)
            {
                if (color == 1)
                {
                    gotoPoseAction.setGoal(ArPose(2800, 613));
                    ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
                               gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
                }
                if (color == 2)
                {
                    printf("Gira 180 grados derecha\n");
                    robot.unlock();
                    turn2.myActivate = 1;
                    turn2.myDirection = 2;
                    turn2.activate();
                    ArUtil::sleep(2000);
                    turn2.deactivate();
                    turn2.myActivate = 0;
                    turn2.myDirection = 0;
                    robot.lock();
                    goalNum = 19;
                }
                if (color == 3)
                {
                    gotoPoseAction.setGoal(ArPose(2800, 1785));
                    ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
                               gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
                }
            }
            else if (goalNum == 12)
            {
                if (color == 1)
                {
                    gotoPoseAction.setGoal(ArPose(3300, 413));
                    ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
                               gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
                }
                if (color == 3)
                {
                    gotoPoseAction.setGoal(ArPose(3300, 1985));
                    ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
                               gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
                }
            }
            else if (goalNum == 13)
            {
                if (color == 1)
                {
                    gotoPoseAction.setGoal(ArPose(3500, 413));
                    ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
                               gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
                }
                if (color == 3)
                {
                    gotoPoseAction.setGoal(ArPose(3500, 1985));
                    ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
                               gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
                }
            }
            else if (goalNum == 14)
            {
                //secuencia de drop de la figura
            }
            else if (goalNum == 15)
            {
                printf("Gira 180 grados derecha\n");
                robot.unlock();
                turn2.myActivate = 1;
                turn2.myDirection = 2;
                turn2.activate();
                ArUtil::sleep(2000);
                turn2.deactivate();
                turn2.myActivate = 0;
                turn2.myDirection = 0;
                robot.lock();
            }
            else if (goalNum == 16)
            {
                if (color == 1)
                {
                    gotoPoseAction.setGoal(ArPose(3300, 413));
                    ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
                               gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
                }
                if (color == 3)
                {
                    gotoPoseAction.setGoal(ArPose(3300, 1985));
                    ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
                               gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
                }
            }
            else if (goalNum == 17)
            {
                if (color == 1)
                {
                    gotoPoseAction.setGoal(ArPose(2800, 603));
                    ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
                               gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
                }
                if (color == 3)
                {
                    gotoPoseAction.setGoal(ArPose(2800, 1795));
                    ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
                               gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
                }
            }
            else if (goalNum == 18)
            {
                if (color == 1)
                {
                    gotoPoseAction.setGoal(ArPose(2800, 860));
                    ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
                               gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
                }
                if (color == 3)
                {
                    gotoPoseAction.setGoal(ArPose(2800, 1540));
                    ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
                               gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
                }
            }
            else if (goalNum == 19)
            {
                gotoPoseAction.setGoal(ArPose(2600, 1199));
                ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
                           gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
            }
            else if (goalNum == 20)
            {
                gotoPoseAction.setGoal(ArPose(1800, 1199));
                ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
                           gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
            }
        }

        if(start.mSecSince() >= duration) {
            ArLog::log(ArLog::Normal, "No puede llegar al punto, y la aplicacion saldra en %d", duration/1000);
            gotoPoseAction.cancelGoal();
            robot.unlock();
            ArUtil::sleep(3000);
            break;
        }

        robot.unlock();
        ArUtil::sleep(10);
    }

    // Robot desconectado al terminal el sleep
    Aria::shutdown();

//----------------------------------------------------------------------------------//

    return 0;
}
Exemplo n.º 24
0
/*
 *  dynamic reconfigure call back
*/
void RosAriaNode::dynamic_reconfigureCB(rosaria::RosAriaConfig &config, uint32_t level)
{
  //
  // Odometry Settings
  //
  robot->lock();
  if(TicksMM != config.TicksMM and config.TicksMM > 0)
  {
    ROS_INFO("Setting TicksMM from Dynamic Reconfigure: %d -> %d ", TicksMM, config.TicksMM);
    TicksMM = config.TicksMM;
    robot->comInt(93, TicksMM);
  }
  
  if(DriftFactor != config.DriftFactor)
  {
    ROS_INFO("Setting DriftFactor from Dynamic Reconfigure: %d -> %d ", DriftFactor, config.DriftFactor);
    DriftFactor = config.DriftFactor;
    robot->comInt(89, DriftFactor);
  }
  
  if(RevCount != config.RevCount and config.RevCount > 0)
  {
    ROS_INFO("Setting RevCount from Dynamic Reconfigure: %d -> %d ", RevCount, config.RevCount);
    RevCount = config.RevCount;
    robot->comInt(88, RevCount);
  }
  
  //
  // Acceleration Parameters
  //
  int value;
  value = config.trans_accel * 1000;
  if(value != robot->getTransAccel() and value > 0)
  {
    ROS_INFO("Setting TransAccel from Dynamic Reconfigure: %d", value);
    robot->setTransAccel(value);
  }
  
  value = config.trans_decel * 1000;
  if(value != robot->getTransDecel() and value > 0)
  {
    ROS_INFO("Setting TransDecel from Dynamic Reconfigure: %d", value);
    robot->setTransDecel(value);
  } 
  
  value = config.lat_accel * 1000;
  if(value != robot->getLatAccel() and value > 0)
  {
    ROS_INFO("Setting LatAccel from Dynamic Reconfigure: %d", value);
    if (robot->getAbsoluteMaxLatAccel() > 0 )
      robot->setLatAccel(value);
  }
  
  value = config.lat_decel * 1000;
  if(value != robot->getLatDecel() and value > 0)
  {
    ROS_INFO("Setting LatDecel from Dynamic Reconfigure: %d", value);
    if (robot->getAbsoluteMaxLatDecel() > 0 )
      robot->setLatDecel(value);
  }
  
  value = config.rot_accel * 180/M_PI;
  if(value != robot->getRotAccel() and value > 0)
  {
    ROS_INFO("Setting RotAccel from Dynamic Reconfigure: %d", value);
    robot->setRotAccel(value);
  }
  
  value = config.rot_decel * 180/M_PI;
  if(value != robot->getRotDecel() and value > 0)
  {
    ROS_INFO("Setting RotDecel from Dynamic Reconfigure: %d", value);
    robot->setRotDecel(value);
  } 
  robot->unlock();
}
int main(int argc, char **argv) 
{

  Aria::init();
  ArRobot robot;
  ArArgumentParser argParser(&argc, argv);
  ArSimpleConnector connector(&argParser);
  ArGripper gripper(&robot);
  ArSonarDevice sonar;
  robot.addRangeDevice(&sonar);

  argParser.loadDefaultArguments();

  if (!Aria::parseArgs() || !argParser.checkHelpAndWarnUnparsed())
  {
    Aria::logOptions();
    Aria::shutdown();
    return 1;
  }
  
  if (!connector.connectRobot(&robot))
  {
    ArLog::log(ArLog::Terse, "gripperExample: Could not connect to robot... exiting");
    Aria::shutdown();
    return 2;
  }
  ArLog::log(ArLog::Normal, "gripperExample: Connected to robot.");

  ArLog::log(ArLog::Normal, "gripperExample: GripperType=%d", gripper.getType());
  gripper.logState();
  if(gripper.getType() == ArGripper::NOGRIPPER)
  {
    ArLog::log(ArLog::Terse, "gripperExample: Error: Robot does not have a gripper. Exiting.");
    Aria::shutdown();
    return -1;
  }

  // Teleoperation actions with obstacle-collision avoidance
  ArActionLimiterTableSensor tableLimit;
  robot.addAction(&tableLimit, 110);
  ArActionLimiterForwards limitNearAction("near", 300, 600, 250);
  robot.addAction(&limitNearAction, 100);
  ArActionLimiterForwards limitFarAction("far", 300, 1100, 400);
  robot.addAction(&limitFarAction, 90);
  ArActionLimiterBackwards limitBackAction;
  robot.addAction(&limitBackAction, 50);
  ArActionJoydrive joydriveAction("joydrive", 400, 15);
  robot.addAction(&joydriveAction, 40);
  joydriveAction.setStopIfNoButtonPressed(false);
  ArActionKeydrive keydriveAction;
  robot.addAction(&keydriveAction, 30);
  

  // Handlers to control the gripper and print out info (classes defined above)
  PrintGripStatus printStatus(&gripper);
  GripperControlHandler gripControl(&gripper);
  printStatus.addRobotTask(&robot);
  gripControl.addKeyHandlers(&robot);

  // enable motors and run (if we lose connection to the robot, exit)
  ArLog::log(ArLog::Normal, "You may now operate the robot with arrow keys or joystick. Operate the gripper with the u, d, o, c, and page up/page down keys.");
  robot.enableMotors();
  robot.run(true);
  
  Aria::shutdown();
  return 0;
}
Exemplo n.º 26
0
int RosAriaNode::Setup()
{
  // Note, various objects are allocated here which are never deleted (freed), since Setup() is only supposed to be
  // called once per instance, and these objects need to persist until the process terminates.

  robot = new ArRobot();
  ArArgumentBuilder *args = new ArArgumentBuilder(); //  never freed
  ArArgumentParser *argparser = new ArArgumentParser(args); // Warning never freed
  argparser->loadDefaultArguments(); // adds any arguments given in /etc/Aria.args.  Useful on robots with unusual serial port or baud rate (e.g. pioneer lx)

  // Now add any parameters given via ros params (see RosAriaNode constructor):

  // if serial port parameter contains a ':' character, then interpret it as hostname:tcpport
  // for wireless serial connection. Otherwise, interpret it as a serial port name.
  size_t colon_pos = serial_port.find(":");
  if (colon_pos != std::string::npos)
  {
    args->add("-remoteHost"); // pass robot's hostname/IP address to Aria
    args->add(serial_port.substr(0, colon_pos).c_str());
    //ROS_INFO( "RosAria: using IP: [%s]", serial_port.substr(0, colon_pos).c_str() );
    args->add("-remoteRobotTcpPort"); // pass robot's TCP port to Aria
    args->add(serial_port.substr(colon_pos+1).c_str());
    //ROS_INFO( "RosAria: using port: [%s]", serial_port.substr(colon_pos+1).c_str() );
  }
  else
  {
    args->add("-robotPort"); // pass robot's serial port to Aria
    args->add(serial_port.c_str());
  }

  // if a baud rate was specified in baud parameter
  if(serial_baud != 0)
  {
    args->add("-robotBaud");
    char tmp[100];
    snprintf(tmp, 100, "%d", serial_baud);
    args->add(tmp);
  }
  
  if( debug_aria )
  {
    // turn on all ARIA debugging
    args->add("-robotLogPacketsReceived"); // log received packets
    args->add("-robotLogPacketsSent"); // log sent packets
    args->add("-robotLogVelocitiesReceived"); // log received velocities
    args->add("-robotLogMovementSent");
    args->add("-robotLogMovementReceived");
    ArLog::init(ArLog::File, ArLog::Verbose, aria_log_filename.c_str(), true);
  }


  // Connect to the robot
  conn = new ArRobotConnector(argparser, robot); // warning never freed
  if (!conn->connectRobot()) {
    ROS_ERROR("RosAria: ARIA could not connect to robot! (Check ~port parameter is correct, and permissions on port device.)");
    return 1;
  }

  // causes ARIA to load various robot-specific hardware parameters from the robot parameter file in /usr/local/Aria/params
  if(!Aria::parseArgs())
  {
    ROS_ERROR("RosAria: ARIA error parsing ARIA startup parameters!");
    return 1;
  }

  readParameters();

  // Start dynamic_reconfigure server
  dynamic_reconfigure_server = new dynamic_reconfigure::Server<rosaria::RosAriaConfig>;
  

  /*
   * 横向速度和纵向速度单位为米
*/

  /*
   * 初始化参数的最小值 其中TickMM=10 DriftFactor=-200 Revount=-32760
   */

  // Setup Parameter Minimums
  rosaria::RosAriaConfig dynConf_min;
  dynConf_min.trans_accel = robot->getAbsoluteMaxTransAccel() / 1000;
  dynConf_min.trans_decel = robot->getAbsoluteMaxTransDecel() / 1000;
  // TODO: Fix rqt dynamic_reconfigure gui to handle empty intervals
  // Until then, set unit length interval.
  dynConf_min.lat_accel = ((robot->getAbsoluteMaxLatAccel() > 0.0) ? robot->getAbsoluteMaxLatAccel() : 0.1) / 1000;
  dynConf_min.lat_decel = ((robot->getAbsoluteMaxLatDecel() > 0.0) ? robot->getAbsoluteMaxLatDecel() : 0.1) / 1000;
  dynConf_min.rot_accel = robot->getAbsoluteMaxRotAccel() * M_PI/180;
  dynConf_min.rot_decel = robot->getAbsoluteMaxRotDecel() * M_PI/180;
  
  // I'm setting these upper bounds relitivly arbitrarily, feel free to increase them.
  dynConf_min.TicksMM     = 10;
  dynConf_min.DriftFactor = -200;
  dynConf_min.RevCount    = -32760;
  
  dynamic_reconfigure_server->setConfigMin(dynConf_min);
  
  
  // 初始化参数的最大值 其中TickMM=200 DriftFactor=200 Revount=32760
  rosaria::RosAriaConfig dynConf_max;
  dynConf_max.trans_accel = robot->getAbsoluteMaxTransAccel() / 1000;
  dynConf_max.trans_decel = robot->getAbsoluteMaxTransDecel() / 1000;
  // TODO: Fix rqt dynamic_reconfigure gui to handle empty intervals
  // Until then, set unit length interval.
  dynConf_max.lat_accel = ((robot->getAbsoluteMaxLatAccel() > 0.0) ? robot->getAbsoluteMaxLatAccel() : 0.1) / 1000;
  dynConf_max.lat_decel = ((robot->getAbsoluteMaxLatDecel() > 0.0) ? robot->getAbsoluteMaxLatDecel() : 0.1) / 1000;
  dynConf_max.rot_accel = robot->getAbsoluteMaxRotAccel() * M_PI/180;
  dynConf_max.rot_decel = robot->getAbsoluteMaxRotDecel() * M_PI/180;
  
  // I'm setting these upper bounds relitivly arbitrarily, feel free to increase them.
  dynConf_max.TicksMM     = 200;
  dynConf_max.DriftFactor = 200;
  dynConf_max.RevCount    = 32760;
  
  dynamic_reconfigure_server->setConfigMax(dynConf_max);
  
   // 初始化参数的默认值
  rosaria::RosAriaConfig dynConf_default;
  dynConf_default.trans_accel = robot->getTransAccel() / 1000;
  dynConf_default.trans_decel = robot->getTransDecel() / 1000;
  dynConf_default.lat_accel   = robot->getLatAccel() / 1000;
  dynConf_default.lat_decel   = robot->getLatDecel() / 1000;
  dynConf_default.rot_accel   = robot->getRotAccel() * M_PI/180;
  dynConf_default.rot_decel   = robot->getRotDecel() * M_PI/180;

  dynConf_default.TicksMM     = TicksMM;
  dynConf_default.DriftFactor = DriftFactor;
  dynConf_default.RevCount    = RevCount;
  
  dynamic_reconfigure_server->setConfigDefault(dynConf_max);
  
  dynamic_reconfigure_server->setCallback(boost::bind(&RosAriaNode::dynamic_reconfigureCB, this, _1, _2));

  // Enable the motors
  robot->enableMotors();

  // disable sonars on startup
  robot->disableSonar();

  // callback will  be called by ArRobot background processing thread for every SIP data packet received from robot
  robot->addSensorInterpTask("ROSPublishingTask", 100, &myPublishCB);

  // Initialize bumpers with robot number of bumpers
  bumpers.front_bumpers.resize(robot->getNumFrontBumpers());
  bumpers.rear_bumpers.resize(robot->getNumRearBumpers());

  // Run ArRobot background processing thread
  robot->runAsync(true);

  return 0;
}
Exemplo n.º 27
0
int main(int argc, char **argv)
{
  Aria::init();
  ArRobot robot;
  ArArgumentParser parser(&argc, argv);
  ArSimpleConnector simpleConnector(&parser);

  // The base server object, manages all connections to clients.
  ArServerBase server;

  // This object simplifies configuration and opening of the ArServerBase
  // object.
  ArServerSimpleOpener simpleOpener(&parser);

  // parse the command line. fail and print the help if the parsing fails
  // or if the help was requested with -help
  parser.loadDefaultArguments();
  if (!simpleConnector.parseArgs() || !simpleOpener.parseArgs() || 
      !parser.checkHelpAndWarnUnparsed())
  {    
    simpleConnector.logOptions();
    simpleOpener.logOptions();
    exit(1);
  }

  // Use the ArSimpleOpener to open the server port
  if (!simpleOpener.open(&server))
  {
    ArLog::log(ArLog::Terse, "Error: Could not open server on port %d", simpleOpener.getPort());
    exit(1);
  }


  //
  // Create services attached to the base server:
  // 
  
  // Robot position etc.:
  ArServerInfoRobot serverInfoRobot(&server, &robot);

  // Robot control modes (only one mode can be active at once):
  ArServerModeStop modeStop(&server, &robot);    
  // old ArServerModeDrive modeDrive(&server, &robot); 
  ArServerModeRatioDrive modeRatioDrive(&server, &robot);
  ArServerModeWander modeWander(&server, &robot);
  modeStop.addAsDefaultMode();
  modeStop.activate();

  // This provides a simple way to add new commands.
  ArServerHandlerCommands commands(&server);

  // Add our custom command. ArServerHandlerCommands also has other methods
  // for adding commands taht take different kinds of arguments, or no
  // arguments.
  ArGlobalFunctor1<ArArgumentBuilder*> customCommandFunctor(&customCommandHandler);
  commands.addStringCommand("ExampleCustomCommand", "Example of a custom command. simpleServerExample will print out the text sent with the command.", &customCommandFunctor);
 
  // These objects provide various debugging and diagnostic custom commands:
  ArServerSimpleComUC uCCommands(&commands, &robot);  // Get information about the robot
  ArServerSimpleComMovementLogging loggingCommands(&commands, &robot);  // Control logging
  modeRatioDrive.addControlCommands(&commands);  // Drive mode diagnostics

  // This provides the client (e.g. MobileEyes) with a simple table of string values
  // (called an InfoGroup). An InfoGroup is kept globally by Aria.
  // The values in the table sent to clients are retrieved periodically by calling a 
  // functor.
  ArServerInfoStrings stringInfo(&server);
  Aria::getInfoGroup()->addAddStringCallback(stringInfo.getAddStringFunctor());

  // Here are some example entries in the InfoGroup:
  Aria::getInfoGroup()->addStringInt(
	  "Motor Packet Count", 10, 
	  new ArConstRetFunctorC<int, ArRobot>(&robot, 
					       &ArRobot::getMotorPacCount));

  //
  // Connect to the robot:
  // 
  
  if (!simpleConnector.connectRobot(&robot))
  {
    printf("Error: Could not connect to robot... exiting\n");
    Aria::shutdown();
    return 1;
  }


  robot.enableMotors();
  robot.runAsync(true);

  // The simple opener might have information to display right before starting 
  // the server thread:
  simpleOpener.checkAndLog();

  // now let the server base run in a new thread, accepting client connections.
  server.runAsync();

  ArLog::log(ArLog::Normal, "Server is now running... Press Ctrl-C to exit.");

  robot.waitForRunExit();
  Aria::shutdown();
  exit(0);  
}
Exemplo n.º 28
0
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);
  }

}
// this is the function called in the new thread
void *Joydrive::runThread(void *arg)
{
  threadStarted();

  int trans, rot;

  // only run while running, ie play nice and pay attention to the thread 
  //being shutdown
  while (myRunning)
  {
    // lock the robot before touching it
    myRobot->lock();
    if (!myRobot->isConnected())
    {
      myRobot->unlock();
      break;
    }
    // print out some information about the robot
    printf("\rx %6.1f  y %6.1f  tth  %6.1f vel %7.1f mpacs %3d   ", 
	   myRobot->getX(), myRobot->getY(), myRobot->getTh(), 
	   myRobot->getVel(), myRobot->getMotorPacCount());
    fflush(stdout);
    // if one of the joystick buttons is pushed, drive the robot
    if (myJoyHandler.haveJoystick() && (myJoyHandler.getButton(1) ||
					myJoyHandler.getButton(2)))
    {
      // get out the values from the joystick
      myJoyHandler.getAdjusted(&rot, &trans);
      // drive the robot
      myRobot->setVel(trans);
      myRobot->setRotVel(-rot);
    }
    // if no buttons are pushed stop the robot
    else
    {
      myRobot->setVel(0);
      myRobot->setRotVel(0);
    }
    // unlock the robot, so everything else can run
    myRobot->unlock();
    // now take a little nap
    ArUtil::sleep(50);
  }
  // return out here, means the thread is done
  return NULL;
}
Exemplo n.º 30
0
int main(int argc, char **argv)
{
  Aria::init();
  ArArgumentParser argParser(&argc, argv);
  argParser.loadDefaultArguments();
  ArRobot robot;
  ArRobotConnector robotConnector(&argParser, &robot);
  ArLaserConnector laserConnector(&argParser, &robot, &robotConnector);

  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.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.");
  }


  // turn on the motors, turn off amigobot sounds
  robot.enableMotors();
  robot.comInt(ArCommands::SOUNDTOG, 0);

  // add a set of actions that combine together to effect the wander behavior //działa
	//general structure: robot.addAction (new Name_of_action (parameters), priority);
	//to find out more about Actions go to ~/catkin_ws/src/rosaria/local/Aria/docs/index.html

// if we're stalled we want to back up and recover
  robot.addAction(new ArActionStallRecover("stall recover", 100,50,100,30), 100); //basic values 225, 50, 150, 45
								//when_stop, cycles_to_move, speed, turn'
//slow down when near obstacle
  robot.addAction(new ArActionLimiterForwards("speed limiter near", 
						200, 200, 200), 95); // basic values 300,600,250
								//stop_distance, slow_distance, slow_speed
  // react to bumpers
  robot.addAction(new ArActionLimiterBackwards, 75);

// turn avoid very close
  robot.addAction(new ArActionAvoidFront("speed limiter far", 150,450,20), 47);	//basic values 450, 200,15
						//when_turn, speed, turn'

// turn avoid things near
  robot.addAction(new ArActionAvoidFront("speed limiter far", 300,450,10), 46);	//basic values 450, 200,15
						//when_turn, speed, turn'

// turn avoid things further away
  robot.addAction(new ArActionAvoidFront("speed limiter far", 800,450,5), 45);	//basic values 450, 200,15
						//when_turn, speed, turn'
  
//move forward
  robot.addAction(new ArActionConstantVelocity("Constant Velocity", 500), 25);



  
  // wait for robot task loop to end before exiting the program
  robot.waitForRunExit();
  
  Aria::exit(0);
  return 0;
}