Пример #1
0
void TakeBlockToWall::setState(State state)
{
  myState = state;
  myNewState = true; 
  myStateStartTime.setToNow();
  myStateStartPos = myRobot->getPose();
}
void basic_turn(int turnAngal)
{
 // ArTime start;
 // G_PTZHandler->reset();
	//ArUtil::sleep(500);
	//G_PTZHandler->tiltRel(-10);
	//CameraMoveCount=0;
  //robot.lock();

	double robotHeading = robot.getPose().getTh();
	robotHeading += turnAngal;
  double robotCurrentX = robot.getPose().getX() ;
  double robotCurrentY = robot.getPose().getY();

	G_PathPlanning->pathPlanToPose(ArPose(robotCurrentX, robotCurrentY , robotHeading),true,true);
	while(G_PathPlanning->getState() != ArPathPlanningTask::REACHED_GOAL );


	//robot.setHeading(robot.getTh()+turnAngal);
 // robot.unlock();
  
  //start.setToNow();
  //while (1)
  //{
	 // robot.lock();
	 // if (robot.isHeadingDone())
	 // {
		//  printf(" Finished turn\n");
		//  
		//  ArUtil::sleep(50);
		//  cout << " current heading: " << "    " << robot.getTh()<<endl;
		//	robot.unlock();
		//  break;
	 // }
	 // if (start.mSecSince() > 5000)
	 // {
		//  printf(" Turn timed out\n");
		//  
		//  cout << " current heading: " << "    " << robot.getTh()<<endl;
		//	robot.unlock();
		//  break;
	 // }
	 // robot.unlock();
	 // ArUtil::sleep(10);
  //}  
}
/*****************************************************************
**								Robot Search Subroutine
******************************************************************/
void S_RobotMotion( ArServerClient *serverclient, ArNetPacket *socket)
{

	G_PTZHandler->reset();
	ArUtil::sleep(500);
	G_PTZHandler->tiltRel(-10);


	double robotHeading=0;
  double robotDstX = robot.getPose().getX();
  double robotDstY = robot.getPose().getY();

	//---------Generate the next motion by random numbers------------
	cout << "-------- Generate the next motion by random numbers --------" <<endl;
	srand( time( NULL ) ); 
	switch(rand()%4) 
	{
	case 0:		//¡û
		robotDstY += 1000;
		robotHeading = 90;
		break;
	case 1:		//¡ü
		robotDstX += 1000;
		robotHeading = 0;
		break;
	case 2:		//¡ú
		robotDstY -= 1000;
		robotHeading = -90;
		break;
	case 3:		//¡ý
		robotDstX -= 1000;
		robotHeading = 180;
		break;
	}


  G_PathPlanning->pathPlanToPose(ArPose(robotDstX, robotDstY, robotHeading), true,true);
	cout << "RobotMotion is processing..." <<endl;

  while(G_PathPlanning->getState() != ArPathPlanningTask::REACHED_GOAL )
	{
		if (G_PathPlanning->getState() == ArPathPlanningTask::ABORTED_PATHPLAN)		{ G_PathPlanning->cancelPathPlan();break;}
		else if(G_PathPlanning->getState() == ArPathPlanningTask::FAILED_PLAN)		{G_PathPlanning->pathPlanToPose(ArPose(-300,-100,0),true,true);}
	}

 
	serverclient->sendPacketTcp(socket);
//-------------------------------------------------------------------------------------------------------

}
Пример #4
0
    void showMenu(){

    	ArLog::log(ArLog::Normal ,"Posicion actual robot:\n");
        robot->getPose().log();

    	cout << "Press the key for your option:" << endl << endl;
    	cout << " flecha arriba     : avanzar" << endl;
    	cout << " flecha abajo      : retroceder" << endl;
    	cout << " flecha izq / drch : girar" << endl;
    	cout << " p   : start" << endl;
    	cout << " s   : stop" << endl;
    	cout << " t   : test parado" << endl;
    	cout << " w   : guardar trayectoria" << endl;
    	cout << " g   : guardar Medidas" << endl;
    	cout << " c   : guardar Medidas continuamente" << endl;
    	cout << " x   : Quit" << endl;

    }
