void turn(int target, int speed) {
GYRO.calibrate(10);
float zero = gyro(4);
float value;
MOTORS.setMotorSpeed(speed, -speed);
do {
value = abs(zero - gyro(4));
} while (value < target);
MOTORS.setMotorSpeed(0, 0);
}
/**
* @example L3G_Demo.cpp
*
* Read the gyrometer data and print it to the terminal
*
* @include PropWare_L3G/CMakeLists.txt
*/
int main () {
int16_t gyroValues[3];
PropWare::SPI *spi = PropWare::SPI::get_instance();
PropWare::L3G gyro(spi, CS);
gyro.start();
gyro.set_dps(PropWare::L3G::DPS_500);
// Though this functional call is not necessary (default value is 0), I
// want to bring attention to this function. It will determine whether the
// l3g_read* functions will always explicitly set the SPI modes before
// each call, or assume that the SPI driver is still running in the proper
// configuration
gyro.always_set_spi_mode(1);
while (1) {
gyro.read_all(gyroValues);
//pwOut << "Gyro vals DPS... X: " << gyro.convert_to_dps(gyroValues[PropWare::L3G::X])
// << "\tY: " << gyro.convert_to_dps(gyroValues[PropWare::L3G::Y])
// << "\tZ: " << gyro.convert_to_dps(gyroValues[PropWare::L3G::Z]) << '\n';
pwOut << "Gyro vals DPS... X: " << gyroValues[PropWare::L3G::X]
<< "\tY: " << gyroValues[PropWare::L3G::Y]
<< "\tZ: " << gyroValues[PropWare::L3G::Z] << '\n';
waitcnt(50*MILLISECOND + CNT);
}
return 0;
}
void ViSensorInterface::ImuCallback(boost::shared_ptr<visensor::ViImuMsg> imu_ptr) {
if (imu_ptr->imu_id != visensor::SensorId::IMU0)
return;
uint32_t timeNSec = imu_ptr->timestamp;
Eigen::Vector3d gyro(imu_ptr->gyro[0], imu_ptr->gyro[1], imu_ptr->gyro[2]);
Eigen::Vector3d acc(imu_ptr->acc[0], imu_ptr->acc[1], imu_ptr->acc[2]);
std::cout << "IMU Accelerometer data: " << acc[0] << "\t "<< acc[1] << "\t"<< acc[2] << "\t" << std::endl;
//Do stuff with IMU...
}
int main(int argc, char *argv[])
{
if (strcmp(argv[1], "mag")==0)
return mag(argc, argv);
if (strcmp(argv[1], "gyro")==0)
return gyro(argc, argv);
if (strcmp(argv[1], "acc")==0)
return acc(argc, argv);
printf("Usage: %s [mag|gyro|acc] <cmd>\r\n");
return 1;
}
/*
process any
*/
bool AP_InertialSensor_MPU6000::update( void )
{
#if !MPU6000_FAST_SAMPLING
if (_sum_count < _sample_count) {
// we don't have enough samples yet
return false;
}
#endif
// we have a full set of samples
uint16_t num_samples;
Vector3f accel, gyro;
hal.scheduler->suspend_timer_procs();
#if MPU6000_FAST_SAMPLING
gyro = _gyro_filtered;
accel = _accel_filtered;
num_samples = 1;
#else
gyro(_gyro_sum.x, _gyro_sum.y, _gyro_sum.z);
accel(_accel_sum.x, _accel_sum.y, _accel_sum.z);
num_samples = _sum_count;
_accel_sum.zero();
_gyro_sum.zero();
#endif
_sum_count = 0;
hal.scheduler->resume_timer_procs();
gyro *= _gyro_scale / num_samples;
_rotate_and_offset_gyro(_gyro_instance, gyro);
accel *= MPU6000_ACCEL_SCALE_1G / num_samples;
_rotate_and_offset_accel(_accel_instance, accel);
if (_last_filter_hz != _imu.get_filter()) {
if (_spi_sem->take(10)) {
_spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_LOW);
_set_filter_register(_imu.get_filter());
_spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_HIGH);
_spi_sem->give();
}
}
return true;
}
bool DummyInterface::readJointStates()
{
// Save the current ROS time
ros::Time now = ros::Time::now();
// Set the feedback positions with some delay and some noise
std::vector<double> cmds = (m_addDelay() ? m_dataBuf.front() : m_dataBuf.back());
for(size_t i = 0; i < m_model->numJoints(); ++i)
{
const boost::shared_ptr<DummyJoint>& joint = boost::reinterpret_pointer_cast<DummyJoint>(m_model->joint(i));
double noisyCmd = cmds[i];
if(m_noiseEnable())
noisyCmd += (drand48() - 0.5) * m_noiseMagnitude();
if(joint->velocityMode())
{
joint->feedback.pos += noisyCmd * m_model->timerDuration();
joint->feedback.pos = angles::normalize_angle(joint->feedback.pos);
}
else
joint->feedback.pos = noisyCmd;
joint->feedback.stamp = now;
}
// Calculate the fake orientation of the robot (nominal values taken from RobotModel constructor)
Eigen::Quaterniond fakeQuat = rot_conv::QuatFromFused(m_fakeAttFusedY(), m_fakeAttFusedX());
Eigen::Vector3d gyro(m_fakeIMUGyroX(), m_fakeIMUGyroY(), m_fakeIMUGyroZ());
Eigen::Vector3d acc(m_fakeIMUAccX(), m_fakeIMUAccY(), m_fakeIMUAccZ());
Eigen::Vector3d mag(m_fakeIMUMagX(), m_fakeIMUMagY(), m_fakeIMUMagZ());
// Write dummy values into the RobotModel
m_model->setRobotOrientation(fakeQuat);
m_model->setAccelerationVector(acc);
m_model->setMagneticFieldVector(mag);
m_model->setRobotAngularVelocity(gyro);
m_model->setTemperature(35.0);
// Return success
return true;
}
void Application::updateNetwork(){
struct servo_packet pkt;
if(sock.recv(&pkt, sizeof(pkt), 0) == sizeof(pkt)){
printf("servo: %d %d %d %d\n", pkt.servo[0], pkt.servo[1], pkt.servo[2], pkt.servo[3]);
for(unsigned c = 0; c < 8; c++){
activeQuad->setServo(c, pkt.servo[c]);
}
struct state_packet state;
memset(&state, 0, sizeof(state));
// create a 3d world to ned frame rotation
glm::quat r1(sin(glm::radians(90.0 / 2)), 0, 0, cos(glm::radians(90.0 / 2)));
glm::quat r2(0, sin(glm::radians(90.0 / 2)), 0, cos(glm::radians(90.0 / 2)));
glm::quat r2ef = r1 * r2;
glm::vec3 gyro(0, 0, 0);
if(!paused)
gyro = activeQuad->getGyro();
glm::vec3 accel = -activeQuad->getAccel();
state.gyro[0] = -gyro.z; state.gyro[1] = gyro.x; state.gyro[2] = -gyro.y;
state.accel[0] = accel.x; state.accel[1] = accel.y; state.accel[2] = accel.z;
//state.accel[0] = (mRCPitch - 1000.0) / 100.0;
//state.accel[1] = (mRCRoll - 1000.0) / 100.0;
//state.accel[2] = (mRCThrottle - 1000.0) / 100.0;
state.rcin[0] = (mRCPitch - 1000.0) / 1000.0;
state.rcin[1] = (mRCRoll - 1000.0) / 1000.0;
state.rcin[2] = (mRCThrottle - 1000.0) / 1000.0;
state.rcin[3] = (mRCYaw - 1000.0) / 1000.0;
printf("sending: acc(%f %f %f) gyr(%f %f %f) rc(%f %f %f %f)\n",
state.accel[0], state.accel[1], state.accel[2],
gyro.x, gyro.y, gyro.z,
state.rcin[0], state.rcin[1], state.rcin[2], state.rcin[3]);
sock.sendto(&state, sizeof(state), "127.0.0.1", 9003);
}
}
void AP_InertialSensor_QURT::accumulate(void)
{
const float ACCEL_SCALE_1G = GRAVITY_MSS / 2048.0;
const float GYRO_SCALE = 0.0174532 / 16.4;
struct mpu9x50_data data;
while (buf.pop(data)) {
Vector3f accel(data.accel_raw[0]*ACCEL_SCALE_1G,
data.accel_raw[1]*ACCEL_SCALE_1G,
data.accel_raw[2]*ACCEL_SCALE_1G);
Vector3f gyro(data.gyro_raw[0]*GYRO_SCALE,
data.gyro_raw[1]*GYRO_SCALE,
data.gyro_raw[2]*GYRO_SCALE);
_rotate_and_correct_accel(accel_instance, accel);
_rotate_and_correct_gyro(gyro_instance, gyro);
_notify_new_gyro_raw_sample(gyro_instance, gyro, data.timestamp);
_notify_new_accel_raw_sample(accel_instance, accel, data.timestamp);
}
}
static void sensors_debug() {
static int divider = 0;
if (divider++ == 20) {
divider = 0;
float heading = g_compass.calculate_heading(g_ahrs.get_dcm_matrix());
hal.console->printf("ahrs: roll %4.1f pitch %4.1f "
"yaw %4.1f hdg %.1f\r\n",
ToDeg(g_ahrs.roll), ToDeg(g_ahrs.pitch),
ToDeg(g_ahrs.yaw),
g_compass.use_for_yaw() ? ToDeg(heading):0.0f);
Vector3f accel(g_ins.get_accel());
Vector3f gyro(g_ins.get_gyro());
hal.console->printf("mpu6000: accel %.2f %.2f %.2f "
"gyro %.2f %.2f %.2f\r\n",
accel.x, accel.y, accel.z,
gyro.x, gyro.y, gyro.z);
hal.console->printf("compass: heading %.2f deg\r\n",
ToDeg(g_compass.calculate_heading(0, 0)));
}
}
int main(int argc, char* argv[])
{
signal(SIGINT, sigproc);
#ifndef WIN32
signal(SIGQUIT, sigproc);
#endif
bool noHelmet = false;
if((argc > 1) && (argv[1] == std::string("-n"))) noHelmet = true;
UdpTransmitSocket transmitSocket( IpEndpointName( ADDRESS, PORT ) );
char buffer[OUTPUT_BUFFER_SIZE];
emokit_device* d;
d = emokit_create();
printf("Current epoc devices connected: %d\n", emokit_get_count(d, EMOKIT_VID, EMOKIT_PID));
if(emokit_open(d, EMOKIT_VID, EMOKIT_PID, 0) != 0 && !noHelmet)
{
printf("CANNOT CONNECT\n");
return 1;
} else if(noHelmet) {
std::cout << "Sending random data" << std::endl;
}
if (!noHelmet) {
while(true)
{
if(emokit_read_data(d) > 0)
{
emokit_get_next_frame(d);
struct emokit_frame frame = d->current_frame;
std::cout << "\r\33[2K" << "gyroX: " << (int)frame.gyroX
<< "; gyroY: " << (int)frame.gyroY
<< "; F3: " << frame.F3
<< "; FC6: " << frame.FC6
<< "; battery: " << (int)d->battery << "%";
flush(std::cout);
osc::OutboundPacketStream channels( buffer, OUTPUT_BUFFER_SIZE );
osc::OutboundPacketStream gyro( buffer, OUTPUT_BUFFER_SIZE );
osc::OutboundPacketStream info( buffer, OUTPUT_BUFFER_SIZE );
channels << osc::BeginMessage( "/emokit/channels" )
<< frame.F3 << frame.FC6 << frame.P7 << frame.T8 << frame.F7 << frame.F8 << frame.T7 << frame.P8 << frame.AF4 << frame.F4 << frame.AF3 << frame.O2 << frame.O1 << frame.FC5 << osc::EndMessage;
transmitSocket.Send( channels.Data(), channels.Size() );
gyro << osc::BeginMessage( "/emokit/gyro" )
<< (int)frame.gyroX << (int)frame.gyroY << osc::EndMessage;
transmitSocket.Send( gyro.Data(), gyro.Size() );
info << osc::BeginMessage( "/emokit/info" )
<< (int)d->battery;
for (int i = 0; i<14 ; i++) info << (int)d->contact_quality[i];
info << osc::EndMessage;
transmitSocket.Send( info.Data(), info.Size() );
}
}
} else {
while (true) {
usleep(FREQ);
osc::OutboundPacketStream channels( buffer, OUTPUT_BUFFER_SIZE );
osc::OutboundPacketStream gyro( buffer, OUTPUT_BUFFER_SIZE );
osc::OutboundPacketStream info( buffer, OUTPUT_BUFFER_SIZE );
channels << osc::BeginMessage( "/emokit/channels" );
for (int i=0 ; i < 14 ; i++) channels << rand() % 10000;
channels << osc::EndMessage;
transmitSocket.Send( channels.Data(), channels.Size() );
gyro << osc::BeginMessage( "/emokit/gyro" )
<< rand() % 100 << rand() % 100 << osc::EndMessage;
transmitSocket.Send( gyro.Data(), gyro.Size() );
info << osc::BeginMessage( "/emokit/info" )
<< rand() % 100;
for (int i = 0; i<14 ; i++) info << rand() % 50;
info << osc::EndMessage;
transmitSocket.Send( info.Data(), info.Size() );
}
}
emokit_close(d);
emokit_delete(d);
return 0;
}
void main_task(void *args)
{
vTaskSetApplicationTaskTag(xTaskGetIdleTaskHandle(), (pdTASK_HOOK_CODE)1);
vTaskSetApplicationTaskTag(NULL, (pdTASK_HOOK_CODE)2);
led_init();
hal.init(0, NULL);
hal.console->printf("AP_HAL Sensor Test\r\n");
hal.scheduler->register_timer_failsafe(failsafe, 1000);
hal.console->printf("init AP_InertialSensor: ");
g_ins.init(AP_InertialSensor::COLD_START, INS_SAMPLE_RATE, flash_leds);
g_ins.init_accel(flash_leds);
hal.console->println();
led_set(0, false);
hal.console->printf("done\r\n");
hal.console->printf("init AP_Compass: "******"done\r\n");
hal.console->printf("init AP_Baro: ");
g_baro.init();
g_baro.calibrate();
hal.console->printf("done\r\n");
hal.console->printf("init AP_AHRS: ");
g_ahrs.init();
g_ahrs.set_compass(&g_compass);
g_ahrs.set_barometer(&g_baro);
hal.console->printf("done\r\n");
portTickType last_print = 0;
portTickType last_compass = 0;
portTickType last_wake = 0;
float heading = 0.0f;
last_wake = xTaskGetTickCount();
for (;;) {
// Delay to run this loop at 100Hz.
vTaskDelayUntil(&last_wake, 10);
portTickType now = xTaskGetTickCount();
if (last_compass == 0 || now - last_compass > 100) {
last_compass = now;
g_compass.read();
g_baro.read();
heading = g_compass.calculate_heading(g_ahrs.get_dcm_matrix());
}
g_ahrs.update();
if (last_print == 0 || now - last_print > 100) {
last_print = now;
hal.console->write("\r\n");
hal.console->printf("ahrs: roll %4.1f pitch %4.1f yaw %4.1f hdg %.1f\r\n",
ToDeg(g_ahrs.roll), ToDeg(g_ahrs.pitch),
ToDeg(g_ahrs.yaw),
g_compass.use_for_yaw() ? ToDeg(heading) : 0.0f);
Vector3f accel(g_ins.get_accel());
Vector3f gyro(g_ins.get_gyro());
hal.console->printf("mpu6000: accel %.2f %.2f %.2f "
"gyro %.2f %.2f %.2f\r\n",
accel.x, accel.y, accel.z,
gyro.x, gyro.y, gyro.z);
hal.console->printf("compass: heading %.2f deg\r\n",
ToDeg(g_compass.calculate_heading(0, 0)));
g_compass.null_offsets();
if (hal.rcin->valid()) {
uint16_t ppm[PPM_MAX_CHANNELS];
size_t count;
count = hal.rcin->read(ppm, PPM_MAX_CHANNELS);
hal.console->write("ppm: ");
for (size_t i = 0; i < count; ++i)
hal.console->printf("%u ", ppm[i]);
hal.console->write("\r\n");
}
}
}
}
int main(int argc, char **argv)
{
Aria::init();
ArRobot robot;
ArServerBase server;
ArArgumentParser parser(&argc, argv);
ArSimpleConnector simpleConnector(&parser);
ArServerSimpleOpener simpleOpener(&parser);
// parse the command line... fail and print the help if the parsing fails
// or if help was requested
parser.loadDefaultArguments();
if (!simpleConnector.parseArgs() || !simpleOpener.parseArgs() ||
!parser.checkHelpAndWarnUnparsed())
{
simpleConnector.logOptions();
simpleOpener.logOptions();
exit(1);
}
// Set up where we'll look for files such as config file, user/password file,
// etc.
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("Error: Bad user/password/permissions file.\n");
else
printf("Error: Could not open server port. Use -help to see options.\n");
exit(1);
}
// Devices
ArAnalogGyro gyro(&robot);
ArSonarDevice sonarDev;
robot.addRangeDevice(&sonarDev);
ArIRs irs;
robot.addRangeDevice(&irs);
ArBumpers bumpers;
robot.addRangeDevice(&bumpers);
ArSick sick(361, 180);
robot.addRangeDevice(&sick);
ArServerInfoRobot serverInfoRobot(&server, &robot);
ArServerInfoSensor serverInfoSensor(&server, &robot);
// This is the service that provides drawing data to the client.
ArServerInfoDrawings drawings(&server);
// Convenience function that sets up drawings for all the robot's current
// range devices (using default shape and color info)
drawings.addRobotsRangeDevices(&robot);
// Add our custom drawings
drawings.addDrawing(
// shape: color: size: layer:
new ArDrawingData("polyLine", ArColor(255, 0, 0), 2, 49),
"exampleDrawing_Home",
new ArGlobalFunctor2<ArServerClient*, ArNetPacket*>(&exampleHomeDrawingNetCallback)
);
drawings.addDrawing(
new ArDrawingData("polyDots", ArColor(0, 255, 0), 250, 48),
"exampleDrawing_Dots",
new ArGlobalFunctor2<ArServerClient*, ArNetPacket*>(&exampleDotsDrawingNetCallback)
);
drawings.addDrawing(
new ArDrawingData("polySegments", ArColor(0, 0, 0), 4, 52),
"exampleDrawing_XMarksTheSpot",
new ArGlobalFunctor2<ArServerClient*, ArNetPacket*>(&exampleXDrawingNetCallback)
);
drawings.addDrawing(
new ArDrawingData("polyArrows", ArColor(255, 0, 255), 500, 100),
"exampleDrawing_Arrows",
new ArGlobalFunctor2<ArServerClient*, ArNetPacket*>(&exampleArrowsDrawingNetCallback)
);
// modes for moving the robot
ArServerModeStop modeStop(&server, &robot);
ArServerModeDrive modeDrive(&server, &robot);
ArServerModeRatioDrive modeRatioDrive(&server, &robot);
ArServerModeWander modeWander(&server, &robot);
modeStop.addAsDefaultMode();
modeStop.activate();
// Connect to the robot.
if (!simpleConnector.connectRobot(&robot))
{
printf("Error: Could not connect to robot... exiting\n");
Aria::shutdown();
return 1;
}
// set up the laser before handing it to the laser mode
simpleConnector.setupLaser(&sick);
robot.enableMotors();
// start the robot cycle running in a background thread
robot.runAsync(true);
// start the laser processing cycle in a background thread
sick.runAsync();
// connect the laser if it was requested
if (!simpleConnector.connectLaser(&sick))
{
printf("Error: Could not connect to laser... exiting\n");
Aria::shutdown();
return 1;
}
// log whatever we wanted to before the runAsync
simpleOpener.checkAndLog();
// run the server thread in the background
server.runAsync();
printf("Server is now running...\n");
// Add a key handler mostly that windows can exit by pressing
// escape, note that the key handler prevents you from running this program
// in the background on Linux.
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");
}
robot.waitForRunExit();
Aria::shutdown();
exit(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;
}
void main_task(void *pvParameters)
{
(void) pvParameters;
vTaskDelay(500 / portTICK_RATE_MS);
SPIMaster spi5(SPI5, SPI_BAUDRATEPRESCALER_32, 0x2000, {
{MEMS_SPI_SCK_PIN, GPIO_MODE_AF_PP, GPIO_PULLDOWN, GPIO_SPEED_MEDIUM, GPIO_AF5_SPI5},
{MEMS_SPI_MISO_PIN, GPIO_MODE_AF_PP, GPIO_PULLDOWN, GPIO_SPEED_MEDIUM, GPIO_AF5_SPI5},
{MEMS_SPI_MOSI_PIN, GPIO_MODE_AF_PP, GPIO_NOPULL, GPIO_SPEED_MEDIUM, GPIO_AF5_SPI5},
}, {
{GYRO_CS_PIN, GPIO_MODE_OUTPUT_PP, GPIO_PULLUP, GPIO_SPEED_MEDIUM, 0},
{ACCEL_CS_PIN, GPIO_MODE_OUTPUT_PP, GPIO_PULLUP, GPIO_SPEED_MEDIUM, 0},
});
SPIMaster spi2(SPI2, SPI_BAUDRATEPRESCALER_32, 0x2000, {
{EXT_MEMS_SPI_SCK_PIN, GPIO_MODE_AF_PP, GPIO_PULLDOWN, GPIO_SPEED_MEDIUM, GPIO_AF5_SPI2},
{EXT_MEMS_SPI_MISO_PIN, GPIO_MODE_AF_PP, GPIO_PULLDOWN, GPIO_SPEED_MEDIUM, GPIO_AF5_SPI2},
{EXT_MEMS_SPI_MOSI_PIN, GPIO_MODE_AF_PP, GPIO_NOPULL, GPIO_SPEED_MEDIUM, GPIO_AF5_SPI2},
}, {
{EXT_GYRO_CS_PIN, GPIO_MODE_OUTPUT_PP, GPIO_PULLUP, GPIO_SPEED_MEDIUM, 0},
{LPS25HB_PRESSURE_CS_PIN, GPIO_MODE_OUTPUT_PP, GPIO_PULLUP, GPIO_SPEED_MEDIUM, 0},
{BMP280_PRESSURE_CS_PIN, GPIO_MODE_OUTPUT_PP, GPIO_PULLUP, GPIO_SPEED_MEDIUM, 0},
});
L3GD20 gyro(spi5, 0);
LPS25HB lps25hb(spi2, 1);
BMP280 bmp2(spi2, 2);
LSM303D accel(spi5, 1);
uint8_t gyro_wtm = 5;
uint8_t acc_wtm = 8;
TimeStamp console_update_time;
TimeStamp sample_dt;
TimeStamp led_toggle_ts;
FlightControl flight_ctl;
static bool print_to_console = false;
LowPassFilter<Vector3f, float> gyro_lpf({0.5});
LowPassFilter<Vector3f, float> acc_lpf_alt({0.9});
LowPassFilter<Vector3f, float> acc_lpf_att({0.990});
LowPassFilter<float, float> pressure_lpf({0.6});
attitudetracker att;
/*
* Apply the boot configuration from flash memory.
*/
dronestate_boot_config(*drone_state);
L3GD20Reader gyro_reader(gyro, GYRO_INT2_PIN, gyro_align);
LSM303Reader acc_reader(accel, ACC_INT2_PIN, acc_align);
UartRpcServer rpcserver(*drone_state, configdata, acc_reader.mag_calibrator_);
bmp2.set_oversamp_pressure(BMP280_OVERSAMP_16X);
bmp2.set_work_mode(BMP280_ULTRA_HIGH_RESOLUTION_MODE);
bmp2.set_filter(BMP280_FILTER_COEFF_OFF);
Bmp280Reader bmp_reader(bmp2);
HAL_NVIC_SetPriority(DMA1_Stream6_IRQn, 1, 1);
HAL_NVIC_EnableIRQ (DMA1_Stream6_IRQn);
HAL_NVIC_SetPriority(DMA1_Stream5_IRQn, 1, 0);
HAL_NVIC_EnableIRQ (DMA1_Stream5_IRQn);
#ifndef ENABLE_UART_TASK
uart2.uart_dmarx_start();
#endif
printf("Priority Group: %lu\n", NVIC_GetPriorityGrouping());
printf("SysTick_IRQn priority: %lu\n", NVIC_GetPriority(SysTick_IRQn) << __NVIC_PRIO_BITS);
printf("configKERNEL_INTERRUPT_PRIORITY: %d\n", configKERNEL_INTERRUPT_PRIORITY);
printf("configMAX_SYSCALL_INTERRUPT_PRIORITY: %d\n", configMAX_SYSCALL_INTERRUPT_PRIORITY);
printf("LPS25HB Device id: %d\n", lps25hb.Get_DeviceID());
vTaskDelay(500 / portTICK_RATE_MS);
gyro_reader.init(gyro_wtm);
gyro_reader.enable_int2(false);
vTaskDelay(500 / portTICK_RATE_MS);
acc_reader.init(acc_wtm);
acc_reader.enable_int2(false);
acc_reader.mag_calibrator_.set_bias(drone_state->mag_bias_);
acc_reader.mag_calibrator_.set_scale_factor(drone_state->mag_scale_factor_);
vTaskDelay(500 / portTICK_RATE_MS);
printf("Calibrating...");
gyro_reader.enable_int2(true);
gyro_reader.calculate_static_bias_filtered(2400);
printf(" Done!\n");
flight_ctl.start_receiver();
printf("Entering main loop...\n");
gyro_reader.enable_int2(true);
sample_dt.time_stamp();
lps25hb.Set_FifoMode(LPS25HB_FIFO_STREAM_MODE);
lps25hb.Set_FifoModeUse(LPS25HB_ENABLE);
lps25hb.Set_Odr(LPS25HB_ODR_25HZ);
lps25hb.Set_Bdu(LPS25HB_BDU_NO_UPDATE);
LPS25HB_FIFOTypeDef_st fifo_config;
memset(&fifo_config, 0, sizeof(fifo_config));
lps25hb.Get_FifoConfig(&fifo_config);
#ifdef USE_LPS25HB
float base_pressure = lps25hb.Get_PressureHpa();
for (int i = 0; i < 100; i++) {
while (lps25hb.Get_FifoStatus().FIFO_EMPTY)
vTaskDelay(50 / portTICK_RATE_MS);
base_pressure = pressure_lpf.do_filter(lps25hb.Get_PressureHpa());
}
#endif
bmp_reader.calibrate();
// Infinite loop
PerfCounter idle_time;
while (1) {
drone_state->iteration_++;
if (drone_state->iteration_ % 120 == 0) {
led1.toggle();
}
if (drone_state->iteration_ % 4 == 0) {
#ifdef USE_LPS25HB
drone_state->temperature_ = lps25hb.Get_TemperatureCelsius();
while (!lps25hb.Get_FifoStatus().FIFO_EMPTY) {
drone_state->pressure_hpa_ = pressure_lpf.do_filter(lps25hb.Get_PressureHpa());
float alt = (powf(base_pressure/drone_state->pressure_hpa_, 0.1902f) - 1.0f) * ((lps25hb.Get_TemperatureCelsius()) + 273.15f)/0.0065;
drone_state->altitude_ = Distance::from_meters(alt);
}
#else
bmp_reader.pressure_filter_.set_alpha(drone_state->altitude_lpf_);
drone_state->altitude_ = bmp_reader.get_altitude(true);
drone_state->pressure_hpa_ = bmp_reader.get_pressure().hpa();
drone_state->temperature_ = bmp_reader.get_temperature(false).celsius();
#endif
}
idle_time.begin_measure();
gyro_reader.wait_for_data();
idle_time.end_measure();
drone_state->dt_ = sample_dt.elapsed();
sample_dt.time_stamp();
if (drone_state->base_throttle_ > 0.1)
att.accelerometer_correction_speed(drone_state->accelerometer_correction_speed_);
else
att.accelerometer_correction_speed(3.0f);
att.gyro_drift_pid(drone_state->gyro_drift_kp_, drone_state->gyro_drift_ki_, drone_state->gyro_drift_kd_);
att.gyro_drift_leak_rate(drone_state->gyro_drift_leak_rate_);
size_t fifosize = gyro_reader.size();
for (size_t i = 0; i < fifosize; i++)
drone_state->gyro_raw_ = gyro_lpf.do_filter(gyro_reader.read_sample());
if (drone_state->gyro_raw_.length_squared() > 0 && drone_state->dt_.microseconds() > 0) {
drone_state->gyro_ = (drone_state->gyro_raw_ - gyro_reader.bias()) * drone_state->gyro_factor_;
att.track_gyroscope(DEG2RAD(drone_state->gyro_) * 1.0f, drone_state->dt_.seconds_float());
}
fifosize = acc_reader.size();
for (size_t i = 0; i < fifosize; i++) {
Vector3f acc_sample = acc_reader.read_sample_acc();
acc_lpf_att.do_filter(acc_sample);
acc_lpf_alt.do_filter(acc_sample);
}
drone_state->accel_raw_ = acc_lpf_att.output();
drone_state->accel_alt_ = acc_lpf_alt.output();
drone_state->accel_ = (drone_state->accel_raw_ - drone_state->accelerometer_adjustment_).normalize();
#define ALLOW_ACCELEROMETER_OFF
#ifdef ALLOW_ACCELEROMETER_OFF
if (drone_state->track_accelerometer_) {
att.track_accelerometer(drone_state->accel_, drone_state->dt_.seconds_float());
}
#else
att.track_accelerometer(drone_state->accel_, drone_state->dt_.seconds_float());
#endif
#define REALTIME_DATA 0
#if REALTIME_DATA
std::cout << drone_state->gyro_.transpose() << drone_state->accel_.transpose() << drone_state->pid_torque_.transpose();
std::cout << drone_state->dt_.seconds_float() << std::endl;
#endif
drone_state->mag_raw_ = acc_reader.read_sample_mag();
drone_state->mag_ = drone_state->mag_raw_.normalize();
if (drone_state->track_magnetometer_) {
att.track_magnetometer(drone_state->mag_, drone_state->dt_.seconds_float());
}
drone_state->attitude_ = att.get_attitude();
drone_state->gyro_drift_error_ = RAD2DEG(att.get_drift_error());
flight_ctl.update_state(*drone_state);
flight_ctl.send_throttle_to_motors();
if (print_to_console && console_update_time.elapsed() > TimeSpan::from_milliseconds(300)) {
Vector3f drift_err = att.get_drift_error();
console_update_time.time_stamp();
printf("Gyro : %5.3f %5.3f %5.3f\n", drone_state->gyro_.at(0), drone_state->gyro_.at(1), drone_state->gyro_.at(2));
printf("Drift Err : %5.3f %5.3f %5.3f\n", RAD2DEG(drift_err.at(0)), RAD2DEG(drift_err.at(1)), RAD2DEG(drift_err.at(2)));
printf("Gyro Raw : %5.3f %5.3f %5.3f\n", drone_state->gyro_raw_.at(0), drone_state->gyro_raw_.at(1), drone_state->gyro_raw_.at(2));
printf("Accel : %5.3f %5.3f %5.3f\n", drone_state->accel_.at(0), drone_state->accel_.at(1), drone_state->accel_.at(2));
printf("Mag : %5.3f %5.3f %5.3f\n", drone_state->mag_.at(0), drone_state->mag_.at(1), drone_state->mag_.at(2));
printf("dT : %lu uSec\n", (uint32_t)drone_state->dt_.microseconds());
printf("Q : %5.3f %5.3f %5.3f %5.3f\n\n", drone_state->attitude_.w, drone_state->attitude_.x, drone_state->attitude_.y,
drone_state->attitude_.z);
#if 1
printf("Motors : %1.2f %1.2f %1.2f %1.2f\n", drone_state->motors_[0], drone_state->motors_[1],
drone_state->motors_[2], drone_state->motors_[3]);
printf("Throttle : %1.2f\n", drone_state->base_throttle_);
printf("Armed : %d\n", drone_state->motors_armed_);
printf("Altitude : %4.2f m\n", drone_state->altitude_.meters());
printf("GPS : Lon: %3.4f Lat: %3.4f Sat %lu Alt: %4.2f m\n",
drone_state->longitude_.degrees(), drone_state->latitude_.degrees(),
drone_state->satellite_count_, drone_state->gps_altitude_.meters());
printf("Battery : %2.1f V, %2.0f%%\n", drone_state->battery_voltage_.volts(), drone_state->battery_percentage_);
#endif
}
#if 0
if (led_toggle_ts.elapsed() > TimeSpan::from_seconds(1)) {
led_toggle_ts.time_stamp();
led0.toggle();
}
#endif
#ifndef ENABLE_UART_TASK
rpcserver.jsonrpc_request_handler(&uart2);
#endif
}
}
int main(int argc, char **argv)
{
// Initialize Aria and Arnl global information
Aria::init();
Arnl::init();
// The robot object
ArRobot robot;
// Parse the command line arguments.
ArArgumentParser parser(&argc, argv);
// Set up our simpleConnector, to connect to the robot and laser
//ArSimpleConnector simpleConnector(&parser);
ArRobotConnector robotConnector(&parser, &robot);
// Connect to the robot
if (!robotConnector.connectRobot())
{
ArLog::log(ArLog::Normal, "Error: Could not connect to robot... exiting");
Aria::exit(3);
}
// Set up where we'll look for files. Arnl::init() set Aria's default
// directory to Arnl's default directory; addDirectories() appends this
// "examples" directory.
char fileDir[1024];
ArUtil::addDirectories(fileDir, sizeof(fileDir), Aria::getDirectory(),
"examples");
// To direct log messages to a file, or to change the log level, use these calls:
//ArLog::init(ArLog::File, ArLog::Normal, "log.txt", true, true);
//ArLog::init(ArLog::File, ArLog::Verbose);
// Add a section to the configuration to change ArLog parameters
ArLog::addToConfig(Aria::getConfig());
// set up a gyro (if the robot is older and its firmware does not
// automatically incorporate gyro corrections, then this object will do it)
ArAnalogGyro gyro(&robot);
// Our networking server
ArServerBase server;
// Set up our simpleOpener, used to set up the networking server
ArServerSimpleOpener simpleOpener(&parser);
// the laser connector
ArLaserConnector laserConnector(&parser, &robot, &robotConnector);
// Tell the laser connector to always connect the first laser since
// this program always requires a laser.
parser.addDefaultArgument("-connectLaser");
// Load default arguments for this computer (from /etc/Aria.args, environment
// variables, and other places)
parser.loadDefaultArguments();
// Parse arguments
if (!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed())
{
logOptions(argv[0]);
Aria::exit(1);
}
// This causes Aria::exit(9) to be called if the robot unexpectedly
// disconnects
ArGlobalFunctor1<int> shutdownFunctor(&Aria::exit, 9);
robot.addDisconnectOnErrorCB(&shutdownFunctor);
// Create an ArSonarDevice object (ArRangeDevice subclass) and
// connect it to the robot.
ArSonarDevice sonarDev;
robot.addRangeDevice(&sonarDev);
// This object will allow robot's movement parameters to be changed through
// a Robot Configuration section in the ArConfig global configuration facility.
ArRobotConfig robotConfig(&robot);
// Include gyro configuration options in the robot configuration section.
robotConfig.addAnalogGyro(&gyro);
// Start the robot thread.
robot.runAsync(true);
// connect the laser(s) if it was requested, this adds them to the
// robot too, and starts them running in their own threads
if (!laserConnector.connectLasers())
{
ArLog::log(ArLog::Normal, "Could not connect to all lasers... exiting\n");
Aria::exit(2);
}
// find the laser we should use for localization and/or mapping,
// which will be the first laser
robot.lock();
ArLaser *firstLaser = robot.findLaser(1);
if (firstLaser == NULL || !firstLaser->isConnected())
{
ArLog::log(ArLog::Normal, "Did not have laser 1 or it is not connected, cannot start localization and/or mapping... exiting");
Aria::exit(2);
}
robot.unlock();
/* Create and set up map object */
// Set up the map object, this will look for files in the examples
// directory (unless the file name starts with a /, \, or .
// You can take out the 'fileDir' argument to look in the program's current directory
// instead.
// When a configuration file is loaded into ArConfig later, if it specifies a
// map file, then that file will be loaded as the map.
ArMap map(fileDir);
// set it up to ignore empty file names (otherwise if a configuration omits
// the map file, the whole configuration change will fail)
map.setIgnoreEmptyFileName(true);
// ignore the case, so that if someone is using MobileEyes or
// MobilePlanner from Windows and changes the case on a map name,
// it will still work.
map.setIgnoreCase(true);
/* Create localization and path planning threads */
ArPathPlanningTask pathTask(&robot, &sonarDev, &map);
ArLog::log(ArLog::Normal, "Creating laser localization task");
// Laser Monte-Carlo Localization
ArLocalizationTask locTask(&robot, firstLaser, &map);
// Set some options on each laser that the laser connector
// connected to.
std::map<int, ArLaser *>::iterator laserIt;
for (laserIt = robot.getLaserMap()->begin();
laserIt != robot.getLaserMap()->end();
laserIt++)
{
int laserNum = (*laserIt).first;
ArLaser *laser = (*laserIt).second;
// Skip lasers that aren't connected
if(!laser->isConnected())
continue;
// add the disconnectOnError CB to shut things down if the laser
// connection is lost
laser->addDisconnectOnErrorCB(&shutdownFunctor);
// set the number of cumulative readings the laser will take
laser->setCumulativeBufferSize(200);
// add the lasers to the path planning task
pathTask.addRangeDevice(laser, ArPathPlanningTask::BOTH);
// set the cumulative clean offset (so that they don't all fire at once)
laser->setCumulativeCleanOffset(laserNum * 100);
// reset the cumulative clean time (to make the new offset take effect)
laser->resetLastCumulativeCleanTime();
// Add the packet count to the Aria info strings (It will be included in
// MobileEyes custom details so you can monitor whether the laser data is
// being received correctly)
std::string laserPacketCountName;
laserPacketCountName = laser->getName();
laserPacketCountName += " Packet Count";
Aria::getInfoGroup()->addStringInt(
laserPacketCountName.c_str(), 10,
new ArRetFunctorC<int, ArLaser>(laser,
&ArLaser::getReadingCount));
}
// Used for optional multirobot features (see below) (TODO move to multirobot
// example?)
ArClientSwitchManager clientSwitch(&server, &parser);
/* Start the server */
// Open the networking server
if (!simpleOpener.open(&server, fileDir, 240))
{
ArLog::log(ArLog::Normal, "Error: Could not open server.");
exit(2);
}
/* Create various services that provide network access to clients (such as
* MobileEyes), as well as add various additional features to ARNL */
// ARNL can optionally get information about the positions of other robots from a
// "central server" (see central server example program), if command
// line options specifying the address of the central server was given.
// If there is no central server, then the address of each other robot
// can instead be given in the configuration, and the multirobot systems
// will connect to each robot (or "peer") individually.
// TODO move this to multirobot example?
bool usingCentralServer = false;
if(clientSwitch.getCentralServerHostName() != NULL)
usingCentralServer = true;
// if we're using the central server then we want to create the
// multiRobot central classes
if (usingCentralServer)
{
// Make the handler for multi robot information (this sends the
// information to the central server)
//ArServerHandlerMultiRobot *handlerMultiRobot =
new ArServerHandlerMultiRobot(&server, &robot,
&pathTask,
&locTask, &map);
// Normally each robot, and the central server, must all have
// the same map name for the central server to share robot
// information. (i.e. they are operating in the same space).
// This changes the map name that ArServerHandlerMutliRobot
// reports to the central server, in case you want this individual
// robot to load a different map file name, but still report
// the common map file to the central server.
//handlerMultiRobot->overrideMapName("central.map");
// the range device that gets the multi robot information from
// the central server and presents it as virtual range readings
// to ARNL
ArMultiRobotRangeDevice *multiRobotRangeDevice = new ArMultiRobotRangeDevice(&server);
robot.addRangeDevice(multiRobotRangeDevice);
pathTask.addRangeDevice(multiRobotRangeDevice,
ArPathPlanningTask::BOTH);
// Set up options for drawing multirobot information in MobileEyes.
multiRobotRangeDevice->setCurrentDrawingData(
new ArDrawingData("polyDots", ArColor(125, 125, 0),
100, 73, 1000), true);
multiRobotRangeDevice->setCumulativeDrawingData(
new ArDrawingData("polyDots", ArColor(125, 0, 125),
100, 72, 1000), true);
// This sets up the localization to use the known poses of other robots
// for its localization in cases where numerous robots crowd out the map.
locTask.setMultiRobotCallback(multiRobotRangeDevice->getOtherRobotsCB());
}
// if we're not using a central server then create the multirobot peer classes
else
{
// set the path planning so it uses the explicit collision range for how far its planning
pathTask.setUseCollisionRangeForPlanningFlag(true);
// make our thing that gathers information from the other servers
ArServerHandlerMultiRobotPeer *multiRobotPeer = NULL;
ArMultiRobotPeerRangeDevice *multiRobotPeerRangeDevice = NULL;
multiRobotPeerRangeDevice = new ArMultiRobotPeerRangeDevice(&map);
// make our thing that sends information to the other servers
multiRobotPeer = new ArServerHandlerMultiRobotPeer(&server, &robot,
&pathTask, &locTask);
// hook the two together so they both know what priority this robot is
multiRobotPeer->setNewPrecedenceCallback(
multiRobotPeerRangeDevice->getSetPrecedenceCallback());
// hook the two together so they both know what priority this
// robot's fingerprint is
multiRobotPeer->setNewFingerprintCallback(
multiRobotPeerRangeDevice->getSetFingerprintCallback());
// hook the two together so that the range device can call on the
// server handler to change its fingerprint
multiRobotPeerRangeDevice->setChangeFingerprintCB(
multiRobotPeer->getChangeFingerprintCB());
// then add the robot to the places it needs to be
robot.addRangeDevice(multiRobotPeerRangeDevice);
pathTask.addRangeDevice(multiRobotPeerRangeDevice,
ArPathPlanningTask::BOTH);
// Set the range device so that we can see the information its using
// to avoid, you can comment these out in order to not see them
multiRobotPeerRangeDevice->setCurrentDrawingData(
new ArDrawingData("polyDots", ArColor(125, 125, 0),
100, 72, 1000), true);
multiRobotPeerRangeDevice->setCumulativeDrawingData(
new ArDrawingData("polyDots", ArColor(125, 0, 125),
100, 72, 1000), true);
// This sets up the localization to use the known poses of other robots
// for its localization in cases where numerous robots crowd out the map.
locTask.setMultiRobotCallback(
multiRobotPeerRangeDevice->getOtherRobotsCB());
}
/* Add additional range devices to the robot and path planning task (so it
avoids obstacles detected by these devices) */
// Add IR range device to robot and path planning task (so it avoids obstacles
// detected by this device)
robot.lock();
ArIRs irs;
robot.addRangeDevice(&irs);
pathTask.addRangeDevice(&irs, ArPathPlanningTask::CURRENT);
// Add bumpers range device to robot and path planning task (so it avoids obstacles
// detected by this device)
ArBumpers bumpers;
robot.addRangeDevice(&bumpers);
pathTask.addRangeDevice(&bumpers, ArPathPlanningTask::CURRENT);
// Add range device which uses forbidden regions given in the map to give virtual
// range device readings to ARNL. (so it avoids obstacles
// detected by this device)
ArForbiddenRangeDevice forbidden(&map);
robot.addRangeDevice(&forbidden);
pathTask.addRangeDevice(&forbidden, ArPathPlanningTask::CURRENT);
robot.unlock();
// Action to slow down robot when localization score drops but not lost.
ArActionSlowDownWhenNotCertain actionSlowDown(&locTask);
pathTask.getPathPlanActionGroup()->addAction(&actionSlowDown, 140);
// Action to stop the robot when localization is "lost" (score too low)
ArActionLost actionLostPath(&locTask, &pathTask);
pathTask.getPathPlanActionGroup()->addAction(&actionLostPath, 150);
// Arnl uses this object when it must replan its path because its
// path is completely blocked. It will use an older history of sensor
// readings to replan this new path. This should not be used with SONARNL
// since sonar readings are not accurate enough and may prevent the robot
// from planning through space that is actually clear.
ArGlobalReplanningRangeDevice replanDev(&pathTask);
// Service to provide drawings of data in the map display :
ArServerInfoDrawings drawings(&server);
drawings.addRobotsRangeDevices(&robot);
drawings.addRangeDevice(&replanDev);
/* Draw a box around the local path planning area use this
(You can enable this particular drawing from custom commands
which is set up down below in ArServerInfoPath) */
ArDrawingData drawingDataP("polyLine", ArColor(200,200,200), 1, 75);
ArFunctor2C<ArPathPlanningTask, ArServerClient *, ArNetPacket *>
drawingFunctorP(&pathTask, &ArPathPlanningTask::drawSearchRectangle);
drawings.addDrawing(&drawingDataP, "Local Plan Area", &drawingFunctorP);
/* Show the sample points used by MCL */
ArDrawingData drawingDataL("polyDots", ArColor(0,255,0), 100, 75);
ArFunctor2C<ArLocalizationTask, ArServerClient *, ArNetPacket *>
drawingFunctorL(&locTask, &ArLocalizationTask::drawRangePoints);
drawings.addDrawing(&drawingDataL, "Localization Points", &drawingFunctorL);
// "Custom" commands. You can add your own custom commands here, they will
// be available in MobileEyes' custom commands (enable in the toolbar or
// access through Robot Tools)
ArServerHandlerCommands commands(&server);
// These provide various kinds of information to the client:
ArServerInfoRobot serverInfoRobot(&server, &robot);
ArServerInfoSensor serverInfoSensor(&server, &robot);
ArServerInfoPath serverInfoPath(&server, &robot, &pathTask);
serverInfoPath.addSearchRectangleDrawing(&drawings);
serverInfoPath.addControlCommands(&commands);
// Provides localization info and allows the client (MobileEyes) to relocalize at a given
// pose:
ArServerInfoLocalization serverInfoLocalization(&server, &robot, &locTask);
ArServerHandlerLocalization serverLocHandler(&server, &robot, &locTask);
// If you're using MobileSim, ArServerHandlerLocalization sends it a command
// to move the robot's true pose if you manually do a localization through
// MobileEyes. To disable that behavior, use this constructor call instead:
// ArServerHandlerLocalization serverLocHandler(&server, &robot, true, false);
// The fifth argument determines whether to send the command to MobileSim.
// Provide the map to the client (and related controls):
ArServerHandlerMap serverMap(&server, &map);
// These objects add some simple (custom) commands to 'commands' for testing and debugging:
ArServerSimpleComUC uCCommands(&commands, &robot); // Send any command to the microcontroller
ArServerSimpleComMovementLogging loggingCommands(&commands, &robot); // configure logging
ArServerSimpleComLogRobotConfig configCommands(&commands, &robot); // trigger logging of the robot config parameters
// ArServerSimpleServerCommands serverCommands(&commands, &server); // monitor networking behavior (track packets sent etc.)
// service that allows the client to monitor the communication link status
// between the robot and the client.
//
ArServerHandlerCommMonitor handlerCommMonitor(&server);
// service that allows client to change configuration parameters in ArConfig
ArServerHandlerConfig handlerConfig(&server, Aria::getConfig(),
Arnl::getTypicalDefaultParamFileName(),
Aria::getDirectory());
/* Set up the possible modes for remote control from a client such as
* MobileEyes:
*/
// Mode To go to a goal or other specific point:
ArServerModeGoto modeGoto(&server, &robot, &pathTask, &map,
locTask.getRobotHome(),
locTask.getRobotHomeCallback());
// Mode To stop and remain stopped:
ArServerModeStop modeStop(&server, &robot);
// Cause the sonar to turn off automatically
// when the robot is stopped, and turn it back on when commands to move
// are sent. (Note, if using SONARNL to localize, then don't do this
// since localization may get lost)
ArSonarAutoDisabler sonarAutoDisabler(&robot);
// Teleoperation modes To drive by keyboard, joystick, etc:
ArServerModeRatioDrive modeRatioDrive(&server, &robot);
// ArServerModeDrive modeDrive(&server, &robot); // Older mode for compatability
// Prevent normal teleoperation driving if localization is lost using
// a high-priority action, which enables itself when the particular mode is
// active.
// (You have to enter unsafe drive mode to drive when lost.)
ArActionLost actionLostRatioDrive(&locTask, &pathTask, &modeRatioDrive);
modeRatioDrive.getActionGroup()->addAction(&actionLostRatioDrive, 110);
// Add drive mode section to the configuration, and also some custom (simple) commands:
modeRatioDrive.addToConfig(Aria::getConfig(), "Teleop settings");
modeRatioDrive.addControlCommands(&commands);
// Wander mode (also prevent wandering if lost):
ArServerModeWander modeWander(&server, &robot);
ArActionLost actionLostWander(&locTask, &pathTask, &modeWander);
modeWander.getActionGroup()->addAction(&actionLostWander, 110);
// This provides a small table of interesting information for the client
// to display to the operator. You can add your own callbacks to show any
// data you want.
ArServerInfoStrings stringInfo(&server);
Aria::getInfoGroup()->addAddStringCallback(stringInfo.getAddStringFunctor());
// Provide a set of informational data (turn on in MobileEyes with
// View->Custom Details)
Aria::getInfoGroup()->addStringInt(
"Motor Packet Count", 10,
new ArConstRetFunctorC<int, ArRobot>(&robot,
&ArRobot::getMotorPacCount));
Aria::getInfoGroup()->addStringDouble(
"Laser Localization Score", 8,
new ArRetFunctorC<double, ArLocalizationTask>(
&locTask, &ArLocalizationTask::getLocalizationScore),
"%.03f");
Aria::getInfoGroup()->addStringInt(
"Laser Loc Num Samples", 8,
new ArRetFunctorC<int, ArLocalizationTask>(
&locTask, &ArLocalizationTask::getCurrentNumSamples),
"%4d");
// Display gyro status if gyro is enabled and is being handled by the firmware (gyro types 2, 3, or 4).
// (If the firmware detects an error communicating with the gyro or IMU it
// returns a flag, and stops using it.)
// (This gyro type parameter, and fault flag, are only in ARCOS, not Seekur firmware)
if(robot.getOrigRobotConfig() && robot.getOrigRobotConfig()->getGyroType() > 1)
{
Aria::getInfoGroup()->addStringString(
"Gyro/IMU Status", 10,
new ArGlobalRetFunctor1<const char*, ArRobot*>(&getGyroStatusString, &robot)
);
}
// Setup the dock if there is a docking system on board.
ArServerModeDock *modeDock = NULL;
modeDock = ArServerModeDock::createDock(&server, &robot, &locTask,
&pathTask);
if (modeDock != NULL)
{
modeDock->checkDock();
modeDock->addAsDefaultMode();
modeDock->addToConfig(Aria::getConfig());
modeDock->addControlCommands(&commands);
}
// Make Stop mode the default (If current mode deactivates without entering
// a new mode, then Stop Mode will be selected)
modeStop.addAsDefaultMode();
// TODO move up near where stop mode is created?
/* Services that allow the client to initiate scanning with the laser to
create maps in Mapper3 (So not possible with SONARNL): */
ArServerHandlerMapping handlerMapping(&server, &robot, firstLaser,
fileDir, "", true);
// make laser localization stop while mapping
handlerMapping.addMappingStartCallback(
new ArFunctor1C<ArLocalizationTask, bool>
(&locTask, &ArLocalizationTask::setIdleFlag, true));
// and then make it start again when we're doine
handlerMapping.addMappingEndCallback(
new ArFunctor1C<ArLocalizationTask, bool>
(&locTask, &ArLocalizationTask::setIdleFlag, false));
// Make it so our "lost" actions don't stop us while mapping
handlerMapping.addMappingStartCallback(actionLostPath.getDisableCB());
handlerMapping.addMappingStartCallback(actionLostRatioDrive.getDisableCB());
handlerMapping.addMappingStartCallback(actionLostWander.getDisableCB());
// And then let them make us stop as usual when done mapping
handlerMapping.addMappingEndCallback(actionLostPath.getEnableCB());
handlerMapping.addMappingEndCallback(actionLostRatioDrive.getEnableCB());
handlerMapping.addMappingEndCallback(actionLostWander.getEnableCB());
// don't let forbidden lines show up as obstacles while mapping
// (they'll just interfere with driving while mapping, and localization is off anyway)
handlerMapping.addMappingStartCallback(forbidden.getDisableCB());
// let forbidden lines show up as obstacles again as usual after mapping
handlerMapping.addMappingEndCallback(forbidden.getEnableCB());
/*
// If we are on a simulator, move the robot back to its starting position,
// and reset its odometry.
// This will allow localizeRobotAtHomeBlocking() below will (probably) work (it
// tries current odometry (which will be 0,0,0) and all the map
// home points.
// (Ignored by a real robot)
//robot.com(ArCommands::SIM_RESET);
*/
// create a pose storage class, this will let the program keep track
// of where the robot is between runs... after we try and restore
// from this file it will start saving the robot's pose into the
// file
ArPoseStorage poseStorage(&robot);
/// if we could restore the pose from then set the sim there (this
/// won't do anything to the real robot)... if we couldn't restore
/// the pose then just reset the position of the robot (which again
/// won't do anything to the real robot)
if (poseStorage.restorePose("robotPose"))
serverLocHandler.setSimPose(robot.getPose());
else
robot.com(ArCommands::SIM_RESET);
/* File transfer services: */
#ifdef WIN32
// Not implemented for Windows yet.
ArLog::log(ArLog::Normal, "Note, file upload/download services are not implemented for Windows; not enabling them.");
#else
// This block will allow you to set up where you get and put files
// to/from, just comment them out if you don't want this to happen
// /*
ArServerFileLister fileLister(&server, fileDir);
ArServerFileToClient fileToClient(&server, fileDir);
ArServerFileFromClient fileFromClient(&server, fileDir, "/tmp");
ArServerDeleteFileOnServer deleteFileOnServer(&server, fileDir);
// */
#endif
/* Video image streaming, and camera controls (Requires SAVserver or ACTS) */
// Forward any video if either ACTS or SAV server are running.
// You can find out more about SAV and ACTS on our website
// http://robots.activmedia.com. ACTS is for color tracking and is
// a seperate product. SAV just does software A/V transmitting and is
// free to all our customers. Just run ACTS or SAV server before you
// start this program and this class here will forward video from the
// server to the client.
ArHybridForwarderVideo videoForwarder(&server, "localhost", 7070);
// make a camera to use in case we have video. the camera collection collects
// multiple ptz cameras
ArPTZ *camera = NULL;
ArServerHandlerCamera *handlerCamera = NULL;
ArCameraCollection *cameraCollection = NULL;
// if we have video then set up a camera
if (videoForwarder.isForwardingVideo())
{
cameraCollection = new ArCameraCollection();
cameraCollection->addCamera("Cam1", "PTZ", "Camera", "PTZ");
videoForwarder.setCameraName("Cam1");
videoForwarder.addToCameraCollection(*cameraCollection);
camera = new ArVCC4(&robot); //, invertedCamera, ArVCC4::COMM_UNKNOWN, true, true);
// To use an RVision SEE camera instead:
// camera = new ArRVisionPTZ(&robot);
camera->init();
handlerCamera = new ArServerHandlerCamera("Cam1",
&server,
&robot,
camera,
cameraCollection);
pathTask.addGoalFinishedCB(
new ArFunctorC<ArServerHandlerCamera>(
handlerCamera,
&ArServerHandlerCamera::cameraModeLookAtGoalClearGoal));
}
// After all of the cameras / videos have been created and added to the collection,
// then start the collection server.
//
if (cameraCollection != NULL) {
new ArServerHandlerCameraCollection(&server, cameraCollection);
}
/* Load configuration values, map, and begin! */
// When parsing the configuration file, also look at the program's command line options
// from the command-line argument parser as well as the configuration file.
// (So you can use any argument on the command line, namely -map.)
Aria::getConfig()->useArgumentParser(&parser);
puts("xxx");puts("aaa"); fflush(stdout);
// Read in parameter files.
ArLog::log(ArLog::Normal, "Loading config file %s into ArConfig (base directory %s)...", Arnl::getTypicalParamFileName(), Aria::getConfig()->getBaseDirectory());
if (!Aria::getConfig()->parseFile(Arnl::getTypicalParamFileName()))
{
ArLog::log(ArLog::Normal, "Trouble loading configuration file, exiting");
Aria::exit(5);
}
// Warn about unknown params.
if (!simpleOpener.checkAndLog() || !parser.checkHelpAndWarnUnparsed())
{
logOptions(argv[0]);
Aria::exit(6);
}
// Warn if there is no map
if (map.getFileName() == NULL || strlen(map.getFileName()) <= 0)
{
ArLog::log(ArLog::Normal, "");
ArLog::log(ArLog::Normal, "### No map file is set up, you can make a map with the following procedure");
ArLog::log(ArLog::Normal, " 0) You can find this information in README.txt or docs/Mapping.txt");
ArLog::log(ArLog::Normal, " 1) Connect to this server with MobileEyes");
ArLog::log(ArLog::Normal, " 2) Go to Tools->Map Creation->Start Scan");
ArLog::log(ArLog::Normal, " 3) Give the map a name and hit okay");
ArLog::log(ArLog::Normal, " 4) Drive the robot around your space (see docs/Mapping.txt");
ArLog::log(ArLog::Normal, " 5) Go to Tools->Map Creation->Stop Scan");
ArLog::log(ArLog::Normal, " 6) Start up Mapper3");
ArLog::log(ArLog::Normal, " 7) Go to File->Open on Robot");
ArLog::log(ArLog::Normal, " 8) Select the .2d you created");
ArLog::log(ArLog::Normal, " 9) Create a .map");
ArLog::log(ArLog::Normal, " 10) Go to File->Save on Robot");
ArLog::log(ArLog::Normal, " 11) In MobileEyes, go to Tools->Robot Config");
ArLog::log(ArLog::Normal, " 12) Choose the Files section");
ArLog::log(ArLog::Normal, " 13) Enter the path and name of your new .map file for the value of the Map parameter.");
ArLog::log(ArLog::Normal, " 14) Press OK and your new map should become the map used");
ArLog::log(ArLog::Normal, "");
}
// Print a log message notifying user of the directory for map files
ArLog::log(ArLog::Normal, "");
ArLog::log(ArLog::Normal,
"Directory for maps and file serving: %s", fileDir);
ArLog::log(ArLog::Normal, "See the ARNL README.txt for more information");
ArLog::log(ArLog::Normal, "");
// Do an initial localization of the robot. It tries all the home points
// in the map, as well as the robot's current odometric position, as possible
// places the robot is likely to be at startup. If successful, it will
// also save the position it found to be the best localized position as the
// "Home" position, which can be obtained from the localization task (and is
// used by the "Go to home" network request).
locTask.localizeRobotAtHomeBlocking();
// Let the client switch manager (for multirobot) spin off into its own thread
// TODO move to multirobot example?
clientSwitch.runAsync();
// Start the networking server's thread
server.runAsync();
// Add a key handler so that you can exit by pressing
// escape. Note that this key handler, however, prevents this program from
// running in the background (e.g. as a system daemon or run from
// the shell with "&") -- it will lock up trying to read the keys;
// remove this if you wish to be able to run this program in the background.
ArKeyHandler *keyHandler;
if ((keyHandler = Aria::getKeyHandler()) == NULL)
{
keyHandler = new ArKeyHandler;
Aria::setKeyHandler(keyHandler);
robot.lock();
robot.attachKeyHandler(keyHandler);
robot.unlock();
puts("Server running. To exit, press escape.");
}
ArnlASyncTaskExample asyncTaskExample(&pathTask, &robot, &modeGoto, &parser);
// Enable the motors and wait until the robot exits (disconnection, etc.) or this program is
// canceled.
robot.enableMotors();
robot.waitForRunExit();
Aria::exit(0);
}
int main(int argc, const char * argv[]) {
std::ofstream to_log(to_log_file_path.c_str());
std::ofstream to_rpy(rpy_file_path.c_str());
std::ofstream gps_out(gps_out_path.c_str());
std::ofstream gps_filter_out(gps_filter_out_path.c_str());
std::ofstream mag_out(mag_out_path.c_str());
try {
// Declaring objects used for inertial navigation
ACCELEROMETER acc(acc_file_path, Captor::COLD_START);
GYRO gyro(gyro_file_path, Captor::COLD_START);
MAGNETOMETER mag(mag_file_path, Captor::COLD_START);
GPS gps(gps_file_path,GPS::UBLOX);
GPS_Filter gps_filter(&gps);
EKF ekf(&gps_filter,&acc,&gyro,&mag);
// Getting offset and gain for accelerometer, gyroscope and magnetometer. Getting GPS HOME field.
// This routine will warn you if one of the offset couldn't be updated
int ret = acc.initOffsets();
if (ret != 1) std::cout << "Couldn't get acc offsets !" << std::endl;
else std::cout << "Acc offsets updated" << std::endl;
ret = acc.initGain();
if (ret != 1) std::cout << "Couldn't get acc gain !" << std::endl;
else std::cout << "Acc gain updated" << std::endl;
// Advanced calibration for acc (offset and gain)
#ifdef ADCALIBRATION
ret = acc.performAdvancedAccCalibration();
if (ret !=1) std::cout << "Couldn't perform acc advanced calibration method" << std::endl;
else {
std::cout << "Acc advanced calibration performed" << std::endl;
acc.correct_GN();
}
#endif
ret = gyro.initOffsets();
if (ret != 1) std::cout << "Couldn't get gyro offsets !" << std::endl;
else std::cout << "Gyro offsets updated" << std::endl;
ret = gyro.initGain();
if (ret != 1) std::cout << "Couldn't get gyro gain !" << std::endl;
else std::cout << "Gyro gain updated" << std::endl;
ret = mag.initOffsets();
if (ret != 1) std::cout << "Couldn't get magnetometer offsets !" << std::endl;
else std::cout << "Mag offsets updated" << std::endl;
ret = gps.setHome();
if (ret != 1) std::cout << "Couldn't get HOME !" << std::endl;
else std::cout << "HOME updated" << std::endl;
// Reading lines
// We create the buffer for the sensor's output values
Eigen::Vector3f acc_vector_buffer;
Eigen::Vector3f gyro_vector_buffer;
Eigen::Vector3d gps_buffer_vector;
Eigen::Vector3d gps_position_vector;
// We update and correct the magnetometer
mag._init_earth_even_magnetic_field();
mag.update_correct();
// We initialize the state vector giving the initial heading
ekf.init_state_vector(mag.getHeading());
//ekf.init_state_vector();
to_rpy << TODEG*(ekf.toRPY(ekf.get_state_vector())).transpose() << std::endl; // Storing operation
while (!acc.line_end && !gyro.line_end){
// Reading from inertial captors and correcting the outputs
acc.getOutput(&acc_vector_buffer);
acc.correctOutput(&acc_vector_buffer, ekf.getDCM());
gyro.getOutput(&gyro_vector_buffer);
gyro.correctOutput(&gyro_vector_buffer);
// Updating and correcting magnetometer
mag.update_correct();
// Updating GPS and storing into the &gps_buffer_vector
gps.update(&gps_buffer_vector);
// Predicting the state vector
// Extended Kalman Filter according step is the prediction step. We first build the Jacobian matrix linearized around the current state and we then multiply the currrent state vector by such a matrix
ekf.build_jacobian_matrix(&acc_vector_buffer, &gyro_vector_buffer);
ekf.predict();
to_rpy << TODEG*(ekf.getRPY()).transpose() << std::endl; // Storing operation for data exploitation
to_log << ekf.get_state_vector().transpose() << std::endl; // Storing operation for data exploitation
//mag_out << TODEG*mag.getHeading() << std::endl; // Storing operation for data exploitation
mag_out << TODEG*mag.getHeading((ekf.getRPY())(1),(ekf.getRPY())(0)) << std::endl; // Storing operation for data exploitation
// Checking inf a new gps data is availaible
if (gps.isAvailable()){
// If yes, we first update the GPS filter
gps.calculatePositionFromHome(&gps_buffer_vector);
gps_out << gps.getActualPosition().transpose() << std::endl;
gps.actualizeInternDatas(&gps_buffer_vector);
gps_filter.predict();
gps_filter.updateFilter();
gps_filter_out << gps_filter.getState().transpose() << std::endl; // Storing operation for data exploitation
// And we then correct the EKF filter by correcting in order position, speed and quaternion attitude vector
//ekf.correct();
//gps_filter.updateSpeedEKF(ekf.getCurrentSpeed());
}
}
}
catch (std::exception const& e)
{ std::cout << e.what() << " file " << std::endl; }
return 0;
}
//******************************************************************************
void SHSensors::handleUpdate()
{
const float fToC = 9.0 / 5.0;
const float fOffset = 32;
//*** read all data ***
while ( imu_->IMURead() )
{
//*** get IMU data ***
RTIMU_DATA imuData = imu_->getIMUData();
//*** pressure / temperature ***
if ( ((enabled_ & IMU_PRESSURE) || (enabled_ & IMU_TEMP)) && pressure_ )
{
pressure_->pressureRead( imuData );
if ( enabled_ & IMU_PRESSURE )
{
//*** pressure in hPa ***
emit pressure( imuData.pressure, RTMath::convertPressureToHeight(imuData.pressure) );
}
if ( enabled_ & IMU_TEMP )
{
//*** temp celsius, fahrenheit ***
emit temperature( imuData.temperature,
imuData.temperature * fToC + fOffset );
}
}
//*** humidity ***
if ( (enabled_ & IMU_HUMIDITY) && humidity_ )
{
humidity_->humidityRead( imuData );
//*** relative humidity ***
emit humidity( imuData.humidity );
}
//*** gyroscope ***
if ( enabled_ & IMU_GYRO )
{
//*** gyroscope in degrees per second ***
emit gyro( imuData.gyro.x() * RTMATH_RAD_TO_DEGREE,
imuData.gyro.y() * RTMATH_RAD_TO_DEGREE,
imuData.gyro.z() * RTMATH_RAD_TO_DEGREE );
}
//*** accelerometer ***
if ( enabled_ & IMU_ACCEL )
{
//*** acceleration in g's ***'
emit accel( imuData.accel.x(), imuData.accel.y(),
imuData.accel.z(), imuData.accel.length() );
}
//*** compass ***
if ( enabled_ & IMU_COMPASS )
{
//*** compas (magnetometer) in uT ***
emit compass( imuData.compass.x(), imuData.compass.y(),
imuData.compass.z(), imuData.compass.length() );
}
//*** always emit fusion data - degrees ***
float fusionX = imuData.fusionPose.x() * RTMATH_RAD_TO_DEGREE;
float fusionY = imuData.fusionPose.y() * RTMATH_RAD_TO_DEGREE;
float fusionZ = imuData.fusionPose.z() * RTMATH_RAD_TO_DEGREE;
emit fusionPose( fusionX, fusionY, fusionZ );
}
}
int main(int argc, char **argv)
{
Aria::init();
//ArLog::init(ArLog::StdOut, ArLog::Verbose);
// robot
ArRobot robot;
/// our server
ArServerBase server;
// set up our parser
ArArgumentParser parser(&argc, argv);
// set up our simple connector
ArSimpleConnector simpleConnector(&parser);
// set up a gyro
ArAnalogGyro gyro(&robot);
// load the default arguments
parser.loadDefaultArguments();
// parse the command line... fail and print the help if the parsing fails
// or if the help was requested
if (!simpleConnector.parseArgs() || !parser.checkHelpAndWarnUnparsed())
{
simpleConnector.logOptions();
exit(1);
}
if (!server.loadUserInfo("userServerTest.userInfo"))
{
printf("Could not load user info, exiting\n");
exit(1);
}
server.logUsers();
// first open the server up
if (!server.open(7272))
{
printf("Could not open server port\n");
exit(1);
}
// sonar, must be added to the robot
ArSonarDevice sonarDev;
// add the sonar to the robot
robot.addRangeDevice(&sonarDev);
ArIRs irs;
robot.addRangeDevice(&irs);
ArBumpers bumpers;
robot.addRangeDevice(&bumpers);
// a laser in case one is used
ArSick sick(361, 180);
// add the laser to the robot
robot.addRangeDevice(&sick);
// attach stuff to the server
ArServerInfoRobot serverInfoRobot(&server, &robot);
ArServerInfoSensor serverInfoSensor(&server, &robot);
ArServerInfoDrawings drawings(&server);
drawings.addRobotsRangeDevices(&robot);
// ways of moving the robot
ArServerModeStop modeStop(&server, &robot);
ArServerModeDrive modeDrive(&server, &robot);
ArServerModeRatioDrive modeRatioDrive(&server, &robot);
ArServerModeWander modeWander(&server, &robot);
modeStop.addAsDefaultMode();
modeStop.activate();
// set up the simple commands
ArServerHandlerCommands commands(&server);
// add the commands for the microcontroller
ArServerSimpleComUC uCCommands(&commands, &robot);
// add the commands for logging
ArServerSimpleComMovementLogging loggingCommands(&commands, &robot);
// add the commands for the gyro
ArServerSimpleComGyro gyroCommands(&commands, &robot, &gyro);
// add the commands to enable and disable safe driving to the simple commands
modeDrive.addControlCommands(&commands);
// Forward any video if we have some to forward.. this will forward
// from SAV or ACTS, you can find both on our website
// http://robots.activmedia.com, ACTS is for color tracking and is
// charged for but SAV just does software A/V transmitting and is
// free to all our customers... just run ACTS or SAV before you
// start this program and this class here will forward video from it
// to MobileEyes
ArHybridForwarderVideo videoForwarder(&server, "localhost", 7070);
// make a camera to use in case we have video
ArPTZ *camera = NULL;
ArServerHandlerCamera *handlerCamera = NULL;
// if we have video then set up a camera
if (videoForwarder.isForwardingVideo())
{
bool invertedCamera = false;
camera = new ArVCC4(&robot, invertedCamera,
ArVCC4::COMM_UNKNOWN, true, true);
camera->init();
handlerCamera = new ArServerHandlerCamera(&server, &robot, camera);
}
server.logCommandGroups();
server.logCommandGroupsToFile("userServerTest.commandGroups");
// now let it spin off in its own thread
server.runAsync();
// set up the robot for connecting
if (!simpleConnector.connectRobot(&robot))
{
printf("Could not connect to robot... exiting\n");
Aria::shutdown();
return 1;
}
// set up the laser before handing it to the laser mode
simpleConnector.setupLaser(&sick);
robot.enableMotors();
// start the robot running, true so that if we lose connection the run stops
robot.runAsync(true);
sick.runAsync();
// connect the laser if it was requested
if (!simpleConnector.connectLaser(&sick))
{
printf("Could not connect to laser... exiting\n");
Aria::shutdown();
return 1;
}
robot.waitForRunExit();
// now exit
Aria::shutdown();
exit(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)
{
// Initialize Aria and Arnl global information
Aria::init();
Arnl::init();
// You can change default ArLog options in this call, but the settings in the parameter file
// (arnl.p) which is loaded below (Aria::getConfig()->parseFile()) will override the options.
//ArLog::init(ArLog::File, ArLog::Normal, "log.txt", true, true);
// Used to parse the command line arguments.
ArArgumentParser parser(&argc, argv);
// Load default arguments for this computer (from /etc/Aria.args, environment
// variables, and other places)
parser.loadDefaultArguments();
#ifdef ARNL_LASER
// Tell the laser connector to always connect the first laser since
// this program always requires a laser.
parser.addDefaultArgument("-connectLaser");
#endif
// The robot object
ArRobot robot;
// handle messages from robot controller firmware and log the contents
robot.addPacketHandler(new ArGlobalRetFunctor1<bool, ArRobotPacket*>(&handleDebugMessage));
// This object is used to connect to the robot, which can be configured via
// command line arguments.
ArRobotConnector robotConnector(&parser, &robot);
// Connect to the robot
if (!robotConnector.connectRobot())
{
ArLog::log(ArLog::Normal, "Error: Could not connect to robot... exiting");
Aria::exit(3);
}
// Set up where we'll look for files. Arnl::init() set Aria's default
// directory to Arnl's default directory; addDirectories() appends this
// "examples" directory.
char fileDir[1024];
ArUtil::addDirectories(fileDir, sizeof(fileDir), Aria::getDirectory(),
"examples");
// To direct log messages to a file, or to change the log level, use these calls:
//ArLog::init(ArLog::File, ArLog::Normal, "log.txt", true, true);
//ArLog::init(ArLog::File, ArLog::Verbose);
// Add a section to the configuration to change ArLog parameters
ArLog::addToConfig(Aria::getConfig());
// set up a gyro (if the robot is older and its firmware does not
// automatically incorporate gyro corrections, then this object will do it)
ArAnalogGyro gyro(&robot);
// Our networking server
ArServerBase server;
#ifdef ARNL_GPSLOC
// GPS connector.
ArGPSConnector gpsConnector(&parser);
#endif
// Set up our simpleOpener, used to set up the networking server
ArServerSimpleOpener simpleOpener(&parser);
#ifdef ARNL_LASER
// the laser connector
ArLaserConnector laserConnector(&parser, &robot, &robotConnector);
#endif
// used to connect to camera PTZ control
ArPTZConnector ptzConnector(&parser, &robot);
#ifdef ARNL_MULTIROBOT
// Used to connect to a "central server" which can be used as a proxy
// for multiple robot servers, and as a way for them to also communicate with
// each other. (objects implementing some of these inter-robot communication
// features are created below).
// NOTE: If the central server is running on the same host as robot server(s),
// then you must use the -serverPort argument to instruct these robot-control
// server(s) to use different ports than the default 7272, since the central
// server will use that port.
ArClientSwitchManager clientSwitch(&server, &parser);
#endif
// Load default arguments for this computer (from /etc/Aria.args, environment
// variables, and other places)
parser.loadDefaultArguments();
// Parse arguments
if (!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed())
{
logOptions(argv[0]);
Aria::exit(1);
}
// This causes Aria::exit(9) to be called if the robot unexpectedly
// disconnects
ArGlobalFunctor1<int> shutdownFunctor(&Aria::exit, 9);
robot.addDisconnectOnErrorCB(&shutdownFunctor);
// Create an ArSonarDevice object (ArRangeDevice subclass) and
// connect it to the robot.
ArSonarDevice sonarDev;
robot.addRangeDevice(&sonarDev);
// This object will allow robot's movement parameters to be changed through
// a Robot Configuration section in the ArConfig global configuration facility.
ArRobotConfig robotConfig(&robot);
// Include gyro configuration options in the robot configuration section.
robotConfig.addAnalogGyro(&gyro);
// Start the robot thread.
robot.runAsync(true);
#ifdef ARNL_GPSLOC
// On the Seekur, power to the GPS receiver is switched on by this command.
// (A third argument of 0 would turn it off). On other robots this command is
// ignored. If this fails, you may need to reset the port with ARIA demo or
// seekurPower program (turn port off then on again). If the port is already
// on, it will have no effect on the GPS (it will remain powered.)
// Do this now before connecting to lasers to give it plenty of time to power
// on, initialize, and find a good position before GPS localization begins.
ArLog::log(ArLog::Normal, "Turning on GPS power... (Seekur/Seekur Jr. power port 6)");
robot.com2Bytes(116, 6, 1);
#endif
#ifdef ARNL_LASER
// connect the laser(s) if it was requested, this adds them to the
// robot too, and starts them running in their own threads
ArLog::log(ArLog::Normal, "Connecting to laser(s) configured in parameters...");
if (!laserConnector.connectLasers())
{
ArLog::log(ArLog::Normal, "Error: Could not connect to laser(s). Exiting.");
Aria::exit(2);
}
ArLog::log(ArLog::Normal, "Done connecting to laser(s).");
#endif
#if defined(ARNL_LASERLOC) || defined(ARNL_MAPPING)
// find the laser we should use for localization and/or mapping,
// which will be the first laser
robot.lock();
ArLaser *firstLaser = robot.findLaser(1);
if (firstLaser == NULL || !firstLaser->isConnected())
{
ArLog::log(ArLog::Normal, "Did not have laser 1 or it is not connected, cannot start localization and/or mapping... exiting");
Aria::exit(2);
}
robot.unlock();
#endif
/* Create and set up map object */
// Set up the map object, this will look for files in the examples
// directory (unless the file name starts with a /, \, or .
// You can take out the 'fileDir' argument to look in the program's current directory
// instead.
// When a configuration file is loaded into ArConfig later, if it specifies a
// map file, then that file will be loaded as the map.
ArMap map(fileDir);
// set it up to ignore empty file names (otherwise if a configuration omits
// the map file, the whole configuration change will fail)
map.setIgnoreEmptyFileName(true);
// ignore the case, so that if someone is using MobileEyes or
// MobilePlanner from Windows and changes the case on a map name,
// it will still work.
map.setIgnoreCase(true);
/* Create localization threads */
#ifdef ARNL_MULTILOC
ArLocalizationManager locManager(&robot, &map);
#define LOCTASK locManager
#endif
#ifdef ARNL_LASERLOC
ArLog::log(ArLog::Normal, "Creating laser localization task");
// Laser Monte-Carlo Localization
ArLocalizationTask locTask(&robot, firstLaser, &map);
#ifdef ARNL_MULTILOC
locManager.addLocalizationTask(&locTask);
#else
#define LOCTASK locTask
#endif
#endif
#ifdef ARNL_SONARLOC
ArLog::log(ArLog::Normal, "Creating sonar localization task");
ArSonarLocalizationTask locTask(&robot, &sonarDev, &map);
#ifdef ARNL_MULTILOC
locManager.addLocalizationTask(&locTask);
#else
#define LOCTASK locTask
#endif
#endif
#ifndef ARNL_GPSLOC
// A callback function, which is called if localization fails
ArGlobalFunctor1<int> locFailedCB(&locFailed);
locTask.setFailedCallBack(&locFailedCB); //, &locTask);
#endif
#ifdef ARNL_GPSLOC
ArLog::log(ArLog::Normal, "Connecting to GPS...");
// Connect to GPS
ArGPS *gps = gpsConnector.createGPS(&robot);
if(!gps || !gps->connect())
{
ArLog::log(ArLog::Terse, "Error connecting to GPS device."
"Try -gpsType, -gpsPort, and/or -gpsBaud command-line arguments."
"Use -help for help. Exiting.");
Aria::exit(5);
}
// set up GPS localization task
ArLog::log(ArLog::Normal, "Creating GPS localization task");
ArGPSLocalizationTask gpsLocTask(&robot, gps, &map);
#ifdef ARNL_MULTILOC
locManager.addLocalizationTask(&gpsLocTask);
#else
#define LOCTASK gpsLocTask
#endif
#endif
#ifdef ARNL_LASER
// Set some options and callbacks on each laser that the laser connector
// connected to.
std::map<int, ArLaser *>::iterator laserIt;
for (laserIt = robot.getLaserMap()->begin();
laserIt != robot.getLaserMap()->end();
laserIt++)
{
int laserNum = (*laserIt).first;
ArLaser *laser = (*laserIt).second;
// Skip lasers that aren't connected
if(!laser->isConnected())
continue;
// add the disconnectOnError CB to shut things down if the laser
// connection is lost
laser->addDisconnectOnErrorCB(&shutdownFunctor);
// set the number of cumulative readings the laser will take
laser->setCumulativeBufferSize(200);
// set the cumulative clean offset (so that they don't all fire at once)
laser->setCumulativeCleanOffset(laserNum * 100);
// reset the cumulative clean time (to make the new offset take effect)
laser->resetLastCumulativeCleanTime();
// Add the packet count to the Aria info strings (It will be included in
// MobileEyes custom details so you can monitor whether the laser data is
// being received correctly)
std::string laserPacketCountName;
laserPacketCountName = laser->getName();
laserPacketCountName += " Packet Count";
Aria::getInfoGroup()->addStringInt(
laserPacketCountName.c_str(), 10,
new ArRetFunctorC<int, ArLaser>(laser,
&ArLaser::getReadingCount));
}
#endif
/* Start the server */
// Open the networking server
if (!simpleOpener.open(&server, fileDir, 240))
{
ArLog::log(ArLog::Normal, "Error: Could not open server.");
exit(2);
}
/* Create various services that provide network access to clients (such as
* MobileEyes), as well as add various additional features to ARNL */
robot.unlock();
// Service to provide drawings of data in the map display :
ArServerInfoDrawings drawings(&server);
drawings.addRobotsRangeDevices(&robot);
#ifdef ARNL_LASERLOC
/* Show the sample points used by MCL */
ArDrawingData drawingDataL("polyDots", ArColor(0,255,0), 100, 75);
ArFunctor2C<ArLocalizationTask, ArServerClient *, ArNetPacket *>
drawingFunctorL(&locTask, &ArLocalizationTask::drawRangePoints);
drawings.addDrawing(&drawingDataL, "Localization Points", &drawingFunctorL);
#endif
#ifdef ARNL_GPSLOC
/* Show the positions calculated by GPS localization */
ArDrawingData drawingDataG("polyDots", ArColor(100,100,255), 130, 61);
ArFunctor2C<ArGPSLocalizationTask, ArServerClient *, ArNetPacket *>
drawingFunctorG(&gpsLocTask, &ArGPSLocalizationTask::drawGPSPoints);
drawings.addDrawing(&drawingDataG, "GPS Points", &drawingFunctorG);
ArDrawingData drawingDataG2("polyDots", ArColor(255,100,100), 100, 62);
ArFunctor2C<ArGPSLocalizationTask, ArServerClient *, ArNetPacket *>
drawingFunctorG2(&gpsLocTask, &ArGPSLocalizationTask::drawKalmanPoints);
drawings.addDrawing(&drawingDataG2, "Kalman Points", &drawingFunctorG2);
ArDrawingData drawingDataG3("polyDots", ArColor(100,255,100), 70, 63);
ArFunctor2C<ArGPSLocalizationTask, ArServerClient *, ArNetPacket *>
drawingFunctorG3(&gpsLocTask, &ArGPSLocalizationTask::drawOdoPoints);
drawings.addDrawing(&drawingDataG3, "Odom. Points", &drawingFunctorG3);
ArDrawingData drawingDataG4("polyDots", ArColor(255,50,50), 100, 75);
ArFunctor2C<ArGPSLocalizationTask, ArServerClient *, ArNetPacket *>
drawingFunctorG4(&gpsLocTask, &ArGPSLocalizationTask::drawKalmanRangePoints);
drawings.addDrawing(&drawingDataG4, "KalRange Points", &drawingFunctorG4);
ArDrawingData drawingDataG5("polySegments", ArColor(100,0,255), 1, 78);
ArFunctor2C<ArGPSLocalizationTask, ArServerClient *, ArNetPacket *>
drawingFunctorG5(&gpsLocTask, &ArGPSLocalizationTask::drawKalmanVariance);
drawings.addDrawing(&drawingDataG5, "VarGPS", &drawingFunctorG5);
#endif
// "Custom" commands. You can add your own custom commands here, they will
// be available in MobileEyes' custom commands (enable in the toolbar or
// access through Robot Tools)
ArServerHandlerCommands commands(&server);
// These provide various kinds of information to the client:
ArServerInfoRobot serverInfoRobot(&server, &robot);
ArServerInfoSensor serverInfoSensor(&server, &robot);
// Provides localization info and allows the client (MobileEyes) to relocalize at a given
// pose:
ArServerInfoLocalization serverInfoLocalization(&server, &robot, &LOCTASK);
ArServerHandlerLocalization serverLocHandler(&server, &robot, &LOCTASK);
// If you're using MobileSim, ArServerHandlerLocalization sends it a command
// to move the robot's true pose if you manually do a localization through
// MobileEyes. To disable that behavior, use this constructor call instead:
// ArServerHandlerLocalization serverLocHandler(&server, &robot, true, false);
// The fifth argument determines whether to send the command to MobileSim.
// Provide the map to the client (and related controls):
ArServerHandlerMap serverMap(&server, &map);
// These objects add some simple (custom) commands to 'commands' for testing and debugging:
ArServerSimpleComUC uCCommands(&commands, &robot); // Send any command to the microcontroller
ArServerSimpleComMovementLogging loggingCommands(&commands, &robot); // configure logging
ArServerSimpleComLogRobotConfig configCommands(&commands, &robot); // trigger logging of the robot config parameters
// ArServerSimpleServerCommands serverCommands(&commands, &server); // monitor networking behavior (track packets sent etc.)
// service that allows the client to monitor the communication link status
// between the robot and the client.
//
ArServerHandlerCommMonitor handlerCommMonitor(&server);
// service that allows client to change configuration parameters in ArConfig
ArServerHandlerConfig handlerConfig(&server, Aria::getConfig(),
Arnl::getTypicalDefaultParamFileName(),
Aria::getDirectory());
// This service causes the client to show simple dialog boxes
ArServerHandlerPopup popupServer(&server);
/* Set up the possible modes for remote control from a client such as
* MobileEyes:
*/
// Mode To stop and remain stopped:
ArServerModeStop modeStop(&server, &robot);
#ifndef ARNL_SONARLOC
// Cause the sonar to turn off automatically
// when the robot is stopped, and turn it back on when commands to move
// are sent. (Note, if using SONARNL to localize, then don't do this
// since localization may get lost)
ArSonarAutoDisabler sonarAutoDisabler(&robot);
#endif
// Teleoperation modes To drive by keyboard, joystick, etc:
ArServerModeRatioDrive modeRatioDrive(&server, &robot);
// Prevent normal teleoperation driving if localization is lost using
// a high-priority action, which enables itself when the particular mode is
// active.
// (You have to enter unsafe drive mode to drive when lost.)
ArActionLost actionLostRatioDrive(&LOCTASK, NULL, &modeRatioDrive);
modeRatioDrive.getActionGroup()->addAction(&actionLostRatioDrive, 110);
// Add drive mode section to the configuration, and also some custom (simple) commands:
modeRatioDrive.addToConfig(Aria::getConfig(), "Teleop settings");
modeRatioDrive.addControlCommands(&commands);
// Wander mode (also prevent wandering if lost):
ArServerModeWander modeWander(&server, &robot);
ArActionLost actionLostWander(&LOCTASK, NULL, &modeWander);
modeWander.getActionGroup()->addAction(&actionLostWander, 110);
// Tool to log data periodically to a file
ArDataLogger dataLogger(&robot, "datalog.txt");
dataLogger.addToConfig(Aria::getConfig()); // make it configurable through ArConfig
// Automatically add anything from the global info group to the data logger.
Aria::getInfoGroup()->addAddStringCallback(dataLogger.getAddStringFunctor());
// This provides a small table of interesting information for the client
// to display to the operator. You can add your own callbacks to show any
// data you want.
ArServerInfoStrings stringInfo(&server);
Aria::getInfoGroup()->addAddStringCallback(stringInfo.getAddStringFunctor());
// The following statements add fields to a set of informational data called
// the InfoGroup. These are served to MobileEyes for displayi (turn on by enabling Details
// and Custom Details in the View menu of MobileEyes.)
Aria::getInfoGroup()->addStringInt(
"Motor Packet Count", 10,
new ArConstRetFunctorC<int, ArRobot>(&robot,
&ArRobot::getMotorPacCount));
#ifdef ARNL_LASERLOC
Aria::getInfoGroup()->addStringDouble(
"Laser Localization Score", 8,
new ArRetFunctorC<double, ArLocalizationTask>(
&locTask, &ArLocalizationTask::getLocalizationScore),
"%.03f");
Aria::getInfoGroup()->addStringInt(
"Laser Loc Num Samples", 8,
new ArRetFunctorC<int, ArLocalizationTask>(
&locTask, &ArLocalizationTask::getCurrentNumSamples),
"%4d");
#elif defined(ARNL_SONARLOC)
Aria::getInfoGroup()->addStringDouble(
"Sonar Localization Score", 8,
new ArRetFunctorC<double, ArSonarLocalizationTask>(
&locTask,
&ArSonarLocalizationTask::getLocalizationScore),
"%.03f");
Aria::getInfoGroup()->addStringInt(
"Sonar Loc Num Samples", 8,
new ArRetFunctorC<int, ArSonarLocalizationTask>(
&locTask, &ArSonarLocalizationTask::getCurrentNumSamples),
"%4d");
#endif
#ifdef ARNL_GPSLOC
const char *dopfmt = "%2.4f";
const char *posfmt = "%2.8f";
const char *altfmt = "%3.6f m";
Aria::getInfoGroup()->addStringString(
"GPS Fix Mode", 25,
new ArConstRetFunctorC<const char*, ArGPS>(gps, &ArGPS::getFixTypeName)
);
Aria::getInfoGroup()->addStringInt(
"GPS Num. Satellites", 4,
new ArConstRetFunctorC<int, ArGPS>(gps, &ArGPS::getNumSatellitesTracked)
);
Aria::getInfoGroup()->addStringDouble(
"GPS HDOP", 12,
new ArConstRetFunctorC<double, ArGPS>(gps, &ArGPS::getHDOP),
dopfmt
);
Aria::getInfoGroup()->addStringDouble(
"GPS VDOP", 5,
new ArConstRetFunctorC<double, ArGPS>(gps, &ArGPS::getVDOP),
dopfmt
);
Aria::getInfoGroup()->addStringDouble(
"GPS PDOP", 5,
new ArConstRetFunctorC<double, ArGPS>(gps, &ArGPS::getPDOP),
dopfmt
);
Aria::getInfoGroup()->addStringDouble(
"Latitude", 15,
new ArConstRetFunctorC<double, ArGPS>(gps, &ArGPS::getLatitude),
posfmt
);
Aria::getInfoGroup()->addStringDouble(
"Longitude", 15,
new ArConstRetFunctorC<double, ArGPS>(gps, &ArGPS::getLongitude),
posfmt
);
Aria::getInfoGroup()->addStringDouble(
"Altitude", 8,
new ArConstRetFunctorC<double, ArGPS>(gps, &ArGPS::getAltitude),
altfmt
);
// only some GPS receivers provide these, but you can uncomment them
// here to enable them if yours does.
/*
const char *errfmt = "%2.4f m";
Aria::getInfoGroup()->addStringDouble(
"GPS Lat. Err.", 6,
new ArConstRetFunctorC<double, ArGPS>(gps, &ArGPS::getLatitudeError),
errfmt
);
Aria::getInfoGroup()->addStringDouble(
"GPS Lon. Err.", 6,
new ArConstRetFunctorC<double, ArGPS>(gps, &ArGPS::getLongitudeError),
errfmt
);
Aria::getInfoGroup()->addStringDouble(
"GPS Alt. Err.", 6,
new ArConstRetFunctorC<double, ArGPS>(gps, &ArGPS::getAltitudeError),
errfmt
);
*/
Aria::getInfoGroup()->addStringDouble(
"MOGS Localization Score", 8,
new ArRetFunctorC<double, ArGPSLocalizationTask>(
&gpsLocTask, &ArGPSLocalizationTask::getLocalizationScore),
"%.03f"
);
#endif
// Display gyro status if gyro is enabled and is being handled by the firmware (gyro types 2, 3, or 4).
// (If the firmware detects an error communicating with the gyro or IMU it
// returns a flag, and stops using it.)
// (This gyro type parameter, and fault flag, are only in ARCOS, not Seekur firmware)
if(robot.getOrigRobotConfig() && robot.getOrigRobotConfig()->getGyroType() > 1)
{
Aria::getInfoGroup()->addStringString(
"Gyro/IMU Status", 10,
new ArGlobalRetFunctor1<const char*, ArRobot*>(&getGyroStatusString, &robot)
);
}
// Display system CPU and wireless network status
ArSystemStatus::startPeriodicUpdate(1000); // update every 1 second
Aria::getInfoGroup()->addStringDouble("CPU Use", 10, ArSystemStatus::getCPUPercentFunctor(), "% 4.0f%%");
Aria::getInfoGroup()->addStringInt("Wireless Link Quality", 9, ArSystemStatus::getWirelessLinkQualityFunctor(), "%d");
Aria::getInfoGroup()->addStringInt("Wireless Link Noise", 9, ArSystemStatus::getWirelessLinkNoiseFunctor(), "%d");
Aria::getInfoGroup()->addStringInt("Wireless Signal", 9, ArSystemStatus::getWirelessLinkSignalFunctor(), "%d");
// stats on how far its driven since software started
Aria::getInfoGroup()->addStringDouble("Distance Travelled (m)", 20, new ArRetFunctorC<double, ArRobot>(&robot, &ArRobot::getOdometerDistanceMeters), "%.2f");
Aria::getInfoGroup()->addStringDouble("Run time (min)", 20, new
ArRetFunctorC<double, ArRobot>(&robot, &ArRobot::getOdometerTimeMinutes),
"%.2f");
#ifdef ARNL_GPSLOC
// Add some "custom commands" for setting up initial GPS offset and heading.
gpsLocTask.addLocalizationInitCommands(&commands);
// Add some commands for manually creating map objects based on GPS positions:
// ArGPSMapTools gpsMapTools(gps, &robot, &commands, &map);
// Add command to set simulated GPS position manually
if(gpsConnector.getGPSType() == ArGPSConnector::Simulator)
{
ArSimulatedGPS *simGPS = dynamic_cast<ArSimulatedGPS*>(gps);
// simGPS->setDummyPosition(42.80709, -71.579047, 100);
commands.addStringCommand("GPS:setDummyPosition",
"Manually set a new dummy position for simulated GPS. Provide latitude (required), longitude (required) and altitude (optional)",
new ArFunctor1C<ArSimulatedGPS, ArArgumentBuilder*>(simGPS, &ArSimulatedGPS::setDummyPositionFromArgs)
);
}
#endif
// Make Stop mode the default (If current mode deactivates without entering
// a new mode, then Stop Mode will be selected)
modeStop.addAsDefaultMode();
// TODO move up near where stop mode is created?
#ifdef ARNL_MAPPING
/* Services that allow the client to initiate scanning with the laser to
create maps in Mapper3 (So not possible with SONARNL): */
ArServerHandlerMapping handlerMapping(&server, &robot, firstLaser,
fileDir, "", true);
#ifdef ARNL_LASERLOC
// make laser localization stop while mapping
handlerMapping.addMappingStartCallback(
new ArFunctor1C<ArLocalizationTask, bool>
(&locTask, &ArLocalizationTask::setIdleFlag, true));
// and then make it start again when we're doine
handlerMapping.addMappingEndCallback(
new ArFunctor1C<ArLocalizationTask, bool>
(&locTask, &ArLocalizationTask::setIdleFlag, false));
#endif
#ifdef ARNL_GPSLOC
// Save GPS positions in the .2d scan log when making a map
handlerMapping.addLocationData("robotGPS",
gpsLocTask.getPoseInterpPositionCallback());
// add the starting latitude and longitude info to the .2d scan log
handlerMapping.addMappingStartCallback(
new ArFunctor1C<ArGPSLocalizationTask, ArServerHandlerMapping *>
(&gpsLocTask, &ArGPSLocalizationTask::addScanInfo,
&handlerMapping));
#endif
// Make it so our "lost" actions don't stop us while mapping
handlerMapping.addMappingStartCallback(actionLostRatioDrive.getDisableCB());
handlerMapping.addMappingStartCallback(actionLostWander.getDisableCB());
// And then let them make us stop as usual when done mapping
handlerMapping.addMappingEndCallback(actionLostRatioDrive.getEnableCB());
handlerMapping.addMappingEndCallback(actionLostWander.getEnableCB());
#endif // ARNL_MAPPING
/*
// If we are on a simulator, move the robot back to its starting position,
// and reset its odometry.
// This will allow localizeRobotAtHomeBlocking() below will (probably) work (it
// tries current odometry (which will be 0,0,0) and all the map
// home points.
// (Ignored by a real robot)
//robot.com(ArCommands::SIM_RESET);
*/
// create a pose storage class, this will let the program keep track
// of where the robot is between runs... after we try and restore
// from this file it will start saving the robot's pose into the
// file
ArPoseStorage poseStorage(&robot);
/// if we could restore the pose from then set the sim there (this
/// won't do anything to the real robot)... if we couldn't restore
/// the pose then just reset the position of the robot (which again
/// won't do anything to the real robot)
if (poseStorage.restorePose("robotPose"))
serverLocHandler.setSimPose(robot.getPose());
//else
// robot.com(ArCommands::SIM_RESET);
/* File transfer services: */
#pragma GPP off
#ifdef WIN32
// Not implemented for Windows yet.
ArLog::log(ArLog::Normal, "Note, file upload/download services are not implemented for Windows; not enabling them.");
#else
// This block will allow you to set up where you get and put files
// to/from, just comment them out if you don't want this to happen
// /*
ArServerFileLister fileLister(&server, fileDir);
ArServerFileToClient fileToClient(&server, fileDir);
ArServerFileFromClient fileFromClient(&server, fileDir, "/tmp");
ArServerDeleteFileOnServer deleteFileOnServer(&server, fileDir);
// */
#endif
#pragma GPP on
/* Video image streaming, and camera controls (Requires SAVserver or ACTS) */
// Forward one video stream if either ACTS, ArVideo videoSubServer,
// or SAV server are running.
// ArHybridForwarderVideo allows this program to be separate from the ArVideo
// library. You could replace videoForwarder and the PTZ connection code below
// with a call to ArVideo::createVideoServers(), and link the program to the
// ArVideo library if you want to include video capture in the same program
// as robot control.
ArHybridForwarderVideo videoForwarder(&server, "localhost", 7070);
// connect to first configured camera PTZ controls (in robot parameter file and
// command line options)
ptzConnector.connect();
ArCameraCollection cameraCollection;
ArPTZ *ptz = ptzConnector.getPTZ(0);
if(ptz)
{
ArLog::log(ArLog::Normal, "Connected to PTZ Camera");
cameraCollection.addCamera("Camera1", ptz->getTypeName(), "Camera", ptz->getTypeName());
videoForwarder.setCameraName("Camera1");
videoForwarder.addToCameraCollection(cameraCollection);
new ArServerHandlerCamera("Camera1",
&server,
&robot,
ptz,
&cameraCollection);
}
// Allows client to find any camera servers created above
ArServerHandlerCameraCollection cameraCollectionServer(&server, &cameraCollection);
/* Load configuration values, map, and begin! */
// When parsing the configuration file, also look at the program's command line options
// from the command-line argument parser as well as the configuration file.
// (So you can use any argument on the command line, namely -map.)
Aria::getConfig()->useArgumentParser(&parser);
// Read in parameter files.
ArLog::log(ArLog::Normal, "Loading config file %s%s into ArConfig...", Aria::getDirectory(), Arnl::getTypicalParamFileName());
if (!Aria::getConfig()->parseFile(Arnl::getTypicalParamFileName()))
{
ArLog::log(ArLog::Normal, "Could not load ARNL configuration file. Set ARNL environment variable to use non-default installation director.y");
Aria::exit(5);
}
// Warn about unknown params.
if (!simpleOpener.checkAndLog() || !parser.checkHelpAndWarnUnparsed())
{
logOptions(argv[0]);
Aria::exit(6);
}
// Warn if there is no map
if (map.getFileName() == NULL || strlen(map.getFileName()) <= 0)
{
ArLog::log(ArLog::Normal, "");
ArLog::log(ArLog::Normal, "### No map file is set up, you can make a map with the following procedure");
#ifdef ARNL
ArLog::log(ArLog::Normal, " 0) You can find this information in README.txt or docs/Mapping.txt");
ArLog::log(ArLog::Normal, " 1) Connect to this server with MobileEyes");
ArLog::log(ArLog::Normal, " 2) Go to Tools->Map Creation->Start Scan");
ArLog::log(ArLog::Normal, " 3) Give the map a name and hit okay");
ArLog::log(ArLog::Normal, " 4) Drive the robot around your space (see docs/Mapping.txt");
ArLog::log(ArLog::Normal, " 5) Go to Tools->Map Creation->Stop Scan");
ArLog::log(ArLog::Normal, " 6) Start up Mapper3");
ArLog::log(ArLog::Normal, " 7) Go to File->Open on Robot");
ArLog::log(ArLog::Normal, " 8) Select the .2d you created");
ArLog::log(ArLog::Normal, " 9) Create a .map");
ArLog::log(ArLog::Normal, " 10) Go to File->Save on Robot");
ArLog::log(ArLog::Normal, " 11) In MobileEyes, go to Tools->Robot Config");
ArLog::log(ArLog::Normal, " 12) Choose the Files section");
ArLog::log(ArLog::Normal, " 13) Enter the path and name of your new .map file for the value of the Map parameter.");
ArLog::log(ArLog::Normal, " 14) Press OK and your new map should become the map used");
ArLog::log(ArLog::Normal, "");
#elif defined(SONARNL)
ArLog::log(ArLog::Normal, " 0) You can find this information in README.txt or docs/SonarMapping.txt");
ArLog::log(ArLog::Normal, " 1) Start up Mapper3Basic");
ArLog::log(ArLog::Normal, " 2) Go to File->New");
ArLog::log(ArLog::Normal, " 3) Draw a line map of your area (make sure it is to scale)");
ArLog::log(ArLog::Normal, " 4) Go to File->Save on Robot");
ArLog::log(ArLog::Normal, " 5) In MobileEyes, go to Tools->Robot Config");
ArLog::log(ArLog::Normal, " 6) Choose the Files section");
ArLog::log(ArLog::Normal, " 7) Enter the path and name of your new .map file for the value of the Map parameter.");
ArLog::log(ArLog::Normal, " 8) Press OK and your new map should become the map used");
ArLog::log(ArLog::Normal, "");
#endif
#ifdef ARNL_GPSLOC
ArLog::log(ArLog::Normal, "\n See docs/GPSMapping.txt for instructions on creating a map for GPS localization");
#endif
}
// Print a log message notifying user of the directory for map files
ArLog::log(ArLog::Normal, "");
ArLog::log(ArLog::Normal,
"Directory for maps and file serving: %s", fileDir);
ArLog::log(ArLog::Normal, "See the ARNL README.txt for more information");
ArLog::log(ArLog::Normal, "");
// Do an initial localization of the robot. ARNL and SONARNL try all the home points
// in the map, as well as the robot's current odometric position, as possible
// places the robot is likely to be at startup. If successful, it will
// also save the position it found to be the best localized position as the
// "Home" position, which can be obtained from the localization task (and is
// used by the "Go to home" network request).
// MOGS instead just initializes at the current GPS position.
// (You will stil have to drive the robot so it can determine the robot's
// heading, however. See GPS Mapping instructions.)
LOCTASK.localizeRobotAtHomeBlocking();
#ifdef ARNL_MULTIROBOT
// Let the client switch manager (for multirobot) spin off into its own thread
// TODO move to multirobot example?
clientSwitch.runAsync();
#endif
// Start the networking server's thread
server.runAsync();
ArLog::log(ArLog::Normal, "Server running. To exit, press CTRL-C.");
// Enable the motors and wait until the robot exits (disconnection, etc.) or this program is
// canceled.
robot.enableMotors();
robot.waitForRunExit();
Aria::exit(0);
}
/**
* @brief Creates an XsOutputConfigurationArray and pushes it to the sensor.
* @details
* - Configures the sensor with desired modules
* - Refer to xsdataidentifier.h
*/
void mtiG::configure(){
XsOutputConfigurationArray configArray;
if(mSettings.orientationData){ //Quaternion - containsOrientation
XsOutputConfiguration quat(XDI_Quaternion, mSettings.orientationFreq);// para pedir quaternion
configArray.push_back(quat);
}
if(mSettings.gpsData){
//LATITUDE E LONGITUDE -containsLatitudeLongitude
XsOutputConfiguration gps(XDI_LatLon, mSettings.gpsFreq);// para pedir gps, //XDI_Quaternion 06/04
configArray.push_back(gps);
XsOutputConfiguration gps_age(XDI_GpsAge, mSettings.gpsFreq);// para pedir gps, //XDI_Quaternion 06/04
configArray.push_back(gps_age);
XsOutputConfiguration gps_sol(XDI_GpsSol, mSettings.gpsFreq);// para pedir gps, //XDI_Quaternion 06/04
configArray.push_back(gps_sol);
XsOutputConfiguration gps_dop(XDI_GpsDop, mSettings.gpsFreq);// para pedir gps, //XDI_Quaternion 06/04
configArray.push_back(gps_dop);
}
if(mSettings.temperatureData){
//TEMPERATURA - containsTemperature
XsOutputConfiguration temp(XDI_Temperature, mSettings.temperatureFreq);
configArray.push_back(temp);
}
if(mSettings.accelerationData){
//ACCELERATION - containsCalibratedAcceleration
XsOutputConfiguration accel(XDI_Acceleration, mSettings.accelerationFreq);
configArray.push_back(accel);
}
if(mSettings.pressureData){
//PRESSURE - containsPressure
XsOutputConfiguration baro(XDI_BaroPressure, mSettings.pressureFreq);
configArray.push_back(baro);
}
if(mSettings.magneticData){
//MAGNETIC FIELD - containsCalibratedMagneticField
XsOutputConfiguration magnet(XDI_MagneticField, mSettings.magneticFreq);
configArray.push_back(magnet);
}
if(mSettings.altitudeData){
//ALTITUDE - containsAltitude
XsOutputConfiguration alt(XDI_AltitudeEllipsoid, mSettings.altitudeFreq);
configArray.push_back(alt);
}
if(mSettings.gyroscopeData){
//GYRO - containsCalibratedGyroscopeData
XsOutputConfiguration gyro(XDI_RateOfTurn, mSettings.gyroscopeFreq);
configArray.push_back(gyro);
}
if(mSettings.velocityData){
//VELOCIDADE XYZ
XsOutputConfiguration vel_xyz(XDI_VelocityXYZ, mSettings.velocityFreq);
configArray.push_back(vel_xyz);
}
// Puts configArray into the device, overwriting the current configuration
if (!device->setOutputConfiguration(configArray))
{
throw std::runtime_error("Could not configure MTmk4 device. Aborting.");
}
}
int _tmain(int argc, char** argv)
{
//-------------- 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);
ActionTurns turn(400, 55);
robot.addAction(&turn, 49);
ActionTurns turn2(400, 55);
robot.addAction(&turn2, 49);
turn.deactivate();
turn2.deactivate();
// 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);
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();
}
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)
{
//secuencia de drop de la figura
}
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();
//----------------------------------------------------------------------------------//
return 0;
}
int main(int argc, char **argv)
{
Aria::init();
argc = 3;
argv[0] = "";
argv[1] = "-rp";
argv[2] = "/dev/ttyUSB0";
ArArgumentParser parser(&argc, argv);
parser.loadDefaultArguments();
ArRobot robot;
ArAnalogGyro gyro(&robot);
ArSonarDevice sonar;
ArRobotConnector robotConnector(&parser, &robot);
if(!robotConnector.connectRobot())
{
if(parser.checkHelpAndWarnUnparsed())
{
Aria::logOptions();
Aria::exit(1);
}
}
if (!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed())
{
Aria::logOptions();
Aria::exit(1);
}
imgdb.setRobot(&robot);
int Rc;
pthread_t slam_thread;
Rc = pthread_create(&slam_thread, NULL, slam_event, NULL);
if (Rc) { printf("Create slam thread failed!\n"); exit(-1); }
ImageList* current = NULL;
while (!current) current = imgdb.getEdge();
// current->person_point
robot.addRangeDevice(&sonar);
robot.runAsync(true);
ArKeyHandler keyHandler;
Aria::setKeyHandler(&keyHandler);
robot.attachKeyHandler(&keyHandler);
printf("You may press escape to exit\n");
robot.setAbsoluteMaxRotVel(30);
ArActionStallRecover recover;
ArActionBumpers bumpers;
double minDistance=10000;
// ArPose endPoint(point.x, point.z);
// current->collect_x
// current->collect_y
// robot.getX()
// robot.getY()
// current = imgdb.getEdge();
// for (int i = 0; i < current->edge.size(); i++) {
// for (int j = 0; j < current->edge[i].size() - 1; j++) {
// double dis = sqrt(pow(current->edge[i][j].z,2)+(pow(current->edge[i][j].x,2)));
// if(dis<minDistance){
// minDistance=dis;
// }
// }
// }
// 引入避障action,前方安全距离为500mm,侧边安全距离为340mm
Avoid avoidSide("Avoid side", ArPose(0, 0), 800, 100, 1000*minDistance);
robot.addAction(&recover, 100);
robot.addAction(&bumpers, 95);
robot.addAction(&avoidSide, 80);
ArActionStop stopAction("stop");
robot.addAction(&stopAction, 50);
robot.enableMotors();
robot.comInt(ArCommands::SOUNDTOG, 0);
const int duration = 500000;
bool first = true;
int goalNum = 0;
ArTime start;
start.setToNow();
while (Aria::getRunning())
{
current = imgdb.getEdge();
for (int i = 0; i < current->edge.size(); i++) {
for (int j = 0; j < current->edge[i].size() - 1; j++) {
// printf("i=%d\n",i);
double dis = sqrt(pow(current->edge[i][j].z,2)+(pow(current->edge[i][j].x,2)));
// printf("dis=%f,mindis=%f\n",dis,minDistance);
if(dis<minDistance){
minDistance=dis;
printf("min=%f\n",minDistance);
}
}
}
//引入避障action,前方安全距离为500mm,侧边安全距离为340mm
// Avoid avoidSide("Avoid side", ArPose(0, 0), 800, 200, minDistance);
// robot.addAction(&avoidSide,80);
robot.lock();
if (first || avoidSide.haveAchievedGoal())
{
first = false;
goalNum++;
printf("count goalNum = %d\n", goalNum);
if (goalNum > 2){
goalNum = 1; // start again at goal #1
}
// avoidSide.setGoal(ArPose(10000,0));
if(goalNum==1){
printf("zuobiaox=%f,zuobiaoy=%f\n",-current->person_point.z,current->person_point.x);
avoidSide.setGoal(ArPose(1000*current->person_point.z, -1000*current->person_point.x));
printf("goalNum == 1\n\n");
}
else if(goalNum==2){
printf("zuobiaox=%f,zuobiaoy=%f\n",-current->person_point.z,current->person_point.x);
avoidSide.setGoal(ArPose(1000*current->person_point.z, -1000*current->person_point.x));
printf("goalNum == 2\n\n");
}
}
if(start.mSecSince() >= duration) {
ArLog::log(ArLog::Normal, "%d seconds have elapsed. Cancelling current goal, waiting 3 seconds, and exiting.", duration/1000);
avoidSide.cancelGoal();
robot.unlock();
ArUtil::sleep(3000);
break;
}
robot.unlock();
ArUtil::sleep(100);
}
Aria::exit(0);
return 0;
}
void SensorFilter::update(FilteredSensorData& filteredSensorData,
const SensorData& theSensorData,
const InertiaSensorData& theInertiaSensorData,
const OrientationData& theOrientationData)
{
// copy sensor data (except gyro and acc)
Vector2<> gyro(filteredSensorData.data[SensorData::gyroX], filteredSensorData.data[SensorData::gyroY]);
Vector3<> acc(filteredSensorData.data[SensorData::accX], filteredSensorData.data[SensorData::accY], filteredSensorData.data[SensorData::accZ]);
(SensorData&)filteredSensorData = theSensorData;
filteredSensorData.data[SensorData::gyroX] = gyro.x;
filteredSensorData.data[SensorData::gyroY] = gyro.y;
filteredSensorData.data[SensorData::accX] = acc.x;
filteredSensorData.data[SensorData::accY] = acc.y;
filteredSensorData.data[SensorData::accZ] = acc.z;
// take calibrated inertia sensor data
for(int i = 0; i < 2; ++i)
{
if(theInertiaSensorData.gyro[i] != InertiaSensorData::off)
filteredSensorData.data[SensorData::gyroX + i] = theInertiaSensorData.gyro[i];
else if(filteredSensorData.data[SensorData::gyroX + i] == SensorData::off)
filteredSensorData.data[SensorData::gyroX + i] = 0.f;
}
filteredSensorData.data[SensorData::gyroZ] = 0.f;
for(int i = 0; i < 3; ++i)
{
if(theInertiaSensorData.acc[i] != InertiaSensorData::off)
filteredSensorData.data[SensorData::accX + i] = theInertiaSensorData.acc[i] / 9.80665f;
else if(filteredSensorData.data[SensorData::accX + i] == SensorData::off)
filteredSensorData.data[SensorData::accX + i] = 0.f;
}
// take orientation data
filteredSensorData.data[SensorData::angleX] = theOrientationData.orientation.x;
filteredSensorData.data[SensorData::angleY] = theOrientationData.orientation.y;
#ifndef RELEASE
if(filteredSensorData.data[SensorData::gyroX] != SensorData::off)
{
gyroAngleXSum += filteredSensorData.data[SensorData::gyroX] * (theSensorData.timeStamp - lastIteration) * 0.001f;
gyroAngleXSum = normalize(gyroAngleXSum);
lastIteration = theSensorData.timeStamp;
}
//PLOT("module:SensorFilter:gyroAngleXSum", gyroAngleXSum);
#endif
/*
PLOT("module:SensorFilter:rawAngleX", theSensorData.data[SensorData::angleX]);
PLOT("module:SensorFilter:rawAngleY", theSensorData.data[SensorData::angleY]);
PLOT("module:SensorFilter:rawAccX", theSensorData.data[SensorData::accX]);
PLOT("module:SensorFilter:rawAccY", theSensorData.data[SensorData::accY]);
PLOT("module:SensorFilter:rawAccZ", theSensorData.data[SensorData::accZ]);
PLOT("module:SensorFilter:rawGyroX", theSensorData.data[SensorData::gyroX]);
PLOT("module:SensorFilter:rawGyroY", theSensorData.data[SensorData::gyroY]);
PLOT("module:SensorFilter:rawGyroZ", theSensorData.data[SensorData::gyroZ]);
PLOT("module:SensorFilter:angleX", filteredSensorData.data[SensorData::angleX]);
PLOT("module:SensorFilter:angleY", filteredSensorData.data[SensorData::angleY]);
PLOT("module:SensorFilter:accX", filteredSensorData.data[SensorData::accX]);
PLOT("module:SensorFilter:accY", filteredSensorData.data[SensorData::accY]);
PLOT("module:SensorFilter:accZ", filteredSensorData.data[SensorData::accZ]);
PLOT("module:SensorFilter:gyroX", filteredSensorData.data[SensorData::gyroX] != float(SensorData::off) ? filteredSensorData.data[SensorData::gyroX] : 0);
PLOT("module:SensorFilter:gyroY", filteredSensorData.data[SensorData::gyroY] != float(SensorData::off) ? filteredSensorData.data[SensorData::gyroY] : 0);
PLOT("module:SensorFilter:gyroZ", filteredSensorData.data[SensorData::gyroZ] != float(SensorData::off) ? filteredSensorData.data[SensorData::gyroZ] : 0);
//PLOT("module:SensorFilter:us", filteredSensorData.data[SensorData::us]);
*/
}
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() {
LocalI2CBus bus(1);
ADXL345 acc(&bus);
L3G4200D gyro(&bus);
acc.setEnabled(false);
acc.setDataRate(ADXL345::DATARATE_100_HZ);
acc.setRange(ADXL345::RANGE_16G);
acc.setFullResolutionEnabled(true);
acc.setEnabled(true);
gyro.setEnabled(false);
gyro.setOutputDataRate(L3G4200D::DATARATE_800_HZ);
//gyro.setBlockDataUpdateEnabled(false);
gyro.setEnabled(true);
timespec ts, ts2;
clock_gettime(CLOCK_REALTIME, &ts);
for (int i=0; i<1000; i++)
clock_gettime(CLOCK_REALTIME, &ts2);
int gettime_delay_ns = ((ts2.tv_sec-ts.tv_sec)*1000000000 + ts2.tv_nsec-ts.tv_nsec) / 1000;
printf("gettime delay: %d\n", gettime_delay_ns);
usleep(1000000);
double accv[32][3];
double rotv[100][3];
double rota[3], rot[3];
for (int i=0; i<10000; i++) {
//acc.getLinearAcceleration(accv[i][0], accv[i][1], accv[i][2]);
//gyro.getAngularVelocity(rotv[i][0], rotv[i][1], rotv[i][2]);
gyro.getAngularVelocity(rot[0], rot[1], rot[2]);
rota[0] += rot[0];
rota[1] += rot[1];
rota[2] += rot[2];
clock_gettime(CLOCK_REALTIME, &ts2);
//usleep(10000);
}
rota[0] /= 10000;
rota[1] /= 10000;
rota[2] /= 10000;
/*
for (int i=0; i<100; i++) {
//printf("\t%.2lf\t%.2lf\t%.2lf\n", accv[i][0], accv[i][1], accv[i][2]);
//printf("\t%.2lf\t%.2lf\t%.2lf\n", rotv[i][0], rotv[i][1], rotv[i][2]);
}
printf("\n\n");
printf("\t%.2lf\t%.2lf\t%.2lf\n", rota[0]/32, rota[1]/32, rota[2]/32);
printf("\n\n");
usleep(10000000);
*/
acc.calibrate(100);
int8 ox, oy, oz;
acc.getOffset(ox, oy, oz);
printf("%d\t%d\t%d\n", ox, oy, oz);
usleep(1000000);
int16 x, y, z;
double acc_x = 0, acc_y = 0, acc_z = 0, mag;
double lacc_x, lacc_y, lacc_z;
double rot_x, rot_y, rot_z;
double ang_x = 0, ang_y = 0, ang_z = 0, interval, dang[3], gvec[3] = {0, 0, 0};
clock_gettime(CLOCK_REALTIME, &ts);
while (true) {
acc.getLinearAcceleration(lacc_x, lacc_y, lacc_z);
gyro.getAngularVelocity(rot_x, rot_y, rot_z);
acc_x = (15 * acc_x + lacc_x) / 16;
acc_y = (15 * acc_y + lacc_y) / 16;
acc_z = (15 * acc_z + lacc_z) / 16;
mag = acc_x*acc_x+acc_y*acc_y+acc_z*acc_z;
//printf("\t%.2lf\t%.2lf\t%.2lf\t%.2lf\t|\t%.2lf\t%.2lf\t%.2lf", acc_x, acc_y, acc_z, mag, rot_x, rot_y, rot_z);
//usleep(10000);
clock_gettime(CLOCK_REALTIME, &ts2);
interval = ((ts2.tv_sec-ts.tv_sec)*1000000000 + ts2.tv_nsec-ts.tv_nsec - gettime_delay_ns)/1000.0;
ts = ts2;
dang[0] = - (rot_x - rota[0]) * interval / 1000000;
dang[1] = - (rot_y - rota[1]) * interval / 1000000;
dang[2] = - (rot_z - rota[2]) * interval / 1000000;
ang_x += dang[0];
ang_y += dang[1];
ang_z += dang[2];
dang[0] *= 3.14 / 180;
dang[1] *= 3.14 / 180;
dang[2] *= 3.14 / 180;
rotateV(gvec, dang);
mag = mag * 100 / (9.81 * 9.81);
if (72.0 < mag && mag < 133.0) {
gvec[0] = (gvec[0] * 600 + acc_x) / 601;
gvec[1] = (gvec[1] * 600 + acc_y) / 601;
gvec[2] = (gvec[2] * 600 + acc_z) / 601;
}
printf("\t%.2lf\t%.2lf\t%.2lf\t|\t%.2lf\t%.2lf\t|\t%.2lf\t%.2lf\t%.2lf\t[%.2lf]\n", ang_x, ang_y, ang_z,
atan2(gvec[1], sqrt(gvec[0]*gvec[0] + gvec[2]*gvec[2])) * 180/ 3.14,
atan2(gvec[0], gvec[2]) * 180/ 3.14, gvec[0], gvec[1], gvec[2], interval);
}
}
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){
//select loggin level
logger_default_config(log4cxx::Level::getInfo());
std::string adc_id;
std::string filename;
size_t offset = 0x08000000; // 128M * 4 -> second memory bank
size_t block_size = 2048;
size_t data_size = 4096;
bool simple_mode = false;
bool halbe_mode = false;
bool statistic_mode = false;
{
namespace po = boost::program_options;
po::options_description desc("Allowed options");
desc.add_options()
("help", "produce help message")
("adc", po::value<std::string>(&adc_id)->required(), "specify ADC board")
("data_size", po::value<size_t>(&data_size), "Amount of data to read (kB), default 4096kB")
("block_size", po::value<size_t>(&block_size), "Read data in blocks of this size(kB), default 2048kB")
("simple_mode", po::bool_switch(&simple_mode), "Run forever and collect statistics.")
("halbe_mode ", po::bool_switch(&halbe_mode), "Run forever and collect statistics.")
("statistic_mode", po::bool_switch(&statistic_mode), "Run forever and collect statistics.")
;
po::variables_map vm;
po::store(po::command_line_parser(argc, argv)
.options(desc)
.run()
, vm);
if (vm.count("help")) {
std::cout << std::endl << desc << std::endl;
return 0;
}
po::notify(vm);
}
if (simple_mode)
{
//create the top of the tree
Vmoduleusb io(usbcomm::note);
if(io.open(usb_vendorid, usb_deviceid, adc_id)){
std::cout << "Open failed" << std::endl;
return -1;
}
else {
std::cout << "Board " << adc_id << " opened" << std::endl;
}
//create sp6 class tree
Vusbmaster usb(&io);
//usbmaster knows three clients, must be in this order!!!
Vusbstatus status(&usb);
Vmemory mem(&usb);
Vocpfifo ocp(&usb);
//ocpfifo clients
Vspiconfrom confrom(&ocp);
Vspigyro gyro(&ocp);
Vspiwireless wireless(&ocp);
std::cout << "Transfering " << data_size << "kB in "
<< calc_blocks(data_size * 1024, block_size * 1024)
<< " block(s) of "
<< calc_words(block_size) << " words." << std::endl;
size_t errorcnt = memtest(mem, calc_words(data_size), offset, calc_words(block_size));
std::cout << "Transmission errors: " << errorcnt << std::endl;
if (errorcnt > 0)
return -1;
}
if (halbe_mode)
{
Vmoduleusb io(usbcomm::note, usb_vendorid, usb_deviceid, adc_id);
std::cout << "Board " << adc_id << " opened" << std::endl;
Vusbmaster usb(&io);
Vusbstatus status(&usb);
Vmemory mem(&usb);
Vocpfifo ocp(&usb);
Vmux_board mux_board(&ocp, mux_board_mode(adc_id));
Vflyspi_adc adc(&ocp);
// write configuration to adc
adc.configure(0);
adc.setup_controller(0, calc_words(data_size),
0 /* single mode */, 0 /* trigger enable */, 0 /* trigger */);
const uint32_t startaddr = adc.get_startaddr();
const uint32_t endaddr = adc.get_endaddr();
const uint32_t num_words = endaddr - startaddr;
if (startaddr != 0 or endaddr != calc_words(data_size))
{
std::cout << "Invalid start or end addresses: startaddr="
<< startaddr << " endaddr=" << endaddr;
exit(-1);
}
std::cout << "Transfering " << num_words << " words in "
<< calc_blocks(data_size * 1024, block_size * 1024)
<< " block(s) of "
<< num_words << " words." << std::endl;
size_t errorcnt = memtest(mem, num_words, offset, calc_words(block_size));
std::cout << "Transmission errors: " << errorcnt << std::endl;
if (errorcnt > 0)
return -1;
}
}
int main(int argc, char **argv)
{
Aria::init();
ArArgumentParser parser(&argc, argv);
parser.loadDefaultArguments();
ArRobot robot;
ArAnalogGyro gyro(&robot);
ArSonarDevice sonar;
ArRobotConnector robotConnector(&parser, &robot);
ArLaserConnector laserConnector(&parser, &robot, &robotConnector);
// 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())
{
ArLog::log(ArLog::Terse, "gotoActionExample: Could not connect to the robot.");
if(parser.checkHelpAndWarnUnparsed())
{
// -help not given
Aria::logOptions();
Aria::exit(1);
}
}
if (!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed())
{
Aria::logOptions();
Aria::exit(1);
}
ArLog::log(ArLog::Normal, "gotoActionExample: Connected to robot.");
robot.addRangeDevice(&sonar);
robot.runAsync(true);
// Make a key handler, so that escape will shut down the program
// cleanly
ArKeyHandler keyHandler;
Aria::setKeyHandler(&keyHandler);
robot.attachKeyHandler(&keyHandler);
printf("You may press escape to exit\n");
// Collision avoidance actions at higher priority
ArActionLimiterForwards limiterAction("speed limiter near", 300, 600, 250);
ArActionLimiterForwards limiterFarAction("speed limiter far", 300, 1100, 400);
ArActionLimiterTableSensor tableLimiterAction;
robot.addAction(&tableLimiterAction, 100);
robot.addAction(&limiterAction, 95);
robot.addAction(&limiterFarAction, 90);
// Goto action at lower priority
ArActionGoto gotoPoseAction("goto");
robot.addAction(&gotoPoseAction, 50);
// Stop action at lower priority, so the robot stops if it has no goal
ArActionStop stopAction("stop");
robot.addAction(&stopAction, 40);
// turn on the motors, turn off amigobot sounds
robot.enableMotors();
robot.comInt(ArCommands::SOUNDTOG, 0);
const int duration = 30000; //msec
ArLog::log(ArLog::Normal, "Going to four goals in turn for %d seconds, then cancelling goal and exiting.", duration/1000);
bool first = true;
int goalNum = 0;
ArTime start;
start.setToNow();
while (Aria::getRunning())
{
robot.lock();
// Choose a new goal if this is the first loop iteration, or if we
// achieved the previous goal.
if (first || gotoPoseAction.haveAchievedGoal())
{
first = false;
goalNum++;
if (goalNum > 4)
goalNum = 1; // start again at goal #1
// set our positions for the different goals
if (goalNum == 1)
gotoPoseAction.setGoal(ArPose(2500, 0));
else if (goalNum == 2)
gotoPoseAction.setGoal(ArPose(2500, 2500));
else if (goalNum == 3)
gotoPoseAction.setGoal(ArPose(0, 2500));
else if (goalNum == 4)
gotoPoseAction.setGoal(ArPose(0, 0));
ArLog::log(ArLog::Normal, "Going to next goal at %.0f %.0f",
gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
}
if(start.mSecSince() >= duration) {
ArLog::log(ArLog::Normal, "%d seconds have elapsed. Cancelling current goal, waiting 3 seconds, and exiting.", duration/1000);
gotoPoseAction.cancelGoal();
robot.unlock();
ArUtil::sleep(3000);
break;
}
robot.unlock();
ArUtil::sleep(100);
}
// Robot disconnected or time elapsed, shut down
Aria::exit(0);
return 0;
}
/*
process any
*/
bool AP_InertialSensor_MPU6000::update( void )
{
#if !MPU6000_FAST_SAMPLING
if (_sum_count < _sample_count) {
// we don't have enough samples yet
return false;
}
#endif
// we have a full set of samples
uint16_t num_samples;
Vector3f accel, gyro;
hal.scheduler->suspend_timer_procs();
#if MPU6000_FAST_SAMPLING
gyro = _gyro_filtered;
accel = _accel_filtered;
num_samples = 1;
#else
gyro(_gyro_sum.x, _gyro_sum.y, _gyro_sum.z);
accel(_accel_sum.x, _accel_sum.y, _accel_sum.z);
num_samples = _sum_count;
_accel_sum.zero();
_gyro_sum.zero();
#endif
_sum_count = 0;
hal.scheduler->resume_timer_procs();
gyro *= _gyro_scale / num_samples;
accel *= MPU6000_ACCEL_SCALE_1G / num_samples;
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXF
accel.rotate(ROTATION_PITCH_180_YAW_90);
gyro.rotate(ROTATION_PITCH_180_YAW_90);
#endif
_publish_accel(_accel_instance, accel);
_publish_gyro(_gyro_instance, gyro);
#if MPU6000_FAST_SAMPLING
if (_last_accel_filter_hz != _accel_filter_cutoff()) {
_accel_filter.set_cutoff_frequency(1000, _accel_filter_cutoff());
_last_accel_filter_hz = _accel_filter_cutoff();
}
if (_last_gyro_filter_hz != _gyro_filter_cutoff()) {
_gyro_filter.set_cutoff_frequency(1000, _gyro_filter_cutoff());
_last_gyro_filter_hz = _gyro_filter_cutoff();
}
#else
if (_last_accel_filter_hz != _accel_filter_cutoff()) {
if (_spi_sem->take(10)) {
_spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_LOW);
_set_filter_register(_accel_filter_cutoff());
_spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_HIGH);
_spi_sem->give();
_last_accel_filter_hz = _accel_filter_cutoff();
}
}
#endif
return true;
}
