SENSSimulator Class Reference

#include <SENSSimulator.h>

Public Member Functions
	SENSSimulator (const std::string &simDirName)
	ctor, transfer directory containing sensor data that was recorded with SENSRecorder More...
	~SENSSimulator ()
SENSSimulatedGps *	getGpsSensorPtr ()
	get sensor pointer of simulated gps sensor and use it like a normal sensor (valid if not null) More...
SENSSimulatedOrientation *	getOrientationSensorPtr ()
	get sensor pointer of simulated orientation sensor and use it like a normal sensor (valid if not null) More...
SENSSimulatedCamera *	getCameraSensorPtr ()
	get sensor pointer of simulated camera sensor and use it like a normal sensor (valid if not null) More...
bool	isRunning () const
	indicates if simulator is currently running More...
bool	getSimulatorErrors (std::vector< std::string > &errorMsgs)
void	pause ()
	pause simulation time More...
bool	isPaused ()
void	resume ()
	resume simulation time More...
SENSTimePt	now ()
	reset simulation time More...
SENSMicroseconds	passedTime ()
	get passed simulation time More...

Private Member Functions
template<typename T >
T *	getActiveSensor ()
void	onStart ()
	callback called from SENSSimulated when the simulated sensor was started More...
void	onSensorSimStopped ()
	callback called from SENSSimulated when the simulated sensor was stopped More...
void	loadGpsData (const std::string &dirName, std::vector< std::pair< SENSTimePt, SENSGps::Location >> &data)
	load data from file and instantiate sensor if valid More...
void	loadOrientationData (const std::string &dirName, std::vector< std::pair< SENSTimePt, SENSOrientation::Quat >> &data)
	load data from file and instantiate sensor if valid More...
void	loadCameraData (const std::string &dirName, std::vector< std::pair< SENSTimePt, int >> &data, std::string &videoFileName, SENSCameraConfig &cameraConfig)
	load data from file and instantiate sensor if valid More...

Private Attributes
std::vector< std::unique_ptr< SENSSimulatedBase > >	_activeSensors
	list of currently activated sensors. Which sensors are activated depends on the content of simulation directory More...
std::unique_ptr< SENSSimClock >	_clock
std::atomic_bool	_running {false}
	flags if simulator is currently running More...

Detailed Description

SENSSimulator This class is owner of SENSSimulated sensor instances. It loads special sensor data from input simulation directory transferred during construction. Depending on if the directory content is valid, a SENSSimulated implementation is instantiated and initialized with loaded sensor data. You can get a sensor pointer to a simulated sensor by calling the getter get..SensorPtr() method and use it like the real sensor, as it implements the original sensor interface. The SENSSimulator is maintainer of the current simulation time. Simulated sensor values are selected depending on this time. The idea is to provide sensor data exactly as it was recorded. ATTENTION: Simulation time starts, as soon as one simulated sensor was started and is resetted when all simulated sensors are stopped.

Definition at line 23 of file SENSSimulator.h.

Constructor & Destructor Documentation

SENSSimulator()

SENSSimulator::SENSSimulator

(

const std::string &

simDirName

)

ctor, transfer directory containing sensor data that was recorded with SENSRecorder

Definition at line 67 of file SENSSimulator.cpp.