Пример #5
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;
}
Пример #6
0
void RosAriaNode::Mas1ToSla_cb( const geometry_msgs::PointStampedConstPtr &msg)
{
	// Master 1 Position
	Vm1 = msg->point.x;
	Xm1 = Xm1 + Vm1;
	// Master force
	Fk1 = msg->point.y;
	// Master 1 Positive Energy
	mst1_slv_cmd_P = msg->point.z;


	Xsd = Scale *(alpha*Xm1 + (1-alpha)*Xm2);// design position

	Xsprv = Xs;
	Position = robot->getPose();
	Xs = Position.getX();
	delta = Xs - Xsprv;

	Vs = PosController.compute(Xsd,Xs);

	// Fs - Sum
	Fs = K_force*(Xsd - Xs);
	Fs1 = alpha*Fs;
	Fs2 = (1-alpha)*Fs;
	/*
	 * Master 1 - Slave Channel
	 */

	// Calculate Negative Energy and dissipate Active energy
	if (Vm1*Fs1>0)
	{
		mst1_slv_cmd_N -=Vm1*Fs1;
	}
	else
	{
		//Do nothing
	}

	// PC:
	if (mst1_slv_cmd_N+mst1_slv_cmd_P<0)
	{
		mst1_slv_cmd_N +=Vm1*Fs1;    // backward 1 step
		Xm1 = Xm1 - Vm1;			// backward 1 step
		// Modify Vm1
		if (Fs1*Fs1>0)
			Vm1 = (mst1_slv_cmd_N+mst1_slv_cmd_P)/Fs1;
		else
			Vm1 = 0;
		//Update
		Xm1 = Xm1 + Vm1;
		Xsd = Scale *(alpha*Xm1 + (1-alpha)*Xm2);// design position
		Vs = PosController.compute(Xsd,Xs);
		// Modify Fs ????

		mst1_slv_cmd_N -=Vm1*Fs1;
	}

	/*
	 * Slave - Master 1 Channel
	 */
	// Calculate Positive Energy

	if (Fk1*Vs>0)
	{
		//sla_mst1_cmd_P += Fk1*Vs;
		sla_mst1_cmd_P += Fk1*delta;
	}
	else
	{
		//Do nothing
	}

	/*
	 * Master 2 - Slave Channel
	 */

	// Calculate Negative Energy and dissipate Active energy
	if (Vm2*Fs2>0)
	{
		mst2_slv_cmd_N -=Vm2*Fs2;
	}
	else
	{
		//Do nothing
	}

	// PC:
	if (mst2_slv_cmd_N+mst2_slv_cmd_P<0)
	{
		mst2_slv_cmd_N +=Vm2*Fs2;    // backward 1 step
		Xm2 = Xm2 - Vm2;			// backward 1 step
		// Modify Vm1
		if (Fs2*Fs2>0)
			Vm2 = (mst2_slv_cmd_N+mst2_slv_cmd_P)/Fs2;
		else
			Vm2 = 0;
		//Update
		Xm2 = Xm2 + Vm2;
		Xsd = Scale *(alpha*Xm1 + (1-alpha)*Xm2);// design position
		Vs = PosController.compute(Xsd,Xs);
		// Modify Fs ????

		mst2_slv_cmd_N -=Vm2*Fs2;
	}

	/*
	 * Slave - Master 2 Channel
	 */
	// Calculate Positive Energy
	if (Fk2*Vs>0)
	{
		sla_mst2_cmd_P += Fk2*Vs;
	}
	else
	{
		//Do nothing
	}



	//Saturation
	if (Vs>MaxVel) Vs = MaxVel;
	if (Vs< - MaxVel) Vs = -MaxVel;

	//ROS_INFO("Velocity: %5f",Vs);
	//ROS_INFO("Ref - Real - Vel : %5f  -- %5f  --%5f",Xsd,Xs,Vs);
	ROS_INFO("Position: %5f - %5f",Xs,Xsd);
	robot->setVel(Vs);


	// Publisher
	SlaToMas1.point.x = Fs1;
	SlaToMas1.point.y = delta;
	SlaToMas1.point.z = sla_mst1_cmd_P;

	SlaToMas1_Pub.publish(SlaToMas1);

	SlaToMas2.point.x = Fs2;
	SlaToMas2.point.y = Vs;
	SlaToMas2.point.z = sla_mst2_cmd_P;

	SlaToMas2_Pub.publish(SlaToMas2);
}
Пример #7
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;
      myAMPTU->panTilt(0, -40);
      myAcquire->activate();
      myAcquire->setChannel(COLOR_BLOCK);
      myPickUp->deactivate();
      myDriveTo->deactivate();
      myDropOff->deactivate();
      myTableLimiter->deactivate();
    }
    if (myGripper->getGripState() == 2 &&
	myGripper->getBreakBeamState() != 0)
    {
      printf("###### AcquireBlock: Successful (have cube?)\n");
      setState(STATE_PICKUP_BACKUP);
      //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 || 
	myStateStartTime.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;
      myAMPTU->panTilt(0, -35);
      myAcquire->deactivate();
      myPickUp->activate();
      myPickUp->setChannel(COLOR_BLOCK);
      myDriveTo->deactivate();
      myDropOff->deactivate();
      myTableLimiter->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_PICKUP_BACKUP);
      //handler();
      return;
    }
    break;
  case STATE_BACKUP:
    if (myNewState)
    {
      myNewState = false;
      myRobot->move(BACKUP_DIST * .75);
      myAcquire->deactivate();
      myPickUp->deactivate();
      myDriveTo->deactivate();
      myDropOff->deactivate();
      myTableLimiter->deactivate();
    }
    if (myRobot->isLeftMotorStalled() || myRobot->isRightMotorStalled())
    {
      printf("###### Backup: Failed, going forwards\n");
      myRobot->clearDirectMotion();
      setState(STATE_FORWARD);      
    }
    if (myStateStartTime.mSecSince() > BACKUP_TIME || 
	myStateStartPos.findDistanceTo(myRobot->getPose()) > BACKUP_DIST * .95 * .75)
    {
      printf("###### Backup: Succeeded\n");
      myRobot->clearDirectMotion();
      setState(STATE_ACQUIRE_BLOCK2);
      //handler();
      return;
    }
    break;
  case STATE_FORWARD:
    if (myNewState)
    {
      myNewState = false;
      myRobot->move(-BACKUP_DIST * .75);
      myAcquire->deactivate();
      myPickUp->deactivate();
      myDriveTo->deactivate();
      myDropOff->deactivate();
      myTableLimiter->deactivate();
    }
    if (myRobot->isLeftMotorStalled() || myRobot->isRightMotorStalled())
    {
      printf("###### Forward: Failed\n");
      myRobot->clearDirectMotion();
      setState(STATE_FAILED);      
    }
    if (myStateStartTime.mSecSince() > BACKUP_TIME || 
	myStateStartPos.findDistanceTo(myRobot->getPose()) > 
	ArMath::fabs(BACKUP_DIST * .95 * .75))
    {
      printf("###### Forward: Succeeded\n");
      myRobot->clearDirectMotion();
      setState(STATE_ACQUIRE_BLOCK2);
      //handler();
      return;
    }
    break;
  case STATE_ACQUIRE_BLOCK2:
    if (myNewState)
    {
      printf("!! Acquire block 2\n");
      myNewState = false;
      myAMPTU->panTilt(0, -40);
      myAcquire->activate();
      myAcquire->setChannel(COLOR_BLOCK);
      myPickUp->deactivate();
      myDriveTo->deactivate();
      myDropOff->deactivate();
      myTableLimiter->deactivate();
    }
    if (myGripper->getGripState() == 2 &&
	myGripper->getBreakBeamState() != 0)
    {
      printf("###### AcquireBlock2: Successful (have cube?)\n");
      setState(STATE_PICKUP_BACKUP);
      //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 ||
	myStateStartTime.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();
      myAMPTU->panTilt(0, -55);
      myPickUp->setChannel(COLOR_BLOCK);
      myDriveTo->deactivate();
      myDropOff->deactivate();
      myTableLimiter->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_PICKUP_BACKUP);
      //handler();
      return;
    }
    break;
  case STATE_PICKUP_BACKUP:
    if (myNewState)
    {
      myNewState = false;
      myRobot->move(BACKUP_DIST);
      myAcquire->deactivate();
      myPickUp->deactivate();
      myDriveTo->deactivate();
      myDropOff->deactivate();
      myTableLimiter->deactivate();
    }
    if (myStateStartTime.mSecSince() > BACKUP_TIME || 
	myStateStartPos.findDistanceTo(myRobot->getPose()) > 
	ArMath::fabs(BACKUP_DIST * .95))
    {
      printf("###### PickUp_BackUp: done\n");
      myRobot->clearDirectMotion();
      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;
      myAMPTU->panTilt(0, -30);
      myAcquire->activate();
      myAcquire->setChannel(myDropWall);
      myPickUp->deactivate();
      myDriveTo->deactivate();
      myDropOff->deactivate();
      myTableLimiter->deactivate();
    }
    if (myGripper->getGripState() != 2 || 
	myGripper->getBreakBeamState() == 0)
    {
      printf("###### AcquireDropWall:: failed (lost cube %d %d)\n",
	     myGripper->getGripState(), myGripper->getBreakBeamState());
      setState(STATE_BACKUP);	       
      //handler();
      return;
    }
    if (myAcquire->getState() == Acquire::STATE_FAILED ||
	myStateStartTime.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("!! DropOff Drop wall, channel %d\n", myDropWall);
      myNewState = false;
      myAcquire->deactivate();
      myPickUp->deactivate();
      myDriveTo->deactivate();
      myDropOff->activate();
      myDropOff->setChannel(myDropWall);
      myTableLimiter->deactivate();
    }
    if (myDropOff->getState() == DropOff::STATE_FAILED)
    {
      printf("###### DropOffDropWall: failed\n");
      setState(STATE_FAILED);
      //handler();
      return;
    }
    else if (myDropOff->getState() == DropOff::STATE_SUCCEEDED)
    {
      printf("###### DropOffDropWall: succesful\n");
      setState(STATE_DROP_BACKUP);
      //handler();
      return;
    }
    break;
  case STATE_DROP_BACKUP:
    if (myNewState)
    {
      myNewState = false;
      myRobot->move(BACKUP_DIST);
      myAcquire->deactivate();
      myPickUp->deactivate();
      myDriveTo->deactivate();
      myDropOff->deactivate();
      myTableLimiter->deactivate();
    }
    if (myStateStartTime.mSecSince() > BACKUP_TIME || 
	myStateStartPos.findDistanceTo(myRobot->getPose()) > 
	ArMath::fabs(BACKUP_DIST * .95))
    {
      printf("###### Drop_Backup: done\n");
      myRobot->clearDirectMotion();
      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;
      myAMPTU->panTilt(0, -30);
      myAcquire->activate();
      myAcquire->setChannel(myLapWall);
      myPickUp->deactivate();
      myDriveTo->deactivate();
      myDropOff->deactivate();
      myTableLimiter->activate();
    }
    if (myAcquire->getState() == Acquire::STATE_FAILED ||
	myStateStartTime.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);
      myDropOff->deactivate();
      myTableLimiter->activate();
    }
    if (myDriveTo->getState() == DriveTo::STATE_FAILED)
    {
      printf("###### DriveToLapWall: failed\n");
      setState(STATE_BACKUP_LAP_WALL);
      //handler();
      return;
    }
    else if (myDriveTo->getState() == DriveTo::STATE_SUCCEEDED)
    {
      printf("###### DriveToLapWall: succesful\n");
      setState(STATE_BACKUP_LAP_WALL);
      //handler();
      return;
    }
    break;
  case STATE_BACKUP_LAP_WALL:
    if (myNewState)
    {
      myNewState = false;
      myRobot->move(BACKUP_DIST * .75);
      myAcquire->deactivate();
      myPickUp->deactivate();
      myDriveTo->deactivate();
      myDropOff->deactivate();
      myTableLimiter->deactivate();
    }
    if (myRobot->isLeftMotorStalled() || myRobot->isRightMotorStalled())
    {
      printf("###### BackupLapWall: Failed, going forwards\n");
      myRobot->clearDirectMotion();
      setState(STATE_FORWARD_LAP_WALL);      
    }
    if (myStateStartTime.mSecSince() > BACKUP_TIME || 
	myStateStartPos.findDistanceTo(myRobot->getPose()) > 
	ArMath::fabs(BACKUP_DIST * .95 * .75))
    {
      printf("###### BackupLapWall: Succeeded\n");
      myRobot->clearDirectMotion();
      setState(STATE_SWITCH);
      //handler();
      return;
    }
    break;
  case STATE_FORWARD_LAP_WALL:
    if (myNewState)
    {
      myNewState = false;
      myRobot->move(-BACKUP_DIST * .75);
      myAcquire->deactivate();
      myPickUp->deactivate();
      myDriveTo->deactivate();
      myDropOff->deactivate();
      myTableLimiter->deactivate();
    }
    if (myRobot->isLeftMotorStalled() || myRobot->isRightMotorStalled())
    {
      printf("###### ForwardLapWall: Failed\n");
      myRobot->clearDirectMotion();
      setState(STATE_FAILED);      
    }
    if (myStateStartTime.mSecSince() > BACKUP_TIME || 
	myStateStartPos.findDistanceTo(myRobot->getPose()) > 
	ArMath::fabs(BACKUP_DIST * .95 * .75))
    {
      printf("###### ForwardLapWall: Succeeded\n");
      myRobot->clearDirectMotion();
      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;
  }

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

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

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


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

  sick.lockDevice();

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

  sick.unlockDevice();
		

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

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

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

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

  //serverclient->sendPacketTcp(socket);

  ArUtil::sleep(3000);


  cout << "RobotMotion is heading home..." <<endl;
  G_PathPlanning->pathPlanToPose(ArPose(0,0,0),true,true);
  
  while(G_PathPlanning->getState() != ArPathPlanningTask::REACHED_GOAL )
	{
		//cout << G_PathPlanning->getState() <<endl;
		if (G_PathPlanning->getState() == ArPathPlanningTask::FAILED_MOVE)
		{
			G_PathPlanning->cancelPathPlan(); break;
		}


	//	//else if(G_PathPlanning->getState() == ArPathPlanningTask::FAILED_PLAN)
 // //  {G_PathPlanning->pathPlanToPose(ArPose(-300,30,0),true,true);}

	}
}
Пример #9
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);
  }

}
int main(int argc, char **argv)
{
  // Initialize Aria and Arnl global information
  Aria::init();
  Arnl::init();


  // The robot object
  ArRobot robot;

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

  // Set up our simpleConnector, to connect to the robot and laser
  //ArSimpleConnector simpleConnector(&parser);
  ArRobotConnector robotConnector(&parser, &robot);

  // Connect to the robot
  if (!robotConnector.connectRobot())
  {
    ArLog::log(ArLog::Normal, "Error: Could not connect to robot... exiting");
    Aria::exit(3);
  }



  // Set up where we'll look for files. Arnl::init() set Aria's default
  // directory to Arnl's default directory; addDirectories() appends this
  // "examples" directory.
  char fileDir[1024];
  ArUtil::addDirectories(fileDir, sizeof(fileDir), Aria::getDirectory(), 
			 "examples");
  
  
  // To direct log messages to a file, or to change the log level, use these  calls:
  //ArLog::init(ArLog::File, ArLog::Normal, "log.txt", true, true);
  //ArLog::init(ArLog::File, ArLog::Verbose);
 
  // Add a section to the configuration to change ArLog parameters
  ArLog::addToConfig(Aria::getConfig());

  // set up a gyro (if the robot is older and its firmware does not
  // automatically incorporate gyro corrections, then this object will do it)
  ArAnalogGyro gyro(&robot);

  // Our networking server
  ArServerBase server;
  

  // Set up our simpleOpener, used to set up the networking server
  ArServerSimpleOpener simpleOpener(&parser);

  // the laser connector
  ArLaserConnector laserConnector(&parser, &robot, &robotConnector);

  // Tell the laser connector to always connect the first laser since
  // this program always requires a laser.
  parser.addDefaultArgument("-connectLaser");
  
  // Load default arguments for this computer (from /etc/Aria.args, environment
  // variables, and other places)
  parser.loadDefaultArguments();

  // Parse arguments 
  if (!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed())
  {
    logOptions(argv[0]);
    Aria::exit(1);
  }
  

  // This causes Aria::exit(9) to be called if the robot unexpectedly
  // disconnects
  ArGlobalFunctor1<int> shutdownFunctor(&Aria::exit, 9);
  robot.addDisconnectOnErrorCB(&shutdownFunctor);


  // Create an ArSonarDevice object (ArRangeDevice subclass) and 
  // connect it to the robot.
  ArSonarDevice sonarDev;
  robot.addRangeDevice(&sonarDev);



  // This object will allow robot's movement parameters to be changed through
  // a Robot Configuration section in the ArConfig global configuration facility.
  ArRobotConfig robotConfig(&robot);

  // Include gyro configuration options in the robot configuration section.
  robotConfig.addAnalogGyro(&gyro);

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

  // connect the laser(s) if it was requested, this adds them to the
  // robot too, and starts them running in their own threads
  if (!laserConnector.connectLasers())
  {
    ArLog::log(ArLog::Normal, "Could not connect to all lasers... exiting\n");
    Aria::exit(2);
  }

  // find the laser we should use for localization and/or mapping,
  // which will be the first laser
  robot.lock();
  ArLaser *firstLaser = robot.findLaser(1);
  if (firstLaser == NULL || !firstLaser->isConnected())
  {
    ArLog::log(ArLog::Normal, "Did not have laser 1 or it is not connected, cannot start localization and/or mapping... exiting");
    Aria::exit(2);
  }
  robot.unlock();


    /* Create and set up map object */
  
  // Set up the map object, this will look for files in the examples
  // directory (unless the file name starts with a /, \, or .
  // You can take out the 'fileDir' argument to look in the program's current directory
  // instead.
  // When a configuration file is loaded into ArConfig later, if it specifies a
  // map file, then that file will be loaded as the map.
  ArMap map(fileDir);
  // set it up to ignore empty file names (otherwise if a configuration omits
  // the map file, the whole configuration change will fail)
  map.setIgnoreEmptyFileName(true);
  // ignore the case, so that if someone is using MobileEyes or
  // MobilePlanner from Windows and changes the case on a map name,
  // it will still work.
  map.setIgnoreCase(true);

    
    /* Create localization and path planning threads */


  ArPathPlanningTask pathTask(&robot, &sonarDev, &map);



  ArLog::log(ArLog::Normal, "Creating laser localization task");
  // Laser Monte-Carlo Localization
  ArLocalizationTask locTask(&robot, firstLaser, &map);



  // Set some options on each laser that the laser connector 
  // connected to.
  std::map<int, ArLaser *>::iterator laserIt;
  for (laserIt = robot.getLaserMap()->begin();
       laserIt != robot.getLaserMap()->end();
       laserIt++)
  {
    int laserNum = (*laserIt).first;
    ArLaser *laser = (*laserIt).second;

    // Skip lasers that aren't connected
    if(!laser->isConnected())
      continue;

    // add the disconnectOnError CB to shut things down if the laser
    // connection is lost
    laser->addDisconnectOnErrorCB(&shutdownFunctor);
    // set the number of cumulative readings the laser will take
    laser->setCumulativeBufferSize(200);
    // add the lasers to the path planning task
    pathTask.addRangeDevice(laser, ArPathPlanningTask::BOTH);
    // set the cumulative clean offset (so that they don't all fire at once)
    laser->setCumulativeCleanOffset(laserNum * 100);
    // reset the cumulative clean time (to make the new offset take effect)
    laser->resetLastCumulativeCleanTime();

    // Add the packet count to the Aria info strings (It will be included in
    // MobileEyes custom details so you can monitor whether the laser data is
    // being received correctly)
    std::string laserPacketCountName;
    laserPacketCountName = laser->getName();
    laserPacketCountName += " Packet Count";
    Aria::getInfoGroup()->addStringInt(
	    laserPacketCountName.c_str(), 10, 
	    new ArRetFunctorC<int, ArLaser>(laser, 
					 &ArLaser::getReadingCount));
  }


  // Used for optional multirobot features (see below) (TODO move to multirobot
  // example?)
  ArClientSwitchManager clientSwitch(&server, &parser);




    /* Start the server */

  // Open the networking server
  if (!simpleOpener.open(&server, fileDir, 240))
  {
    ArLog::log(ArLog::Normal, "Error: Could not open server.");
    exit(2);
  }



    /* Create various services that provide network access to clients (such as
     * MobileEyes), as well as add various additional features to ARNL */


  // ARNL can optionally get information about the positions of other robots from a
  // "central server" (see central server example program), if command
  // line options specifying the address of the central server was given.
  // If there is no central server, then the address of each other robot
  // can instead be given in the configuration, and the multirobot systems
  // will connect to each robot (or "peer") individually.

        // TODO move this to multirobot example?


  bool usingCentralServer = false;
  if(clientSwitch.getCentralServerHostName() != NULL)
    usingCentralServer = true;

  // if we're using the central server then we want to create the
  // multiRobot central classes
  if (usingCentralServer)
  {
    // Make the handler for multi robot information (this sends the
    // information to the central server)
    //ArServerHandlerMultiRobot *handlerMultiRobot = 
    new ArServerHandlerMultiRobot(&server, &robot, 
						      &pathTask,
						      &locTask, &map);
    
    // Normally each robot, and the central server, must all have
    // the same map name for the central server to share robot
    // information.  (i.e. they are operating in the same space).
    // This changes the map name that ArServerHandlerMutliRobot 
    // reports to the central server, in case you want this individual
    // robot to load a different map file name, but still report 
    // the common map file to the central server.
    //handlerMultiRobot->overrideMapName("central.map");

    // the range device that gets the multi robot information from
    // the central server and presents it as virtual range readings
    // to ARNL
    ArMultiRobotRangeDevice *multiRobotRangeDevice = new ArMultiRobotRangeDevice(&server);
    
    robot.addRangeDevice(multiRobotRangeDevice);
    pathTask.addRangeDevice(multiRobotRangeDevice, 
			    ArPathPlanningTask::BOTH);
    
    // Set up options for drawing multirobot information in MobileEyes.
    multiRobotRangeDevice->setCurrentDrawingData(
	    new ArDrawingData("polyDots", ArColor(125, 125, 0),
			      100, 73, 1000), true);
    multiRobotRangeDevice->setCumulativeDrawingData(
	    new ArDrawingData("polyDots", ArColor(125, 0, 125),
			      100, 72, 1000), true);

    // This sets up the localization to use the known poses of other robots
    // for its localization in cases where numerous robots crowd out the map.
    locTask.setMultiRobotCallback(multiRobotRangeDevice->getOtherRobotsCB());
  }
  // if we're not using a central server then create the multirobot peer classes
  else
  {
    // set the path planning so it uses the explicit collision range for how far its planning
    pathTask.setUseCollisionRangeForPlanningFlag(true);
    // make our thing that gathers information from the other servers
    ArServerHandlerMultiRobotPeer *multiRobotPeer = NULL;
    ArMultiRobotPeerRangeDevice *multiRobotPeerRangeDevice = NULL;
    multiRobotPeerRangeDevice = new ArMultiRobotPeerRangeDevice(&map);
    // make our thing that sends information to the other servers
    multiRobotPeer = new ArServerHandlerMultiRobotPeer(&server, &robot, 
						     &pathTask, &locTask);
    // hook the two together so they both know what priority this robot is
    multiRobotPeer->setNewPrecedenceCallback(
	    multiRobotPeerRangeDevice->getSetPrecedenceCallback());
    // hook the two together so they both know what priority this
    // robot's fingerprint is
    multiRobotPeer->setNewFingerprintCallback(
	    multiRobotPeerRangeDevice->getSetFingerprintCallback());
    // hook the two together so that the range device can call on the
    // server handler to change its fingerprint
    multiRobotPeerRangeDevice->setChangeFingerprintCB(
	    multiRobotPeer->getChangeFingerprintCB());
    // then add the robot to the places it needs to be
    robot.addRangeDevice(multiRobotPeerRangeDevice);
    pathTask.addRangeDevice(multiRobotPeerRangeDevice, 
			    ArPathPlanningTask::BOTH);

    // Set the range device so that we can see the information its using
    // to avoid, you can comment these out in order to not see them
    multiRobotPeerRangeDevice->setCurrentDrawingData(
	    new ArDrawingData("polyDots", ArColor(125, 125, 0),
			      100, 72, 1000), true);
    multiRobotPeerRangeDevice->setCumulativeDrawingData(
	    new ArDrawingData("polyDots", ArColor(125, 0, 125),
			      100, 72, 1000), true);
    // This sets up the localization to use the known poses of other robots
    // for its localization in cases where numerous robots crowd out the map.
    locTask.setMultiRobotCallback(
	    multiRobotPeerRangeDevice->getOtherRobotsCB());
  }




  /* Add additional range devices to the robot and path planning task (so it
     avoids obstacles detected by these devices) */
  
  // Add IR range device to robot and path planning task (so it avoids obstacles
  // detected by this device)
  robot.lock();
  ArIRs irs;
  robot.addRangeDevice(&irs);
  pathTask.addRangeDevice(&irs, ArPathPlanningTask::CURRENT);

  // Add bumpers range device to robot and path planning task (so it avoids obstacles
  // detected by this device)
  ArBumpers bumpers;
  robot.addRangeDevice(&bumpers);
  pathTask.addRangeDevice(&bumpers, ArPathPlanningTask::CURRENT);

  // Add range device which uses forbidden regions given in the map to give virtual
  // range device readings to ARNL.  (so it avoids obstacles
  // detected by this device)
  ArForbiddenRangeDevice forbidden(&map);
  robot.addRangeDevice(&forbidden);
  pathTask.addRangeDevice(&forbidden, ArPathPlanningTask::CURRENT);

  robot.unlock();


  // Action to slow down robot when localization score drops but not lost.
  ArActionSlowDownWhenNotCertain actionSlowDown(&locTask);
  pathTask.getPathPlanActionGroup()->addAction(&actionSlowDown, 140);

  // Action to stop the robot when localization is "lost" (score too low)
  ArActionLost actionLostPath(&locTask, &pathTask);
  pathTask.getPathPlanActionGroup()->addAction(&actionLostPath, 150);

  // Arnl uses this object when it must replan its path because its
  // path is completely blocked.  It will use an older history of sensor
  // readings to replan this new path.  This should not be used with SONARNL
  // since sonar readings are not accurate enough and may prevent the robot
  // from planning through space that is actually clear.
  ArGlobalReplanningRangeDevice replanDev(&pathTask);

  
  // Service to provide drawings of data in the map display :
  ArServerInfoDrawings drawings(&server);
  drawings.addRobotsRangeDevices(&robot);
  drawings.addRangeDevice(&replanDev);

  /* Draw a box around the local path planning area use this 
    (You can enable this particular drawing from custom commands 
    which is set up down below in ArServerInfoPath) */
  ArDrawingData drawingDataP("polyLine", ArColor(200,200,200), 1, 75);
  ArFunctor2C<ArPathPlanningTask, ArServerClient *, ArNetPacket *> 
    drawingFunctorP(&pathTask, &ArPathPlanningTask::drawSearchRectangle);
  drawings.addDrawing(&drawingDataP, "Local Plan Area", &drawingFunctorP); 

  /* Show the sample points used by MCL */
  ArDrawingData drawingDataL("polyDots", ArColor(0,255,0), 100, 75);
  ArFunctor2C<ArLocalizationTask, ArServerClient *, ArNetPacket *> 
    drawingFunctorL(&locTask, &ArLocalizationTask::drawRangePoints);
  drawings.addDrawing(&drawingDataL, "Localization Points", &drawingFunctorL);


  // "Custom" commands. You can add your own custom commands here, they will
  // be available in MobileEyes' custom commands (enable in the toolbar or
  // access through Robot Tools)
  ArServerHandlerCommands commands(&server);


  // These provide various kinds of information to the client:
  ArServerInfoRobot serverInfoRobot(&server, &robot);
  ArServerInfoSensor serverInfoSensor(&server, &robot);
  ArServerInfoPath serverInfoPath(&server, &robot, &pathTask);
  serverInfoPath.addSearchRectangleDrawing(&drawings);
  serverInfoPath.addControlCommands(&commands);

  // Provides localization info and allows the client (MobileEyes) to relocalize at a given
  // pose:
  ArServerInfoLocalization serverInfoLocalization(&server, &robot, &locTask);
  ArServerHandlerLocalization serverLocHandler(&server, &robot, &locTask);

  // If you're using MobileSim, ArServerHandlerLocalization sends it a command
  // to move the robot's true pose if you manually do a localization through 
  // MobileEyes.  To disable that behavior, use this constructor call instead:
  // ArServerHandlerLocalization serverLocHandler(&server, &robot, true, false);
  // The fifth argument determines whether to send the command to MobileSim.

  // Provide the map to the client (and related controls):
  ArServerHandlerMap serverMap(&server, &map);

  // These objects add some simple (custom) commands to 'commands' for testing and debugging:
  ArServerSimpleComUC uCCommands(&commands, &robot);                   // Send any command to the microcontroller
  ArServerSimpleComMovementLogging loggingCommands(&commands, &robot); // configure logging
  ArServerSimpleComLogRobotConfig configCommands(&commands, &robot);   // trigger logging of the robot config parameters
//  ArServerSimpleServerCommands serverCommands(&commands, &server);     // monitor networking behavior (track packets sent etc.)


  // service that allows the client to monitor the communication link status
  // between the robot and the client.
  //
  ArServerHandlerCommMonitor handlerCommMonitor(&server);



  // service that allows client to change configuration parameters in ArConfig 
  ArServerHandlerConfig handlerConfig(&server, Aria::getConfig(),
				      Arnl::getTypicalDefaultParamFileName(),
				      Aria::getDirectory());



  /* Set up the possible modes for remote control from a client such as
   * MobileEyes:
   */

  // Mode To go to a goal or other specific point:
  ArServerModeGoto modeGoto(&server, &robot, &pathTask, &map,
			    locTask.getRobotHome(),
			    locTask.getRobotHomeCallback());


  // Mode To stop and remain stopped:
  ArServerModeStop modeStop(&server, &robot);

  // Cause the sonar to turn off automatically
  // when the robot is stopped, and turn it back on when commands to move
  // are sent. (Note, if using SONARNL to localize, then don't do this
  // since localization may get lost)
  ArSonarAutoDisabler sonarAutoDisabler(&robot);

  // Teleoperation modes To drive by keyboard, joystick, etc:
  ArServerModeRatioDrive modeRatioDrive(&server, &robot);  
//  ArServerModeDrive modeDrive(&server, &robot);            // Older mode for compatability



  // Prevent normal teleoperation driving if localization is lost using
  // a high-priority action, which enables itself when the particular mode is
  // active.
  // (You have to enter unsafe drive mode to drive when lost.)
  ArActionLost actionLostRatioDrive(&locTask, &pathTask, &modeRatioDrive);
  modeRatioDrive.getActionGroup()->addAction(&actionLostRatioDrive, 110);

  // Add drive mode section to the configuration, and also some custom (simple) commands:
  modeRatioDrive.addToConfig(Aria::getConfig(), "Teleop settings");
  modeRatioDrive.addControlCommands(&commands);

  // Wander mode (also prevent wandering if lost):
  ArServerModeWander modeWander(&server, &robot);
  ArActionLost actionLostWander(&locTask, &pathTask, &modeWander);
  modeWander.getActionGroup()->addAction(&actionLostWander, 110);


  // This provides a small table of interesting information for the client
  // to display to the operator. You can add your own callbacks to show any
  // data you want.
  ArServerInfoStrings stringInfo(&server);
  Aria::getInfoGroup()->addAddStringCallback(stringInfo.getAddStringFunctor());
  
  // Provide a set of informational data (turn on in MobileEyes with
  // View->Custom Details)

  Aria::getInfoGroup()->addStringInt(
	  "Motor Packet Count", 10, 
	  new ArConstRetFunctorC<int, ArRobot>(&robot, 
					       &ArRobot::getMotorPacCount));

  Aria::getInfoGroup()->addStringDouble(
	  "Laser Localization Score", 8, 
	  new ArRetFunctorC<double, ArLocalizationTask>(
		  &locTask, &ArLocalizationTask::getLocalizationScore),
	  "%.03f");
  Aria::getInfoGroup()->addStringInt(
	  "Laser Loc Num Samples", 8, 
	  new ArRetFunctorC<int, ArLocalizationTask>(
		  &locTask, &ArLocalizationTask::getCurrentNumSamples),
	  "%4d");


  // Display gyro status if gyro is enabled and is being handled by the firmware (gyro types 2, 3, or 4).
  // (If the firmware detects an error communicating with the gyro or IMU it
  // returns a flag, and stops using it.)
  // (This gyro type parameter, and fault flag, are only in ARCOS, not Seekur firmware)
  if(robot.getOrigRobotConfig() && robot.getOrigRobotConfig()->getGyroType() > 1)
  {
    Aria::getInfoGroup()->addStringString(
          "Gyro/IMU Status", 10,
          new ArGlobalRetFunctor1<const char*, ArRobot*>(&getGyroStatusString, &robot)
      );
  }


  // Setup the dock if there is a docking system on board.
  ArServerModeDock *modeDock = NULL;
  modeDock = ArServerModeDock::createDock(&server, &robot, &locTask, 
					  &pathTask);
  if (modeDock != NULL)
  {
    modeDock->checkDock();
    modeDock->addAsDefaultMode();
    modeDock->addToConfig(Aria::getConfig());
    modeDock->addControlCommands(&commands);
  }



  // Make Stop mode the default (If current mode deactivates without entering
  // a new mode, then Stop Mode will be selected)
  modeStop.addAsDefaultMode();
    // TODO move up near where stop mode is created?





  /* Services that allow the client to initiate scanning with the laser to
     create maps in Mapper3 (So not possible with SONARNL): */

  ArServerHandlerMapping handlerMapping(&server, &robot, firstLaser, 
					fileDir, "", true);

  // make laser localization stop while mapping
  handlerMapping.addMappingStartCallback(
	  new ArFunctor1C<ArLocalizationTask, bool>
	  (&locTask, &ArLocalizationTask::setIdleFlag, true));

  // and then make it start again when we're doine
  handlerMapping.addMappingEndCallback(
	  new ArFunctor1C<ArLocalizationTask, bool>
	  (&locTask, &ArLocalizationTask::setIdleFlag, false));


  // Make it so our "lost" actions don't stop us while mapping
  handlerMapping.addMappingStartCallback(actionLostPath.getDisableCB());
  handlerMapping.addMappingStartCallback(actionLostRatioDrive.getDisableCB());
  handlerMapping.addMappingStartCallback(actionLostWander.getDisableCB());

  // And then let them make us stop as usual when done mapping
  handlerMapping.addMappingEndCallback(actionLostPath.getEnableCB());
  handlerMapping.addMappingEndCallback(actionLostRatioDrive.getEnableCB());
  handlerMapping.addMappingEndCallback(actionLostWander.getEnableCB());

  // don't let forbidden lines show up as obstacles while mapping
  // (they'll just interfere with driving while mapping, and localization is off anyway)
  handlerMapping.addMappingStartCallback(forbidden.getDisableCB());

  // let forbidden lines show up as obstacles again as usual after mapping
  handlerMapping.addMappingEndCallback(forbidden.getEnableCB());


  /*
  // If we are on a simulator, move the robot back to its starting position,
  // and reset its odometry.
  // This will allow localizeRobotAtHomeBlocking() below will (probably) work (it
  // tries current odometry (which will be 0,0,0) and all the map
  // home points.
  // (Ignored by a real robot)
  //robot.com(ArCommands::SIM_RESET);
  */


  // create a pose storage class, this will let the program keep track
  // of where the robot is between runs...  after we try and restore
  // from this file it will start saving the robot's pose into the
  // file
  ArPoseStorage poseStorage(&robot);
  /// if we could restore the pose from then set the sim there (this
  /// won't do anything to the real robot)... if we couldn't restore
  /// the pose then just reset the position of the robot (which again
  /// won't do anything to the real robot)
  if (poseStorage.restorePose("robotPose"))
    serverLocHandler.setSimPose(robot.getPose());
  else
    robot.com(ArCommands::SIM_RESET);



  /* File transfer services: */
  
#ifdef WIN32
  // Not implemented for Windows yet.
  ArLog::log(ArLog::Normal, "Note, file upload/download services are not implemented for Windows; not enabling them.");
#else
  // This block will allow you to set up where you get and put files
  // to/from, just comment them out if you don't want this to happen
  // /*
  ArServerFileLister fileLister(&server, fileDir);
  ArServerFileToClient fileToClient(&server, fileDir);
  ArServerFileFromClient fileFromClient(&server, fileDir, "/tmp");
  ArServerDeleteFileOnServer deleteFileOnServer(&server, fileDir);
  // */
#endif

    /* Video image streaming, and camera controls (Requires SAVserver or ACTS) */

  // Forward any 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 seperate 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);
  
  // make a camera to use in case we have video. the camera collection collects
  // multiple ptz cameras 
  ArPTZ *camera = NULL;
  ArServerHandlerCamera *handlerCamera = NULL;
  ArCameraCollection *cameraCollection = NULL;

  // if we have video then set up a camera 
  if (videoForwarder.isForwardingVideo())
  {

    cameraCollection = new ArCameraCollection();
    cameraCollection->addCamera("Cam1", "PTZ", "Camera", "PTZ");

    videoForwarder.setCameraName("Cam1");
    videoForwarder.addToCameraCollection(*cameraCollection);

    camera = new ArVCC4(&robot); //,	invertedCamera, ArVCC4::COMM_UNKNOWN, true, true);
    // To use an RVision SEE camera instead:
    // camera = new ArRVisionPTZ(&robot);
    camera->init();

    handlerCamera = new ArServerHandlerCamera("Cam1", 
		                                           &server, 
					                                     &robot,
					                                     camera, 
					                                     cameraCollection);

    pathTask.addGoalFinishedCB(
	    new ArFunctorC<ArServerHandlerCamera>(
		    handlerCamera, 
		    &ArServerHandlerCamera::cameraModeLookAtGoalClearGoal));
  }

  // After all of the cameras / videos have been created and added to the collection,
  // then start the collection server.
  //
  if (cameraCollection != NULL) {
    new ArServerHandlerCameraCollection(&server, cameraCollection);
  }




    /* Load configuration values, map, and begin! */

  
  // When parsing the configuration file, also look at the program's command line options 
  // from the command-line argument parser as well as the configuration file.
  // (So you can use any argument on the command line, namely -map.) 
  Aria::getConfig()->useArgumentParser(&parser);
  puts("xxx");puts("aaa"); fflush(stdout);
  // Read in parameter files.
  ArLog::log(ArLog::Normal, "Loading config file %s into ArConfig (base directory %s)...", Arnl::getTypicalParamFileName(), Aria::getConfig()->getBaseDirectory());
  if (!Aria::getConfig()->parseFile(Arnl::getTypicalParamFileName()))
  {
    ArLog::log(ArLog::Normal, "Trouble loading configuration file, exiting");
    Aria::exit(5);
  }

  // Warn about unknown params.
  if (!simpleOpener.checkAndLog() || !parser.checkHelpAndWarnUnparsed())
  {
    logOptions(argv[0]);
    Aria::exit(6);
  }

  // Warn if there is no map
  if (map.getFileName() == NULL || strlen(map.getFileName()) <= 0)
  {
    ArLog::log(ArLog::Normal, "");
    ArLog::log(ArLog::Normal, "### No map file is set up, you can make a map with the following procedure");
    ArLog::log(ArLog::Normal, "   0) You can find this information in README.txt or docs/Mapping.txt");
    ArLog::log(ArLog::Normal, "   1) Connect to this server with MobileEyes");
    ArLog::log(ArLog::Normal, "   2) Go to Tools->Map Creation->Start Scan");
    ArLog::log(ArLog::Normal, "   3) Give the map a name and hit okay");
    ArLog::log(ArLog::Normal, "   4) Drive the robot around your space (see docs/Mapping.txt");
    ArLog::log(ArLog::Normal, "   5) Go to Tools->Map Creation->Stop Scan");
    ArLog::log(ArLog::Normal, "   6) Start up Mapper3");
    ArLog::log(ArLog::Normal, "   7) Go to File->Open on Robot");
    ArLog::log(ArLog::Normal, "   8) Select the .2d you created");
    ArLog::log(ArLog::Normal, "   9) Create a .map");
    ArLog::log(ArLog::Normal, "  10) Go to File->Save on Robot");
    ArLog::log(ArLog::Normal, "  11) In MobileEyes, go to Tools->Robot Config");
    ArLog::log(ArLog::Normal, "  12) Choose the Files section");
    ArLog::log(ArLog::Normal, "  13) Enter the path and name of your new .map file for the value of the Map parameter.");
    ArLog::log(ArLog::Normal, "  14) Press OK and your new map should become the map used");
    ArLog::log(ArLog::Normal, "");    
  }

  // Print a log message notifying user of the directory for map files
  ArLog::log(ArLog::Normal, "");
  ArLog::log(ArLog::Normal, 
	     "Directory for maps and file serving: %s", fileDir);
  
  ArLog::log(ArLog::Normal, "See the ARNL README.txt for more information");
  ArLog::log(ArLog::Normal, "");

  // Do an initial localization of the robot. It tries all the home points
  // in the map, as well as the robot's current odometric position, as possible
  // places the robot is likely to be at startup.   If successful, it will
  // also save the position it found to be the best localized position as the
  // "Home" position, which can be obtained from the localization task (and is
  // used by the "Go to home" network request).
  locTask.localizeRobotAtHomeBlocking();
  
  // Let the client switch manager (for multirobot) spin off into its own thread
  // TODO move to multirobot example?
  clientSwitch.runAsync();

  // Start the networking server's thread
  server.runAsync();


  // Add a key handler so that you can exit by pressing
  // escape. Note that this key handler, however, prevents this program from
  // running in the background (e.g. as a system daemon or run from 
  // the shell with "&") -- it will lock up trying to read the keys; 
  // remove this if you wish to be able to run this program in the background.
  ArKeyHandler *keyHandler;
  if ((keyHandler = Aria::getKeyHandler()) == NULL)
  {
    keyHandler = new ArKeyHandler;
    Aria::setKeyHandler(keyHandler);
    robot.lock();
    robot.attachKeyHandler(keyHandler);
    robot.unlock();
    puts("Server running. To exit, press escape.");
  }


 	
   ArnlASyncTaskExample asyncTaskExample(&pathTask, &robot, &modeGoto, &parser);



  // Enable the motors and wait until the robot exits (disconnection, etc.) or this program is
  // canceled.
  robot.enableMotors();
  robot.waitForRunExit();
  Aria::exit(0);
}
int main(int argc, char **argv)
{
  // Initialize Aria and Arnl global information
  Aria::init();
  Arnl::init();

  // You can change default ArLog options in this call, but the settings in the parameter file
  // (arnl.p) which is loaded below (Aria::getConfig()->parseFile())  will override the options.
  //ArLog::init(ArLog::File, ArLog::Normal, "log.txt", true, true);

  // Used to parse the command line arguments.
  ArArgumentParser parser(&argc, argv);
  
  // Load default arguments for this computer (from /etc/Aria.args, environment
  // variables, and other places)
  parser.loadDefaultArguments();

#ifdef ARNL_LASER
  // Tell the laser connector to always connect the first laser since
  // this program always requires a laser.
  parser.addDefaultArgument("-connectLaser");
#endif

  


  // The robot object
  ArRobot robot;

  // handle messages from robot controller firmware and log the contents
  robot.addPacketHandler(new ArGlobalRetFunctor1<bool, ArRobotPacket*>(&handleDebugMessage));

  // This object is used to connect to the robot, which can be configured via
  // command line arguments.
  ArRobotConnector robotConnector(&parser, &robot);

  // Connect to the robot
  if (!robotConnector.connectRobot())
  {
    ArLog::log(ArLog::Normal, "Error: Could not connect to robot... exiting");
    Aria::exit(3);
  }



  // Set up where we'll look for files. Arnl::init() set Aria's default
  // directory to Arnl's default directory; addDirectories() appends this
  // "examples" directory.
  char fileDir[1024];
  ArUtil::addDirectories(fileDir, sizeof(fileDir), Aria::getDirectory(), 
			 "examples");
  
  
  // To direct log messages to a file, or to change the log level, use these  calls:
  //ArLog::init(ArLog::File, ArLog::Normal, "log.txt", true, true);
  //ArLog::init(ArLog::File, ArLog::Verbose);
 
  // Add a section to the configuration to change ArLog parameters
  ArLog::addToConfig(Aria::getConfig());

  // set up a gyro (if the robot is older and its firmware does not
  // automatically incorporate gyro corrections, then this object will do it)
  ArAnalogGyro gyro(&robot);

  // Our networking server
  ArServerBase server;
  
#ifdef ARNL_GPSLOC
  // GPS connector.
  ArGPSConnector gpsConnector(&parser);
#endif

  // Set up our simpleOpener, used to set up the networking server
  ArServerSimpleOpener simpleOpener(&parser);

#ifdef ARNL_LASER
  // the laser connector
  ArLaserConnector laserConnector(&parser, &robot, &robotConnector);
#endif

  // used to connect to camera PTZ control
  ArPTZConnector ptzConnector(&parser, &robot);

#ifdef ARNL_MULTIROBOT
  // Used to connect to a "central server" which can be used as a proxy 
  // for multiple robot servers, and as a way for them to also communicate with
  // each other.  (objects implementing some of these inter-robot communication
  // features are created below).  
  // NOTE: If the central server is running on the same host as robot server(s),
  // then you must use the -serverPort argument to instruct these robot-control
  // server(s) to use different ports than the default 7272, since the central
  // server will use that port.
  ArClientSwitchManager clientSwitch(&server, &parser);
#endif
  
  // Load default arguments for this computer (from /etc/Aria.args, environment
  // variables, and other places)
  parser.loadDefaultArguments();

  // Parse arguments 
  if (!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed())
  {
    logOptions(argv[0]);
    Aria::exit(1);
  }
  

  // This causes Aria::exit(9) to be called if the robot unexpectedly
  // disconnects
  ArGlobalFunctor1<int> shutdownFunctor(&Aria::exit, 9);
  robot.addDisconnectOnErrorCB(&shutdownFunctor);


  // Create an ArSonarDevice object (ArRangeDevice subclass) and 
  // connect it to the robot.
  ArSonarDevice sonarDev;
  robot.addRangeDevice(&sonarDev);



  // This object will allow robot's movement parameters to be changed through
  // a Robot Configuration section in the ArConfig global configuration facility.
  ArRobotConfig robotConfig(&robot);

  // Include gyro configuration options in the robot configuration section.
  robotConfig.addAnalogGyro(&gyro);

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

#ifdef ARNL_GPSLOC
  // On the Seekur, power to the GPS receiver is switched on by this command.
  // (A third argument of 0 would turn it off). On other robots this command is
  // ignored. If this fails, you may need to reset the port with ARIA demo or 
  // seekurPower program (turn port off then on again).  If the port is already
  // on, it will have no effect on the GPS (it will remain powered.)
  // Do this now before connecting to lasers to give it plenty of time to power
  // on, initialize, and find a good position before GPS localization begins.
  ArLog::log(ArLog::Normal, "Turning on GPS power... (Seekur/Seekur Jr. power port 6)");
  robot.com2Bytes(116, 6, 1);
#endif
  
#ifdef ARNL_LASER

  // connect the laser(s) if it was requested, this adds them to the
  // robot too, and starts them running in their own threads
  ArLog::log(ArLog::Normal, "Connecting to laser(s) configured in parameters...");
  if (!laserConnector.connectLasers())
  {
    ArLog::log(ArLog::Normal, "Error: Could not connect to laser(s). Exiting.");
    Aria::exit(2);
  }
  ArLog::log(ArLog::Normal, "Done connecting to laser(s).");
#endif

#if defined(ARNL_LASERLOC) || defined(ARNL_MAPPING)
  // find the laser we should use for localization and/or mapping,
  // which will be the first laser
  robot.lock();
  ArLaser *firstLaser = robot.findLaser(1);
  if (firstLaser == NULL || !firstLaser->isConnected())
  {
    ArLog::log(ArLog::Normal, "Did not have laser 1 or it is not connected, cannot start localization and/or mapping... exiting");
    Aria::exit(2);
  }
  robot.unlock();
#endif  


    /* Create and set up map object */
  
  // Set up the map object, this will look for files in the examples
  // directory (unless the file name starts with a /, \, or .
  // You can take out the 'fileDir' argument to look in the program's current directory
  // instead.
  // When a configuration file is loaded into ArConfig later, if it specifies a
  // map file, then that file will be loaded as the map.
  ArMap map(fileDir);
  // set it up to ignore empty file names (otherwise if a configuration omits
  // the map file, the whole configuration change will fail)
  map.setIgnoreEmptyFileName(true);
  // ignore the case, so that if someone is using MobileEyes or
  // MobilePlanner from Windows and changes the case on a map name,
  // it will still work.
  map.setIgnoreCase(true);

    
    /* Create localization threads */

#ifdef ARNL_MULTILOC
  ArLocalizationManager locManager(&robot, &map);
#define LOCTASK locManager
#endif


#ifdef ARNL_LASERLOC
  ArLog::log(ArLog::Normal, "Creating laser localization task");
  // Laser Monte-Carlo Localization
  ArLocalizationTask locTask(&robot, firstLaser, &map);
#ifdef ARNL_MULTILOC
  locManager.addLocalizationTask(&locTask);
#else
#define LOCTASK locTask
#endif
#endif
  

#ifdef ARNL_SONARLOC
  ArLog::log(ArLog::Normal, "Creating sonar localization task");
  ArSonarLocalizationTask locTask(&robot, &sonarDev, &map);
#ifdef ARNL_MULTILOC
  locManager.addLocalizationTask(&locTask);
#else
#define LOCTASK locTask
#endif
#endif

#ifndef ARNL_GPSLOC
  // A callback function, which is called if localization fails
  ArGlobalFunctor1<int> locFailedCB(&locFailed);
  locTask.setFailedCallBack(&locFailedCB); //, &locTask);
#endif

#ifdef ARNL_GPSLOC
  ArLog::log(ArLog::Normal, "Connecting to GPS...");

  // Connect to GPS
  ArGPS *gps = gpsConnector.createGPS(&robot);
  if(!gps || !gps->connect())
  {
    ArLog::log(ArLog::Terse, "Error connecting to GPS device."
      "Try -gpsType, -gpsPort, and/or -gpsBaud command-line arguments."
      "Use -help for help. Exiting.");
    Aria::exit(5);
  }

  // set up GPS localization task
  ArLog::log(ArLog::Normal, "Creating GPS localization task");
  ArGPSLocalizationTask gpsLocTask(&robot, gps, &map);
#ifdef ARNL_MULTILOC
  locManager.addLocalizationTask(&gpsLocTask);
#else
#define LOCTASK gpsLocTask
#endif
#endif

#ifdef ARNL_LASER
  // Set some options  and callbacks on each laser that the laser connector 
  // connected to.
  std::map<int, ArLaser *>::iterator laserIt;
  for (laserIt = robot.getLaserMap()->begin();
       laserIt != robot.getLaserMap()->end();
       laserIt++)
  {
    int laserNum = (*laserIt).first;
    ArLaser *laser = (*laserIt).second;

    // Skip lasers that aren't connected
    if(!laser->isConnected())
      continue;

    // add the disconnectOnError CB to shut things down if the laser
    // connection is lost
    laser->addDisconnectOnErrorCB(&shutdownFunctor);
    // set the number of cumulative readings the laser will take
    laser->setCumulativeBufferSize(200);
    // set the cumulative clean offset (so that they don't all fire at once)
    laser->setCumulativeCleanOffset(laserNum * 100);
    // reset the cumulative clean time (to make the new offset take effect)
    laser->resetLastCumulativeCleanTime();

    // Add the packet count to the Aria info strings (It will be included in
    // MobileEyes custom details so you can monitor whether the laser data is
    // being received correctly)
    std::string laserPacketCountName;
    laserPacketCountName = laser->getName();
    laserPacketCountName += " Packet Count";
    Aria::getInfoGroup()->addStringInt(
	    laserPacketCountName.c_str(), 10, 
	    new ArRetFunctorC<int, ArLaser>(laser, 
					 &ArLaser::getReadingCount));
  }
#endif





    /* Start the server */

  // Open the networking server
  if (!simpleOpener.open(&server, fileDir, 240))
  {
    ArLog::log(ArLog::Normal, "Error: Could not open server.");
    exit(2);
  }



    /* Create various services that provide network access to clients (such as
     * MobileEyes), as well as add various additional features to ARNL */


  robot.unlock();



  
  // Service to provide drawings of data in the map display :
  ArServerInfoDrawings drawings(&server);
  drawings.addRobotsRangeDevices(&robot);

#ifdef ARNL_LASERLOC
  /* Show the sample points used by MCL */
  ArDrawingData drawingDataL("polyDots", ArColor(0,255,0), 100, 75);
  ArFunctor2C<ArLocalizationTask, ArServerClient *, ArNetPacket *> 
    drawingFunctorL(&locTask, &ArLocalizationTask::drawRangePoints);
  drawings.addDrawing(&drawingDataL, "Localization Points", &drawingFunctorL);
#endif

#ifdef ARNL_GPSLOC
  /* Show the positions calculated by GPS localization */

  ArDrawingData drawingDataG("polyDots", ArColor(100,100,255), 130, 61);
  ArFunctor2C<ArGPSLocalizationTask, ArServerClient *, ArNetPacket *> 
    drawingFunctorG(&gpsLocTask, &ArGPSLocalizationTask::drawGPSPoints);
  drawings.addDrawing(&drawingDataG, "GPS Points", &drawingFunctorG);

  ArDrawingData drawingDataG2("polyDots", ArColor(255,100,100), 100, 62);
  ArFunctor2C<ArGPSLocalizationTask, ArServerClient *, ArNetPacket *> 
    drawingFunctorG2(&gpsLocTask, &ArGPSLocalizationTask::drawKalmanPoints);
  drawings.addDrawing(&drawingDataG2, "Kalman Points", &drawingFunctorG2);

  ArDrawingData drawingDataG3("polyDots", ArColor(100,255,100), 70, 63);
  ArFunctor2C<ArGPSLocalizationTask, ArServerClient *, ArNetPacket *> 
    drawingFunctorG3(&gpsLocTask, &ArGPSLocalizationTask::drawOdoPoints);
  drawings.addDrawing(&drawingDataG3, "Odom. Points", &drawingFunctorG3);

  ArDrawingData drawingDataG4("polyDots", ArColor(255,50,50), 100, 75);
  ArFunctor2C<ArGPSLocalizationTask, ArServerClient *, ArNetPacket *> 
    drawingFunctorG4(&gpsLocTask, &ArGPSLocalizationTask::drawKalmanRangePoints);
  drawings.addDrawing(&drawingDataG4, "KalRange Points", &drawingFunctorG4);

  ArDrawingData drawingDataG5("polySegments", ArColor(100,0,255), 1, 78);
  ArFunctor2C<ArGPSLocalizationTask, ArServerClient *, ArNetPacket *> 
    drawingFunctorG5(&gpsLocTask, &ArGPSLocalizationTask::drawKalmanVariance);
  drawings.addDrawing(&drawingDataG5, "VarGPS", &drawingFunctorG5);
#endif

  // "Custom" commands. You can add your own custom commands here, they will
  // be available in MobileEyes' custom commands (enable in the toolbar or
  // access through Robot Tools)
  ArServerHandlerCommands commands(&server);


  // These provide various kinds of information to the client:
  ArServerInfoRobot serverInfoRobot(&server, &robot);
  ArServerInfoSensor serverInfoSensor(&server, &robot);

  // Provides localization info and allows the client (MobileEyes) to relocalize at a given
  // pose:
  ArServerInfoLocalization serverInfoLocalization(&server, &robot, &LOCTASK);
  ArServerHandlerLocalization serverLocHandler(&server, &robot, &LOCTASK);

  // If you're using MobileSim, ArServerHandlerLocalization sends it a command
  // to move the robot's true pose if you manually do a localization through 
  // MobileEyes.  To disable that behavior, use this constructor call instead:
  // ArServerHandlerLocalization serverLocHandler(&server, &robot, true, false);
  // The fifth argument determines whether to send the command to MobileSim.

  // Provide the map to the client (and related controls):
  ArServerHandlerMap serverMap(&server, &map);

  // These objects add some simple (custom) commands to 'commands' for testing and debugging:
  ArServerSimpleComUC uCCommands(&commands, &robot);                   // Send any command to the microcontroller
  ArServerSimpleComMovementLogging loggingCommands(&commands, &robot); // configure logging
  ArServerSimpleComLogRobotConfig configCommands(&commands, &robot);   // trigger logging of the robot config parameters
//  ArServerSimpleServerCommands serverCommands(&commands, &server);     // monitor networking behavior (track packets sent etc.)


  // service that allows the client to monitor the communication link status
  // between the robot and the client.
  //
  ArServerHandlerCommMonitor handlerCommMonitor(&server);



  // service that allows client to change configuration parameters in ArConfig 
  ArServerHandlerConfig handlerConfig(&server, Aria::getConfig(),
				      Arnl::getTypicalDefaultParamFileName(),
				      Aria::getDirectory());


  // This service causes the client to show simple dialog boxes
  ArServerHandlerPopup popupServer(&server);




  /* Set up the possible modes for remote control from a client such as
   * MobileEyes:
   */

  // Mode To stop and remain stopped:
  ArServerModeStop modeStop(&server, &robot);

#ifndef ARNL_SONARLOC
  // Cause the sonar to turn off automatically
  // when the robot is stopped, and turn it back on when commands to move
  // are sent. (Note, if using SONARNL to localize, then don't do this
  // since localization may get lost)
  ArSonarAutoDisabler sonarAutoDisabler(&robot);
#endif

  // Teleoperation modes To drive by keyboard, joystick, etc:
  ArServerModeRatioDrive modeRatioDrive(&server, &robot);  



  // Prevent normal teleoperation driving if localization is lost using
  // a high-priority action, which enables itself when the particular mode is
  // active.
  // (You have to enter unsafe drive mode to drive when lost.)
  ArActionLost actionLostRatioDrive(&LOCTASK, NULL, &modeRatioDrive);
  modeRatioDrive.getActionGroup()->addAction(&actionLostRatioDrive, 110);

  // Add drive mode section to the configuration, and also some custom (simple) commands:
  modeRatioDrive.addToConfig(Aria::getConfig(), "Teleop settings");
  modeRatioDrive.addControlCommands(&commands);

  // Wander mode (also prevent wandering if lost):
  ArServerModeWander modeWander(&server, &robot);
  ArActionLost actionLostWander(&LOCTASK, NULL, &modeWander);
  modeWander.getActionGroup()->addAction(&actionLostWander, 110);

  // Tool to log data periodically to a file
  ArDataLogger dataLogger(&robot, "datalog.txt");
  dataLogger.addToConfig(Aria::getConfig()); // make it configurable through ArConfig

  // Automatically add anything from the global info group to the data logger.
  Aria::getInfoGroup()->addAddStringCallback(dataLogger.getAddStringFunctor());

  // This provides a small table of interesting information for the client
  // to display to the operator. You can add your own callbacks to show any
  // data you want.
  ArServerInfoStrings stringInfo(&server);
  Aria::getInfoGroup()->addAddStringCallback(stringInfo.getAddStringFunctor());
  
  // The following statements add fields to a set of informational data called
  // the InfoGroup. These are served to MobileEyes for displayi (turn on by enabling Details
  // and Custom Details in the View menu of MobileEyes.)

  Aria::getInfoGroup()->addStringInt(
	  "Motor Packet Count", 10, 
	  new ArConstRetFunctorC<int, ArRobot>(&robot, 
					       &ArRobot::getMotorPacCount));

#ifdef ARNL_LASERLOC
  Aria::getInfoGroup()->addStringDouble(
	  "Laser Localization Score", 8, 
	  new ArRetFunctorC<double, ArLocalizationTask>(
		  &locTask, &ArLocalizationTask::getLocalizationScore),
	  "%.03f");
  Aria::getInfoGroup()->addStringInt(
	  "Laser Loc Num Samples", 8, 
	  new ArRetFunctorC<int, ArLocalizationTask>(
		  &locTask, &ArLocalizationTask::getCurrentNumSamples),
	  "%4d");
#elif defined(ARNL_SONARLOC)
  Aria::getInfoGroup()->addStringDouble(
	  "Sonar Localization Score", 8, 
	  new ArRetFunctorC<double, ArSonarLocalizationTask>(
		  &locTask, 
      &ArSonarLocalizationTask::getLocalizationScore),
	  "%.03f");
  Aria::getInfoGroup()->addStringInt(
	  "Sonar Loc Num Samples", 8, 
	  new ArRetFunctorC<int, ArSonarLocalizationTask>(
		  &locTask, &ArSonarLocalizationTask::getCurrentNumSamples),
	  "%4d");
#endif

#ifdef ARNL_GPSLOC
  const char *dopfmt = "%2.4f";
  const char *posfmt = "%2.8f";
  const char *altfmt = "%3.6f m";
  Aria::getInfoGroup()->addStringString(
	    "GPS Fix Mode", 25,
	    new ArConstRetFunctorC<const char*, ArGPS>(gps, &ArGPS::getFixTypeName)
    );
  Aria::getInfoGroup()->addStringInt(
	    "GPS Num. Satellites", 4,
	    new ArConstRetFunctorC<int, ArGPS>(gps, &ArGPS::getNumSatellitesTracked)
    );
  Aria::getInfoGroup()->addStringDouble(
	    "GPS HDOP", 12,
	    new ArConstRetFunctorC<double, ArGPS>(gps, &ArGPS::getHDOP),
      dopfmt
    );
  Aria::getInfoGroup()->addStringDouble(
	    "GPS VDOP", 5,
	    new ArConstRetFunctorC<double, ArGPS>(gps, &ArGPS::getVDOP),
      dopfmt
    );
  Aria::getInfoGroup()->addStringDouble(
	    "GPS PDOP", 5,
	    new ArConstRetFunctorC<double, ArGPS>(gps, &ArGPS::getPDOP),
      dopfmt
    );
  Aria::getInfoGroup()->addStringDouble(
	    "Latitude", 15,
	    new ArConstRetFunctorC<double, ArGPS>(gps, &ArGPS::getLatitude),
      posfmt
    );
  Aria::getInfoGroup()->addStringDouble(
	    "Longitude", 15,
	    new ArConstRetFunctorC<double, ArGPS>(gps, &ArGPS::getLongitude),
      posfmt
    );
  Aria::getInfoGroup()->addStringDouble(
	    "Altitude", 8,
	    new ArConstRetFunctorC<double, ArGPS>(gps, &ArGPS::getAltitude),
      altfmt
    );

  // only some GPS receivers provide these, but you can uncomment them
  // here to enable them if yours does.
  /*
  const char *errfmt = "%2.4f m";
  Aria::getInfoGroup()->addStringDouble(
	    "GPS Lat. Err.", 6,
	    new ArConstRetFunctorC<double, ArGPS>(gps, &ArGPS::getLatitudeError),
      errfmt
    );
  Aria::getInfoGroup()->addStringDouble(
	    "GPS Lon. Err.", 6,
	    new ArConstRetFunctorC<double, ArGPS>(gps, &ArGPS::getLongitudeError),
      errfmt
    );
  Aria::getInfoGroup()->addStringDouble(
	    "GPS Alt. Err.", 6,
	    new ArConstRetFunctorC<double, ArGPS>(gps, &ArGPS::getAltitudeError),
      errfmt
    );
  */

  Aria::getInfoGroup()->addStringDouble(
    "MOGS Localization Score", 8,
    new ArRetFunctorC<double, ArGPSLocalizationTask>(
      &gpsLocTask, &ArGPSLocalizationTask::getLocalizationScore),
    "%.03f"
  );

#endif

  // Display gyro status if gyro is enabled and is being handled by the firmware (gyro types 2, 3, or 4).
  // (If the firmware detects an error communicating with the gyro or IMU it
  // returns a flag, and stops using it.)
  // (This gyro type parameter, and fault flag, are only in ARCOS, not Seekur firmware)
  if(robot.getOrigRobotConfig() && robot.getOrigRobotConfig()->getGyroType() > 1)
  {
    Aria::getInfoGroup()->addStringString(
          "Gyro/IMU Status", 10,
          new ArGlobalRetFunctor1<const char*, ArRobot*>(&getGyroStatusString, &robot)
      );
  }

  // Display system CPU and wireless network status
  ArSystemStatus::startPeriodicUpdate(1000); // update every 1 second
  Aria::getInfoGroup()->addStringDouble("CPU Use", 10, ArSystemStatus::getCPUPercentFunctor(), "% 4.0f%%");
  Aria::getInfoGroup()->addStringInt("Wireless Link Quality", 9, ArSystemStatus::getWirelessLinkQualityFunctor(), "%d");
  Aria::getInfoGroup()->addStringInt("Wireless Link Noise", 9, ArSystemStatus::getWirelessLinkNoiseFunctor(), "%d");
  Aria::getInfoGroup()->addStringInt("Wireless Signal", 9, ArSystemStatus::getWirelessLinkSignalFunctor(), "%d");
  

  // stats on how far its driven since software started
  Aria::getInfoGroup()->addStringDouble("Distance Travelled (m)", 20, new ArRetFunctorC<double, ArRobot>(&robot, &ArRobot::getOdometerDistanceMeters), "%.2f");
  Aria::getInfoGroup()->addStringDouble("Run time (min)", 20, new
ArRetFunctorC<double, ArRobot>(&robot, &ArRobot::getOdometerTimeMinutes),
"%.2f");


#ifdef ARNL_GPSLOC
  // Add some "custom commands" for setting up initial GPS offset and heading.
  gpsLocTask.addLocalizationInitCommands(&commands);
  
  // Add some commands for manually creating map objects based on GPS positions:
//  ArGPSMapTools gpsMapTools(gps, &robot, &commands, &map);

  // Add command to set simulated GPS position manually
  if(gpsConnector.getGPSType() == ArGPSConnector::Simulator)
  {
    ArSimulatedGPS *simGPS = dynamic_cast<ArSimulatedGPS*>(gps);
//    simGPS->setDummyPosition(42.80709, -71.579047, 100);
    commands.addStringCommand("GPS:setDummyPosition", 
      "Manually set a new dummy position for simulated GPS. Provide latitude (required), longitude (required) and altitude (optional)", 
      new ArFunctor1C<ArSimulatedGPS, ArArgumentBuilder*>(simGPS, &ArSimulatedGPS::setDummyPositionFromArgs)
    );
  }
#endif


  // Make Stop mode the default (If current mode deactivates without entering
  // a new mode, then Stop Mode will be selected)
  modeStop.addAsDefaultMode();
    // TODO move up near where stop mode is created?




#ifdef ARNL_MAPPING

  /* Services that allow the client to initiate scanning with the laser to
     create maps in Mapper3 (So not possible with SONARNL): */

  ArServerHandlerMapping handlerMapping(&server, &robot, firstLaser, 
					fileDir, "", true);

#ifdef ARNL_LASERLOC
  // make laser localization stop while mapping
  handlerMapping.addMappingStartCallback(
	  new ArFunctor1C<ArLocalizationTask, bool>
	  (&locTask, &ArLocalizationTask::setIdleFlag, true));

  // and then make it start again when we're doine
  handlerMapping.addMappingEndCallback(
	  new ArFunctor1C<ArLocalizationTask, bool>
	  (&locTask, &ArLocalizationTask::setIdleFlag, false));
#endif

#ifdef ARNL_GPSLOC
  // Save GPS positions in the .2d scan log when making a map
  handlerMapping.addLocationData("robotGPS", 
			    gpsLocTask.getPoseInterpPositionCallback());

  // add the starting latitude and longitude info to the .2d scan log
  handlerMapping.addMappingStartCallback(
	  new ArFunctor1C<ArGPSLocalizationTask, ArServerHandlerMapping *>
	  (&gpsLocTask, &ArGPSLocalizationTask::addScanInfo, 
	   &handlerMapping));
#endif

  // Make it so our "lost" actions don't stop us while mapping
  handlerMapping.addMappingStartCallback(actionLostRatioDrive.getDisableCB());
  handlerMapping.addMappingStartCallback(actionLostWander.getDisableCB());

  // And then let them make us stop as usual when done mapping
  handlerMapping.addMappingEndCallback(actionLostRatioDrive.getEnableCB());
  handlerMapping.addMappingEndCallback(actionLostWander.getEnableCB());

#endif // ARNL_MAPPING


  /*
  // If we are on a simulator, move the robot back to its starting position,
  // and reset its odometry.
  // This will allow localizeRobotAtHomeBlocking() below will (probably) work (it
  // tries current odometry (which will be 0,0,0) and all the map
  // home points.
  // (Ignored by a real robot)
  //robot.com(ArCommands::SIM_RESET);
  */


  // create a pose storage class, this will let the program keep track
  // of where the robot is between runs...  after we try and restore
  // from this file it will start saving the robot's pose into the
  // file
  ArPoseStorage poseStorage(&robot);
  /// if we could restore the pose from then set the sim there (this
  /// won't do anything to the real robot)... if we couldn't restore
  /// the pose then just reset the position of the robot (which again
  /// won't do anything to the real robot)
  if (poseStorage.restorePose("robotPose"))
    serverLocHandler.setSimPose(robot.getPose());
  //else
 //   robot.com(ArCommands::SIM_RESET);



  /* File transfer services: */
  
#pragma GPP off
#ifdef WIN32
  // Not implemented for Windows yet.
  ArLog::log(ArLog::Normal, "Note, file upload/download services are not implemented for Windows; not enabling them.");
#else
  // This block will allow you to set up where you get and put files
  // to/from, just comment them out if you don't want this to happen
  // /*
  ArServerFileLister fileLister(&server, fileDir);
  ArServerFileToClient fileToClient(&server, fileDir);
  ArServerFileFromClient fileFromClient(&server, fileDir, "/tmp");
  ArServerDeleteFileOnServer deleteFileOnServer(&server, fileDir);
  // */
#endif
#pragma GPP on

    /* Video image streaming, and camera controls (Requires SAVserver or ACTS) */

  // Forward one video stream if either ACTS, ArVideo videoSubServer, 
  // or SAV server are running.
  // ArHybridForwarderVideo allows this program to be separate from the ArVideo
  // library. You could replace videoForwarder and the PTZ connection code below
  // with a call to ArVideo::createVideoServers(), and link the program to the
  // ArVideo library if you want to include video capture in the same program
  // as robot control.
  ArHybridForwarderVideo videoForwarder(&server, "localhost", 7070);
  
  // connect to first configured camera PTZ controls (in robot parameter file and
  // command line options)
  ptzConnector.connect();
  ArCameraCollection cameraCollection;
  ArPTZ *ptz = ptzConnector.getPTZ(0);
  if(ptz)
  {
    ArLog::log(ArLog::Normal, "Connected to PTZ Camera");
    cameraCollection.addCamera("Camera1", ptz->getTypeName(), "Camera", ptz->getTypeName());

    videoForwarder.setCameraName("Camera1");
    videoForwarder.addToCameraCollection(cameraCollection);

    new ArServerHandlerCamera("Camera1", 
      &server, 
      &robot,
      ptz, 
      &cameraCollection);

  } 

  // Allows client to find any camera servers created above
  ArServerHandlerCameraCollection cameraCollectionServer(&server, &cameraCollection);



    /* Load configuration values, map, and begin! */

  
  // When parsing the configuration file, also look at the program's command line options 
  // from the command-line argument parser as well as the configuration file.
  // (So you can use any argument on the command line, namely -map.) 
  Aria::getConfig()->useArgumentParser(&parser);

  // Read in parameter files.
  ArLog::log(ArLog::Normal, "Loading config file %s%s into ArConfig...", Aria::getDirectory(), Arnl::getTypicalParamFileName());
  if (!Aria::getConfig()->parseFile(Arnl::getTypicalParamFileName()))
  {
    ArLog::log(ArLog::Normal, "Could not load ARNL configuration file. Set ARNL environment variable to use non-default installation director.y");
    Aria::exit(5);
  }

  // Warn about unknown params.
  if (!simpleOpener.checkAndLog() || !parser.checkHelpAndWarnUnparsed())
  {
    logOptions(argv[0]);
    Aria::exit(6);
  }

  // Warn if there is no map
  if (map.getFileName() == NULL || strlen(map.getFileName()) <= 0)
  {
    ArLog::log(ArLog::Normal, "");
    ArLog::log(ArLog::Normal, "### No map file is set up, you can make a map with the following procedure");
#ifdef ARNL
    ArLog::log(ArLog::Normal, "   0) You can find this information in README.txt or docs/Mapping.txt");
    ArLog::log(ArLog::Normal, "   1) Connect to this server with MobileEyes");
    ArLog::log(ArLog::Normal, "   2) Go to Tools->Map Creation->Start Scan");
    ArLog::log(ArLog::Normal, "   3) Give the map a name and hit okay");
    ArLog::log(ArLog::Normal, "   4) Drive the robot around your space (see docs/Mapping.txt");
    ArLog::log(ArLog::Normal, "   5) Go to Tools->Map Creation->Stop Scan");
    ArLog::log(ArLog::Normal, "   6) Start up Mapper3");
    ArLog::log(ArLog::Normal, "   7) Go to File->Open on Robot");
    ArLog::log(ArLog::Normal, "   8) Select the .2d you created");
    ArLog::log(ArLog::Normal, "   9) Create a .map");
    ArLog::log(ArLog::Normal, "  10) Go to File->Save on Robot");
    ArLog::log(ArLog::Normal, "  11) In MobileEyes, go to Tools->Robot Config");
    ArLog::log(ArLog::Normal, "  12) Choose the Files section");
    ArLog::log(ArLog::Normal, "  13) Enter the path and name of your new .map file for the value of the Map parameter.");
    ArLog::log(ArLog::Normal, "  14) Press OK and your new map should become the map used");
    ArLog::log(ArLog::Normal, "");    
#elif defined(SONARNL)
    ArLog::log(ArLog::Normal, "   0) You can find this information in README.txt or docs/SonarMapping.txt");
    ArLog::log(ArLog::Normal, "   1) Start up Mapper3Basic");
    ArLog::log(ArLog::Normal, "   2) Go to File->New");
    ArLog::log(ArLog::Normal, "   3) Draw a line map of your area (make sure it is to scale)");
    ArLog::log(ArLog::Normal, "   4) Go to File->Save on Robot");
    ArLog::log(ArLog::Normal, "   5) In MobileEyes, go to Tools->Robot Config");
    ArLog::log(ArLog::Normal, "   6) Choose the Files section");
    ArLog::log(ArLog::Normal, "   7) Enter the path and name of your new .map file for the value of the Map parameter.");
    ArLog::log(ArLog::Normal, "   8) Press OK and your new map should become the map used");
    ArLog::log(ArLog::Normal, "");    
#endif
#ifdef ARNL_GPSLOC
    ArLog::log(ArLog::Normal, "\n   See docs/GPSMapping.txt for instructions on creating a map for GPS localization");
#endif
  }

  // Print a log message notifying user of the directory for map files
  ArLog::log(ArLog::Normal, "");
  ArLog::log(ArLog::Normal, 
	     "Directory for maps and file serving: %s", fileDir);
  
  ArLog::log(ArLog::Normal, "See the ARNL README.txt for more information");
  ArLog::log(ArLog::Normal, "");

  // Do an initial localization of the robot. ARNL and SONARNL try all the home points
  // in the map, as well as the robot's current odometric position, as possible
  // places the robot is likely to be at startup.   If successful, it will
  // also save the position it found to be the best localized position as the
  // "Home" position, which can be obtained from the localization task (and is
  // used by the "Go to home" network request).
  // MOGS instead just initializes at the current GPS position.
  // (You will stil have to drive the robot so it can determine the robot's
  // heading, however. See GPS Mapping instructions.)
  LOCTASK.localizeRobotAtHomeBlocking();
  
#ifdef ARNL_MULTIROBOT
  // Let the client switch manager (for multirobot) spin off into its own thread
  // TODO move to multirobot example?
  clientSwitch.runAsync();
#endif

  // Start the networking server's thread
  server.runAsync();

  ArLog::log(ArLog::Normal, "Server running. To exit, press CTRL-C.");

  // Enable the motors and wait until the robot exits (disconnection, etc.) or this program is
  // canceled.
  robot.enableMotors();
  robot.waitForRunExit();
  Aria::exit(0);
}
Пример #12
0
void Joydrive::drive(void)
{
  int trans, rot;
  ArPose pose;
  ArPose rpose;
  ArTransform transform;
  ArRangeDevice *dev;
  ArSensorReading *son;

  if (!myRobot->isConnected())
  {
    printf("Lost connection to the robot, exiting\n");
    exit(0);
  }
  printf("\rx %6.1f  y %6.1f  th  %6.1f", 
	 myRobot->getX(), myRobot->getY(), myRobot->getTh());
  fflush(stdout);
  if (myJoyHandler.haveJoystick() && myJoyHandler.getButton(1))
  {
    if (ArMath::fabs(myRobot->getVel()) < 10.0)
      myRobot->comInt(ArCommands::ENABLE, 1);
    myJoyHandler.getAdjusted(&rot, &trans);
    myRobot->setVel(trans);
    myRobot->setRotVel(-rot);
  }
  else
  {
    myRobot->setVel(0);
    myRobot->setRotVel(0);
  }
  if (myJoyHandler.haveJoystick() && myJoyHandler.getButton(2) &&
      time(NULL) - myLastPress > 1)
  {
    myLastPress = time(NULL);
    printf("\n");
    switch (myTest)
    {
    case 1:
      printf("Moving back to the origin.\n");
      pose.setPose(0, 0, 0);
      myRobot->moveTo(pose);
      break;
    case 2:
      printf("Moving over a meter.\n");
      pose.setPose(myRobot->getX() + 1000, myRobot->getY(), 0);
      myRobot->moveTo(pose);
      break;
    case 3:
      printf("Doing a transform test....\n");
      printf("\nOrigin should be transformed to the robots coords.\n");
      transform = myRobot->getToGlobalTransform();
      pose.setPose(0, 0, 0);
      pose = transform.doTransform(pose);
      rpose = myRobot->getPose();
      printf("Pos:  ");
      pose.log();
      printf("Robot:  ");
      rpose.log();

      if (pose.findDistanceTo(rpose) < .1)
	printf("Success\n");
      else
	printf("#### FAILURE\n");
    
      printf("\nRobot coords should be transformed to the origin.\n");
      transform = myRobot->getToLocalTransform();
      pose = myRobot->getPose();
      pose = transform.doTransform(pose);
      rpose.setPose(0, 0, 0);
      printf("Pos:  ");
      pose.log();
      printf("Robot:  ");
      rpose.log();
      if (pose.findDistanceTo(rpose) < .1)
	printf("Success\n");
      else
	printf("#### FAILURE\n");
      break;
    case 4:
      printf("Doing a tranform test...\n");
      printf("A point 1 meter to the -x from the robot (in local coords) should be transformed into global coordinates.\n");
      transform = myRobot->getToGlobalTransform();
      pose.setPose(-1000, 0, 0);
      pose = transform.doTransform(pose);
      rpose = myRobot->getPose();
      printf("Pos:  ");
      pose.log();
      printf("Robot:  ");
      rpose.log();

      if (ArMath::fabs(pose.findDistanceTo(rpose) - 1000.0) < .1)
	printf("Probable Success\n");
      else
	printf("#### FAILURE\n");
      break;
    case 5:
      printf("Doing a transform test on range devices..\n");
      printf("Moving the robot +4 meters x and +4 meters y and seeing if the moveTo will move the sonar readings along with it.\n");
      dev = myRobot->findRangeDevice("sonar");
      if (dev == NULL)
      {
	printf("No sonar on the robot, can't do the test.\n");
	break;
      }
      printf("Closest sonar reading to the robot is %.0f away\n", dev->currentReadingPolar(1, 0));
      printf("Sonar 0 reading is at ");
      son = myRobot->getSonarReading(0);
      if (son != NULL)
      {
	pose = son->getPose();
	pose.log();
      }
      pose = myRobot->getPose();
      pose.setX(pose.getX() + 4000);
      pose.setY(pose.getY() + 4000);
      myRobot->moveTo(pose);
      printf("Moved robot.\n");
      printf("Closest sonar reading to the robot is %.0f away\n", dev->currentReadingPolar(1, 0));
      printf("Sonar 0 reading is at ");
      son = myRobot->getSonarReading(0);
      if (son != NULL)
      {
	pose = son->getPose();
	pose.log();
      }

      break;
    case 6:
      printf("Robot position now is:\n");
      pose = myRobot->getPose();
      pose.log();
      printf("Disconnecting from the robot, then reconnecting.\n");
      myRobot->disconnect();
      myRobot->blockingConnect();      
      printf("Robot position now is:\n");
      pose = myRobot->getPose();
      pose.log();
      break;
    default:
      printf("No test for second button.\n");
      break;
    } 
  }
}
Пример #13
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;
}
Пример #14
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.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;
  }

  if (robot->areSonarsEnabled())
  {
    int i = 0;
    int j = 0;
    ArSensorReading* reading = NULL;
    if(sonars__crossed_the_streams)
    {
      i = 8;
      j = 8;
    }
    
    for(; i < 16; i++)
    {
      ranges.data[i].header.stamp = ros::Time::now();
      
      ArSensorReading* _reading = NULL;
      _reading = robot->getSonarReading(i-j);
      ranges.data[i].range = _reading->getRange() / 1000.0f;
      range_pub[i].publish(ranges.data[i]);
    }
    ranges.header.stamp = ros::Time::now();
    combined_range_pub.publish(ranges);
  }  
}
Пример #15
0
void readPosition(ArRobot& robot){
	ArPose pose = robot.getPose();
	fseek(G_pose_fd, SEEK_SET, 0);
	fprintf(G_pose_fd, "x=%0.6f;y=%0.6f;th=%0.6f;\n", pose.getX(), pose.getY(), pose.getTh());
}