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

/*!
 * 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");

}

// Collects a laser reading from the robot, and corrects the XY-rotation
vector<PointXY> CollectLaser(ArSick &sick, double maxRange)
{
	vector<PointXY> laserPoints;

	sick.lockDevice();

	vector<ArSensorReading> * readings = sick.getRawReadingsAsVector();
	for(vector<ArSensorReading>::const_iterator it = readings->begin(); it != readings->end(); it++)
	{
		if(it->getRange() < maxRange)
		{
			// Correct coordinates right here so we don't have to do it later!
			double x = -it->getLocalY();
			double y =  it->getLocalX();
			laserPoints.push_back(PointXY(x, y));
		}
	}

	sick.unlockDevice();
	//ArUtil::sleep(100);

	return laserPoints;
}

//.........这里部分代码省略.........
  {
    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);

  ArTime startTime;
  // move a couple meters
  robot.move(distToTravel);
  robot.unlock();
  startTime.setToNow();
  do {
    ArUtil::sleep(100);
    robot.lock();
    robot.setHeading(0);
    done = robot.isMoveDone(60);
    robot.unlock();
  } while (!done);

  /*
  // rotate a few times
  robot.lock();
  robot.setVel(0);
  robot.setRotVel(60);
  robot.unlock();
  ArUtil::sleep(12000);
  */

  robot.lock();
  robot.setHeading(180);
  robot.unlock();
  do {
    ArUtil::sleep(100);
    robot.lock();
    robot.setHeading(180);
    done = robot.isHeadingDone();
    robot.unlock();
  } while (!done);

  // move a couple meters
  robot.lock();
  robot.move(distToTravel);
  robot.unlock();
  startTime.setToNow();
  do {
    ArUtil::sleep(100);
    robot.lock();
    robot.setHeading(180);
    done = robot.isMoveDone(60);
    robot.unlock();
  } while (!done);

  robot.lock();
  robot.setHeading(0);
  robot.setVel(0);
  robot.unlock();
  startTime.setToNow();
  do {
    ArUtil::sleep(100);
    robot.lock();
    robot.setHeading(0);
    done = robot.isHeadingDone();
    robot.unlock();
  } while (!done);


  sick.lockDevice();
  sick.disconnect();
  sick.unlockDevice();
  robot.lock();
  robot.disconnect();
  robot.unlock();
  // now exit
  Aria::shutdown();
  return 0;
}