{
    try
    {
        //check directory content and enable simulated sensors depending on this
        std::string dirName = Utils::unifySlashes(simDirName);
 
        if (Utils::dirExists(dirName))
        {
            std::vector<std::pair<SENSTimePt, SENSGps::Location>> gpsData;
            loadGpsData(dirName, gpsData);
            std::vector<std::pair<SENSTimePt, SENSOrientation::Quat>> orientationData;
            loadOrientationData(dirName, orientationData);
            std::vector<std::pair<SENSTimePt, int>> cameraData;
            std::string                             videoFileName;
            SENSCameraConfig                        cameraConfig;
            loadCameraData(dirName, cameraData, videoFileName, cameraConfig);
 
            //search for common sim time start point
            //start time point of simulation in the past
            SENSTimePt simStartTimePt;
 
            bool initialized = false;
 
            if (gpsData.size())
                findSimStartTimePt(simStartTimePt, initialized, gpsData.front().first);
            if (orientationData.size())
                findSimStartTimePt(simStartTimePt, initialized, orientationData.front().first);
            if (cameraData.size())
                findSimStartTimePt(simStartTimePt, initialized, cameraData.front().first);
 
            if (initialized)
            {
                //instantiate simulator clock
                _clock = std::make_unique<SENSSimClock>(SENSClock::now(), simStartTimePt);
 
                if (gpsData.size())
                {
                    //make_unique has problems with fiendship and private constructor so we use unique_ptr
                    _activeSensors.push_back(std::unique_ptr<SENSSimulatedGps>(
                      new SENSSimulatedGps(std::bind(&SENSSimulator::onStart, this),
                                           std::bind(&SENSSimulator::onSensorSimStopped, this),
                                           std::move(gpsData),
                                           *_clock)));
                }
 
                if (orientationData.size())
                {
                    //make_unique has problems with fiendship and private constructor so we use unique_ptr
                    _activeSensors.push_back(std::unique_ptr<SENSSimulatedOrientation>(
                      new SENSSimulatedOrientation(std::bind(&SENSSimulator::onStart, this),
                                                   std::bind(&SENSSimulator::onSensorSimStopped, this),
                                                   std::move(orientationData),
                                                   *_clock)));
                }
 
                if (cameraData.size())
                {
                    //make_unique has problems with fiendship and private constructor so we use unique_ptr
                    _activeSensors.push_back(std::unique_ptr<SENSSimulatedCamera>(
                      new SENSSimulatedCamera(std::bind(&SENSSimulator::onStart, this),
                                              std::bind(&SENSSimulator::onSensorSimStopped, this),
                                              std::move(cameraData),
                                              videoFileName,
                                              cameraConfig,
                                              *_clock)));
                }
            }
        }
        else
            Utils::log("SENS", "SENSSimulator: Directory does not exist: %s", simDirName.c_str());
    }
    catch (...)
    {
        Utils::log("SENS", "SENSSimulator: Exception while parsing sensor files");
    }
}

~SENSSimulator()

SENSSimulator::~SENSSimulator

(

)

Definition at line 3 of file SENSSimulator.cpp.

{
}

Member Function Documentation

getActiveSensor()

template<typename T >

T* SENSSimulator::getActiveSensor ( )

inlineprivate

Definition at line 53 of file SENSSimulator.h.

    {
        for (int i = 0; i < _activeSensors.size(); ++i)
        {
            SENSSimulatedBase* base    = _activeSensors[i].get();
            T*                 derived = dynamic_cast<T*>(base);
            if (derived)
                return derived;
        }
 
        return nullptr;
    }

getCameraSensorPtr()

SENSSimulatedCamera* SENSSimulator::getCameraSensorPtr ( )

inline

get sensor pointer of simulated camera sensor and use it like a normal sensor (valid if not null)

Definition at line 35 of file SENSSimulator.h.

{ return getActiveSensor<SENSSimulatedCamera>(); }

getGpsSensorPtr()

SENSSimulatedGps* SENSSimulator::getGpsSensorPtr ( )

inline

get sensor pointer of simulated gps sensor and use it like a normal sensor (valid if not null)

Definition at line 31 of file SENSSimulator.h.

{ return getActiveSensor<SENSSimulatedGps>(); }

getOrientationSensorPtr()

SENSSimulatedOrientation* SENSSimulator::getOrientationSensorPtr ( )

inline

get sensor pointer of simulated orientation sensor and use it like a normal sensor (valid if not null)

Definition at line 33 of file SENSSimulator.h.

{ return getActiveSensor<SENSSimulatedOrientation>(); }

getSimulatorErrors()

bool SENSSimulator::getSimulatorErrors

(

std::vector< std::string > &

errorMsgs

)

Definition at line 7 of file SENSSimulator.cpp.

{
    for (int i = 0; i < _activeSensors.size(); ++i)
    {
        SENSSimulatedBase* sensor = _activeSensors[i].get();
        std::string        msg;
        if (sensor->getErrorMsg(msg))
            errorMsgs.emplace_back(msg);
    }
 
    return errorMsgs.size() != 0;
}

isPaused()

bool SENSSimulator::isPaused

