/home/jgoppert/Projects/uvsim/uvsim/src/uvsim/systems/UgvSystem.cc

Go to the documentation of this file.

/*
 * UgvSystem.cpp
 *
 *  Created on: Apr 26, 2009
 *      Author: jgoppert
 */

#include "UgvSystem.h"
#include <iostream>
#include <uvsim/utilities/RealtimeThread.h>

namespace uvsim
{

VisionThread::VisionThread(const char * port, float freq, int priority) :
        RealtimeThread(priority, freq)
{
    std::cout << "\nVision thread initialized." << std::endl;
}

void VisionThread::loop()
{
    //std::cout << "\nVision thread running." << std::endl;
}

VisionThread::~VisionThread()
{
}

ControlThread::ControlThread(std::string port, float freq, int priority,
                             bool useJoystick) :
        RealtimeThread(priority, freq)
{
    m_controlComm = new ControlComm(port, useJoystick);
    std::cout << "Control thread initialized." << std::endl;
}

void ControlThread::loop()
{
    //std::cout << "\nControl thread running." << std::endl;
    m_controlComm->update();
}

ControlThread::~ControlThread()
{
    // TODO: Fix memory leak for controlComm
    //delete m_controlComm;
}

NavigationThread::NavigationThread(const char * port, float freq, int priority) :
        RealtimeThread(priority, freq)
{
    vector<double> freqCutMb(3), freqCutFb(3), freqCutWb(3);

    // magnetometer cutoff freq
    freqCutMb(0) = 312;
    freqCutMb(1) = 312;
    freqCutMb(2) = 312;

    // accel cutoff freq
    freqCutFb(0) = 350;
    freqCutFb(1) = 350;
    freqCutFb(2) = 150;

    // gyro cutoff freq
    freqCutWb(0) = 120;
    freqCutWb(1) = 120;
    freqCutWb(2) = 120;

    // setup imu
    imu = new Imu6DofSensor(port, freqCutMb, freqCutFb, freqCutWb);

    // setup ins
    ins = new Ins(std::string(DATADIR)+="/data/Ins15g.dat",
                  std::string(DATADIR)+="/data/Gyro.dat",
                  std::string(DATADIR)+="/data/Mag.dat",
                  freq,imu);
}

void NavigationThread::loop()
{
    std::cout << "\nNavigation thread running." << std::endl;
}

NavigationThread::~NavigationThread()
{
    delete imu;
    delete ins;
}

UgvSystem::UgvSystem()
{
    m_visionThread = NULL;
    m_navigationThread = NULL;
    m_controlThread = NULL;
}

UgvSystem::UgvSystem(const char * visionPort, float visionFreq,
                     int visionPriority, const char * controlPort, float controlFreq,
                     int controlPriority, bool useJoystick, const char * navigationPort,
                     float navigationFreq, int navigationPriority)
{
    m_visionThread = new VisionThread(visionPort, visionFreq, visionPriority);
    m_controlThread = new ControlThread(controlPort, controlFreq, controlPriority, useJoystick);
    m_navigationThread = new NavigationThread(navigationPort, navigationFreq, navigationPriority);
        std::cout << "\nThreads initialized." << std::endl;
}

UgvSystem::~UgvSystem()
{
    // Join all threads
    if (m_controlThread != NULL) m_controlThread->stop();
    if (m_navigationThread != NULL) m_navigationThread->stop();
    if (m_visionThread != NULL) m_visionThread->stop();

    // Delete all threads
    if (m_visionThread != NULL) delete m_visionThread;
    if (m_controlThread != NULL) delete m_controlThread;
    if (m_navigationThread != NULL) delete m_navigationThread;
}

void UgvSystem::run()
{
        std::cout << "\nLaunching threads." << std::endl;
    if (m_visionThread != NULL) m_visionThread->start();
    if (m_navigationThread != NULL) m_navigationThread->start();
    if (m_controlThread != NULL) m_controlThread->start();
}

}

---