//* NAV Course simplified Pioneer robot interface - implementation file - 11/03/2010 12:25PM *

#include "nav_aria_iface.h" #include <iostream> NAV_Robot::NAV_Robot(const char* serial) { Aria::init(Aria::SIGHANDLE_THREAD); // init area with a dedicated signal handling thread robot = new ArRobot(); chandler = new ConnHandler(robot); // attach sonar sensor as range reader sonar = new ArSonarDevice(64, 1024); robot->addRangeDevice(sonar); gyro = new ArAnalogGyro(robot); laser = new ArSick(); robot->addRangeDevice(laser); ArSerialConnection *scon = new ArSerialConnection(); scon->setPort(serial); con = scon; connect(); } NAV_Robot::NAV_Robot(const char* server, unsigned short port) { Aria::init(Aria::SIGHANDLE_THREAD); // init area with a dedicated signal handling thread robot = new ArRobot(); chandler = new ConnHandler(robot); // attach sonar sensor as range reader sonar = new ArSonarDevice(64, 1024); robot->addRangeDevice(sonar); gyro = new ArAnalogGyro(robot); laser = new ArSick(); robot->addRangeDevice(laser); ArTcpConnection *tcon = new ArTcpConnection(); tcon->setPort(server, port); con = tcon; connect(); } NAV_Robot::~NAV_Robot() { disconnect(); Aria::shutdown(); delete gyro; delete sonar; delete laser; delete chandler; delete robot; delete con; } void NAV_Robot::connect() { con->openSimple(); // set the robots connection robot->setDeviceConnection(con); // try to connect, if we fail, the connection handler should bail if (!robot->blockingConnect()) { std::cerr << "ERROR: asyncConnect failed because robot is not running in its own thread." << std::endl; return; } robot->runAsync(true); // run the robot interface in its own thread // true - stop the robot if connection lost // laser->asyncConnect(); // connect the laser device to its interface if ( laser->isConnected() ) laser->runAsync(); // run laser device interface in its own thread lock(); enableMotors(); // enable motors enableSonar(); // enable sonars unlock(); } void NAV_Robot::disconnect() { lock(); Stop(); disableMotors(); disableSonar(); if ( laser->isConnected() ) { laser->stopRunning(); // gracefuly stop the laser thread laser->disconnect(); } robot->stopRunning(); // gracefuly stop the robot thread if ( robot->isConnected() ) robot->disconnect(); unlock(); con->close(); }