示例#1
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);

}
示例#2
0
int main(int argc, char **argv) 
{
  std::string str;
  int ret;
  
  ArGlobalRetFunctor1<bool, ArRobotPacket *> tcm2PrinterCB(&tcm2Printer);
  ArSerialConnection con;
  Aria::init();
  
  robot = new ArRobot;

  robot->addPacketHandler(&tcm2PrinterCB, ArListPos::FIRST);
  if ((ret = con.open()) != 0)
  {
    str = con.getOpenMessage(ret);
    printf("Open failed: %s\n", str.c_str());
    exit(0);
  }

  robot->setDeviceConnection(&con);
  if (!robot->blockingConnect())
  {
    printf("Could not connect to robot... exiting\n");
    exit(0);
  }
  
  printf("%6s %6s %6s %6s %6s %6s %6s %10s %4s %4s %6s\n", 
	 "comp", "pitch", "roll", "magX", "magY", "magZ", "temp", "error",
	 "calH", "calV", "calM");
	 robot->comInt(ArCommands::TCM2, 3);

  robot->run(true);
  Aria::shutdown();
  
}
示例#3
0
int main(int argc, char **argv) 
{
  std::string str;
  int ret;
  ArTime start;
  
  // 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);

  // 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);

  int i;
  while (Aria::getRunning())
  {
    robot.lock();
    robot.comStr(ArCommands::TTY3, "1234567890");
    robot.unlock();
  }

  robot.disconnect();
  // shutdown and ge tout
  Aria::shutdown();
  return 0;
}
int main(int argc, char **argv)
{
    // just some stuff for returns
    std::string str;
    int ret;

    // robots connection
    ArSerialConnection con;
    // the robot, this turns state reflection off
    ArRobot robot(NULL, false);
    // the joydrive as defined above, this also adds itself as a user task
    Joydrive joyd(&robot);

    // mandatory init
    Aria::init();

    // open the connection, if it fails, exit
    if ((ret = con.open()) != 0)
    {
        str = con.getOpenMessage(ret);
        printf("Open failed: %s\n", str.c_str());
        Aria::shutdown();
        return 1;
    }

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

    // turn off the sonar, enable the motors, turn off amigobot sounds
    robot.comInt(ArCommands::SONAR, 0);
    robot.comInt(ArCommands::ENABLE, 1);
    robot.comInt(ArCommands::SOUNDTOG, 0);

    // run, if we lose connection to the robot, exit
    robot.run(true);

    // shutdown and go away
    Aria::shutdown();
    return 0;
}
示例#5
0
int main(void)
{
  
  ArSerialConnection serConn;

  if (serConn.open(ArUtil::COM1) != 0)
  {
    printf("Could not open serial port\n");
    exit(1);
  }

  while (1)
  {
    printf("\rDCD %d", serConn.getDCD());
    fflush(stdout);
  }
  return 0;
}
int main()
{
  ArModuleLoader::Status status;
  ArSerialConnection con;
  ArRobot robot;
  int ret;
  std::string str;

  Aria::init();

  status=ArModuleLoader::load("./joydriveActionMod", &robot);
  printStatus(status);

  if (status == ArModuleLoader::STATUS_INIT_FAILED)
    return(1);

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

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

  robot.run(true);
  
  status=ArModuleLoader::close("./joydriveActionMod");
  printStatus(status);

  Aria::shutdown();
  return 0;
}
示例#7
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;
}
示例#8
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;
}
示例#9
0
// Create an ArGPS object. If some options were obtained from command-line
// parameters by parseArgs(), use those, otherwise get values from robot
// parameters (the .p file) if we have a valid robot with valid parameters.
AREXPORT ArGPS* ArGPSConnector::createGPS(ArRobot *robot)
{
  // If we have a robot with parameters (i.e. have connected and read the .p
  // file), use those values unless already set by parseArgs() from command-line 
  if(robot && robot->getRobotParams())
  {
    if(myPort == NULL) {
      myPort = robot->getRobotParams()->getGPSPort();
      if(strcmp(myPort, "COM1") == 0)
        myPort = ArUtil::COM1;
      if(strcmp(myPort, "COM2") == 0)
        myPort = ArUtil::COM2;
      if(strcmp(myPort, "COM3") == 0)
        myPort = ArUtil::COM3;
      if(strcmp(myPort, "COM4") == 0)
        myPort = ArUtil::COM4;
    }
    if(myBaud == -1) {
      myBaud = robot->getRobotParams()->getGPSBaud();
    }
    if(myDeviceType == Invalid) {
      myDeviceType = deviceTypeFromString(robot->getRobotParams()->getGPSType());
    }
  }
  else
  {
    if(myPort == NULL) myPort = ARGPS_DEFAULT_SERIAL_PORT;
    if(myBaud == -1) myBaud = ARGPS_DEFAULT_SERIAL_BAUD;
    if(myDeviceType == Invalid) myDeviceType = Standard;
  }

  // Create gps:
  ArGPS* newGPS = NULL;
  switch (myDeviceType)
  {
    case Novatel:
      ArLog::log(ArLog::Normal, "ArGPSConnector: Using Novatel GPS");
      newGPS = new ArNovatelGPS;
      break;
    case Trimble:
      ArLog::log(ArLog::Normal, "ArGPSConnector: Using Trimble GPS");
      newGPS = new ArTrimbleGPS;
      break;
    case NovatelSPAN:
      ArLog::log(ArLog::Normal, "ArGPSConnector: Using Novatel SPAN GPS");
      newGPS = new ArNovatelSPAN;
      break;
    default:
      ArLog::log(ArLog::Normal, "ArGPSConnector: Using standard NMEA GPS");
      newGPS = new ArGPS;
      break;
  }

  if (myTCPHost == NULL)
  {
    // Setup serial connection
    ArSerialConnection *serialCon = new ArSerialConnection;
    ArLog::log(ArLog::Normal, "ArGPSConnector: Connecting to GPS on port %s at %d baud...", myPort, myBaud);
    if (!serialCon->setBaud(myBaud)) { delete serialCon; return false; }
    if (serialCon->open(myPort) != 0) {
      ArLog::log(ArLog::Terse, "ArGPSConnector: Error: could not open GPS serial port %s.", myPort);
      delete serialCon;
      return NULL;
    }
    newGPS->setDeviceConnection(serialCon);
    myDeviceCon = serialCon;
  }
  else
  {
    // Setup TCP connection
    ArTcpConnection *tcpCon = new ArTcpConnection;
    ArLog::log(ArLog::Normal, "ArGPSConnector: Opening TCP connection to %s:%d...", myTCPHost, myTCPPort);
    int openState = tcpCon->open(myTCPHost, myTCPPort);
    if (openState != 0) {
      ArLog::log(ArLog::Terse, "ArGPSConnector: Error: could not open TCP connection to %s port %d: %s", tcpCon->getOpenMessage(openState));
      delete tcpCon;
      return NULL;
    }
    newGPS->setDeviceConnection(tcpCon);
    myDeviceCon = tcpCon;
  }

  return newGPS;
}
int main(int argc, char **argv)
{
    std::string str;
    int ret;
    time_t lastTime;
    int trans, rot;

    ArJoyHandler joyHandler;
    ArSerialConnection con;
    ArRobot robot(NULL, false);

    joyHandler.init();
    joyHandler.setSpeeds(100, 700);

    if (joyHandler.haveJoystick())
    {
        printf("Have a joystick\n\n");
    }
    else
    {
        printf("Do not have a joystick, set up the joystick then rerun the program\n\n");
//    exit(0);
    }

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


    robot.setDeviceConnection(&con);
    if (!robot.blockingConnect())
    {
        printf("Could not connect to robot... exiting\n");
        exit(0);
    }

    robot.comInt(ArCommands::SONAR, 0);
    robot.comInt(ArCommands::ENABLE, 1);
    robot.comInt(ArCommands::SOUNDTOG, 0);
    //robot.comInt(ArCommands::ENCODER, 1);

    //robot.comStrN(ArCommands::SAY, "\1\6\2\105", 4);

    lastTime = time(NULL);
    while (1)
    {
        if (!robot.isConnected())
        {
            printf("No longer connected to robot, exiting.\n");
            exit(0);
        }
        robot.loopOnce();
        if (lastTime != time(NULL))
        {
            printf("\rx %6.1f  y %6.1f  tth  %6.1f vel %7.1f mpacs %3d", robot.getX(),
                   robot.getY(), robot.getTh(), robot.getVel(),
                   robot.getMotorPacCount());
            fflush(stdout);
            lastTime = time(NULL);
        }
        if (joyHandler.haveJoystick() && (joyHandler.getButton(1) ||
                                          joyHandler.getButton(2)))
        {
            if (ArMath::fabs(robot.getVel()) < 10.0)
                robot.comInt(ArCommands::ENABLE, 1);
            joyHandler.getAdjusted(&rot, &trans);
            robot.comInt(ArCommands::VEL, trans);
            robot.comInt(ArCommands::RVEL, -rot);
        }
        else
        {
            robot.comInt(ArCommands::VEL, 0);
            robot.comInt(ArCommands::RVEL, 0);
        }
        ArUtil::sleep(100);
    }


}
示例#11
0
int main(int argc, char **argv) 
{
  std::string str;
  int ret;
  ArTime start;
  
  // 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);

  // 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
  printf("Telling the robot to go 300 mm for 5 seconds\n");
  robot.lock();
  robot.setVel(500);
  robot.unlock();
  start.setToNow();
  while (1)
  {
    robot.lock();
    if (start.mSecSince() > 5000)
    {
      robot.unlock();
      break;
    }   
    printf("Trans: %10g Rot: %10g\n", robot.getVel(), robot.getRotVel());
    robot.unlock();
    ArUtil::sleep(100);
  }
  
  printf("Telling the robot to turn at 50 deg/sec for 10 seconds\n");
  robot.lock();
  robot.setVel(0);
  robot.setRotVel(50);
  robot.unlock();
  start.setToNow();
  while (1)
  {
    robot.lock();
    if (start.mSecSince() > 10000)
    {
      robot.unlock();
      break;
    }   
    printf("Trans: %10g Rot: %10g\n", robot.getVel(), robot.getRotVel());
    robot.unlock();
    ArUtil::sleep(100);
  }

  printf("Telling the robot to turn at 100 deg/sec for 10 seconds\n");
  robot.lock();
  robot.setVel(0);
  robot.setRotVel(100);
  robot.unlock();
  start.setToNow();
  while (1)
  {
    robot.lock();
    if (start.mSecSince() > 10000)
    {
      robot.unlock();
      break;
    }   
    printf("Trans: %10g Rot: %10g\n", robot.getVel(), robot.getRotVel());
    robot.unlock();
    ArUtil::sleep(100);
  }

  printf("Done with tests, exiting\n");
  robot.disconnect();
  // shutdown and ge tout
  Aria::shutdown();
  return 0;
}
示例#12
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;
  //ArLog::init(ArLog::StdOut, ArLog::Verbose);
  srand(time(NULL));
  robot.runAsync(false);
