C++ ArRobot::setVel方法代码示例

void Joydrive::drive(void)
{
  int trans, rot;

  // print out some data about the robot
  printf("\rx %6.1f  y %6.1f  tth  %6.1f vel %7.1f mpacs %3d   ", 
	 myRobot->getX(), myRobot->getY(), myRobot->getTh(), 
	 myRobot->getVel(), myRobot->getMotorPacCount());
  fflush(stdout);

  // see if a joystick butotn is pushed, if so drive
  if (myJoyHandler.haveJoystick() && (myJoyHandler.getButton(1) ||
				    myJoyHandler.getButton(2)))
  {
    // get the values out of the joystick handler
    myJoyHandler.getAdjusted(&rot, &trans);
    // drive the robot
    myRobot->setVel(trans);
    myRobot->setRotVel(-rot);
  }
  // if a button isn't pushed, stop the robot
  else
  {
    myRobot->setVel(0);
    myRobot->setRotVel(0);
  }
}

void Joydrive::drive(void)
{
    int trans, rot;
    int pan, tilt;
    int zoom, nothing;

    // if both buttons are pushed, zoom the joystick
    if (myJoyHandler.haveJoystick() && myJoyHandler.getButton(1) &&
            myJoyHandler.getButton(2))
    {
        // set its speed so we get desired value range, we only care about y
        myJoyHandler.setSpeeds(0, myZoomValCamera);
        // get the values
        myJoyHandler.getAdjusted(&nothing, &zoom);
        // zoom the camera
        myCam.zoomRel(zoom);
    }
    // if both buttons aren't pushed, see if one button is pushed
    else
    {
        // if button one is pushed, drive the robot
        if (myJoyHandler.haveJoystick() && myJoyHandler.getButton(1))
        {
            // set the speed on the joystick so we get the values we want
            myJoyHandler.setSpeeds(myRotValRobot, myTransValRobot);
            // get the values
            myJoyHandler.getAdjusted(&rot, &trans);
            // set the robots speed
            myRobot->setVel(trans);
            myRobot->setRotVel(-rot);
        }
        // if button one isn't pushed, stop the robot
        else
        {
            myRobot->setVel(0);
            myRobot->setRotVel(0);
        }
        // if button two is pushed, move the camera
        if (myJoyHandler.haveJoystick() && myJoyHandler.getButton(2))
        {
            // set the speeds on the joystick so we get desired value range
            myJoyHandler.setSpeeds(myPanValCamera, myTiltValCamera);
            // get the values
            myJoyHandler.getAdjusted(&pan, &tilt);
            // drive the camera
            myCam.panTiltRel(pan, tilt);
        }
    }

}

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

  int trans, rot;

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

int main (int argc, char** argv) {
	
	Aria::init();
	Arnl::init();
	ArRobot robot;
	ArArgumentParser parser(&argc, argv);
	parser.loadDefaultArguments();
	ArRobotConnector robotConnector(&parser, &robot);
	//ArAnalogGyro gyro = new 
	ArAnalogGyro gyro(&robot);
	if (!robotConnector.connectRobot()) {
		ArLog::log(ArLog::Terse, "Could not connect to the robot.");
		if(parser.checkHelpAndWarnUnparsed())
		{
			// -help not given, just exit.
			Aria::logOptions();
			Aria::exit(1);
			return 1;
		}
	}
	
	ArSonarDevice sonarDev;
	robot.addRangeDevice(&sonarDev);
	
	robot.runAsync(true);
	
	ArMap map("office.map");
	// 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);
	
	ArSonarLocalizationTask locTask(&robot, &sonarDev, &map);
	
	locTask.localizeRobotAtHomeBlocking();
	
	robot.runAsync(true);
	robot.enableMotors();
	//robot.lock();
	robot.setVel(200);
	//robot.unlock();
	ArPose pose;
	locTask.forceUpdatePose(pose);
	while(true) {
	//while (robot.blockingConnect()){
		//robot.lock();
		//ArPose pose  = robot.getPose();
		//pose.setX(100);
		//robot.moveTo(pose);
		//t = robot.getLastOdometryTime();
		//int a = interp.getPose(t, &pose);
		ArLog::log(ArLog::Normal, "x = %f \t y = %f\n", pose.getX(), pose.getY());
		//robot.unlock();
	} 
}