void S_TargetApproach( ArServerClient *serverclient, ArNetPacket *socket)
{
	//Important: to halt current movement of camera, making the reading curret.
	G_PTZHandler->haltPanTilt();
	cout << "The last step: TargetApproach!" <<endl;

   //G_PathPlanning->setCollisionRange(1000);
	//G_PathPlanning->setFrontClearance(40);
	//G_PathPlanning->setGoalDistanceTolerance(2500);
  double camAngle = -1 * G_PTZHandler->getPan();
  double robotHeading = robot.getPose().getTh();
  double robotCurrentX = robot.getPose().getX() ;
  double robotCurrentY = robot.getPose().getY();
  double angle = 0.0;
  double distance =0.0;
  
  double targetX, targetY;
  int disThreshold = 500;
   //test mode
//     G_PTZHandler->panRel(-30);
	//G_PathPlanning->setFrontClearance(40);
	//G_PathPlanning->setObsThreshold(1);

  cout << "--------------Path Planning Information--------------------" <<endl;
  cout << "SafeCollisionRange = " << G_PathPlanning->getSafeCollisionRange()  << endl;
	cout << "FrontClearance     = " << G_PathPlanning->getFrontClearance()      << endl
		   << "GoalDistanceTolerance = " << G_PathPlanning->getGoalDistanceTolerance() << endl
			 << "setGoalOccupiedFailDistance = " << G_PathPlanning->getGoalOccupiedFailDistance() << endl
			 << "getObsThreshold = " << G_PathPlanning->getObsThreshold() <<endl
			 << "getLocalPathFailDistance = " << G_PathPlanning->getLocalPathFailDistance() << endl;
			 //getCurrentGoal


  
  
//--------------- Set up the laser range device to read the distance from the target -----------------------------
 // for(int i =0; i< 20;i++)
	//sick.currentReadingPolar(robotHeading+ camAngle -2.5, robotHeading+ camAngle +2.5, &angle);
	
//Begin:  

  sick.lockDevice();

	//if (distance == 0 || distance>disThreshold)
  distance = sick.currentReadingPolar(/*robotHeading+*/ camAngle -2.5, /*robotHeading+*/ camAngle +2.5, &angle)-disThreshold;
   //double distance = sick.currentReadingPolar(89, 90, &angle);
  cout << "The closest reading is " << distance << " at " << angle << " degree , " << "disThreshold : " <<disThreshold << " " << robotHeading << " " << camAngle<< endl;

  sick.unlockDevice();
		

  
//----------------------------------------------------------------------------------------------------------------
  
  //basic_turn(camAngle);
  
  cout << "Camera Angle is " << camAngle << endl;

  cout << "before calculation, check originalX, originalY " << robotCurrentX << " " << robotCurrentY << " robotHeading is " << robotHeading << endl<<endl;  
  coordinateCalculation(robotCurrentX,robotCurrentY,&targetX,&targetY,camAngle,robotHeading,distance);
  //
  //cout << "before movement, check targetX, target Y" << targetX << " " << targetY << "robotHeading is "<< robotHeading << endl << endl;
  

	cout << "targetX : " <<targetX << " targetY" <<targetY;
  G_PathPlanning->pathPlanToPose(ArPose(targetX,targetY,camAngle),true,true);
  cout << "RobotMotion is processing..." <<endl;

  G_PTZHandler->reset();
	ArUtil::sleep(200);
	G_PTZHandler->tiltRel(-10);
  while(G_PathPlanning->getState() != ArPathPlanningTask::REACHED_GOAL )
  {
		if (G_PathPlanning->getState() == ArPathPlanningTask::FAILED_MOVE)
		{
			G_PathPlanning->cancelPathPlan();cout <<  "x " << robot.getPose().getX()<< " y " <<robot.getPose().getY() <<endl; break;
		}
		else if(G_PathPlanning->getState() == ArPathPlanningTask::FAILED_PLAN)
		{
//			getchar();
//getchar();
//getchar();
//			getchar();
//			disThreshold+=50;
//			
//			goto Begin;
		}
  }

  //serverclient->sendPacketTcp(socket);

  ArUtil::sleep(3000);


  cout << "RobotMotion is heading home..." <<endl;
  G_PathPlanning->pathPlanToPose(ArPose(0,0,0),true,true);
  
  while(G_PathPlanning->getState() != ArPathPlanningTask::REACHED_GOAL )
	{
		//cout << G_PathPlanning->getState() <<endl;
//.........这里部分代码省略.........

int main(int argc, char **argv)
{
  // robot
  ArRobot robot;
  // the laser
  ArSick sick;


  // 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", 1600, 0, 0, 1.3);
  // limiter for far away obstacles
  //ArActionLimiterForwards limiterFar("speed limiter near", 300, 1000, 450, 1.1);
  //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", 1500);
  // turn the orbot if its slowed down
  ArActionTurn turn;

  // mandatory init
  Aria::init();

  // Parse all our args
  ArSimpleConnector connector(&argc, argv);
  if (!connector.parseArgs() || argc > 1)
  {
    connector.logOptions();
    exit(1);
  }
  
  // add the sonar to the robot
  //robot.addRangeDevice(&sonar);
  // add the laser to the robot
  robot.addRangeDevice(&sick);

  // try to connect, if we fail exit
  if (!connector.connectRobot(&robot))
  {
    printf("Could not connect to robot... exiting\n");
    Aria::shutdown();
    return 1;
  }

  robot.comInt(ArCommands::SONAR, 0);

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

  // add the actions
  //robot.addAction(&recover, 100);
  //robot.addAction(&bumpers, 75);
  robot.addAction(&limiter, 49);
  //robot.addAction(&limiter, 48);
  //robot.addAction(&tableLimiter, 50);
  robot.addAction(&turn, 30);
  robot.addAction(&constantVelocity, 20);

  robot.setStateReflectionRefreshTime(50);
  limiter.activate();
  turn.activate();
  constantVelocity.activate();

  robot.clearDirectMotion();
  //robot.setStateReflectionRefreshTime(50);
  robot.setRotVelMax(50);
  robot.setTransAccel(1500);
  robot.setTransDecel(100);

  // start the robot running, true so that if we lose connection the run stops
  robot.runAsync(true);

  connector.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");
    Aria::shutdown();
    return 1;
  }
  
  sick.lockDevice();
  sick.setMinRange(250);
  sick.unlockDevice();
  robot.lock();
  ArGlobalFunctor1<ArRobot *> userTaskCB(&userTask, &robot);
  robot.addUserTask("iotest", 100, &userTaskCB);
  requestTime.setToNow();
//.........这里部分代码省略.........

//.........这里部分代码省略.........
  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);
  robot->setVel(1000);
  robot->unlock();

  ArUtil::sleep(speed / 500.0 * 1000.0);
  printf("Should be up to speed, moving on first side\n");
  ArUtil::sleep(squareSide / speed * 1000);
  printf("Turning to second side\n");
  robot->lock();
  robot->setHeading(90);
  robot->setVel(speed);
  robot->unlock();
  ArUtil::sleep(squareSide / speed * 1000);
  printf("Turning to third side\n");
  robot->lock();
  robot->setHeading(180);
  robot->setVel(speed);
  robot->unlock();
  ArUtil::sleep(squareSide / speed * 1000);
  printf("Turning to last side\n");
  robot->lock();
  robot->setHeading(-90);
  robot->setVel(speed);
  robot->unlock();
  ArUtil::sleep(squareSide / speed * 1000);
  printf("Pointing back original direction and stopping\n");
  robot->lock();
  robot->setHeading(0);
  robot->setVel(0);
  robot->unlock();
  ArUtil::sleep(300);

  printf("Stopped\n");
  sick.lockDevice();
  sick.disconnect();
  sick.unlockDevice();
  robot->lock();
  robot->disconnect();
  robot->unlock();

  // now exit
  Aria::shutdown();
  return 0;
}

