C++ ArSick::disconnect方法代码示例

/*!
 * Shuts down the system.
 *
 */
void
Advanced::shutDown(void)
{
  Aria::exit(0);
  //
  // Stop the path planning thread.
  //
  if(myPathPlanningTask){
    myPathPlanningTask->stopRunning();
//    delete myPathPlanningTask;
    printf("Stopped Path Planning Thread\n");
  }
  //
  // Stop the localization thread.
  //
  if(myLocaTask){
    myLocaTask->stopRunning();
    delete myLocaTask;
    printf("Stopped Localization Thread\n");
  }
  //
  // Stop the laser thread.
  //
  if(mySick)
  {
    mySick->lockDevice();
    mySick->disconnect();
    mySick->unlockDevice();
    printf("Stopped Laser Thread\n");
  }
  //
  // Stop the robot thread.
  //
  myRobot->lock();
  myRobot->stopRunning();
  myRobot->unlock();
  printf("Stopped Robot Thread\n");
  //
  // Exit Aria
  //
  Aria::shutdown();
  printf("Aria Shutdown\n");

}

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->setAbsoluteMaxTransVel(2000);
  robot->setTransVelMax(2000);
  robot->setTransAccel(500);
  robot->setTransDecel(500);
  /*
  robot->comInt(82, 30); // rotkp
  robot->comInt(83, 200); // rotkv
  robot->comInt(84, 0); // rotki
  robot->comInt(85, 15); // transkp
  robot->comInt(86, 450); // transkv
  robot->comInt(87, 4); // transki
  */
  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
  // run the robot, true here so that the run will exit if connection lost
  robot->runAsync(true);



  // set up the laser before handing it to the laser mode
  simpleConnector.setupLaser(&sick);

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

  robot->lock();
  robot->addUserTask("printer", 50, new ArGlobalFunctor(&printer));
  robot->unlock();

#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

  printf("Starting moving\n");
  robot->lock();
  // enable the motors, disable amigobot sounds
  robot->comInt(ArCommands::ENABLE, 1);
  robot->setHeading(0);