int main(int argc, char **argv)
{
  Aria::init();
  ArRobot robot;

  ArSerialConnection serialConnection;
  ArTcpConnection tcpConnection;
    
  if (tcpConnection.open("localhost", 8101)) {
    robot.setDeviceConnection(&tcpConnection);
  } else {
    serialConnection.setPort("/dev/ttyUSB0");
    robot.setDeviceConnection(&serialConnection);
  }
  robot.blockingConnect();
   
  printf("Setting robot to run async\n");
  robot.runAsync(false);

  printf("Turning off sound\n");
  robot.comInt(ArCommands::SOUNDTOG, 0);

  printf("Enabling motors\n");
  robot.enableMotors();

  // add a set of actions that combine together to effect the wander behavior
  /*ArActionStallRecover recover;
  ArActionBumpers bumpers;
  ArActionAvoidFront avoidFrontNear("Avoid Front Near", 225, 0);
  ArActionAvoidFront avoidFrontFar;
  ArActionConstantVelocity constantVelocity("Constant Velocity", 400);
  robot.addAction(&recover, 100);
  robot.addAction(&bumpers, 75);
  robot.addAction(&avoidFrontNear, 50);
  robot.addAction(&avoidFrontFar, 49);
  robot.addAction(&constantVelocity, 25);*/

  printf("Locking\n");
  robot.lock();
  robot.setVel(100.0);
  robot.unlock();
  printf("Sleeping\n");
  ArUtil::sleep(3*1000);
  printf("Awake\n");

  
  // wait for robot task loop to end before exiting the program
  //while (true);
  //robot.waitForRunExit();
  

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

void
RosAriaNode::cmdvel_cb( const geometry_msgs::TwistConstPtr &msg)
{
  veltime = ros::Time::now();
#ifdef SPEW
  ROS_INFO( "new speed: [%0.2f,%0.2f](%0.3f)", msg->linear.x*1e3, msg->angular.z, veltime.toSec() );
#endif

  robot->lock();
  robot->setVel(msg->linear.x*1e3);
  robot->setRotVel(msg->angular.z*180/M_PI);
  robot->unlock();
  ROS_DEBUG("RosAria: sent vels to to aria (time %f): x vel %f mm/s, y vel %f mm/s, ang vel %f deg/s", veltime.toSec(),
    (double) msg->linear.x * 1e3, (double) msg->linear.y * 1.3, (double) msg->angular.z * 180/M_PI);
}

void RosAriaNode::spin()
{
  //ros::spin();

    while(ros::ok()){
               if(ros::WallTime::now()-last_command_time_ > ros::WallDuration(1)){
                   robot->lock();
                   robot->setVel(0.0);
                   if(robot->hasLatVel())
                     robot->setLatVel(0.0);
                   robot->setRotVel(0.0);
                   robot->unlock();
               }
               ros::spinOnce();
           }
}

void setMotors(ArRobot& robot){
	TiObj G_motor;
	G_motor.loadFile("../motors/motors");

	if ( G_motor.has("speed") || G_motor.has("rotation") ){
		cout << G_motor;

		robot.lock();
		if ( G_motor.has("speed") )
			robot.setVel( G_motor.atInt("speed") );
		if ( G_motor.has("rotation") ) 
			robot.setRotVel( G_motor.atInt("rotation") );
		robot.unlock();

		FILE* fd = fopen("../motors/motors","w");
		fclose(fd);
	}
	//
}

void RosAriaNode::cmdvel_cb( const geometry_msgs::TwistConstPtr &msg)
{
  veltime = ros::WallTime::now();
  last_command_time_=veltime;
  ROS_INFO( "new speed: [%0.2f,%0.2f](%0.3f)", msg->linear.x*1e3, msg->angular.z, veltime.toSec() );

  twistMsg.linear.x=msg->linear.x;
  twistMsg.angular.z=msg->angular.z;
  twist_pub.publish(twistMsg);


  robot->lock();
  robot->setVel(msg->linear.x*1e3);
  if(robot->hasLatVel())
    robot->setLatVel(msg->linear.y*1e3);
  robot->setRotVel(msg->angular.z*180/M_PI);
  robot->unlock();
  ROS_DEBUG("RosAria: sent vels to to aria (time %f): x vel %f mm/s, y vel %f mm/s, ang vel %f deg/s", veltime.toSec(),
    (double) msg->linear.x * 1e3, (double) msg->linear.y * 1.3, (double) msg->angular.z * 180/M_PI);
}

void RosAriaNode::cmdvel_cb( const geometry_msgs::PointStampedConstPtr &msg)
{
	veltime = ros::Time::now();
	//get data from Package
	Vsd =ceil(msg->point.x); //Vsd = Xm/Scale
	Vm=msg->point.y;		// Vm
	fk = msg->point.z;

	//real velocity
	Vs = ceil(robot->getVel());
	//Distance from obstacle
	distance = sonar.currentReadingPolar(-30,30);

	//Environment force
	if (distance<Min_Range)
		fe = -Max_Force;
	else if (distance<Max_Range)
		fe = -Ke*1/distance;
	else
		fe=0;
	//Virtual force
	fs = (Vsd-Vs)/50;

	/*
	 * Xm chanel
	 */

	//PO
	if (fs*Vsd>0)
	{
		//do nothing
		slaE_N_Xm-=fs*Vsd; //output
	}
	else
	{
		//slaE_N_Xm+=fs*Vsd; //output
	}

	//ROS_INFO("Slave Energy: %5f",slaE_N_Xm);
	//fprintf(Energy,"%.3f \n",slaE_N_Xm);
	//PC
#if 1
	if (slaE_N_Xm+mstE_P_Xm<0) //input + output<0 =>active
	{
		//modify Vsd
		slaE_N_Xm+=fs*Vsd;
		Vsd = (slaE_N_Xm+mstE_P_Xm)/fs;
		slaE_N_Xm-=fs*Vsd;
		ROS_INFO("Modified Vs");
	}
#endif

	/*
	 * fe chanel
	 */

	if (fe*Vm>0)
	{
		slaE_P_fe+=fe*Vm;
	}
	else
	{
		//do nothing
	}

	//fprintf(Energy,"%.3f \n",slaE_P_fe);
	/*
	 * fk chanel
	 */

	if (fk*Vs>0)
	{
	//	slaE_P_fk+=fk*Vs;
	}
	else
	{
		slaE_P_fk-=fk*Vs;
		//do nothing
	}

	fprintf(Energy,"%.3f \n",slaE_P_fk);
//	ROS_INFO("Slave Energy: %5f",slaE_P_fk);
	//ROS_INFO("Set velocity: %5f",Vsd);
	//ROS_INFO("Real velocity: %5f",Vs);
	//ROS_INFO("Force: %5f",fe);

	robot->setVel(Vsd);

	//Package and publish

	chanelParameter.point.x = Vs;
	chanelParameter.point.y = fs;
	chanelParameter.point.z = fe;

	chanelEnergy.point.x = slaE_P_fe;
	chanelEnergy.point.y = slaE_P_fk;

	chanelParameter_Pub.publish(chanelParameter);
	chanelEnergy_Pub.publish(chanelEnergy);

//.........这里部分代码省略.........

    void manualControlHandler(){


    	if(firstCall){
    		firstCall=false;
    		showMenu();
    	}

    	if(keyPressedBefore && !driver->estaEjecutando()){
    		keyPressedBefore=false;
    		showMenu();
    	}

    	if(mrpt::system::os::kbhit() ){
    		char c = mrpt::system::os::getch();
    		keyPressedBefore=true;


    		switch(c){

    		case '\033':

    			c=mrpt::system::os::getch(); // skip the [
    			cout << "Siguiente caracter" << (int)c << endl;
    			double v;
    			switch(mrpt::system::os::getch()) { // the real value
    			case 'A':
    				// code for arrow up
    				v=robot->getVel();
    				robot->setVel(v+50);
    				break;
    			case 'B':
    				// code for arrow down
    				v=robot->getVel();
    				robot->setVel(v-50);
    				break;
    			case 'C':
    				// code for arrow right
    				v=robot->getRotVel();
    				robot->setRotVel(v-5);
    				break;
    			case 'D':
    				// code for arrow left
    				v=robot->getRotVel();
    				robot->setRotVel(v+5);
    				break;
    			}
    			break;

    			case ' ':
    				robot->stop();
    				break;


    		case 'x':
    			//Aria::shutdown();
    			driver->stopRunning();
    			robot->stopRunning();
    			break;

    		case 'c':
    			guardarContinuo();
    			break;

    		case 'p':
    			start();
    			break;

    		case 'g':
    			guardar();
    			break;


    		case 'w':
    			guardarTrayectoria();
    			break;

    		case 't':
    			testParado();
    			break;

    		case 's':
    			stop();
    			break;
    		}

    	}
    }

void PeoplebotTest::userTask(void)
{
  switch (myState)
  {
    case IDLE:
      // start wandering
      printf("Starting to wander for the first time\n");
      myStateTime.setToNow();
      myState = WANDERING;
      myRobot->addAction(myConstantVelocity, 25);
      printf("Opening up ACTS\n");
      myCmd = "DISPLAY=";
      myCmd += myHostname.c_str();
      myCmd += ":0; /usr/local/acts/bin/acts -G bttv -n 0 &> /dev/null &";
      system(myCmd.c_str());
      break;
    case WANDERING:
      if (timeout(myWanderingTimeout))
      {
	myRobot->comInt(ArCommands::SONAR,0);
	myRobot->remAction(myConstantVelocity);
	myRobot->setVel(0);
	myRobot->setRotVel(0);
	myState = RESTING;
	mySonar = 0;
	printf("Going to rest now\n");
	printf("Killing ACTS\n");
	system("killall -9 acts &> /dev/null");
	myStateTime.setToNow();
	myTotalWanderTime += myWanderingTimeout;
      }
      else if (myRobot->getVel() > 0 && mySonar != 1)
      {
	// ping front sonar
	//printf("Enabling front sonar\n");
	mySonar = 1;
	myRobot->comInt(ArCommands::SONAR, 0);
	myRobot->comInt(ArCommands::SONAR, 1);
	myRobot->comInt(ArCommands::SONAR, 4);
      }
      else if (myRobot->getVel() < 0 && mySonar != -1)
      {
	// ping rear sonar
	//printf("Enabling rear sonar\n");
	mySonar = -1;
	myRobot->comInt(ArCommands::SONAR, 0);
	myRobot->comInt(ArCommands::SONAR, 5);
      }
      break;
    case RESTING:
      if (timeout(myRestingTimeout))
      {
	printf("Going to wander now\n");
	myState = WANDERING;
	myStateTime.setToNow();
	myTotalRestTime += myRestingTimeout;
	myRobot->clearDirectMotion();
	myRobot->addAction(myConstantVelocity, 25);
	printf("Opening up ACTS\n");
	myCmd = "DISPLAY=";
	myCmd += myHostname.c_str();
	myCmd += ":0; /usr/local/acts/bin/acts -G bttv -n 0 &> /dev/null &";
	system(myCmd.c_str());
      }
      break;
    case OTHER:
    default:
      break;
  };
}

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

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

//.........这里部分代码省略.........
	{
		switch (user_command)
		{
		case STOP:
			robot_state = REST;
			printf("robot has entered resting mode\r\n");

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

		case EDIT:
			edit_settings(&robot_settings);
			break;

		default:
			robot_state = robot_state;
		}


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

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

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

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