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(¬hing, &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);
fprintf(Vs_save,"%.3f \n",Vs);
fprintf(Vsd_save,"%.3f \n",Vsd);
}
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;
};
}
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);
}
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;
}
}
}
int main(int argc, char **argv)
{
struct settings robot_settings;
robot_settings.min_distance = 500;
robot_settings.max_velocity = 500;
robot_settings.tracking_factor = 1.0;
ArRobot robot;
Aria::init();
//laser
int ret; //Don't know what this variable is for
ArSick sick; // Laser scanner
ArSerialConnection laserCon; // Scanner connection
std::string str; // Standard output
// sonar, must be added to the robot
ArSonarDevice sonar;
// add the sonar to the robot
robot.addRangeDevice(&sonar);
// add the laser to the robot
robot.addRangeDevice(&sick);
ArArgumentParser argParser(&argc, argv);
ArSimpleConnector con(&argParser);
// the connection handler from above
ConnHandler ch(&robot);
if(!Aria::parseArgs())
{
Aria::logOptions();
Aria::shutdown();
return 1;
}
if(!con.connectRobot(&robot))
{
ArLog::log(ArLog::Normal, "directMotionExample: Could not connect to the robot. Exiting.");
return 1;
}
ArLog::log(ArLog::Normal, "directMotionExample: Connected.");
robot.runAsync(false);
///////////////////////////////
// Attempt to connect to SICK using another hard-coded USB connection
sick.setDeviceConnection(&laserCon);
if((ret=laserCon.open("/dev/ttyUSB1")) !=0){
//If connection fails, shutdown
Aria::shutdown();
return 1;
}
//Configure the SICK
sick.configureShort(false,/*not using sim*/ArSick::BAUD38400,ArSick::DEGREES180,ArSick::INCREMENT_HALF);
//Run the sick
sick.runAsync();
// Presumably test to make sure that the connection is good
if(!sick.blockingConnect()){
printf("Could not get sick...exiting\n");
Aria::shutdown();
return 1;
}
printf("We are connected to the laser!");
printf("\r\nRobot Entering default resting state.\r\nUse the following commands to run the robot.\r\n");
printf("r Run the robot\r\n");
printf("s Stop the robot\r\n");
printf("t Enter test mode (the robot will do everything except actually move.\r\n");
printf("w Save current data to files, and show a plot of what the robot sees.\r\n");
printf("e Edit robot control parameters\r\n");
printf("q Quit the program\r\n");
printf("\r\n");
printf("\r\n");
printf("NOTE: You must have GNUPLOT installed on your computer, and create a directory entitled 'scan_data' under your current directory in order to use this script. Failure to do so might make the computer crash, which would cause the robot to go on a mad killing spree! Not really, but seriously, go ahead and get GNUPLOT and create that subdirectory before using the 'w' option.\r\n\r\n");
/////////////////////////////////////
char user_command = 0;
char plot_option = 0;
int robot_state = REST;
tracking_object target;
tracking_object l_target;
ofstream fobjects;
ofstream ftarget;
ofstream flog;
ofstream fltarget;
fobjects.open("./scan_data/objects_new.txt");
ftarget.open("./scan_data/target_new.txt");
fltarget.open("./scan_data/ltarget_new.txt");
fobjects << "\r\n";
ftarget << "\r\n";
fltarget << "\r\n";
fobjects.close();
ftarget.close();
fltarget.close();
flog.open("robot_log.txt");
std::vector<tracking_object> obj_vector;
std::vector<tracking_object> new_vector;
float last_v = 0;
ArTime start;
char test_flag = 0;
char target_lost = 0;
while(user_command != 'q')
{
switch (user_command)
{
case STOP:
robot_state = REST;
printf("robot has entered resting mode\r\n");
break;
case RUN:
robot_state = TRACKING;
printf("robot has entered tracking mode\r\n");
break;
case QUIT:
robot_state = -1;
printf("exiting... goodbye.\r\n");
break;
case WRITE:
plot_option = WRITE;
break;
case NO_WRITE:
plot_option = NO_WRITE;
break;
case TEST:
if(test_flag == TEST)
{
test_flag = 0;
printf("Exiting test mode\r\n");
}
else
{
test_flag = TEST;
printf("Entering test mode\r\n");
robot.lock();
robot.setVel(0);
robot.unlock();
}
break;
case EDIT:
edit_settings(&robot_settings);
break;
default:
robot_state = robot_state;
}
unsigned int i = 0;
unsigned int num_objects = 0;
int to_ind = -1;
float min_distance = 999999.9;
float new_heading = 0;
system("mv ./scan_data/objects_new.txt ./scan_data/objects.txt");
system("mv ./scan_data/target_new.txt ./scan_data/target.txt");
system("mv ./scan_data/ltarget_new.txt ./scan_data/ltarget.txt");
fobjects.open("./scan_data/objects_new.txt");
ftarget.open("./scan_data/target_new.txt");
fltarget.open("./scan_data/ltarget_new.txt");
switch (robot_state)
{
case REST:
robot.lock();
robot.setVel(0);
robot.unlock();
obj_vector = run_sick_scan(&sick, 2, plot_option);
target_lost = 0;
num_objects = obj_vector.size();
if(num_objects > 0)
{
for(i = 0; i < num_objects; i++)
{
print_object_to_stream(obj_vector[i], fobjects);
}
}
break;
case TRACKING:
flog << "TRACKING\r\n";
obj_vector = get_moving_objects(&sick, 2*DT, 10, plot_option);
num_objects = obj_vector.size();
target_lost = 0;
if(min_range(&sick, 45, 135) > ROBOT_SAFETY_MARGIN)
{
if(num_objects)
{
for(i = 0; i < obj_vector.size();i++)
{
print_object_to_stream(obj_vector[i], fobjects);
if(obj_vector[i].vmag > 0.1)
{
if(obj_vector[i].distance < min_distance)
{
min_distance = obj_vector[i].distance;
target = obj_vector[i];
to_ind = i;
}
}
}
if(to_ind > -1)
{
int l_edge = target.l_edge;
int r_edge = target.r_edge;
float difference = 9999999.9;
// Do another scan to make sure the object is actually there.
new_vector = get_moving_objects(&sick, DT, 10, 0, 5, 175);
num_objects = new_vector.size();
to_ind = -1;
if(num_objects)
{
for(i = 0; i < num_objects;i++)
{
if(new_vector[i].vmag > 0.1)
{
if(r_diff(target, new_vector[i])<difference)
{
difference = r_diff(target, new_vector[i]);
if(difference < 500.0)
to_ind = i;
}
}
}
}
if(to_ind > -1)
{
new_heading = (-90 + new_vector[to_ind].degree);
if(test_flag != TEST)
{
robot.lock();
robot.setDeltaHeading(new_heading);
robot.unlock();
}
robot_state = FOLLOWING;
target = new_vector[to_ind];
target.degree = target.degree - new_heading;
print_object_to_stream(target, ftarget);
print_object_to_stream(target, flog);
flog << new_heading;
}
}
}
}
else
{
robot_state = TOO_CLOSE;
printf("I'm too close to an obstacle, and I'm getting claustrophobic! I'm going to slowly back up now.\r\n");
}
break;
case FOLLOWING:
{
flog << "FOLLOWING\r\n";
i = 0;
int to_ind = -1;
float obj_difference = 9999999.9;
int num_scans = 0;
if(min_range(&sick, 45, 135) > ROBOT_SAFETY_MARGIN)
{
while((num_scans < 3)&&(to_ind == -1))
{
obj_vector = run_sick_scan(&sick, 2, 0, 10, 350);
num_objects = obj_vector.size();
if(num_objects)
{
for(i = 0; i < num_objects;i++)
{
if(r_diff(target, obj_vector[i]) < obj_difference)
{
obj_difference = r_diff(target, obj_vector[i]);
if (obj_difference < 500.0)
to_ind = i;
}
}
for(i = 0; i < num_objects;i++)
{
if((int)i == to_ind)
{
print_object_to_stream(obj_vector[i], ftarget);
}
else
{
print_object_to_stream(obj_vector[i], fobjects);
}
}
}
num_scans++;
}
float new_vel = 0;
if(to_ind > -1)
{
printf("Tracking target\r\n");
flog << "Following target\t";
target_lost = 0;
target = obj_vector[to_ind];
new_vel = (obj_vector[to_ind].distance - robot_settings.min_distance)*robot_settings.tracking_factor;
if(new_vel < 0)
new_vel = 0;
if(new_vel > robot_settings.max_velocity)
new_vel = robot_settings.max_velocity;
new_heading = (-90 + target.degree)*0.25;
new_heading = get_safe_path(&sick, new_heading, target.distance);
if(test_flag != TEST)
{
robot.lock();
robot.setVel(new_vel);
robot.unlock();
last_v = new_vel;
robot.lock();
robot.setDeltaHeading(new_heading);
robot.unlock();
target.degree = target.degree - new_heading;
}
if(new_vel < 1.0)
{
robot_state = TRACKING;
printf("entering tracking mode\r\n");
flog<<"entering tracking mode\r\n";
}
}
else
{
if(target_lost)
{
l_target.distance = l_target.distance - last_v*(start.mSecSince()/1000.0);
int temp_max_v = min_range(&sick, 5, 175);
if(temp_max_v < last_v)
last_v = temp_max_v;
if(last_v > robot_settings.max_velocity)
last_v = robot_settings.max_velocity;
if(test_flag != TEST)
{
robot.lock();
robot.setVel(last_v);
robot.unlock();
new_heading = -90.0 + l_target.degree;
new_heading = get_safe_path(&sick, new_heading, l_target.distance);
l_target.degree = target.degree - new_heading;
robot.lock();
robot.setDeltaHeading(new_heading);
robot.unlock();
}
print_object_to_stream(l_target, fltarget);
}
else
{
target_lost = 1;
l_target = target;
}
printf("target lost\r\n");
flog << "target lost\r\n";
start.setToNow();
if(target.distance < ROBOT_SAFETY_MARGIN)
{
robot_state = TRACKING;
target_lost = 0;
}
target_lost = 1;
}
printf("Velocity: %f\t",last_v);
flog << "Velocity:\t";
flog << last_v;
printf("Heading: %f\t",new_heading);
flog << "\tHeading\t";
flog << new_heading;
flog << "\r\n";
printf("\r\n");
}
else
{
robot_state = TOO_CLOSE;
printf("I'm too close to an obstacle, and I'm getting claustrophobic! I'm going to slowly back up now.\r\n");
}
break;
}
case TOO_CLOSE:
{
flog << "Too close\r\n";
if(min_range(&sick, 45, 135) > (ROBOT_SAFETY_MARGIN + 100))
{
robot_state = TRACKING;
printf("I feel better now! I'm going to reenter tracking mode.\r\n");
robot.lock();
robot.setVel(0);
robot.unlock();
}
else
{
if(test_flag != TEST)
{
printf("Backing up...\r\n");
robot.lock();
robot.setVel(-50);
robot.unlock();
}
}
break;
}
}
fobjects.close();
ftarget.close();
fltarget.close();
if(plot_option == WRITE)
{
system("./scan_data/plot_script.sh >/dev/null");
}
//////////////////////////////////////////////////////////////////////
// Everything past this point is code to grab the user input
user_command = 0;
fd_set rfds;
struct timeval tv;
int retval;
FD_ZERO(&rfds);
FD_SET(0, &rfds);
tv.tv_sec = 0;
tv.tv_usec = 100;
retval = select(1, &rfds, NULL, NULL, &tv);
if(retval == -1)
perror("select()");
else if(retval)
{
cin >> user_command;
printf("input detected from user\r\n");
}
plot_option = NO_WRITE;
//////////////////////////////////////////////////////////////////////
}
flog.close();
Aria::shutdown();
printf("Shutting down");
return 0;
}
int main(int argc, char **argv)
{
bool done;
double distToTravel = 2300;
// whether to use the sim for the laser or not, if you use the sim
// for hte laser, you have to use the sim for the robot too
bool useSim = false;
// the laser
ArSick sick;
// connection
ArDeviceConnection *con;
// Laser connection
ArSerialConnection laserCon;
// robot
ArRobot robot;
// set a default filename
//std::string filename = "c:\\log\\1scans.2d";
std::string filename = "1scans.2d";
// see if we want to use a different filename
//if (argc > 1)
//Lfilename = argv[1];
printf("Logging to file %s\n", filename.c_str());
// start the logger with good values
sick.configureShort(useSim, ArSick::BAUD38400,
ArSick::DEGREES180, ArSick::INCREMENT_HALF);
ArSickLogger logger(&robot, &sick, 300, 25, filename.c_str());
// mandatory init
Aria::init();
// add it to the robot
robot.addRangeDevice(&sick);
//ArAnalogGyro gyro(&robot);
// if we're not using the sim, make a serial connection and set it up
if (!useSim)
{
ArSerialConnection *serCon;
serCon = new ArSerialConnection;
serCon->setPort();
//serCon->setBaud(38400);
con = serCon;
}
// if we are using the sim, set up a tcp connection
else
{
ArTcpConnection *tcpCon;
tcpCon = new ArTcpConnection;
tcpCon->setPort();
con = tcpCon;
}
// set the connection on the robot
robot.setDeviceConnection(con);
// try to connect, if we fail exit
if (!robot.blockingConnect())
{
printf("Could not connect to robot... exiting\n");
Aria::shutdown();
return 1;
}
// set up a key handler so escape exits and attach to the robot
ArKeyHandler keyHandler;
robot.attachKeyHandler(&keyHandler);
// run the robot, true here so that the run will exit if connection lost
robot.runAsync(true);
// if we're not using the sim, set up the port for the laser
if (!useSim)
{
laserCon.setPort(ArUtil::COM3);
sick.setDeviceConnection(&laserCon);
}
// now that we're connected to the robot, connect to the laser
sick.runAsync();
if (!sick.blockingConnect())
{
printf("Could not connect to SICK laser... exiting\n");
robot.disconnect();
Aria::shutdown();
return 1;
}
#ifdef WIN32
// wait until someone pushes the motor button to go
while (1)
{
robot.lock();
if (!robot.isRunning())
exit(0);
if (robot.areMotorsEnabled())
{
robot.unlock();
break;
}
robot.unlock();
ArUtil::sleep(100);
}
#endif
// basically from here on down the robot just cruises around a bit
robot.lock();
// enable the motors, disable amigobot sounds
robot.comInt(ArCommands::ENABLE, 1);
ArTime startTime;
// move a couple meters
robot.move(distToTravel);
robot.unlock();
startTime.setToNow();
do {
ArUtil::sleep(100);
robot.lock();
robot.setHeading(0);
done = robot.isMoveDone(60);
robot.unlock();
} while (!done);
/*
// rotate a few times
robot.lock();
robot.setVel(0);
robot.setRotVel(60);
robot.unlock();
ArUtil::sleep(12000);
*/
robot.lock();
robot.setHeading(180);
robot.unlock();
do {
ArUtil::sleep(100);
robot.lock();
robot.setHeading(180);
done = robot.isHeadingDone();
robot.unlock();
} while (!done);
// move a couple meters
robot.lock();
robot.move(distToTravel);
robot.unlock();
startTime.setToNow();
do {
ArUtil::sleep(100);
robot.lock();
robot.setHeading(180);
done = robot.isMoveDone(60);
robot.unlock();
} while (!done);
robot.lock();
robot.setHeading(0);
robot.setVel(0);
robot.unlock();
startTime.setToNow();
do {
ArUtil::sleep(100);
robot.lock();
robot.setHeading(0);
done = robot.isHeadingDone();
robot.unlock();
} while (!done);
sick.lockDevice();
sick.disconnect();
sick.unlockDevice();
robot.lock();
robot.disconnect();
robot.unlock();
// now exit
Aria::shutdown();
return 0;
}
int main(int argc, char **argv)
{
std::string str;
int ret;
ArTime start;
// connection to the robot
ArSerialConnection con;
// the robot
ArRobot robot;
// the connection handler from above
ConnHandler ch(&robot);
// init area with a dedicated signal handling thread
Aria::init(Aria::SIGHANDLE_THREAD);
// open the connection with the defaults, exit if failed
if ((ret = con.open()) != 0)
{
str = con.getOpenMessage(ret);
printf("Open failed: %s\n", str.c_str());
Aria::shutdown();
return 1;
}
// set the robots connection
robot.setDeviceConnection(&con);
// try to connect, if we fail, the connection handler should bail
if (!robot.blockingConnect())
{
// this should have been taken care of by the connection handler
// but just in case
printf(
"asyncConnect failed because robot is not running in its own thread.\n");
Aria::shutdown();
return 1;
}
// run the robot in its own thread, so it gets and processes packets and such
robot.runAsync(false);
// just a big long set of printfs, direct motion commands and sleeps,
// it should be self-explanatory
printf("Telling the robot to go 300 mm for 5 seconds\n");
robot.lock();
robot.setVel(500);
robot.unlock();
start.setToNow();
while (1)
{
robot.lock();
if (start.mSecSince() > 5000)
{
robot.unlock();
break;
}
printf("Trans: %10g Rot: %10g\n", robot.getVel(), robot.getRotVel());
robot.unlock();
ArUtil::sleep(100);
}
printf("Telling the robot to turn at 50 deg/sec for 10 seconds\n");
robot.lock();
robot.setVel(0);
robot.setRotVel(50);
robot.unlock();
start.setToNow();
while (1)
{
robot.lock();
if (start.mSecSince() > 10000)
{
robot.unlock();
break;
}
printf("Trans: %10g Rot: %10g\n", robot.getVel(), robot.getRotVel());
robot.unlock();
ArUtil::sleep(100);
}
printf("Telling the robot to turn at 100 deg/sec for 10 seconds\n");
robot.lock();
robot.setVel(0);
robot.setRotVel(100);
robot.unlock();
start.setToNow();
while (1)
{
robot.lock();
if (start.mSecSince() > 10000)
{
robot.unlock();
break;
}
printf("Trans: %10g Rot: %10g\n", robot.getVel(), robot.getRotVel());
robot.unlock();
ArUtil::sleep(100);
}
printf("Done with tests, exiting\n");
robot.disconnect();
// shutdown and ge tout
Aria::shutdown();
return 0;
}
int main(int argc, char **argv)
{
Aria::init();
ArArgumentParser argParser(&argc, argv);
ArSimpleConnector con(&argParser);
ArRobot robot;
// the connection handler from above
ConnHandler ch(&robot);
if(!Aria::parseArgs())
{
Aria::logOptions();
Aria::shutdown();
return 1;
}
if(!con.connectRobot(&robot))
{
ArLog::log(ArLog::Normal, "directMotionExample: Could not connect to the robot. Exiting.");
return 1;
}
ArLog::log(ArLog::Normal, "directMotionExample: Connected.");
// Run the robot processing cycle in its own thread. Note that after starting this
// thread, we must lock and unlock the ArRobot object before and after
// accessing it.
robot.runAsync(false);
// Send the robot a series of motion commands directly, sleeping for a
// few seconds afterwards to give the robot time to execute them.
printf("directMotionExample: Setting rot velocity to 100 deg/sec then sleeping 3 seconds\n");
robot.lock();
robot.setRotVel(100);
robot.unlock();
ArUtil::sleep(3*1000);
printf("Stopping\n");
robot.lock();
robot.setRotVel(0);
robot.unlock();
ArUtil::sleep(200);
printf("directMotionExample: Telling the robot to go 300 mm on left wheel and 100 mm on right wheel for 5 seconds\n");
robot.lock();
robot.setVel2(300, 100);
robot.unlock();
ArTime start;
start.setToNow();
while (1)
{
robot.lock();
if (start.mSecSince() > 5000)
{
robot.unlock();
break;
}
robot.unlock();
ArUtil::sleep(50);
}
printf("directMotionExample: Telling the robot to move forwards one meter, then sleeping 5 seconds\n");
robot.lock();
robot.move(1000);
robot.unlock();
start.setToNow();
while (1)
{
robot.lock();
if (robot.isMoveDone())
{
printf("directMotionExample: Finished distance\n");
robot.unlock();
break;
}
if (start.mSecSince() > 5000)
{
printf("directMotionExample: Distance timed out\n");
robot.unlock();
break;
}
robot.unlock();
ArUtil::sleep(50);
}
printf("directMotionExample: Telling the robot to move backwards one meter, then sleeping 5 seconds\n");
robot.lock();
robot.move(-1000);
robot.unlock();
start.setToNow();
while (1)
{
robot.lock();
if (robot.isMoveDone())
{
printf("directMotionExample: Finished distance\n");
robot.unlock();
break;
}
if (start.mSecSince() > 10000)
{
printf("directMotionExample: Distance timed out\n");
robot.unlock();
break;
}
robot.unlock();
ArUtil::sleep(50);
}
printf("directMotionExample: Telling the robot to turn to 180, then sleeping 4 seconds\n");
robot.lock();
robot.setHeading(180);
robot.unlock();
start.setToNow();
while (1)
{
robot.lock();
if (robot.isHeadingDone(5))
{
printf("directMotionExample: Finished turn\n");
robot.unlock();
break;
}
if (start.mSecSince() > 5000)
{
printf("directMotionExample: Turn timed out\n");
robot.unlock();
break;
}
robot.unlock();
ArUtil::sleep(100);
}
printf("directMotionExample: Telling the robot to turn to 90, then sleeping 2 seconds\n");
robot.lock();
robot.setHeading(90);
robot.unlock();
start.setToNow();
while (1)
{
robot.lock();
if (robot.isHeadingDone(5))
{
printf("directMotionExample: Finished turn\n");
robot.unlock();
break;
}
if (start.mSecSince() > 5000)
{
printf("directMotionExample: turn timed out\n");
robot.unlock();
break;
}
robot.unlock();
ArUtil::sleep(100);
}
printf("directMotionExample: Setting vel2 to 200 mm/sec on both wheels, then sleeping 3 seconds\n");
robot.lock();
robot.setVel2(200, 200);
robot.unlock();
ArUtil::sleep(3000);
printf("directMotionExample: Stopping the robot, then sleeping for 2 seconds\n");
robot.lock();
robot.stop();
robot.unlock();
ArUtil::sleep(2000);
printf("directMotionExample: Setting velocity to 200 mm/sec then sleeping 3 seconds\n");
robot.lock();
robot.setVel(200);
robot.unlock();
ArUtil::sleep(3000);
printf("directMotionExample: Stopping the robot, then sleeping for 2 seconds\n");
robot.lock();
robot.stop();
robot.unlock();
ArUtil::sleep(2000);
printf("directMotionExample: Setting vel2 with 0 on left wheel, 200 mm/sec on right, then sleeping 5 seconds\n");
robot.lock();
robot.setVel2(0, 200);
robot.unlock();
ArUtil::sleep(5000);
printf("directMotionExample: Telling the robot to rotate at 50 deg/sec then sleeping 5 seconds\n");
robot.lock();
robot.setRotVel(50);
robot.unlock();
ArUtil::sleep(5000);
printf("directMotionExample: Telling the robot to rotate at -50 deg/sec then sleeping 5 seconds\n");
robot.lock();
robot.setRotVel(-50);
robot.unlock();
ArUtil::sleep(5000);
printf("directMotionExample: Setting vel2 with 0 on both wheels, then sleeping 3 seconds\n");
robot.lock();
robot.setVel2(0, 0);
robot.unlock();
ArUtil::sleep(3000);
printf("directMotionExample: Now having the robot change heading by -125 degrees, then sleeping for 6 seconds\n");
robot.lock();
robot.setDeltaHeading(-125);
robot.unlock();
ArUtil::sleep(6000);
printf("directMotionExample: Now having the robot change heading by 45 degrees, then sleeping for 6 seconds\n");
robot.lock();
robot.setDeltaHeading(45);
robot.unlock();
ArUtil::sleep(6000);
printf("directMotionExample: Setting vel2 with 200 on left wheel, 0 on right wheel, then sleeping 5 seconds\n");
robot.lock();
robot.setVel2(200, 0);
robot.unlock();
ArUtil::sleep(5000);
printf("directMotionExample: Done, shutting down Aria and exiting.\n");
Aria::shutdown();
return 0;
}
ArActionDesired* ActionReadSonar::fire(ArActionDesired currentDesired)
{
ArRobot *robot = this->getRobot();
int total = robot->getNumSonar(); // get the total number of sonar on the robot
ArSensorReading* value; // This class abstracts range and angle read from sonar
//cout << " 0 : " << robot->getSonarReading(0)->getSensorTh() << " 1 : " << robot->getSonarReading(1)->getSensorTh()
// << " 2 : " << robot->getSonarReading(2)->getSensorTh() << " 3 : " << robot->getSonarReading(3)->getSensorTh()
// << " 4 : " << robot->getSonarReading(4)->getSensorTh() << " 5 : " << robot->getSonarReading(5)->getSensorTh()
// << " 6 : " << robot->getSonarReading(6)->getSensorTh() << " 7 : " << robot->getSonarReading(7)->getSensorTh()
// << "r :" << robot->getTh() << endl;
double limit = 800;
double distance;
// reset the actionDesired (must be done), to clear
// its previous values.
myDesired.reset();
// if the sonar is null we can't do anything, so deactivate
if (mySonar == NULL)
{
deactivate();
return NULL;
}
// gets value of object between -20 degrees and 20 degrees of foward
double angle = 0;
distance = mySonar->currentReadingPolar(-20, 20, &angle);
//cout << "distance from nearest object =" << distance << endl;
if (distance <= limit) {
int heading = 15;
//cout << "angle :" << angle << endl;
if (angle > 10) {
heading = -heading;
}
else if (angle < -10) {
heading = heading;
}
//cout << "x" << robot->getX() << " ," << robot->getY() << endl;
cout << "distance from nearest object =" << distance << endl;
robot->lock();
robot->setVel(0);
robot->unlock();
robot->lock();
robot->setDeltaHeading(heading);
robot->unlock();
ArUtil::sleep(50);
}
return &myDesired;
}
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;
}
int main(int argc, char **argv)
{
Aria::init();
ArRobot robot;
ArArgumentParser parser(&argc, argv);
parser.loadDefaultArguments();
ArLog::log(ArLog::Terse, "WARNING: this program does no sensing or avoiding of obstacles, the robot WILL collide with any objects in the way! Make sure the robot has approximately 3 meters of free space on all sides.");
// ArRobotConnector connects to the robot, get some initial data from it such as type and name,
// and then loads parameter files for this robot.
ArRobotConnector robotConnector(&parser, &robot);
if(!robotConnector.connectRobot())
{
ArLog::log(ArLog::Terse, "simpleMotionCommands: Could not connect to the robot.");
if(parser.checkHelpAndWarnUnparsed())
{
Aria::logOptions();
Aria::exit(1);
return 1;
}
}
if (!Aria::parseArgs())
{
Aria::logOptions();
Aria::exit(1);
return 1;
}
ArLog::log(ArLog::Normal, "simpleMotionCommands: Connected.");
// Start the robot processing cycle running in the background.
// True parameter means that if the connection is lost, then the
// run loop ends.
robot.runAsync(true);
// Print out some data from the SIP.
// We must "lock" the ArRobot object
// before calling its methods, and "unlock" when done, to prevent conflicts
// with the background thread started by the call to robot.runAsync() above.
// See the section on threading in the manual for more about this.
// Make sure you unlock before any sleep() call or any other code that will
// take some time; if the robot remains locked during that time, then
// ArRobot's background thread will be blocked and unable to communicate with
// the robot, call tasks, etc.
robot.lock();
ArLog::log(ArLog::Normal, "simpleMotionCommands: Pose=(%.2f,%.2f,%.2f), Trans. Vel=%.2f, Rot. Vel=%.2f, Battery=%.2fV",
robot.getX(), robot.getY(), robot.getTh(), robot.getVel(), robot.getRotVel(), robot.getBatteryVoltage());
robot.unlock();
// Sleep for 3 seconds.
ArLog::log(ArLog::Normal, "simpleMotionCommands: Will start driving in 3 seconds...");
ArUtil::sleep(3000);
// Set forward velocity to 50 mm/s
ArLog::log(ArLog::Normal, "simpleMotionCommands: Driving forward at 250 mm/s for 5 sec...");
robot.lock();
robot.enableMotors();
robot.setVel(250);
robot.unlock();
ArUtil::sleep(5000);
ArLog::log(ArLog::Normal, "simpleMotionCommands: Stopping.");
robot.lock();
robot.stop();
robot.unlock();
ArUtil::sleep(1000);
ArLog::log(ArLog::Normal, "simpleMotionCommands: Rotating at 10 deg/s for 5 sec...");
robot.lock();
robot.setRotVel(10);
robot.unlock();
ArUtil::sleep(5000);
ArLog::log(ArLog::Normal, "simpleMotionCommands: Rotating at -10 deg/s for 10 sec...");
robot.lock();
robot.setRotVel(-10);
robot.unlock();
ArUtil::sleep(10000);
ArLog::log(ArLog::Normal, "simpleMotionCommands: Driving forward at 150 mm/s for 5 sec...");
robot.lock();
robot.setRotVel(0);
robot.setVel(150);
robot.unlock();
ArUtil::sleep(5000);
ArLog::log(ArLog::Normal, "simpleMotionCommands: Stopping.");
robot.lock();
robot.stop();
robot.unlock();
ArUtil::sleep(1000);
// Other motion command functions include move(), setHeading(),
// setDeltaHeading(). You can also adjust acceleration and deceleration
// values used by the robot with setAccel(), setDecel(), setRotAccel(),
// setRotDecel(). See the ArRobot class documentation for more.
robot.lock();
ArLog::log(ArLog::Normal, "simpleMotionCommands: Pose=(%.2f,%.2f,%.2f), Trans. Vel=%.2f, Rot. Vel=%.2f, Battery=%.2fV",
robot.getX(), robot.getY(), robot.getTh(), robot.getVel(), robot.getRotVel(), robot.getBatteryVoltage());
robot.unlock();
ArLog::log(ArLog::Normal, "simpleMotionCommands: Ending robot thread...");
robot.stopRunning();
// wait for the thread to stop
robot.waitForRunExit();
// exit
ArLog::log(ArLog::Normal, "simpleMotionCommands: Exiting.");
Aria::exit(0);
return 0;
}