int main( int argc, char **argv ){
   // parse our args and make sure they were all accounted for
   ArSimpleConnector connector(&argc, argv);

   ArRobot robot;
   ArSick sick;
   double dist, angle = 0;

   // Allow for esc to release robot
   ArKeyHandler keyHandler;
   Aria::setKeyHandler(&keyHandler);
   robot.attachKeyHandler(&keyHandler);
   printf("You may press escape to exit\n");

   if( !connector.parseArgs() || argc > 1 ){
      connector.logOptions();
      exit(1);
   }

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

   // try to connect, if we fail exit
   if( !connector.connectRobot(&robot) ){
      printf("Could not connect to robot... exiting\n");
      Aria::shutdown();
      return 1;
   }

   // start the robot running, true so that if we lose connection the run stops
   robot.runAsync(true);

   // now set up the laser
   connector.setupLaser(&sick);

   sick.runAsync();

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

   robot.comInt(ArCommands::ENABLE, 1);
   ArPose pose(0, -1000, 0);
   robot.moveTo(pose);
   ArPose prev_pose = robot.getPose();
   double total_distance = 0;

   printf("Connected\n");
   ArUtil::sleep(1000);

   PathLog log("../Data/reactive.dat");
   int iterations_wo_movement = 0;
   while( iterations_wo_movement < CONSECUTIVE_NON_MOTIONS ){

      // Get updated measurement
      sick.lockDevice();
      dist = sick.currentReadingPolar(-90, 90, &angle);
      sick.unlockDevice();

      dist = (dist > 30000) ? 0 : dist - IDEAL_DISTANCE;
      trackRobot(&robot, dist, angle);
      ArUtil::sleep(500);
      pose = robot.getPose();
      log.write(pose);
      total_distance += getDistance(prev_pose, pose);
      prev_pose = pose;

      // Determine if the robot is done tracking
      isRobotTracking(&iterations_wo_movement, dist, angle);

   }

   ArUtil::sleep(1000);
   log.close();

   ofstream output;
   output.open("../Data/reactive_dist.dat", ios::out | ios::trunc);
   output << "Reactive 1 " << total_distance << endl;
   output.close();

   Aria::exit(0);
   return 0;
}