(

)

Definition at line 26 of file SENSSimulator.cpp.

{
    if (_clock)
        return _clock->isPaused();
    else
        return false;
}

isRunning()

bool SENSSimulator::isRunning ( ) const

inline

indicates if simulator is currently running

Definition at line 37 of file SENSSimulator.h.

{ return _running; }

loadCameraData()

void SENSSimulator::loadCameraData ( const std::string &  dirName, std::vector< std::pair< SENSTimePt, int >> &  data, std::string &  videoFileName, SENSCameraConfig &  cameraConfig  )

private

load data from file and instantiate sensor if valid

Definition at line 259 of file SENSSimulator.cpp.

{
    std::string textFileName   = dirName + "camera.txt";
    videoFileName              = dirName + "video.avi";
    std::string configFileName = dirName + "cameraConfig.json";
 
    //check if directory contains gps.txt
    if (Utils::fileExists(textFileName) && Utils::fileExists(videoFileName) && Utils::fileExists(configFileName))
    {
        //load config file
        {
            cv::FileStorage fs;
            fs.open(configFileName, cv::FileStorage::READ);
            if (fs.isOpened())
            {
                fs["deviceId"] >> cameraConfig.deviceId;
                fs["widthPix"] >> cameraConfig.streamConfig.widthPix;
                fs["heightPix"] >> cameraConfig.streamConfig.heightPix;
                fs["focalLengthPix"] >> cameraConfig.streamConfig.focalLengthPix;
                int focusMode;
                fs["focusMode"] >> focusMode;
                cameraConfig.focusMode = (SENSCameraFocusMode)focusMode;
                int facing;
                fs["facing"] >> facing;
                cameraConfig.facing = (SENSCameraFacing)facing;
            }
            else
                Utils::log("SENS", "SENSSimulator: Unable to open file: %s ", configFileName.c_str());
        }
 
        //load frame info
        {
            std::string line;
            ifstream    file(textFileName);
            if (file.is_open())
            {
                while (std::getline(file, line))
                {
                    //cout << line << '\n';
                    long long                readTimePt;
                    int                      frameIndex;
                    std::vector<std::string> values;
                    Utils::splitString(line, ' ', values);
                    if (values.size() == 2)
                    {
                        readTimePt = std::stoll(values[0]);
                        frameIndex = std::stoi(values[1]);
 
                        SENSMicroseconds readTimePtUs(readTimePt);
                        SENSTimePt       tPt(readTimePtUs);
                        data.push_back(std::make_pair(tPt, frameIndex));
                    }
                }
                file.close();
            }
            else
                Utils::log("SENS", "SENSSimulator: Unable to open file: %s ", textFileName.c_str());
        }
    }
}

loadGpsData()

void SENSSimulator::loadGpsData ( const std::string &  dirName, std::vector< std::pair< SENSTimePt, SENSGps::Location >> &  data  )

private

load data from file and instantiate sensor if valid

Definition at line 185 of file SENSSimulator.cpp.

{
    std::string gpsFileName = dirName + "gps.txt";
    //check if directory contains gps.txt
    if (Utils::fileExists(gpsFileName))
    {
        std::string line;
        ifstream    file(gpsFileName);
        if (file.is_open())
        {
            while (std::getline(file, line))
            {
                //cout << line << '\n';
                long long                readTimePt;
                SENSGps::Location        loc;
                std::vector<std::string> values;
                Utils::splitString(line, ' ', values);
                if (values.size() == 5)
                {
                    readTimePt       = std::stoll(values[0]);
                    loc.latitudeDEG  = std::stof(values[1]);
                    loc.longitudeDEG = std::stof(values[2]);
                    loc.altitudeM    = std::stof(values[3]);
                    loc.accuracyM    = std::stof(values[4]);
 
                    SENSMicroseconds readTimePtUs(readTimePt);
                    SENSTimePt       tPt(readTimePtUs);
                    data.push_back(std::make_pair(tPt, loc));
                }
            }
            file.close();
        }
        else
            Utils::log("SENS", "SENSSimulator: Unable to open file: %s", gpsFileName.c_str());
    }
}

loadOrientationData()

