void laserRequest_and_odom(ArServerClient *client, ArNetPacket *packet) { robot.lock(); ArNetPacket sending; sending.empty(); ArLaser* laser = robot.findLaser(1); if(!laser){ printf("Could not connect to Laser... exiting\n"); Aria::exit(1);} laser->lockDevice(); const std::list<ArSensorReading*> *sensorReadings = laser->getRawReadings(); // see ArRangeDevice interface doc sending.byte4ToBuf((ArTypes::Byte4)(sensorReadings->size())); for (std::list<ArSensorReading*>::const_iterator it2= sensorReadings->begin(); it2 != sensorReadings->end(); ++it2){ ArSensorReading* laserRead =*it2; sending.byte4ToBuf((ArTypes::Byte4)(laserRead->getRange())); //printf("%i,%i:",laserRead->getRange(),laserRead->getIgnoreThisReading()); } sending.byte4ToBuf((ArTypes::Byte4)(robot.getX())); sending.byte4ToBuf((ArTypes::Byte4)(robot.getY())); sending.byte4ToBuf((ArTypes::Byte4)(robot.getTh())); sending.byte4ToBuf((ArTypes::Byte4)(robot.getVel())); sending.byte4ToBuf((ArTypes::Byte4)(robot.getRotVel())); //printf("%1f,%1f,%1f\n",robot.getX(),robot.getY(),robot.getTh()); laser->unlockDevice(); robot.unlock(); sending.finalizePacket(); //sending.printHex(); client->sendPacketTcp(&sending); }
void SensorDetectPopup::popupClosed(ArTypes::Byte4 popupID, int button) { // A client closed the popup ArLog::log(ArLog::Normal, "popupExample: a client closed popup dialog window with id=%d. Button=%d...", popupID, button); myPopupDisplayed = false; if(button < 0) { ArLog::log(ArLog::Normal, "\t...popup timed out or closed due to an error."); return; } if (button == 0) { ArLog::log(ArLog::Normal, "\t...OK pressed."); return; } if(button == 1) { ArLog::log(ArLog::Normal, "\t...180 degree rotate requested."); myRobot->lock(); myRobot->setDeltaHeading(180); myRobot->unlock(); return; } if(button == 2) { ArLog::log(ArLog::Normal, "\t...exit requested."); myRobot->stopRunning(); Aria::shutdown(); Aria::exit(0); } }
// 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; }
bool RosAriaNode::disable_motors_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response) { ROS_INFO("RosAria: Disable motors request."); robot->lock(); robot->disableMotors(); robot->unlock(); // todo could wait and see if motors do become disabled, and send a response with an error flag if not return true; }
bool RosAriaNode::enable_motors_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response) { ROS_INFO("RosAria: Enable motors request."); robot->lock(); if(robot->isEStopPressed()) ROS_WARN("RosAria: Warning: Enable motors requested, but robot also has E-Stop button pressed. Motors will not enable."); robot->enableMotors(); robot->unlock(); // todo could wait and see if motors do become enabled, and send a response with an error flag if not return true; }
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; }
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); int i; while (Aria::getRunning()) { robot.lock(); robot.comStr(ArCommands::TTY3, "1234567890"); robot.unlock(); } robot.disconnect(); // shutdown and ge tout Aria::shutdown(); return 0; }
void RosAriaNode::sonarDisconnectCb() { if (!robot->tryLock()) { ROS_ERROR("Skipping sonarConnectCb because could not lock"); return; } if (robot->areSonarsEnabled()) { robot->disableSonar(); sonar_tf_timer.stop(); } robot->unlock(); }
void RosAriaNode::sonarConnectCb() { robot->lock(); if (sonar_pub.getNumSubscribers() == 0) { robot->disableSonar(); use_sonar = false; } else { robot->enableSonar(); use_sonar = true; } robot->unlock(); }
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(); } }
/// Called when another node subscribes or unsubscribes from sonar topic. void RosAriaNode::sonarConnectCb() { publish_sonar = (sonar_pub.getNumSubscribers() > 0); publish_sonar_pointcloud2 = (sonar_pointcloud2_pub.getNumSubscribers() > 0); robot->lock(); if (publish_sonar || publish_sonar_pointcloud2) { robot->enableSonar(); sonar_enabled = false; } else if(!publish_sonar && !publish_sonar_pointcloud2) { robot->disableSonar(); sonar_enabled = true; } robot->unlock(); }
/* * To Read the parameter */ void RosAriaNode::readParameters() { // Robot Parameters robot->lock(); ros::NodeHandle n_("~"); if (n_.hasParam("TicksMM")) { n_.getParam( "TicksMM", TicksMM); ROS_INFO("Setting TicksMM from ROS Parameter: %d", TicksMM); robot->comInt(93, TicksMM); } else { TicksMM = robot->getOrigRobotConfig()->getTicksMM(); n_.setParam( "TicksMM", TicksMM); ROS_INFO("Setting TicksMM from robot EEPROM: %d", TicksMM); } if (n_.hasParam("DriftFactor")) { n_.getParam( "DriftFactor", DriftFactor); ROS_INFO("Setting DriftFactor from ROS Parameter: %d", DriftFactor); robot->comInt(89, DriftFactor); } else { DriftFactor = robot->getOrigRobotConfig()->getDriftFactor(); n_.setParam( "DriftFactor", DriftFactor); ROS_INFO("Setting DriftFactor from robot EEPROM: %d", DriftFactor); } if (n_.hasParam("RevCount")) { n_.getParam( "RevCount", RevCount); ROS_INFO("Setting RevCount from ROS Parameter: %d", RevCount); robot->comInt(88, RevCount); } else { RevCount = robot->getOrigRobotConfig()->getRevCount(); n_.setParam( "RevCount", RevCount); ROS_INFO("Setting RevCount from robot EEPROM: %d", RevCount); } robot->unlock(); }
int main(int argc, char **argv){ Aria::init(); ArRobot robot; ArArgumentParser parser(&argc, argv); ArSimpleConnector connector(& parser); parser.loadDefaultArguments(); Aria::logOptions(); if (!connector.parseArgs()){ cout << "Unknown settings\n"; Aria::exit(0); exit(1); } if (!connector.connectRobot(&robot)){ cout << "Unable to connect\n"; Aria::exit(0); exit(1); } robot.runAsync(true); robot.lock(); robot.comInt(ArCommands::ENABLE, 1); robot.unlock(); ArSonarDevice sonar; robot.addRangeDevice(&sonar); G_id = 0; G_SONAR_FD = fopen("../sensors/sonars","w"); G_pose_fd = fopen("../sensors/pose","w"); int numSonar = robot.getNumSonar(); while(1){ readPosition(robot); readSonars(robot, 8); setMotors(robot); usleep(20000); } fclose(G_SONAR_FD); fclose(G_pose_fd); Aria::exit(0); }
/*! * Shuts down the system. * */ void Advanced::shutDown(void) { Aria::exit(0); // // Stop the path planning thread. // if(myPathPlanningTask){ myPathPlanningTask->stopRunning(); // delete myPathPlanningTask; printf("Stopped Path Planning Thread\n"); } // // Stop the localization thread. // if(myLocaTask){ myLocaTask->stopRunning(); delete myLocaTask; printf("Stopped Localization Thread\n"); } // // Stop the laser thread. // if(mySick) { mySick->lockDevice(); mySick->disconnect(); mySick->unlockDevice(); printf("Stopped Laser Thread\n"); } // // Stop the robot thread. // myRobot->lock(); myRobot->stopRunning(); myRobot->unlock(); printf("Stopped Robot Thread\n"); // // Exit Aria // Aria::shutdown(); printf("Aria Shutdown\n"); }
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); }
/*! * Sets up for path planning. * * @return true if successful. */ bool Advanced::initializePathPlanningTask(void) { #ifdef SONARNL if(myRobot && mySonar && myMap) myPathPlanningTask = new ArPathPlanningTask(myRobot, mySonar, myMap); #else if(myRobot && mySick && mySonar && myMap) myPathPlanningTask = new ArPathPlanningTask(myRobot, mySick, mySonar, myMap); #endif if(!myPathPlanningTask){ return false; }else{ myRobot->lock(); myRobot->enableMotors(); myRobot->unlock(); return true; } }
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) { // mandatory init Aria::init(); //ArLog::init(ArLog::StdOut, ArLog::Verbose); // set up our parser ArArgumentParser parser(&argc, argv); // load the default arguments parser.loadDefaultArguments(); // robot ArRobot robot; // set up our simple connector ArRobotConnector robotConnector(&parser, &robot); // add a gyro, it'll see if it should attach to the robot or not ArAnalogGyro gyro(&robot); // set up the robot for connecting if (!robotConnector.connectRobot()) { printf("Could not connect to robot... exiting\n"); Aria::exit(1); } ArDataLogger dataLogger(&robot, "dataLog.txt"); dataLogger.addToConfig(Aria::getConfig()); // our base server object ArServerBase server; ArLaserConnector laserConnector(&parser, &robot, &robotConnector); ArServerSimpleOpener simpleOpener(&parser); ArClientSwitchManager clientSwitchManager(&server, &parser); // parse the command line... fail and print the help if the parsing fails // or if the help was requested if (!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed()) { Aria::logOptions(); Aria::exit(1); } // Set up where we'll look for files such as user/password char fileDir[1024]; ArUtil::addDirectories(fileDir, sizeof(fileDir), Aria::getDirectory(), "ArNetworking/examples"); // first open the server up if (!simpleOpener.open(&server, fileDir, 240)) { if (simpleOpener.wasUserFileBad()) printf("Bad user/password/permissions file\n"); else printf("Could not open server port\n"); exit(1); } // Range devices: ArSonarDevice sonarDev; robot.addRangeDevice(&sonarDev); ArIRs irs; robot.addRangeDevice(&irs); ArBumpers bumpers; robot.addRangeDevice(&bumpers); // attach services to the server ArServerInfoRobot serverInfoRobot(&server, &robot); ArServerInfoSensor serverInfoSensor(&server, &robot); ArServerInfoDrawings drawings(&server); // modes for controlling robot movement ArServerModeStop modeStop(&server, &robot); ArServerModeRatioDrive modeRatioDrive(&server, &robot); ArServerModeWander modeWander(&server, &robot); modeStop.addAsDefaultMode(); modeStop.activate(); // set up the simple commands ArServerHandlerCommands commands(&server); ArServerSimpleComUC uCCommands(&commands, &robot); // send commands directly to microcontroller ArServerSimpleComMovementLogging loggingCommands(&commands, &robot); // control debug logging ArServerSimpleComGyro gyroCommands(&commands, &robot, &gyro); // configure gyro ArServerSimpleComLogRobotConfig configCommands(&commands, &robot); // control more debug logging ArServerSimpleServerCommands serverCommands(&commands, &server); // control ArNetworking debug logging ArServerSimpleLogRobotDebugPackets logRobotDebugPackets(&commands, &robot, "."); // debugging tool // ArServerModeDrive is an older drive mode. ArServerModeRatioDrive is newer and generally performs better, // but you can use this for old clients if neccesary. //ArServerModeDrive modeDrive(&server, &robot); //modeDrive.addControlCommands(&commands); // configure the drive modes (e.g. enable/disable safe drive) ArServerHandlerConfig serverHandlerConfig(&server, Aria::getConfig()); // make a config handler ArLog::addToConfig(Aria::getConfig()); // let people configure logging modeRatioDrive.addToConfig(Aria::getConfig(), "Teleop settings"); // able to configure teleop settings modeRatioDrive.addControlCommands(&commands); // Forward 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 separate 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); // Control a pan/tilt/zoom camera, if one is installed, and the video // forwarder was enabled above. ArPTZ *camera = NULL; ArServerHandlerCamera *handlerCamera = NULL; ArCameraCollection *cameraCollection = NULL; if (videoForwarder.isForwardingVideo()) { bool invertedCamera = false; camera = new ArVCC4(&robot, invertedCamera, ArVCC4::COMM_UNKNOWN, true, true); camera->init(); cameraCollection = new ArCameraCollection(); cameraCollection->addCamera("Cam1", "VCC4", "Camera", "VCC4"); handlerCamera = new ArServerHandlerCamera("Cam1", &server, &robot, camera, cameraCollection); } // You can use this class to send a set of arbitrary strings // for MobileEyes to display, this is just a small example ArServerInfoStrings stringInfo(&server); Aria::getInfoGroup()->addAddStringCallback(stringInfo.getAddStringFunctor()); Aria::getInfoGroup()->addStringInt( "Motor Packet Count", 10, new ArConstRetFunctorC<int, ArRobot>(&robot, &ArRobot::getMotorPacCount)); /* Aria::getInfoGroup()->addStringInt( "Laser Packet Count", 10, new ArRetFunctorC<int, ArSick>(&sick, &ArSick::getSickPacCount)); */ // start the robot running, true means that if we lose connection the run thread stops robot.runAsync(true); // connect the laser(s) if it was requested if (!laserConnector.connectLasers()) { printf("Could not connect to lasers... exiting\n"); Aria::exit(2); } drawings.addRobotsRangeDevices(&robot); // log whatever we wanted to before the runAsync simpleOpener.checkAndLog(); // now let it spin off in its own thread server.runAsync(); printf("Server is now running...\n"); // Add a key handler so that you can exit by pressing // escape. Note that a key handler prevents you from running // a program in the background on Linux, since it expects an // active terminal to read keys from; remove this if you want // to run it in the background. ArKeyHandler *keyHandler; if ((keyHandler = Aria::getKeyHandler()) == NULL) { keyHandler = new ArKeyHandler; Aria::setKeyHandler(keyHandler); robot.lock(); robot.attachKeyHandler(keyHandler); robot.unlock(); printf("To exit, press escape.\n"); } // Read in parameter files. std::string configFile = "serverDemoConfig.txt"; Aria::getConfig()->setBaseDirectory("./"); if (Aria::getConfig()->parseFile(configFile.c_str(), true, true)) { ArLog::log(ArLog::Normal, "Loaded config file %s", configFile.c_str()); } else { if (ArUtil::findFile(configFile.c_str())) { ArLog::log(ArLog::Normal, "Trouble loading configuration file %s, continuing", configFile.c_str()); } else { ArLog::log(ArLog::Normal, "No configuration file %s, will try to create if config used", configFile.c_str()); } } clientSwitchManager.runAsync(); robot.lock(); robot.enableMotors(); robot.unlock(); robot.waitForRunExit(); Aria::exit(0); }
int main(int argc, char *argv[]) { // Initialize location of Aria, Arnl and their args. Aria::init(); Arnl::init(); // The robot object ArRobot robot; // Parse the command line arguments. ArArgumentParser parser(&argc, argv); // Read data_index if exists int data_index; bool exist_data_index; parser.checkParameterArgumentInteger("dataIndex",&data_index,&exist_data_index); // Load default arguments for this computer (from /etc/Aria.args, environment // variables, and other places) parser.loadDefaultArguments(); // Object that connects to the robot or simulator using program options ArRobotConnector robotConnector(&parser, &robot); // set up a gyro ArAnalogGyro gyro(&robot); ArLog::init(ArLog::File,ArLog::Normal,"run.log",false,true,true); // Parse arguments for the simple connector. if (!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed()) { ArLog::log(ArLog::Normal, "\nUsage: %s -map mapfilename\n", argv[0]); Aria::logOptions(); Aria::exit(1); } // Collision avoidance actions at higher priority ArActionAvoidFront avoidAction("avoid",200); ArActionLimiterForwards limiterAction("speed limiter near", 150, 500, 150); ArActionLimiterForwards limiterFarAction("speed limiter far", 300, 1100, 400); ArActionLimiterTableSensor tableLimiterAction; //robot.addAction(&tableLimiterAction, 100); //robot.addAction(&avoidAction,100); //robot.addAction(&limiterAction, 95); //robot.addAction(&limiterFarAction, 90); // Goto action at lower priority ArActionGoto gotoPoseAction("goto"); //robot.addAction(&gotoPoseAction, 50); gotoPoseAction.setCloseDist(750); // Stop action at lower priority, so the robot stops if it has no goal ArActionStop stopAction("stop"); //robot.addAction(&stopAction, 40); // Connect the robot if (!robotConnector.connectRobot()) { ArLog::log(ArLog::Normal, "Could not connect to robot... exiting"); Aria::exit(3); } if(!robot.isConnected()) { ArLog::log(ArLog::Terse, "Internal error: robot connector succeeded but ArRobot::isConnected() is false!"); } // Connector for laser rangefinders ArLaserConnector laserConnector(&parser, &robot, &robotConnector); // Sonar, must be added to the robot, used by teleoperation and wander to // detect obstacles, and for localization if SONARNL ArSonarDevice sonarDev; // Add the sonar to the robot robot.addRangeDevice(&sonarDev); // Start the robot thread. robot.runAsync(true); // Connect to the laser(s) if lasers were configured in this robot's parameter // file or on the command line, and run laser processing thread if applicable // for that laser class. For the purposes of this demo, add all // possible lasers to ArRobot's list rather than just the ones that were // connected by this call so when you enter laser mode, you // can then interactively choose which laser to use from that list of all // lasers mentioned in robot parameters and on command line. Normally, // only connected lasers are put in ArRobot's list. if (!laserConnector.connectLasers( false, // continue after connection failures true, // add only connected lasers to ArRobot true // add all lasers to ArRobot )) { ArLog::log(ArLog::Normal ,"Could not connect to lasers... exiting\n"); Aria::exit(2); } // Puntero a laser ArSick* sick=(ArSick*)robot.findLaser(1); // Conectamos el laser sick->asyncConnect(); //Esperamos a que esté encendido while(!sick->isConnected()) { ArUtil::sleep(100); } ArLog::log(ArLog::Normal ,"Laser conectado\n"); // Set up things so data can be logged (only do it with the laser // since it can overrun a 9600 serial connection which the sonar is // more likely to have) ArDataLogger dataLogger(&robot); dataLogger.addToConfig(Aria::getConfig()); // add our logging to the config //ArLog::addToConfig(Aria::getConfig()); // Set up a class that'll put the movement and gyro parameters into ArConfig ArRobotConfig robotConfig(&robot); robotConfig.addAnalogGyro(&gyro); // Add additional range devices to the robot and path planning task. // IRs if the robot has them. robot.lock(); ArIRs irs; robot.addRangeDevice(&irs); // Bumpers. ArBumpers bumpers; robot.addRangeDevice(&bumpers); // cause the sonar to turn off automatically // when the robot is stopped, and turn it back on when commands to move // are sent. (Note, this should not be done if you need the sonar // data to localize, or for other purposes while stopped) ArSonarAutoDisabler sonarAutoDisabler(&robot); // Read in parameter files. Aria::getConfig()->useArgumentParser(&parser); if (!Aria::getConfig()->parseFile(Arnl::getTypicalParamFileName())) { ArLog::log(ArLog::Normal, "Trouble loading configuration file, exiting"); Aria::exit(5); } //Configuracion del laser sick->setMinDistBetweenCurrent(0); robot.enableMotors(); robot.setAbsoluteMaxTransVel(1000); /* Finally, get ready to run the robot: */ robot.unlock(); Controlador driver(&robot); if(exist_data_index){ driver.setDataIndex(data_index); } driver.runAsync(); ControlHandler handler(&driver,&robot); // Use manual key handler //handler.addKeyHandlers(&robot); robot.addSensorInterpTask("ManualKeyHandler",50,handler.getFunctor()); ArLog::log(ArLog::Normal ,"Añadido manejador teclado\n"); // double x,y,dist,angle; // ArPose punto; // ArPose origen=robot.getPose(); // sick->lockDevice(); // for(int i=-90;i<90;i++){ // // Obtengo la medida de distancia y angulo // dist=sick->currentReadingPolar(i,i+1,&angle); // // Obtengo coordenadas del punto usando el laser como referencia // x=dist*ArMath::cos(angle); // y=dist*ArMath::sin(angle); // //Roto los puntos // ArMath::pointRotate(&x,&y,-origen.getTh()); // punto.setX(x); // punto.setY(y); // punto=punto + origen; // printf("Medida: %d\t Angulo:%.2f\t Angulo:%.2f\t Distancia:%0.2f\t X:%0.2f\t Y:%0.2f\n",i,angle,angle+origen.getTh(),dist,punto.getX(),punto.getY()); // } // printf("Medidas adquiridas\n"); // sick->unlockDevice(); robot.waitForRunExit(); //ArUtil::sleep(10000); return 0; }
int main(int argc, char **argv) { int ret; std::string str; // the serial connection (robot) ArSerialConnection serConn; // tcp connection (sim) ArTcpConnection tcpConn; // the robot ArRobot robot; // the laser ArSick sick; // the laser connection ArSerialConnection laserCon; bool useSimForLaser = false; std::string hostname = "prod.local.net"; // timeouts in minutes int wanderTime = 0; int restTime = 0; // check arguments if (argc == 3 || argc == 4) { wanderTime = atoi(argv[1]); restTime = atoi(argv[2]); if (argc == 4) hostname = argv[3]; } else { printf("\nUsage:\n\tpeoplebotTest <wanderTime> <restTime> <hostname>\n\n"); printf("Times are in minutes. Hostname is the machine to pipe the ACTS display to\n\n"); wanderTime = 15; restTime = 45; } printf("Wander time - %d minutes\nRest time - %d minutes\n", wanderTime, restTime); printf("Sending display to %s.\n\n", hostname.c_str()); // sonar, must be added to the robot ArSonarDevice sonar; // the actions we'll use to wander ArActionStallRecover recover; ArActionBumpers bumpers; ArActionAvoidFront avoidFrontNear("Avoid Front Near", 225, 0); ArActionAvoidFront avoidFrontFar; // Make a key handler, so that escape will shut down the program // cleanly ArKeyHandler keyHandler; // mandatory init Aria::init(); // Add the key handler to Aria so other things can find it Aria::setKeyHandler(&keyHandler); // Attach the key handler to a robot now, so that it actually gets // some processing time so it can work, this will also make escape // exit robot.attachKeyHandler(&keyHandler); // First we see if we can open the tcp connection, if we can we'll // assume we're connecting to the sim, and just go on... if we // can't open the tcp it means the sim isn't there, so just try the // robot // modify this next line if you're not using default tcp connection tcpConn.setPort(); // see if we can get to the simulator (true is success) if (tcpConn.openSimple()) { // we could get to the sim, so set the robots device connection to the sim printf("Connecting to simulator through tcp.\n"); robot.setDeviceConnection(&tcpConn); } else { // we couldn't get to the sim, so set the port on the serial // connection and then set the serial connection as the robots // device // modify the next line if you're not using the first serial port // to talk to your robot serConn.setPort(); printf( "Could not connect to simulator, connecting to robot through serial.\n"); robot.setDeviceConnection(&serConn); } // add the sonar to the robot robot.addRangeDevice(&sonar); // add the laser robot.addRangeDevice(&sick); // try to connect, if we fail exit if (!robot.blockingConnect()) { printf("Could not connect to robot... exiting\n"); Aria::shutdown(); return 1; } // turn on the motors, turn off amigobot sounds //robot.comInt(ArCommands::ENABLE, 1); robot.comInt(ArCommands::SOUNDTOG, 0); // turn off the sonar to start with robot.comInt(ArCommands::SONAR, 0); // add the actions robot.addAction(&recover, 100); robot.addAction(&bumpers, 75); robot.addAction(&avoidFrontNear, 50); robot.addAction(&avoidFrontFar, 49); // start the robot running, true so that if we lose connection the run stops robot.runAsync(true); if (!useSimForLaser) { sick.setDeviceConnection(&laserCon); if ((ret = laserCon.open("/dev/ttyS2")) != 0) { str = tcpConn.getOpenMessage(ret); printf("Open failed: %s\n", str.c_str()); Aria::shutdown(); return 1; } sick.configureShort(false); } else { sick.configureShort(true); } sick.runAsync(); if (!sick.blockingConnect()) { printf("Could not connect to SICK laser... exiting\n"); Aria::shutdown(); return 1; } robot.lock(); robot.comInt(ArCommands::ENABLE, 1); robot.unlock(); // add the peoplebot test PeoplebotTest pbTest(&robot, wanderTime, restTime, hostname); robot.waitForRunExit(); // now exit Aria::shutdown(); return 0; }
int main(int argc, char **argv) { std::string str; int ret; int successes = 0, failures = 0; int action; bool exitOnFailure = true; ArSerialConnection con; ArRobot robot; //ArLog::init(ArLog::StdOut, ArLog::Verbose); srand(time(NULL)); robot.runAsync(false); // if (!exitOnFailure) // ArLog::init(ArLog::None, ArLog::Terse); //else //ArLog::init(ArLog::None); while (1) { if (con.getStatus() != ArDeviceConnection::STATUS_OPEN && (ret = con.open()) != 0) { str = con.getOpenMessage(ret); printf("Open failed: %s\n", str.c_str()); ++failures; if (exitOnFailure) { printf("Failed\n"); exit(0); } else { ArUtil::sleep(200); robot.unlock(); continue; } } robot.lock(); robot.setDeviceConnection(&con); robot.unlock(); ArUtil::sleep((rand() % 5) * 100); if (robot.asyncConnect()) { robot.waitForConnectOrConnFail(); robot.lock(); if (!robot.isConnected()) { if (exitOnFailure) { printf("Failed after %d tries.\n", successes); exit(0); } printf("Failed to connect successfully"); ++failures; } robot.comInt(ArCommands::SONAR, 0); robot.comInt(ArCommands::SOUNDTOG, 0); //robot.comInt(ArCommands::PLAYLIST, 0); robot.comInt(ArCommands::ENCODER, 1); ArUtil::sleep(((rand() % 20) + 3) * 100); ++successes; // okay, now try to leave it in a messed up state action = rand() % 8; robot.dropConnection(); switch (action) { case 0: printf("Discon 0 "); robot.disconnect(); ArUtil::sleep(100); robot.com(0); break; case 1: printf("Discon 1 "); robot.disconnect(); ArUtil::sleep(100); robot.com(0); ArUtil::sleep(100); robot.com(1); break; case 2: printf("Discon 2 "); robot.disconnect(); ArUtil::sleep(100); robot.com(0); ArUtil::sleep(100); robot.com(1); ArUtil::sleep(100); robot.com(2); break; case 3: printf("Discon 10 "); robot.disconnect(); ArUtil::sleep(100); robot.com(10); break; case 4: printf("Discon "); robot.disconnect(); break; default: printf("Leave "); break; } robot.unlock(); } else { if (exitOnFailure) { printf("Failed after %d tries.\n", successes); exit(0); } printf("Failed to start connect "); ++failures; } if ((rand() % 2) == 0) { printf(" ! RadioDisconnect ! "); con.write("|||\15", strlen("!!!\15")); ArUtil::sleep(100); con.write("WMD\15", strlen("WMD\15")); ArUtil::sleep(200); } if ((rand() % 2) == 0) { printf(" ! ClosePort !\n"); con.close(); } else printf("\n"); printf("#### %d successes %d failures, %% %.2f success\n", successes, failures, (float)successes/(float)(successes+failures)*100); ArUtil::sleep((rand() % 2)* 1000); } return 0; }
int main(int argc, char** argv) { ros::init(argc, argv, "terabot_arm_hardware_interface"); ros::NodeHandle node; std::cout << "argc= "<<argc<<" argv= "<<argv<<std::endl; hardware_interface::JointStateInterface jnt_state_interface_; hardware_interface::PositionJointInterface jnt_pos_interface_; //********************************************************** //********************************************************** Aria::init(); static float defaultJointSpeed = 15; ArLog::init(ArLog::StdErr, ArLog::Normal); ArArgumentParser parser(&argc, argv); parser.loadDefaultArguments(); ArRobot robot; ArTerabotArm arm(&robot); ArRobotConnector robotConnector(&parser, &robot); if(!robotConnector.connectRobot()) { ArLog::log(ArLog::Terse, "terabotArm: Could not connect to the robot."); if(parser.checkHelpAndWarnUnparsed()) { Aria::logOptions(); Aria::exit(1); } } if (!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed()) { Aria::logOptions(); Aria::exit(1); } ArLog::log(ArLog::Normal, "terabotArm: Connected to mobile robot."); if(!arm.open()) { ArLog::log(ArLog::Terse, "terabotArm: Error opening serial connection to arm"); Aria::exit(1); } robot.runAsync(true); arm.powerOn(); arm.reset(); arm.enable(); arm.setAllJointSpeeds(defaultJointSpeed); ArUtil::sleep(5000); robot.lock(); robot.enableMotors(); robot.unlock(); TerabotArmInterface robot1(&robot, &arm, node); //********************************************************** //********************************************************** // TerabotArmInterface robot1(&robot, &arm); std::cout << "after creating the object"<<std::endl; robot1.init(jnt_state_interface_, jnt_pos_interface_); controller_manager::ControllerManager cm(&robot1, node); ros::AsyncSpinner spinner(4); spinner.start(); ros::Time previous=ros::Time::now(); ros::Rate rate(10.0); while (ros::ok()) { ros::Duration period; robot1.readHW(); ros::Time now=ros::Time::now(); period=now-previous; //std::cout << "period:"<<period<<std::endl; cm.update(now, period); robot1.writeHW(); rate.sleep(); } spinner.stop(); return 0; }
int main ( int argc, char *argv[] ){ //cout << "running....\n"; try{ // Create the socket ServerSocket server ( 30000 ); Aria::init(); Arnl::init(); ArRobot robot; ArArgumentParser parser(&argc, argv); parser.loadDefaultArguments(); ArSonarDevice sonar; ArSimpleConnector simpleConnector(&parser); // Our server for mobile eyes ArServerBase moServer; // Set up our simpleOpener ArServerSimpleOpener simpleOpener(&parser); parser.loadDefaultArguments(); if (!Aria::parseArgs () || !parser.checkHelpAndWarnUnparsed()){ Aria::logOptions (); Aria::exit (1); } //Add the sonar to the robot robot.addRangeDevice(&sonar); // Look for map in the current directory ArMap arMap; // set it up to ignore empty file names (otherwise the parseFile // on the config will fail) arMap.setIgnoreEmptyFileName (true); // First open the server if (!simpleOpener.open(&moServer)){ if (simpleOpener.wasUserFileBad()) ArLog::log(ArLog::Normal, "Bad user file"); else ArLog::log(ArLog::Normal, "Could not open server port"); exit(2); } // Connect to the robot if (!simpleConnector.connectRobot (&robot)){ ArLog::log (ArLog::Normal, "Could not connect to robot... exiting"); Aria::exit (3); } // Create the localization task (it will start its own thread here) ArSonarLocalizationTask locTask(&robot, &sonar, &arMap); ArLocalizationManager locManager(&robot, &arMap); ArLog::log(ArLog::Normal, "Creating sonar localization task"); locManager.addLocalizationTask(&locTask); // Set the initial pose to the robot's "Home" position from the map, or // (0,0,0) if none, then let the localization thread take over. locTask.localizeRobotAtHomeNonBlocking(); //Create the path planning task ArPathPlanningTask pathTask(&robot,&sonar,&arMap); ArLog::log(ArLog::Normal, "Robot Server: Connected."); robot.enableMotors(); robot.clearDirectMotion(); // 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); // Read in parameter files. Aria::getConfig ()->useArgumentParser (&parser); if (!Aria::getConfig ()->parseFile (Arnl::getTypicalParamFileName ())){ ArLog::log (ArLog::Normal, "Trouble loading configuration file, exiting"); Aria::exit (5); } //Create the three states robot.lock(); Follow follow = Follow(&robot,&sonar); GoTo goTo(&robot,&pathTask,&arMap); Search s(&robot,&sonar); // Bumpers. ArBumpers bumpers; robot.addRangeDevice(&bumpers); pathTask.addRangeDevice(&bumpers, ArPathPlanningTask::CURRENT); // Forbidden regions from the map ArForbiddenRangeDevice forbidden(&arMap); robot.addRangeDevice(&forbidden); pathTask.addRangeDevice(&forbidden, ArPathPlanningTask::CURRENT); // Mode To stop and remain stopped: ArServerModeStop modeStop(&moServer, &robot); // 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); // These provide various kinds of information to the client: ArServerInfoRobot serverInfoRobot(&moServer, &robot); ArServerInfoSensor serverInfoSensor(&moServer, &robot); ArServerInfoPath serverInfoPath(&moServer, &robot, &pathTask); // Provide the map to the client (and related controls): // This uses both lines and points now, since everything except // sonar localization uses both (path planning with sonar still uses both) ArServerHandlerMap serverMap(&moServer, &arMap); // Provides localization info and allows the client (MobileEyes) to relocalize at a given // pose: ArServerInfoLocalization serverInfoLocalization(&moServer, &robot, &locTask); ArServerHandlerLocalization serverLocHandler(&moServer, &robot, &locTask); robot.unlock(); moServer.runAsync(); //Main loop while (true){ //The socket to accept connection ServerSocket new_sock; server.accept ( new_sock ); int state = 1; //1 = Follow, 2 = Search, 3 = GoTo int lastPos[2]; //Storing last position of BB to search the target int data[2]; //matrix with X,Y of BB try{ while ( true ){ //receive data from tld new_sock >> data; //cout << data[0] << "," << data[1] << endl; if(data[0] != -1) lastPos[0] = data[0]; //cout << state <<endl; //for debugging //Main logic switch(state){ case 1: cout << "Following target\n"; state = follow.run(data); break; case 2: cout << "Searching for target\n"; state = s.seek(lastPos, data); break; case 3: cout << "Going to ...\n"; state = goTo.run(data); break; default: cout << "Not a case for state\n"; break; } std::cout << "Loc score: " << locTask.getLocalizationScore() << std::endl; } } catch ( SocketException& ) { cout << "Lost Connection" << endl; robot.lock(); robot.stop(); robot.unlock(); } } } catch ( SocketException& e ){ std::cout << "Exception was caught:" << e.description() << "\nExiting.\n"; } ArLog::log(ArLog::Normal, "RobotServer: Exiting."); return 0; }
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 __cdecl _tmain (int argc, char** argv) { //------------ I N I C I O M A I N D E L P R O G R A M A D E L R O B O T-----------// //inicializaion de variables Aria::init(); ArArgumentParser parser(&argc, argv); parser.loadDefaultArguments(); ArSimpleConnector simpleConnector(&parser); ArRobot robot; ArSonarDevice sonar; ArAnalogGyro gyro(&robot); robot.addRangeDevice(&sonar); ActionGos go(500, 350); robot.addAction(&go, 48); ActionTurns turn(400, 110); robot.addAction(&turn, 49); ActionTurns turn2(400, 110); robot.addAction(&turn2, 49); // presionar tecla escape para salir del programa ArKeyHandler keyHandler; Aria::setKeyHandler(&keyHandler); robot.attachKeyHandler(&keyHandler); printf("Presionar ESC para salir\n"); // uso de sonares para evitar colisiones con las paredes u // obstaculos grandes, mayores a 8cm de alto ArActionLimiterForwards limiterAction("limitador velocidad cerca", 300, 600, 250); ArActionLimiterForwards limiterFarAction("limitador velocidad lejos", 300, 1100, 400); ArActionLimiterTableSensor tableLimiterAction; robot.addAction(&tableLimiterAction, 100); robot.addAction(&limiterAction, 95); robot.addAction(&limiterFarAction, 90); // Inicializon la funcion de goto ArActionGoto gotoPoseAction("goto"); robot.addAction(&gotoPoseAction, 50); // Finaliza el goto si es que no hace nada ArActionStop stopAction("stop"); robot.addAction(&stopAction, 40); // Parser del CLI if (!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed()) { Aria::logOptions(); exit(1); } // Conexion del robot if (!simpleConnector.connectRobot(&robot)) { printf("Could not connect to robot... exiting\n"); Aria::exit(1); } robot.runAsync(true); // enciende motores, apaga sonidos robot.enableMotors(); robot.comInt(ArCommands::SOUNDTOG, 0); // Imprimo algunos datos del robot como posicion velocidad y bateria robot.lock(); ArLog::log(ArLog::Normal, "Posicion=(%.2f,%.2f,%.2f), Trans. Vel=%.2f, Bateria=%.2fV", robot.getX(), robot.getY(), robot.getTh(), robot.getVel(), robot.getBatteryVoltage()); robot.unlock(); const int duration = 100000; //msec ArLog::log(ArLog::Normal, "Completados los puntos en %d segundos", duration/1000); // ============================ INICIO CONFIG COM =================================// CSerial serial; LONG lLastError = ERROR_SUCCESS; // Trata de abrir el com seleccionado lLastError = serial.Open(_T("COM3"),0,0,false); if (lLastError != ERROR_SUCCESS) return ::ShowError(serial.GetLastError(), _T("Imposible abrir el COM")); // Inicia el puerto serial (9600,8N1) lLastError = serial.Setup(CSerial::EBaud9600,CSerial::EData8,CSerial::EParNone,CSerial::EStop1); if (lLastError != ERROR_SUCCESS) return ::ShowError(serial.GetLastError(), _T("Imposible setear la config del COM")); // Register only for the receive event lLastError = serial.SetMask(CSerial::EEventBreak | CSerial::EEventCTS | CSerial::EEventDSR | CSerial::EEventError | CSerial::EEventRing | CSerial::EEventRLSD | CSerial::EEventRecv); if (lLastError != ERROR_SUCCESS) return ::ShowError(serial.GetLastError(), _T("Unable to set COM-port event mask")); // Use 'non-blocking' reads, because we don't know how many bytes // will be received. This is normally the most convenient mode // (and also the default mode for reading data). lLastError = serial.SetupReadTimeouts(CSerial::EReadTimeoutNonblocking); if (lLastError != ERROR_SUCCESS) return ::ShowError(serial.GetLastError(), _T("Unable to set COM-port read timeout.")); // ============================ FIN CONFIG COM =================================// bool first = true; int goalNum = 0; int color = 3; ArTime start; start.setToNow(); while (Aria::getRunning()) { robot.lock(); // inicia el primer punto if (first || gotoPoseAction.haveAchievedGoal()) { first = false; goalNum++; //cambia de 0 a 1 el contador printf("El contador esta en: --> %d <---\n",goalNum); if (goalNum > 20) goalNum = 1; //comienza la secuencia de puntos if (goalNum == 1) { gotoPoseAction.setGoal(ArPose(1150, 0)); ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY()); // Imprimo algunos datos del robot como posicion velocidad y bateria robot.lock(); ArLog::log(ArLog::Normal, "Posicion=(%.2f,%.2f,%.2f), Trans. Vel=%.2f, Bateria=%.2fV", robot.getX(), robot.getY(), robot.getTh(), robot.getVel(), robot.getBatteryVoltage()); robot.unlock(); // Create the sound queue. ArSoundsQueue soundQueue; // Run the sound queue in a new thread soundQueue.runAsync(); std::vector<const char*> filenames; filenames.push_back("sound-r2a.wav"); soundQueue.play(filenames[0]); } else if (goalNum == 2) { printf("Gira 90 grados izquierda\n"); robot.unlock(); turn.myActivate = 1; turn.myDirection = 1; turn.activate(); ArUtil::sleep(1000); turn.deactivate(); turn.myActivate = 0; turn.myDirection = 0; robot.lock(); } else if (goalNum == 3) { gotoPoseAction.setGoal(ArPose(1150, 2670)); ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY()); // Imprimo algunos datos del robot como posicion velocidad y bateria robot.lock(); ArLog::log(ArLog::Normal, "Posicion=(%.2f,%.2f,%.2f), Trans. Vel=%.2f, Bateria=%.2fV", robot.getX(), robot.getY(), robot.getTh(), robot.getVel(), robot.getBatteryVoltage()); robot.unlock(); } else if (goalNum == 4) { printf("Gira 90 grados izquierda\n"); robot.unlock(); turn2.myActivate = 1; turn2.myDirection = 1; turn2.activate(); ArUtil::sleep(1000); turn2.deactivate(); turn2.myActivate = 0; turn2.myDirection = 0; robot.lock(); } else if (goalNum == 5) { gotoPoseAction.setGoal(ArPose(650, 2670)); ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY()); } else if (goalNum == 6) { printf("Gira 90 grados izquierda\n"); robot.unlock(); turn2.myActivate = 1; turn2.myDirection = 1; turn2.activate(); ArUtil::sleep(1000); turn2.deactivate(); turn2.myActivate = 0; turn2.myDirection = 0; robot.lock(); } else if (goalNum == 7) { gotoPoseAction.setGoal(ArPose(650, 0)); ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY()); } else if (goalNum == 8) { gotoPoseAction.setGoal(ArPose(1800,1199)); ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY()); } else if (goalNum == 9) { gotoPoseAction.setGoal(ArPose(2600, 1199)); ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY()); } else if (goalNum == 10) { if (color == 1) { gotoPoseAction.setGoal(ArPose(2800, 850)); ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY()); } if (color == 2) { gotoPoseAction.setGoal(ArPose(3500, 1199)); ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY()); } if (color == 3) { gotoPoseAction.setGoal(ArPose(2800, 1550)); ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY()); } } else if (goalNum == 11) { if (color == 1) { gotoPoseAction.setGoal(ArPose(2800, 613)); ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY()); } if (color == 2) { printf("Gira 180 grados derecha\n"); robot.unlock(); turn2.myActivate = 1; turn2.myDirection = 2; turn2.activate(); ArUtil::sleep(2000); turn2.deactivate(); turn2.myActivate = 0; turn2.myDirection = 0; robot.lock(); goalNum = 19; } if (color == 3) { gotoPoseAction.setGoal(ArPose(2800, 1785)); ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY()); } } else if (goalNum == 12) { if (color == 1) { gotoPoseAction.setGoal(ArPose(3300, 413)); ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY()); } if (color == 3) { gotoPoseAction.setGoal(ArPose(3300, 1985)); ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY()); } } else if (goalNum == 13) { if (color == 1) { gotoPoseAction.setGoal(ArPose(3500, 413)); ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY()); } if (color == 3) { gotoPoseAction.setGoal(ArPose(3500, 1985)); ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY()); } } else if (goalNum == 14) { robot.unlock(); //Valor para el while bool fContinue = true; // <<<<<<------------- 1 Parte Secuencia: BAJA BRAZO ------------->>>>>> // lLastError = serial.Write("b"); if (lLastError != ERROR_SUCCESS) return ::ShowError(serial.GetLastError(), _T("Unable to send data")); //-------------------------E S C U C H A C O M ----------------------------// do { // Wait for an event lLastError = serial.WaitEvent(); if (lLastError != ERROR_SUCCESS) return ::ShowError(serial.GetLastError(), _T("Unable to wait for a COM-port event.")); // Save event const CSerial::EEvent eEvent = serial.GetEventType(); // Handle break event if (eEvent & CSerial::EEventBreak) { printf("\n### BREAK received ###\n"); } // Handle CTS event if (eEvent & CSerial::EEventCTS) { printf("\n### Clear to send %s ###\n", serial.GetCTS()?"on":"off"); } // Handle DSR event if (eEvent & CSerial::EEventDSR) { printf("\n### Data set ready %s ###\n", serial.GetDSR()?"on":"off"); } // Handle error event if (eEvent & CSerial::EEventError) { printf("\n### ERROR: "); switch (serial.GetError()) { case CSerial::EErrorBreak: printf("Break condition"); break; case CSerial::EErrorFrame: printf("Framing error"); break; case CSerial::EErrorIOE: printf("IO device error"); break; case CSerial::EErrorMode: printf("Unsupported mode"); break; case CSerial::EErrorOverrun: printf("Buffer overrun"); break; case CSerial::EErrorRxOver: printf("Input buffer overflow"); break; case CSerial::EErrorParity: printf("Input parity error"); break; case CSerial::EErrorTxFull: printf("Output buffer full"); break; default: printf("Unknown"); break; } printf(" ###\n"); } // Handle ring event if (eEvent & CSerial::EEventRing) { printf("\n### RING ###\n"); } // Handle RLSD/CD event if (eEvent & CSerial::EEventRLSD) { printf("\n### RLSD/CD %s ###\n", serial.GetRLSD()?"on":"off"); } // Handle data receive event if (eEvent & CSerial::EEventRecv) { // Read data, until there is nothing left DWORD dwBytesRead = 0; char szBuffer[101]; do { // Lee datos del Puerto COM lLastError = serial.Read(szBuffer,sizeof(szBuffer)-1,&dwBytesRead); if (lLastError != ERROR_SUCCESS) return ::ShowError(serial.GetLastError(), _T("Unable to read from COM-port.")); if (dwBytesRead > 0) { //Preseteo color int color = 0; // Finaliza el dato, asi que sea una string valida szBuffer[dwBytesRead] = '\0'; // Display the data printf("%s", szBuffer); // <<<<<<----------- 2 Parte Secuencia: CIERRA GRIPPER ----------->>>>>> // if (strchr(szBuffer,76)) { lLastError = serial.Write("c"); if (lLastError != ERROR_SUCCESS) return ::ShowError(serial.GetLastError(), _T("Unable to send data")); } // <<<<<<------------- 3 Parte Secuencia: SUBE BRAZO ------------->>>>>> // if (strchr(szBuffer,117)) { lLastError = serial.Write("s"); if (lLastError != ERROR_SUCCESS) return ::ShowError(serial.GetLastError(), _T("Unable to send data")); } // <<<<<<------------- 4 Parte Secuencia: COLOR ------------->>>>>> // if (strchr(szBuffer,72)) { lLastError = serial.Write("C"); if (lLastError != ERROR_SUCCESS) return ::ShowError(serial.GetLastError(), _T("Unable to send data")); } // <<<<<<---------- 5.1 Parte Secuencia: COLOR ROJO---------->>>>>> // if (strchr(szBuffer,82)) { color = 1; //salir del bucle fContinue = false; } // <<<<<<---------- 5.2 Parte Secuencia: COLOR AZUL ---------->>>>>> // if (strchr(szBuffer,66)) { color = 2; //salir del bucle fContinue = false; } // <<<<<<---------- 5.3 Parte Secuencia: COLOR VERDE ---------->>>>>> // if (strchr(szBuffer,71)) { color = 3; //salir del bucle fContinue = false; } } } while (dwBytesRead == sizeof(szBuffer)-1); } } while (fContinue); // Close the port again serial.Close(); robot.lock(); } else if (goalNum == 15) { printf("Gira 180 grados derecha\n"); robot.unlock(); turn2.myActivate = 1; turn2.myDirection = 2; turn2.activate(); ArUtil::sleep(2000); turn2.deactivate(); turn2.myActivate = 0; turn2.myDirection = 0; robot.lock(); } else if (goalNum == 16) { if (color == 1) { gotoPoseAction.setGoal(ArPose(3300, 413)); ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY()); } if (color == 3) { gotoPoseAction.setGoal(ArPose(3300, 1985)); ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY()); } } else if (goalNum == 17) { if (color == 1) { gotoPoseAction.setGoal(ArPose(2800, 603)); ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY()); } if (color == 3) { gotoPoseAction.setGoal(ArPose(2800, 1795)); ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY()); } } else if (goalNum == 18) { if (color == 1) { gotoPoseAction.setGoal(ArPose(2800, 860)); ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY()); } if (color == 3) { gotoPoseAction.setGoal(ArPose(2800, 1540)); ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY()); } } else if (goalNum == 19) { gotoPoseAction.setGoal(ArPose(2600, 1199)); ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY()); } else if (goalNum == 20) { gotoPoseAction.setGoal(ArPose(1800, 1199)); ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY()); } } if(start.mSecSince() >= duration) { ArLog::log(ArLog::Normal, "No puede llegar al punto, y la aplicacion saldra en %d", duration/1000); gotoPoseAction.cancelGoal(); robot.unlock(); ArUtil::sleep(3000); break; } robot.unlock(); ArUtil::sleep(10); } // Robot desconectado al terminal el sleep Aria::shutdown(); //------------ F I N M A I N D E L P R O G R A M A D E L R O B O T-----------// return 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; }
int main(int argc, char** argv) { // Initialize some global data Aria::init(); // If you want ArLog to print "Verbose" level messages uncomment this: //ArLog::init(ArLog::StdOut, ArLog::Verbose); // This object parses program options from the command line ArArgumentParser parser(&argc, argv); // Load some default values for command line arguments from /etc/Aria.args // (Linux) or the ARIAARGS environment variable. parser.loadDefaultArguments(); // Central object that is an interface to the robot and its integrated // devices, and which manages control of the robot by the rest of the program. ArRobot robot; // Object that connects to the robot or simulator using program options ArRobotConnector robotConnector(&parser, &robot); // If the robot has an Analog Gyro, this object will activate it, and // if the robot does not automatically use the gyro to correct heading, // this object reads data from it and corrects the pose in ArRobot ArAnalogGyro gyro(&robot); // Connect to the robot, get some initial data from it such as type and name, // and then load parameter files for this robot. if (!robotConnector.connectRobot()) { // Error connecting: // if the user gave the -help argumentp, then just print out what happened, // and continue so options can be displayed later. if (!parser.checkHelpAndWarnUnparsed()) { ArLog::log(ArLog::Terse, "Could not connect to robot, will not have parameter file so options displayed later may not include everything"); } // otherwise abort else { ArLog::log(ArLog::Terse, "Error, could not connect to robot."); Aria::logOptions(); Aria::exit(1); } } // Connector for laser rangefinders ArLaserConnector laserConnector(&parser, &robot, &robotConnector); // Connector for compasses ArCompassConnector compassConnector(&parser); // Parse the command line options. Fail and print the help message if the parsing fails // or if the help was requested with the -help option if (!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed()) { Aria::logOptions(); exit(1); } // Used to access and process sonar range data ArSonarDevice sonarDev; // Used to perform actions when keyboard keys are pressed ArKeyHandler keyHandler; Aria::setKeyHandler(&keyHandler); // ArRobot contains an exit action for the Escape key. It also // stores a pointer to the keyhandler so that other parts of the program can // use the same keyhandler. robot.attachKeyHandler(&keyHandler); printf("You may press escape to exit\n"); // Attach sonarDev to the robot so it gets data from it. robot.addRangeDevice(&sonarDev); // Start the robot task loop running in a new background thread. The 'true' argument means if it loses // connection the task loop stops and the thread exits. robot.runAsync(true); // Connect to the laser(s) if lasers were configured in this robot's parameter // file or on the command line, and run laser processing thread if applicable // for that laser class. (For the purposes of this demo, add all // possible lasers to ArRobot's list rather than just the ones that were // specified with the connectLaser option (so when you enter laser mode, you // can then interactively choose which laser to use from the list which will // show both connected and unconnected lasers.) if (!laserConnector.connectLasers(false, false, true)) { printf("Could not connect to lasers... exiting\n"); Aria::exit(2); } // Create and connect to the compass if the robot has one. ArTCM2 *compass = compassConnector.create(&robot); if(compass && !compass->blockingConnect()) { compass = NULL; } // Sleep for a second so some messages from the initial responses // from robots and cameras and such can catch up ArUtil::sleep(1000); // We need to lock the robot since we'll be setting up these modes // while the robot task loop thread is already running, and they // need to access some shared data in ArRobot. robot.lock(); // now add all the modes for this demo // these classes are defined in ArModes.cpp in ARIA's source code. ArModeLaser laser(&robot, "laser", 'l', 'L'); ArModeTeleop teleop(&robot, "teleop", 't', 'T'); ArModeUnguardedTeleop unguardedTeleop(&robot, "unguarded teleop", 'u', 'U'); ArModeWander wander(&robot, "wander", 'w', 'W'); ArModeGripper gripper(&robot, "gripper", 'g', 'G'); ArModeCamera camera(&robot, "camera", 'c', 'C'); ArModeSonar sonar(&robot, "sonar", 's', 'S'); ArModeBumps bumps(&robot, "bumps", 'b', 'B'); ArModePosition position(&robot, "position", 'p', 'P', &gyro); ArModeIO io(&robot, "io", 'i', 'I'); ArModeActs actsMode(&robot, "acts", 'a', 'A'); ArModeCommand command(&robot, "command", 'd', 'D'); ArModeTCM2 tcm2(&robot, "tcm2", 'm', 'M', compass); // activate the default mode teleop.activate(); // turn on the motors robot.comInt(ArCommands::ENABLE, 1); robot.unlock(); // Block execution of the main thread here and wait for the robot's task loop // thread to exit (e.g. by robot disconnecting, escape key pressed, or OS // signal) robot.waitForRunExit(); Aria::exit(0); }
int main(int argc, char **argv) { //----------------------initialized robot server------------------------------------------ Aria::init(); Arnl::init(); ArServerBase server; //----------------------------------------------------------------------------------- VCCHandler ptz(&robot); //create keyboard for control vcc50i G_PTZHandler->reset(); ArUtil::sleep(300); G_PTZHandler->panSlew(30); //----------------------------------------------------------------------------------- argc = 2 ; argv[0] = "-map"; argv[1] = "map20121111.map"; // Parse the command line arguments. ArArgumentParser parser(&argc, argv); // Set up our simpleConnector ArSimpleConnector simpleConnector(&parser); // Set up our simpleOpener ArServerSimpleOpener simpleOpener(&parser); //******* // Set up our client for the central server // ArClientSwitchManager clientSwitch(&server, &parser); //************ // Load default arguments for this computer (from /etc/Aria.args, environment // variables, and other places) // parser.loadDefaultArguments(); // set up a gyro ArAnalogGyro gyro(&robot); //gyro.activate(); // Parse arguments for the simple connector. // if (!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed()) // { // ArLog::log(ArLog::Normal, "\nUsage: %s -map mapfilename\n", argv[0]); // Aria::logOptions(); // Aria::exit(1); // } // The laser object, will be used if we have one // Add the laser to the robot robot.addRangeDevice(&sick); // Sonar, must be added to the robot, used by teleoperation and wander to // detect obstacles, and for localization if SONARNL ArSonarDevice sonarDev; // Add the sonar to the robot robot.addRangeDevice(&sonarDev); // Set up where we'll look for files char fileDir[1024]; ArUtil::addDirectories(fileDir, sizeof(fileDir), "./", "maps"); ArLog::log(ArLog::Normal, "Installation directory is: %s\nMaps directory is: %s\n", Aria::getDirectory(), fileDir); // Set up the map, 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 current directory // instead ArMap arMap(fileDir); // set it up to ignore empty file names (otherwise the parseFile // on the config will fail) arMap.setIgnoreEmptyFileName(true); //******************************** //Localization //******************************** ArLocalizationManager locManager(&robot, &arMap); ArLog::log(ArLog::Normal, "Creating laser localization task"); ArLocalizationTask locTask (&robot, &sick, &arMap); locManager.addLocalizationTask(&locTask); //******************************* //Path planning //******************************* // Make the path task planning task ArPathPlanningTask pathTask(&robot, &sick, &sonarDev, &arMap); G_PathPlanning = &pathTask; // Set up things so data can be logged (only do it with the laser // since it can overrun a 9600 serial connection which the sonar is // more likely to have) ArDataLogger dataLogger(&robot); dataLogger.addToConfig(Aria::getConfig()); // add our logging to the config // ArLog::addToConfig(Aria::getConfig()); // First open the server if (!simpleOpener.open(&server, fileDir, 240)) { if (simpleOpener.wasUserFileBad()) ArLog::log(ArLog::Normal, "Bad user file"); else ArLog::log(ArLog::Normal, "Could not open server port"); exit(2); } // Connect the robot if (!simpleConnector.connectRobot(&robot)) { ArLog::log(ArLog::Normal, "Could not connect to robot... exiting"); Aria::exit(3); } //----------------------------------------------- //************************** // Set up a class that'll put the movement and gyro parameters into ArConfig ArRobotConfig robotConfig(&robot); robotConfig.addAnalogGyro(&gyro); //***************************** robot.enableMotors(); robot.clearDirectMotion(); // if we are connected to a simulator, reset it to its start position robot.comInt(ArCommands::RESETSIMTOORIGIN, 1); robot.moveTo(ArPose(0,0,0)); // Set up laser using connector (command line arguments, etc.) simpleConnector.setupLaser(&sick); // Start the robot thread. robot.runAsync(true); // Start the laser thread. sick.runAsync(); // Try to connect the laser if (!sick.blockingConnect()) ArLog::log(ArLog::Normal, "Warning: Couldn't connect to SICK laser, it won't be used"); else ArLog::log(ArLog::Normal, "Connected to laser."); //*************************************** // Add additional range devices to the robot and path planning task. // IRs if the robot has them. robot.lock(); // ArIRs irs; // robot.addRangeDevice(&irs); // pathTask.addRangeDevice(&irs, ArPathPlanningTask::CURRENT); //****************************************** // Forbidden regions from the map ArForbiddenRangeDevice forbidden(&arMap); robot.addRangeDevice(&forbidden); // This is the place to add a range device which will hold sensor data // and delete it appropriately to replan around blocked paths. ArGlobalReplanningRangeDevice replanDev(&pathTask); // Create objects that add network services: // Drawing in the map display: ArServerInfoDrawings drawings(&server); drawings.addRobotsRangeDevices(&robot); drawings.addRangeDevice(&replanDev); /* If you want to draw the destination put this code back in: ArServerDrawingDestination destination( &drawings, &pathTask, "destination", 500, 500, new ArDrawingData("polyDots", ArColor(0xff, 0xff, 0x0), 800, // size 49), // just below the robot */ /* If you want to see 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); */ /* If you want to see the points making up the local path in addition to the * main path use this. ArDrawingData drawingDataP2("polyDots", ArColor(0,128,0), 100, 70); ArFunctor2C<ArPathPlanningTask, ArServerClient *, ArNetPacket *> drawingFunctorP2(pathTask, &ArPathPlanningTask::drawPathPoints); drawings.addDrawing(&drawingDataP2, "Path Points", &drawingFunctorP2); */ // Misc. simple commands: 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); //-------------------------receive commands or events from client--------------------- // ArServerHandlerCommands commands(&server); server.addData("RobotVideo" ,"", new ArGlobalFunctor2<ArServerClient *, ArNetPacket*>(&VideoServerBase::RobotVideoCB) ,"",""); server.addData("turn" ,"", new ArGlobalFunctor2<ArServerClient *, ArNetPacket*>(&turn_func) ,"",""); server.addData("RobotMotion" ,"", new ArGlobalFunctor2<ArServerClient *, ArNetPacket*>(&S_RobotMotion) ,"",""); server.addData("CameraMotion" ,"", new ArGlobalFunctor2<ArServerClient *, ArNetPacket*>(&S_CameraMotion) ,"",""); server.addData("RobotTurnLeft" ,"", new ArGlobalFunctor2<ArServerClient *, ArNetPacket*>(&S_RobotTurnLeft) ,"",""); server.addData("RobotTurnRight" ,"", new ArGlobalFunctor2<ArServerClient *, ArNetPacket*>(&S_RobotTurnRight) ,"",""); server.addData("TargetApproach" ,"", new ArGlobalFunctor2<ArServerClient *, ArNetPacket*>(&S_TargetApproach) ,"",""); server.addData("TargetApproachObstacles" ,"", new ArGlobalFunctor2<ArServerClient *, ArNetPacket*>(&S_TargetApproach_Obstacles) ,"",""); server.addData("GlassesCancel" ,"", new ArGlobalFunctor2<ArServerClient *, ArNetPacket*>(&S_GlassesCancel) ,"",""); server.addData("Calibration" ,"", new ArGlobalFunctor2<ArServerClient *, ArNetPacket*>(&S_Calibration) ,"",""); server.addClientRemovedCallback(new ArGlobalFunctor1< ArServerClient * >(&clientCloseCallback)); //-------------------------receive commands or events from client--------------------- //*********************** ArServerInfoLocalization serverInfoLocalization (&server, &robot, &locManager); ArServerHandlerLocalization serverLocHandler (&server, &robot, &locManager); // Provide the map to the client (and related controls): // This uses both lines and points now, since everything except // sonar localization uses both (path planning with sonar still uses both) ArServerHandlerMap serverMap(&server, &arMap); // Add some simple (custom) commands for testing and debugging: ArServerSimpleComUC uCCommands(&commands, &robot); // Send any command to the microcontroller ArServerSimpleComMovementLogging loggingCommands(&commands, &robot); // configure logging ArServerSimpleComGyro gyroCommands(&commands, &robot, &gyro); // monitor the gyro ArServerSimpleComLogRobotConfig configCommands(&commands, &robot); // trigger logging of the robot config parameters ArServerSimpleServerCommands serverCommands(&commands, &server); // monitor networking behavior (track packets sent etc.) /* 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, &arMap, ArPose(0,0,0)); // Add a simple (custom) command that allows you to give a list of // goals to tour, instead of all. Useful for testing and debugging. modeGoto.addTourGoalsInListSimpleCommand(&commands); // 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, this should not be done if you need the sonar // data to localize, or for other purposes while stopped) ArSonarAutoDisabler sonarAutoDisabler(&robot); // Teleoperation modes To drive by keyboard, joystick, etc: ArServerModeRatioDrive modeRatioDrive(&server, &robot); // New, improved mode ArServerModeDrive modeDrive(&server, &robot); // Older mode for compatability // Drive mode's configuration and custom (simple) commands: modeRatioDrive.addToConfig(Aria::getConfig(), "Teleop settings"); modeDrive.addControlCommands(&commands); modeRatioDrive.addControlCommands(&commands); // Wander mode // ArServerModeWander modeWander(&server, &robot); //********************************* // Prevent driving if localization is lost: ArActionLost actionLostRatioDrive (&locManager, NULL, &modeRatioDrive); modeRatioDrive.getActionGroup ()->addAction (&actionLostRatioDrive, 110); // Prevent wandering if lost: // ArActionLost // actionLostWander (&locManager, NULL, &modeWander); // modeWander.getActionGroup ()->addAction (&actionLostWander, 110); // This provides a small table of interesting information for the client // to display to the operator: ArServerInfoStrings stringInfo(&server); Aria::getInfoGroup()->addAddStringCallback(stringInfo.getAddStringFunctor()); Aria::getInfoGroup()->addStringInt( "Motor Packet Count", 10, new ArConstRetFunctorC<int, ArRobot>(&robot, &ArRobot::getMotorPacCount)); // Make Stop mode the default (If current mode deactivates without entering // a new mode, then Stop Mode will be selected) modeStop.addAsDefaultMode(); // Create the service that allows the client to monitor the communication // between the robot and the client. // ArServerHandlerCommMonitor handlerCommMonitor(&server); // Create service that allows client to change configuration parameters in ArConfig ArServerHandlerConfig handlerConfig(&server, Aria::getConfig(), Arnl::getTypicalDefaultParamFileName(), Aria::getDirectory()); // Read in parameter files. read the paras from input Aria::getConfig()->useArgumentParser(&parser); 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()) { ArLog::log(ArLog::Normal, "\nUsage: %s -map mapfilename\n", argv[0]); simpleConnector.logOptions(); simpleOpener.logOptions(); Aria::exit(6); } // Warn if there is no map if (arMap.getFileName() == NULL || strlen(arMap.getFileName()) <= 0) { ArLog::log(ArLog::Normal, ""); ArLog::log(ArLog::Normal, "### Warning, No map file is set up, you can make a map with sickLogger or arnlServer, and Mapper3; More info in docs/Mapping.txt and README.txt. Set the map with the -map command line option, or by changing the config with MobileEyes or by editing the config file."); ArLog::log(ArLog::Normal, ""); } // find out where we'll want to put 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, ""); // If you want MobileSim to try and load up the same map as you are // using in guiServer then uncomment out the next line and this object // will send a command to MobileSim to do so, but make sure you start // MobileSim from the Arnl/examples directory or use the --cwd option, // so that the map names used by MobileSim match the map names used // by guiServer //ArSimMapSwitcher mapSwitcher(&robot, &arMap); /****************************************************** * **************************************************** * Camera * **************************************************** * *****************************************************/ robot.unlock(); // Localize robot at home. locTask.localizeRobotAtHomeBlocking(); locTask.forceUpdatePose(ArPose(0,0,0)); resetMotion(); // robot.enableMotors(); // robot.runAsync(true); //locTask.localizeRobotAtHomeBlocking(); server.runAsync(); VideoServerBase videoserver; videoserver.runAsync(); robot.comInt(ArCommands::SOUNDTOG, 0); //G_PathPlanning->pathPlanToPose(ArPose(1500, -1500 , -32),true,true); //while(G_PathPlanning->getState() != ArPathPlanningTask::REACHED_GOAL ); //S_TargetApproach_Obstacles1(); //ArUtil::sleep(5000); // G_PTZHandler->panRel(60); // ArUtil::sleep(3000); //while(1) //S_TargetApproach1(); cout << "done!!!!!!!---------------------" <<endl; robot.waitForRunExit(); Aria::exit(0); }