// if (!exitOnFailure)
//    ArLog::init(ArLog::None, ArLog::Terse);
  //else
  //ArLog::init(ArLog::None);
  while (1)
  {
    if (con.getStatus() != ArDeviceConnection::STATUS_OPEN &&
	(ret = con.open()) != 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);
	robot.unlock();
	continue;
      }
    }
    robot.lock();
    robot.setDeviceConnection(&con);
    robot.unlock();
    ArUtil::sleep((rand() % 5) * 100);
    if (robot.asyncConnect())
    {
      robot.waitForConnectOrConnFail();
      robot.lock();
      if (!robot.isConnected())
      {
	if (exitOnFailure)
	{
	  printf("Failed after %d tries.\n", successes);
	  exit(0);
	}
	printf("Failed to connect successfully");
	++failures;
      }
      robot.comInt(ArCommands::SONAR, 0);
      robot.comInt(ArCommands::SOUNDTOG, 0);
      //robot.comInt(ArCommands::PLAYLIST, 0);
      robot.comInt(ArCommands::ENCODER, 1);
      ArUtil::sleep(((rand() % 20) + 3) * 100);
      ++successes;
      // okay, now try to leave it in a messed up state
      action = rand() % 8;
      robot.dropConnection();
      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;
      }
      robot.unlock();
    }
    else
    {
      if (exitOnFailure)
      {
	printf("Failed after %d tries.\n", successes);
	exit(0);
      }
      printf("Failed to start connect ");
      ++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; 
}
示例#13
0
int main(int argc, char **argv)
{
  int ret;
  std::string str;
  // the serial connection (robot)
  ArSerialConnection serConn;
  // tcp connection (sim)
  ArTcpConnection tcpConn;
  // the robot
  ArRobot robot;
  // the laser
  ArSick sick;
  // the laser connection
  ArSerialConnection laserCon;

  bool useSimForLaser = false;


  std::string hostname = "prod.local.net";

  // timeouts in minutes
  int wanderTime = 0;
  int restTime = 0;


  // check arguments
  if (argc == 3 || argc == 4)
  {
    wanderTime = atoi(argv[1]);
    restTime = atoi(argv[2]);
    if (argc == 4)
      hostname = argv[3];
  }
  else
  {
    printf("\nUsage:\n\tpeoplebotTest <wanderTime> <restTime> <hostname>\n\n");
    printf("Times are in minutes.  Hostname is the machine to pipe the ACTS display to\n\n");
    wanderTime = 15;
    restTime = 45;
  }

  printf("Wander time - %d minutes\nRest time - %d minutes\n", wanderTime, restTime);
  printf("Sending display to %s.\n\n", hostname.c_str());

  // sonar, must be added to the robot
  ArSonarDevice sonar;

  // the actions we'll use to wander
  ArActionStallRecover recover;
  ArActionBumpers bumpers;
  ArActionAvoidFront avoidFrontNear("Avoid Front Near", 225, 0);
  ArActionAvoidFront avoidFrontFar;

  // Make a key handler, so that escape will shut down the program
  // cleanly
  ArKeyHandler keyHandler;

  // mandatory init
  Aria::init();

  // Add the key handler to Aria so other things can find it
  Aria::setKeyHandler(&keyHandler);

  // Attach the key handler to a robot now, so that it actually gets
  // some processing time so it can work, this will also make escape
  // exit
  robot.attachKeyHandler(&keyHandler);


  // First we see if we can open the tcp connection, if we can we'll
  // assume we're connecting to the sim, and just go on...  if we
  // can't open the tcp it means the sim isn't there, so just try the
  // robot

  // modify this next line if you're not using default tcp connection
  tcpConn.setPort();

  // see if we can get to the simulator  (true is success)
  if (tcpConn.openSimple())
  {
    // we could get to the sim, so set the robots device connection to the sim
    printf("Connecting to simulator through tcp.\n");
    robot.setDeviceConnection(&tcpConn);
  }
  else
  {
    // we couldn't get to the sim, so set the port on the serial
    // connection and then set the serial connection as the robots
    // device

    // modify the next line if you're not using the first serial port
    // to talk to your robot
    serConn.setPort();
    printf(
      "Could not connect to simulator, connecting to robot through serial.\n");
    robot.setDeviceConnection(&serConn);
  }
  
  
  // add the sonar to the robot
  robot.addRangeDevice(&sonar);

  // add the laser
  robot.addRangeDevice(&sick);

  // try to connect, if we fail 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::ENABLE, 1);
  robot.comInt(ArCommands::SOUNDTOG, 0);

  // turn off the sonar to start with
  robot.comInt(ArCommands::SONAR, 0);

  // add the actions
  robot.addAction(&recover, 100);
  robot.addAction(&bumpers, 75);
  robot.addAction(&avoidFrontNear, 50);
  robot.addAction(&avoidFrontFar, 49);
  
  // start the robot running, true so that if we lose connection the run stops
  robot.runAsync(true);

  if (!useSimForLaser)
  { 
    sick.setDeviceConnection(&laserCon);

    if ((ret = laserCon.open("/dev/ttyS2")) != 0)
    {
      str = tcpConn.getOpenMessage(ret);
      printf("Open failed: %s\n", str.c_str());
      Aria::shutdown();
      return 1;
    }
    sick.configureShort(false);
  }
  else 
  {
    sick.configureShort(true);
  }

  sick.runAsync();

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

  robot.lock();
  robot.comInt(ArCommands::ENABLE, 1);
  robot.unlock();

  // add the peoplebot test
  PeoplebotTest pbTest(&robot, wanderTime, restTime, hostname);

  robot.waitForRunExit();

  // now exit
  Aria::shutdown();
  return 0;
}
示例#14
0
int main(int argc, char **argv) 
{
	struct settings robot_settings;

	robot_settings.min_distance = 500;
	robot_settings.max_velocity = 500;
	robot_settings.tracking_factor = 1.0;

	ArRobot robot;
	Aria::init();
	//laser
	int ret; //Don't know what this variable is for
	ArSick sick; // Laser scanner
	ArSerialConnection laserCon; // Scanner connection
	std::string str; // Standard output
	// sonar, must be added to the robot
	ArSonarDevice sonar;
	// add the sonar to the robot
	robot.addRangeDevice(&sonar);
	// add the laser to the robot
	robot.addRangeDevice(&sick);

	ArArgumentParser argParser(&argc, argv);
	ArSimpleConnector con(&argParser);

	// 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.");
	robot.runAsync(false);

	///////////////////////////////
	// Attempt to connect to SICK using another hard-coded USB connection
	sick.setDeviceConnection(&laserCon);
	if((ret=laserCon.open("/dev/ttyUSB1")) !=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!");

	printf("\r\nRobot Entering default resting state.\r\nUse the following commands to run the robot.\r\n");
	printf("r  Run the robot\r\n");
	printf("s  Stop the robot\r\n");
	printf("t  Enter test mode (the robot will do everything except actually move.\r\n");
	printf("w  Save current data to files, and show a plot of what the robot sees.\r\n");
	printf("e  Edit robot control parameters\r\n");
	printf("q  Quit the program\r\n");

	printf("\r\n");
	printf("\r\n");
	printf("NOTE: You must have GNUPLOT installed on your computer, and create a directory entitled 'scan_data' under your current directory in order to use this script. Failure to do so might make the computer crash, which would cause the robot to go on a mad killing spree! Not really, but seriously, go ahead and get GNUPLOT and create that subdirectory before using the 'w' option.\r\n\r\n"); 

	/////////////////////////////////////
	char user_command = 0;
	char plot_option = 0;

	int robot_state = REST;

	tracking_object target;
	tracking_object l_target;

	ofstream fobjects;
	ofstream ftarget;
	ofstream flog;
	ofstream fltarget;


	fobjects.open("./scan_data/objects_new.txt");
	ftarget.open("./scan_data/target_new.txt");
	fltarget.open("./scan_data/ltarget_new.txt");
	fobjects << "\r\n";
	ftarget << "\r\n";
	fltarget << "\r\n";
	fobjects.close();
	ftarget.close();
	fltarget.close();

	flog.open("robot_log.txt");

	std::vector<tracking_object> obj_vector;
	std::vector<tracking_object> new_vector;


	float last_v = 0;

	ArTime start;



	char test_flag = 0;
	char target_lost = 0;


	while(user_command != 'q')
	{
		switch (user_command)
		{
		case STOP:
			robot_state = REST;
			printf("robot has entered resting mode\r\n");

			break;
		case RUN:
			robot_state = TRACKING;
			printf("robot has entered tracking mode\r\n");
			break;
		case QUIT:
			robot_state = -1;
			printf("exiting... goodbye.\r\n");
			break;
		case WRITE:
			plot_option = WRITE;
			break;
		case NO_WRITE:
			plot_option = NO_WRITE;
			break;
		case TEST:
			if(test_flag == TEST)
			{
				test_flag = 0;
				printf("Exiting test mode\r\n");
			}
			else
			{
				test_flag = TEST;
				printf("Entering test mode\r\n");
				robot.lock();
				robot.setVel(0);
				robot.unlock();
			}
			break;

		case EDIT:
			edit_settings(&robot_settings);
			break;

		default:
			robot_state = robot_state;
		}


		unsigned int i = 0;
		unsigned int num_objects = 0;
		int to_ind = -1;
		float min_distance = 999999.9;
		float new_heading = 0;

		system("mv ./scan_data/objects_new.txt ./scan_data/objects.txt");
		system("mv ./scan_data/target_new.txt ./scan_data/target.txt");
		system("mv ./scan_data/ltarget_new.txt ./scan_data/ltarget.txt");

		fobjects.open("./scan_data/objects_new.txt");
		ftarget.open("./scan_data/target_new.txt");
		fltarget.open("./scan_data/ltarget_new.txt");

		switch (robot_state)
		{
		case REST:
			robot.lock();
			robot.setVel(0);
			robot.unlock();

			obj_vector = run_sick_scan(&sick, 2, plot_option);

			target_lost = 0;
			num_objects = obj_vector.size();
			if(num_objects > 0)
			{

				for(i = 0; i < num_objects; i++)
				{
					print_object_to_stream(obj_vector[i], fobjects);	
				}
			}

			break;

		case TRACKING:
			flog << "TRACKING\r\n";

			obj_vector = get_moving_objects(&sick, 2*DT, 10, plot_option);
			num_objects = obj_vector.size();

			target_lost = 0;

			if(min_range(&sick, 45, 135) > ROBOT_SAFETY_MARGIN)
			{
				if(num_objects)
				{
 					for(i = 0; i < obj_vector.size();i++)
					{	
						print_object_to_stream(obj_vector[i], fobjects);
						if(obj_vector[i].vmag > 0.1)
						{	
							if(obj_vector[i].distance < min_distance)
							{
								min_distance = obj_vector[i].distance;
								target = obj_vector[i];
								to_ind = i;
							}
						}
					}

					if(to_ind > -1)
					{
						int l_edge = target.l_edge;
						int r_edge = target.r_edge;
						float difference = 9999999.9;

// Do another scan to make sure the object is actually there.
						new_vector = get_moving_objects(&sick, DT, 10, 0, 5, 175);

						num_objects = new_vector.size();
						to_ind = -1;
						if(num_objects)
						{
							for(i = 0; i < num_objects;i++)
							{
								if(new_vector[i].vmag > 0.1)
								{	
									if(r_diff(target, new_vector[i])<difference)
									{
										difference = r_diff(target, new_vector[i]);
										if(difference < 500.0)
											to_ind = i;
									}
								}
							}
						}

						if(to_ind > -1)
						{
							new_heading = (-90 + new_vector[to_ind].degree);

							if(test_flag != TEST)
							{
								robot.lock();
								robot.setDeltaHeading(new_heading);
								robot.unlock();
							}

							robot_state = FOLLOWING;
							target = new_vector[to_ind];

							target.degree = target.degree - new_heading;


							print_object_to_stream(target, ftarget);

							print_object_to_stream(target, flog);
							flog << new_heading;
						}

					}
				}
			}
			else		      
			{
				robot_state = TOO_CLOSE;
				printf("I'm too close to an obstacle, and I'm getting claustrophobic! I'm going to slowly back up now.\r\n");
			}



			break;

		case FOLLOWING:
		{
			flog << "FOLLOWING\r\n";

			i = 0;
			int to_ind = -1;
			float obj_difference = 9999999.9;
			int num_scans = 0;

			if(min_range(&sick, 45, 135) > ROBOT_SAFETY_MARGIN)
			{
				while((num_scans < 3)&&(to_ind == -1))
				{
					obj_vector = run_sick_scan(&sick, 2, 0, 10, 350);
					num_objects = obj_vector.size();
					if(num_objects)
					{
						for(i = 0; i < num_objects;i++)
						{	
							if(r_diff(target, obj_vector[i]) < obj_difference)
							{	
								obj_difference = r_diff(target, obj_vector[i]);
								if (obj_difference < 500.0)
									to_ind = i;

							}
						}

						for(i = 0; i < num_objects;i++)
						{	
							if((int)i == to_ind)
							{
								print_object_to_stream(obj_vector[i], ftarget);
							}
							else
							{
								print_object_to_stream(obj_vector[i], fobjects);
							}
						}
					}
					num_scans++;
				}

				float new_vel = 0;

				if(to_ind > -1)
				{
					printf("Tracking target\r\n");
					flog << "Following target\t";

					target_lost = 0;

					target = obj_vector[to_ind];

					new_vel = (obj_vector[to_ind].distance - robot_settings.min_distance)*robot_settings.tracking_factor;
					if(new_vel < 0)
						new_vel = 0;

					if(new_vel > robot_settings.max_velocity)
						new_vel = robot_settings.max_velocity;

					new_heading = (-90 + target.degree)*0.25;
					new_heading = get_safe_path(&sick, new_heading, target.distance);

					if(test_flag != TEST)
					{
						robot.lock();
						robot.setVel(new_vel);
						robot.unlock();

						last_v = new_vel;

						robot.lock();
						robot.setDeltaHeading(new_heading);
						robot.unlock();
						target.degree = target.degree - new_heading;
					}
					if(new_vel < 1.0)
					{
						robot_state = TRACKING;
						printf("entering tracking mode\r\n");
						flog<<"entering tracking mode\r\n";
					}

				}
				else		      
				{
					if(target_lost)
					{
						
						l_target.distance = l_target.distance - last_v*(start.mSecSince()/1000.0);
 
						int temp_max_v = min_range(&sick, 5, 175);
						if(temp_max_v < last_v)
							last_v = temp_max_v;

						if(last_v > robot_settings.max_velocity)
							last_v = robot_settings.max_velocity;

						if(test_flag != TEST)
						{

							robot.lock();
							robot.setVel(last_v);
							robot.unlock();


							new_heading = -90.0 + l_target.degree;
							new_heading = get_safe_path(&sick, new_heading, l_target.distance);

							l_target.degree = target.degree - new_heading;

							robot.lock();
							robot.setDeltaHeading(new_heading);
							robot.unlock();
						}

						print_object_to_stream(l_target, fltarget);



					}
					else
					{
						target_lost = 1;
						l_target = target;
					}

					printf("target lost\r\n");
					flog << "target lost\r\n";
					start.setToNow();

					if(target.distance < ROBOT_SAFETY_MARGIN)
					{
						robot_state = TRACKING;
						target_lost = 0;
					}

					target_lost = 1;

				}
				printf("Velocity: %f\t",last_v);
				flog << "Velocity:\t";
				flog << last_v;

				printf("Heading: %f\t",new_heading);
				flog << "\tHeading\t";
				flog << new_heading;
				flog << "\r\n";
				printf("\r\n");

			}		
			else		      
			{
				robot_state = TOO_CLOSE;
				printf("I'm too close to an obstacle, and I'm getting claustrophobic! I'm going to slowly back up now.\r\n");
			}

			break;



		}

		case TOO_CLOSE:
		{
			flog << "Too close\r\n";
			if(min_range(&sick, 45, 135) > (ROBOT_SAFETY_MARGIN + 100))
			{		
				robot_state = TRACKING;
				printf("I feel better now! I'm going to reenter tracking mode.\r\n");
				robot.lock();
				robot.setVel(0);
				robot.unlock();
			}
			else
			{
				if(test_flag != TEST)
				{
					printf("Backing up...\r\n");
					robot.lock();
					robot.setVel(-50);
					robot.unlock();
				}

			}
			break;
 		}

		}

		fobjects.close();
		ftarget.close();
		fltarget.close();


		if(plot_option == WRITE)
		{
			system("./scan_data/plot_script.sh >/dev/null");
		}







////////////////////////////////////////////////////////////////////// 
// Everything past this point is code to grab the user input
		user_command = 0;


		fd_set rfds;
		struct timeval tv;
		int retval;

		FD_ZERO(&rfds);
		FD_SET(0, &rfds);

		tv.tv_sec = 0;
		tv.tv_usec = 100;

		retval = select(1, &rfds, NULL, NULL, &tv);

		if(retval == -1)
			perror("select()");
		else if(retval)
		{
			cin >> user_command;
			printf("input detected from user\r\n");
		}

		plot_option = NO_WRITE;
//////////////////////////////////////////////////////////////////////
		
	}
	flog.close();
	Aria::shutdown();
	printf("Shutting down");
	return 0;
}
示例#15
0
int main(void)
{
  ArSerialConnection con;
  ArRobot robot;
  int ret;
  std::string str;
  ArActionLimiterForwards limiter("speed limiter near", 225, 600, 250);
  ArActionLimiterForwards limiterFar("speed limiter far", 225, 1100, 400);
  ArActionTableSensorLimiter tableLimiter;
  ArActionLimiterBackwards backwardsLimiter;
  ArActionConstantVelocity stop("stop", 0);
  ArSonarDevice sonar;
  ArACTS_1_2 acts;
  ArPTZ *ptz;
  ptz = new ArVCC4(&robot, true);
  ArGripper gripper(&robot);
  
  Acquire acq(&acts, &gripper);
  DriveTo driveTo(&acts, &gripper, ptz);
  DropOff dropOff(&acts, &gripper, ptz);
  PickUp pickUp(&acts, &gripper, ptz);
  

  TakeBlockToWall takeBlock(&robot, &gripper, ptz, &acq, &driveTo, &pickUp,
			    &dropOff, &tableLimiter);

  if (!acts.openPort(&robot))
  {
    printf("Could not connect to acts, exiting\n");
    exit(0);    
  }
  Aria::init();
  
  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;
  }

  ptz->init();
  ArUtil::sleep(8000);
  printf("### 2222\n");
  ptz->panTilt(0, -40);
  printf("### whee\n");
  ArUtil::sleep(8000);
  robot.setAbsoluteMaxTransVel(400);

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

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

  robot.run(true);
  
  Aria::shutdown();
  return 0;
}
示例#16
0
int main(int argc, char **argv)
{
  // this is how long to wait after there's been no data to close the
  // connection.. if its 0 and its using robot it'll set it to 5000 (5
  // seconds), if its 0 and using laser, it'll set it to 60000 (60
  // seconds, which is needed if the sick driver is controlling power
  int timeout = 0;
  // true will print out packets as they come and go, false won't
  bool tracePackets = false;

  // The socket objects
  ArSocket masterSock, clientSock;
  // The connections
  ArTcpConnection clientConn;
  ArSerialConnection robotConn;
  // the receivers, first for the robot
  ArRobotPacketReceiver clientRec(&clientConn);
  ArRobotPacketReceiver robotRec(&robotConn);
  // then for the laser
  ArSickPacketReceiver clientSickRec(&clientConn, 0, false, true);
  ArSickPacketReceiver robotSickRec(&robotConn);
  // how about a packet
  ArBasePacket *packet;
  // our timer for how often we test the client
  ArTime lastClientTest;
  ArTime lastData;
  // where we're forwarding from and to
  int portNumber;
  const char *portName;
  // if we're using the robot or the laser
  bool useRobot;
  
  if (argc == 1)
  {
    printf("Using robot and port 8101 and serial connection %s, by default.\n", ArUtil::COM1);
    useRobot = true;
    portNumber = 8101;
    portName = ArUtil::COM1;
  }
  else if (argc == 2)
  {
    // if laser isn't the last arg, somethings wrong
    if (strcmp(argv[1], "laser") != 0)
    {
      usage(argv[0]);
      return -1;
    }
    useRobot = false;
    portNumber = 8102;
    portName = ArUtil::COM3;
    printf("Using laser and port %d and serial connection %s, by command line arguments.\n", portNumber, portName);
    printf("(Note: Requests to change BAUD rate cannot be fulfilled; use 9600 rate only.)\n");
  }
  else if (argc == 3)
  {
    if ((portNumber = atoi(argv[1])) <= 0)
    {
      usage(argv[0]);
      return -1;
    }
    portName = argv[2];
    printf("Using robot and port %d and serial connection %s, by command line arguments.\n", portNumber, portName);
  }
  else if (argc == 4)
  {
    if ((portNumber = atoi(argv[1])) <= 0)
    {
      usage(argv[0]);
      return -1;
    }
    // if laser isn't the last arg, somethings wrong
    if (strcmp(argv[3], "laser") != 0)
    {
      usage(argv[0]);
      return -1;
    }
    useRobot = false;
    portName = argv[2];
    printf("Using laser and port %d and serial connection %s, by command line arguments.\n", portNumber, portName);
    printf("(Note: Requests to change BAUD rate cannot be fulfilled; use 9600 rate only.)\n");
  }
  else
  {
    usage(argv[0]);
    return -1;
  }
  if (timeout == 0 && useRobot)
    timeout = 5000;
  else if (timeout == 0)
    timeout = 60000;

  // Initialize Aria. For Windows, this absolutely must be done. Because
  // Windows does not initialize the socket layer for each program. Each
  // program must initialize the sockets itself.
  Aria::init(Aria::SIGHANDLE_NONE);

  // Lets open the master socket
  if (masterSock.open(portNumber, ArSocket::TCP))
    printf("Opened the master port at %d\n", portNumber);
  else
  {
    printf("Failed to open the master port at %d: %s\n",
	   portNumber, masterSock.getErrorStr().c_str());
    return -1;
  }

  // just go forever
  while (1)
  {
    // Lets wait for the client to connect to us.
    if (masterSock.accept(&clientSock))
      printf("Client has connected\n");
    else
      printf("Error in accepting a connection from the client: %s\n",
	     masterSock.getErrorStr().c_str());
   
    // now set up our connection so our packet receivers work
    clientConn.setSocket(&clientSock);
    clientConn.setStatus(ArDeviceConnection::STATUS_OPEN);
    lastClientTest.setToNow();
    lastData.setToNow();
    // open up the robot port
    if (robotConn.open(portName) != 0)
    {
      printf("Could not open robot port %s.\n", portName);
      return -1;
    }

    // while we're open, just read from one port and write to the other
    while (clientSock.getFD() >= 0)
    {
      // get our packet
      if (useRobot)
	packet = clientRec.receivePacket(1);
      else
	packet = clientSickRec.receivePacket(1);
      // see if we had one
      if (packet != NULL)
      {
	if (tracePackets)
	{
	  printf("Client ");
	  packet->log();
	}
	robotConn.writePacket(packet);
	lastData.setToNow();
      }
      // get our packet
      if (useRobot)
	packet = robotRec.receivePacket(1);
      else
	packet = robotSickRec.receivePacket(1);
      // see if we had one
      if (packet != NULL)
      {
	if (tracePackets)
	{
	  printf("Robot ");
	  packet->log();
	}
	clientConn.writePacket(packet);
	lastData.setToNow();
      }
      ArUtil::sleep(1);
      // If no datas gone by in timeout ms assume our connection is broken
      if (lastData.mSecSince() > timeout)
      {
	printf("No data received in %d milliseconds, closing connection.\n", 
	       timeout);
	clientConn.close();
      }
    }
    // Now lets close the connection to the client
    clientConn.close();
    printf("Socket to client closed\n");
    robotConn.close();
  }
  // And lets close the master port
  masterSock.close();
  printf("Master socket closed and program exiting\n");

  // Uninitialize Aria
  Aria::uninit();

  // All done
  return(0);
}
int main(int argc, char **argv)
{
  int ret;
  std::string str;
  ArSerialConnection con;
  double dist, angle;
  std::list<ArPoseWithTime *> *readings;
  std::list<ArPoseWithTime *>::iterator it;
  double farDist, farAngle;
  bool found;
  ArGlobalFunctor failedConnectCB(&failedConnect);
  
  Aria::init();
  
  sick = new ArSick;
  // open the connection, if it fails, exit
  if ((ret = con.open("/dev/ttyS2")) != 0)
  {
    str = con.getOpenMessage(ret);
    printf("Open failed: %s\n", str.c_str());
    Aria::shutdown();
    return 1;
  }
  
  sick->configure(false);
  sick->setDeviceConnection(&con);

  sick->setFilterNearDist(0);
  sick->setMinRange(0);
  sick->addFailedConnectCB(&failedConnectCB, ArListPos::FIRST);
  sick->runAsync();

  ArUtil::sleep(100);
  sick->lockDevice();
  sick->asyncConnect();
  sick->unlockDevice();
  while (!sick->isConnected())
    ArUtil::sleep(100);

  printf("Connected\n");
//  while (sick->isConnected())
  int times = 0;
  while (sick->getRunning())
  {
    //dist = sick->getCurrentBuffer().getClosestPolar(-90, 90, ArPose(0, 0), 30000, &angle);
    sick->lockDevice();
    dist = sick->currentReadingPolar(-90, 90, &angle);
    if (dist < sick->getMaxRange())
      printf("Closest reading %.2f mm away at %.2f degrees\n", dist, angle);
    else
      printf("No close reading.\n");
    readings = sick->getCurrentBuffer();
    int i = 0;
    for (it = readings->begin(), found = false; it != readings->end(); it++)
    {
      i++;
      dist = (*it)->findDistanceTo(ArPose(0, 0));
      angle = (*it)->findAngleTo(ArPose(0, 0));
      if (!found || dist > farDist)
      {
	found = true;
	farDist = dist;
	farAngle = angle;
      }
    }
    if (found)
      printf("Furthest reading %.2f mm away at %.2f degrees\n", 
	     farDist, farAngle);
    else
      printf("No far reading found.\n");
    printf("%d readings\n\n", i);
    
    sick->unlockDevice();
    ArUtil::sleep(100);
  }
  sick->lockDevice();
  sick->stopRunning();
  sick->disconnect();
  sick->unlockDevice();
  system("echo 'succeeded' >> results");
  return 0;
}
示例#18
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{
示例#19
0
int main(int argc, char **argv)
{
  int ret;
  std::string str;
  ArSerialConnection con;
  ArSickPacket sick;
  ArSickPacket *packet;
  ArSickPacketReceiver receiver(&con);
  ArTime start;
  unsigned int value;
  int numReadings;
  ArTime lastReading;
  ArTime packetTime;

  start.setToNow();

  // open the connection, if it fails, exit
  if ((ret = con.open()) != 0)
  {
    str = con.getOpenMessage(ret);
    printf("Open failed: %s\n", str.c_str());
    Aria::shutdown();
    return 1;
  }

  start.setToNow();

  printf("Waiting for laser to power on\n");
  sick.empty();
  sick.uByteToBuf(0x10);
  sick.finalizePacket();
  con.write(sick.getBuf(), sick.getLength());

  while (start.secSince() < 70 && 
	 ((packet = receiver.receivePacket(100)) == NULL
	  ||  (packet->getID() != 0x90)));
  if (packet != NULL)
    printf("Laser powered on\n");
  else
    exit(1);

  printf("Changing baud\n");
  sick.empty();
  sick.byteToBuf(0x20);
  sick.byteToBuf(0x40);
  sick.finalizePacket();
  con.write(sick.getBuf(), sick.getLength());

  ArUtil::sleep(10);
  if (!con.setBaud(38400))
  {
    printf("Could not set baud, exiting\n");
  }
  
  
  /*packet = receiver.receivePacket(100);
  if (packet != NULL) 
    packet->log();
  */
  sick.empty();
  sick.uByteToBuf(0x3B);
  sick.uByte2ToBuf(180);
  sick.uByte2ToBuf(100);
  sick.finalizePacket();
  con.write(sick.getBuf(), sick.getLength());

  packet = receiver.receivePacket(100);
  if (packet != NULL) 
    packet->log();

  sick.empty();
  sick.byteToBuf(0x20);
  sick.byteToBuf(0x24);
  sick.finalizePacket();
  con.write(sick.getBuf(), sick.getLength());

  packet = receiver.receivePacket(100);
  if (packet != NULL) 
    packet->log();



  printf("Starting to report back from port, it took %ld ms to get here:\n",
	 start.mSecSince());
  start.setToNow();
  while (start.secSince() < 6)
  {
    packetTime.setToNow();
    packet = receiver.receivePacket();
    if (packet != NULL)
      printf("####### %ld ms was how long the packet took\n", packetTime.mSecSince());
    if (packet != NULL)
    {
      if (packet->getLength() < 10)
	packet->log();
      else if (packet->getID() == 0x90)
      {
	char strBuf[512];
	packet->log();
	//printf("%x\n", packet->bufToUByte());
	packet->bufToStr(strBuf, 512);
	printf("0x%x %s\n", packet->getID(), strBuf);
	sick.empty();
	sick.uByteToBuf(0x3B);
	sick.uByte2ToBuf(180);
	sick.uByte2ToBuf(100);
	sick.finalizePacket();
	con.write(sick.getBuf(), sick.getLength());
	packet = receiver.receivePacket(100);
	sick.empty();
	sick.uByteToBuf(0x20);
	sick.uByteToBuf(0x24);
	sick.finalizePacket();
	con.write(sick.getBuf(), sick.getLength());
      }
      else
      {
	value = packet->bufToUByte2();
	numReadings = value & 0x3ff;
	printf("%ld ms after last reading.\n", lastReading.mSecSince());
	/*
	printf("Reading number %d, complete %d, unit: %d %d:\n", value & 0x3ff, !(bool)(value & ArUtil::BIT13), (bool)(value & ArUtil::BIT14), (bool)(value & ArUtil::BIT15));
	for (i = 0; i < numReadings; i++)
	{
	  value = packet->bufToUByte2();
	  if (value & ArUtil::BIT13)
	    printf("D");
	  printf("%d ", value & 0x1fff);
	}
	printf("\n");
	*/
	lastReading.setToNow();
      }
    }
    else
    {
      //printf("No packet\n");
    }
  }
}
示例#20
0
int main(int argc, char **argv)
{
  int ret;
  std::string str;
  ArSerialConnection con;
  const std::list<ArSensorReading *> *readings;
  std::list<ArSensorReading *>::const_iterator it;
  int i;
  ArGlobalFunctor failedConnectCB(&failedConnect);
  
  Aria::init();
  sick = new ArSick;
  // open the connection, if it fails, exit
  if ((ret = con.open("/dev/ttyS2")) != 0)
  {
    str = con.getOpenMessage(ret);
    printf("Open failed: %s\n", str.c_str());
    Aria::shutdown();
    return 1;
  }
  
  sick->configure(false);
  sick->setDeviceConnection(&con);

  sick->addFailedConnectCB(&failedConnectCB, ArListPos::FIRST);
  sick->runAsync();

  ArUtil::sleep(100);
  sick->lockDevice();
  sick->asyncConnect();
  sick->unlockDevice();
  while (!sick->isConnected())
    ArUtil::sleep(100);
  printf("Connected\n");
//  while (sick->isConnected())
  while (1)
  {
    //dist = sick->getCurrentBuffer().getClosestPolar(-90, 90, ArPose(0, 0), 30000, &angle);
    sick->lockDevice();
    if (!sick->getRunning() || !sick->isConnected())
      {
	break;
      }
    printf("\r");
    readings = sick->getRawReadings();
    if (readings != NULL)
    {
      for (i = 0, it = readings->begin(); it != readings->end(); it++, i++)
      {
	if (abs(i - readings->size()/2) < 3)
	  printf("(%.2f %d) ", (*it)->getSensorTh(), (*it)->getRange());
      }
    }
    // switch the commenting in out of the fflush and the
    // printf("\n"); if you want to print on the same line or have it
    // scrolling
    fflush(stdout);
    //printf("\n");

    sick->unlockDevice();
    ArUtil::sleep(100);
  }
  sick->lockDevice();
  sick->stopRunning();
  sick->disconnect();
  sick->unlockDevice();
   return 0;
}