void SENSSimulator::loadOrientationData ( const std::string &  dirName, std::vector< std::pair< SENSTimePt, SENSOrientation::Quat >> &  data  )

private

load data from file and instantiate sensor if valid

Definition at line 222 of file SENSSimulator.cpp.

{
    std::string orientationFileName = dirName + "orientation.txt";
    //check if directory contains orientation.txt
    if (Utils::fileExists(orientationFileName))
    {
        std::string line;
        ifstream    file(orientationFileName);
        if (file.is_open())
        {
            while (std::getline(file, line))
            {
                //cout << line << '\n';
                long long                readTimePt;
                SENSOrientation::Quat    quat;
                std::vector<std::string> values;
                Utils::splitString(line, ' ', values);
                if (values.size() == 5)
                {
                    readTimePt = std::stoll(values[0]);
                    quat.quatX = std::stof(values[1]);
                    quat.quatY = std::stof(values[2]);
                    quat.quatZ = std::stof(values[3]);
                    quat.quatW = std::stof(values[4]);
 
                    SENSMicroseconds readTimePtUs(readTimePt);
                    SENSTimePt       tPt(readTimePtUs);
                    data.push_back(std::make_pair(tPt, quat));
                }
            }
            file.close();
        }
        else
            Utils::log("SENS", "SENSSimulator: Unable to open file: %s", orientationFileName.c_str());
    }
}

now()

SENSTimePt SENSSimulator::now

(

)

reset simulation time

Definition at line 48 of file SENSSimulator.cpp.

{
    SENSTimePt simNow;
    if (_clock)
        simNow = _clock->now();
    return simNow;
}

onSensorSimStopped()

void SENSSimulator::onSensorSimStopped ( )

private

callback called from SENSSimulated when the simulated sensor was stopped

Definition at line 155 of file SENSSimulator.cpp.

{
    //if no sensor is running anymore, we stop the simulation
    bool aSensorIsRunning = false;
    for (int i = 0; i < _activeSensors.size(); ++i)
    {
        if (_activeSensors[i]->isThreadRunning())
        {
            aSensorIsRunning = true;
            break;
        }
    }
 
    if (!aSensorIsRunning)
        _running = false;
 
    //THIS DOES NOT WORK BECAUSE WE WOULD JOIN A THREAD FROM THE CURRENT THREAD
    //assuming sensor implementations (e.g. SENSGps) are still in running we can restart the backend threads
    /*
    if (!_running && _repeat)
    {
        _clock.reset();
        for (int i = 0; i < _activeSensors.size(); ++i)
        {
            _activeSensors[i]->startSim();
        }
    }
    */
}

onStart()

void SENSSimulator::onStart ( )

private

callback called from SENSSimulated when the simulated sensor was started

Definition at line 145 of file SENSSimulator.cpp.

{
    if (!_running)
    {
        if (_clock)
            _clock->reset();
        _running = true;
    }
}

passedTime()

SENSMicroseconds SENSSimulator::passedTime

(

)

get passed simulation time

Definition at line 40 of file SENSSimulator.cpp.

{
    SENSMicroseconds passedTime;
    if (_clock)
        passedTime = _clock->passedTime();
    return passedTime;
}

pause()

void SENSSimulator::pause

(

)

pause simulation time

Definition at line 20 of file SENSSimulator.cpp.

{
    if (_clock)
        _clock->pause();
}

resume()

void SENSSimulator::resume

(

)

resume simulation time

Definition at line 34 of file SENSSimulator.cpp.

{
    if (_clock)
        _clock->resume();
}

Member Data Documentation

_activeSensors

std::vector<std::unique_ptr<SENSSimulatedBase> > SENSSimulator::_activeSensors

private

list of currently activated sensors. Which sensors are activated depends on the content of simulation directory

Definition at line 78 of file SENSSimulator.h.

_clock

std::unique_ptr<SENSSimClock> SENSSimulator::_clock

private

Definition at line 80 of file SENSSimulator.h.

_running

std::atomic_bool SENSSimulator::_running {false}

private

flags if simulator is currently running

Definition at line 82 of file SENSSimulator.h.

---

The documentation for this class was generated from the following files:

• SENSSimulator.h
• SENSSimulator.cpp