#include <SENSSimulated.h>

Inheritance diagram for SENSSimulated< T >:

Public Member Functions
virtual	~SENSSimulated ()

Public Member Functions inherited from SENSSimulatedBase
virtual	~SENSSimulatedBase ()

Protected Types
using	StartSimCB = std::function< void(void)>

using	SensorSimStoppedCB = std::function< void(void)>

Protected Member Functions
	SENSSimulated (const std::string name, StartSimCB startSimCB, SensorSimStoppedCB sensorSimStoppedCB, std::vector< std::pair< SENSTimePt, T >> &&data, const SENSSimClock &clock)

void	startSim ()
	start the sensor simulation thread More...

void	stopSim ()
	stop the sensor simulation thread More...

bool	getErrorMsg (std::string &msg) override
	get error msg (valid if function returns true) More...

bool	isThreadRunning () const override
	indicates if simulation thread is running More...

virtual void	feedSensorData (const int counter)=0
	feed new sensor data to sensor More...

virtual void	prepareSensorData (const int counter)

virtual void	onLatencyProblem (int &counter)
	special treatment when there is latency (e.g. when feeding camera frames from cv::videocapture updating the frame pos may take ages) More...

Protected Attributes
std::vector< std::pair< SENSTimePt, T > >	_data

SensorSimStoppedCB	_sensorSimStoppedCB

StartSimCB	_startSimCB

std::thread	_thread

std::condition_variable	_condVar

std::mutex	_mutex

bool	_stop = false

SENSTimePt	_commonSimStartTimePt

bool	_threadIsRunning = false

const SENSSimClock &	_clock

std::string	_name

std::mutex	_msgMutex

std::string	_errorMsg

Private Member Functions
void	feedSensor ()
	thread run routine to feed sensor with data. More...

Detailed Description

template<typename T>
class SENSSimulated< T >

SENSSimulated Implements the sensor simulation backend. SENSSimulated runs a thread (see feedSensor()) that calls feedSensorData() interface function which feeds the next data value to a sensor (e.g. SENSGps). The current simulation time is retrieved from the SENSSimulator clock and depending on this the next data value is selected and fed a the respective time. This class contains common functionality for SENSSimulated implementations.

Definition at line 39 of file SENSSimulated.h.

Member Typedef Documentation

◆ SensorSimStoppedCB

template<typename T >

using SENSSimulated< T >::SensorSimStoppedCB = std::function<void(void)>

protected

Definition at line 46 of file SENSSimulated.h.

◆ StartSimCB

template<typename T >

using SENSSimulated< T >::StartSimCB = std::function<void(void)>

protected

Definition at line 45 of file SENSSimulated.h.

Constructor & Destructor Documentation

◆ ~SENSSimulated()

template<typename T >

virtual SENSSimulated< T >::~SENSSimulated ( )

inlinevirtual

Definition at line 42 of file SENSSimulated.h.

42 {}

◆ SENSSimulated()

template<typename T >

SENSSimulated< T >::SENSSimulated	(	const std::string	name,
		StartSimCB	startSimCB,
		SensorSimStoppedCB	sensorSimStoppedCB,
		std::vector< std::pair< SENSTimePt, T >> &&	data,
		const SENSSimClock &	clock
	)

protected

Transfer callbacks to SENSSimulator during construction. The SENSSimulator is informed by these if the SENSSimulated state changes (start, stop). We use callbacks to avoid circular dependencies.

Definition at line 6 of file SENSSimulated.cpp.

   : _name(name),
     _startSimCB(startSimCB),
     _sensorSimStoppedCB(sensorSimStoppedCB),
     _data(data),
     _clock(clock)
 {
 }

Member Function Documentation

◆ feedSensor()

template<typename T >

void SENSSimulated< T >::feedSensor

private

thread run routine to feed sensor with data.

Definition at line 60 of file SENSSimulated.cpp.

 {
     _threadIsRunning = true;
     //index of current data set to feed
     int counter = 0;
  
     SENSTimePt simTime = _clock.now();
     //bring counter close to startTimePt (maybe we skip some values to synchronize simulated sensors)
     while (counter < _data.size() && _data[counter].first < simTime)
         counter++;
  
     if (counter >= _data.size())
     {
         //end of simulation
         _threadIsRunning = false;
         _sensorSimStoppedCB();
         return;
     }
  
     while (true)
     {
         //estimate current sim time
         SENSTimePt simTime = _clock.now();
  
         //feed latest data
         int oldCounter = counter;
         if (counter < _data.size() && _data[counter].first <= simTime)
         {
             //SENS_DEBUG("feed sensor index %d with latency: %d us, SimT: %d, ValueT: %d", counter, std::chrono::duration_cast<SENSMicroseconds>(_data[counter].first - simTime).count(), std::chrono::time_point_cast<SENSMicroseconds>(simTime).time_since_epoch().count(), std::chrono::time_point_cast<SENSMicroseconds>(_data[counter].first).time_since_epoch().count());
             SENS_DEBUG("feed %s sensor index %d with latency: %d us", _name.c_str(), counter, std::chrono::duration_cast<SENSMicroseconds>(_data[counter].first - simTime).count());
             feedSensorData(counter);
             if (_stop)
                 break;
  
             //update the counter
             simTime = _clock.now();
             while (counter < _data.size() && _data[counter].first < simTime)
                 counter++;
         }
  
         if (counter - oldCounter > 1)
         {
             //for feeding camera frames from cv::videocapture we need a special treatment in this case
             //(THIS FUNCTION MAY CHANGE THE COUNTER)
             onLatencyProblem(counter);
             //report an error
             int numberOfSkippedData = counter - oldCounter;
  
             std::stringstream ss;
             ss << "Data feeding is too slow. Skipped " << numberOfSkippedData << " data sets!";
             SENS_WARN("SENSSimulated feedSensor: %s", ss.str().c_str());
             {
                 std::lock_guard<std::mutex> lock(_msgMutex);
                 _errorMsg = ss.str();
             }
         }
  
         //end of simulation
         if (counter >= _data.size())
             break;
  
         //prepare things that may take some time (e.g. for video reading we can already read the frame and do decoding and maybe it also blocks for some reasons..)
         prepareSensorData(counter);
  
         //WAITING DOES NOT WORK VERY EXACTLY ON WINDOWS (it does wake too late)
 #ifdef WAIT
         simTime = _clock.now();
         //simTime should now be smaller than valueTime because valueTime is in the simulation future
         SENSMicroseconds waitTimeUs = std::chrono::duration_cast<SENSMicroseconds>(_data[counter].first - simTime);
         //We reduce the wait time because thread sleep is not very exact (best is not to wait at all)
         SENSMicroseconds reducedWaitTimeUs((long)(0.01 * (double)waitTimeUs.count()));
  
         //HighResTimer t;
         std::unique_lock<std::mutex> lock(_mutex);
         _condVar.wait_for(lock, reducedWaitTimeUs, [&]
                           { return _stop == true; });
         //SENS_DEBUG("wait time %d us", waitTimeUs.count());
         //SENS_DEBUG("woke after %d us", t.elapsedTimeInMicroSec());
 #endif
         if (_stop)
             break;
     }
  
     _threadIsRunning = false;
     _sensorSimStoppedCB();
 }

◆ feedSensorData()

template<typename T >

virtual void SENSSimulated< T >::feedSensorData ( const int counter )

protectedpure virtual

feed new sensor data to sensor

Implemented in SENSSimulatedCamera, SENSSimulatedOrientation, and SENSSimulatedGps.

◆ getErrorMsg()

template<typename T >

bool SENSSimulated< T >::getErrorMsg ( std::string & msg )

overrideprotectedvirtual

get error msg (valid if function returns true)

Implements SENSSimulatedBase.

Definition at line 47 of file SENSSimulated.cpp.

 {
     std::lock_guard<std::mutex> lock(_msgMutex);
     if (_errorMsg.empty())
         return false;
     else
     {
         msg = _errorMsg;
         return true;
     }
 }

◆ isThreadRunning()

template<typename T >

bool SENSSimulated< T >::isThreadRunning ( ) const

inlineoverrideprotectedvirtual

indicates if simulation thread is running

Implements SENSSimulatedBase.

Definition at line 64 of file SENSSimulated.h.

64 { return _threadIsRunning; }

◆ onLatencyProblem()

template<typename T >

virtual void SENSSimulated< T >::onLatencyProblem ( int & counter )

inlineprotectedvirtual

special treatment when there is latency (e.g. when feeding camera frames from cv::videocapture updating the frame pos may take ages)

Reimplemented in SENSSimulatedCamera.

Definition at line 72 of file SENSSimulated.h.

72 {}

◆ prepareSensorData()

template<typename T >

virtual void SENSSimulated< T >::prepareSensorData ( const int counter )

inlineprotectedvirtual

prepare things that may take some time for the next writing of sensor data (e.g. for video reading we can already read the frame and do decoding and maybe it also blocks for some reasons..)

Reimplemented in SENSSimulatedCamera.

Definition at line 70 of file SENSSimulated.h.

70 {};

◆ startSim()

template<typename T >

void SENSSimulated< T >::startSim

protected

start the sensor simulation thread

Definition at line 20 of file SENSSimulated.cpp.

 {
     //inform SENSSimulator about start
     _startSimCB();
     //stop the local simulation thread if running
     stopSim();
     _errorMsg.clear();
     //start the simulation thread
     _thread = std::thread(&SENSSimulated::feedSensor, this);
 }

◆ stopSim()

template<typename T >

void SENSSimulated< T >::stopSim

protected

stop the sensor simulation thread

Definition at line 32 of file SENSSimulated.cpp.

 {
     std::unique_lock<std::mutex> lock(_mutex);
     _stop = true;
     lock.unlock();
     _condVar.notify_one();
  
     if (_thread.joinable())
         _thread.join();
  
     lock.lock();
     _stop = false;
 }

Member Data Documentation

◆ _clock

template<typename T >

const SENSSimClock& SENSSimulated< T >::_clock

protected

Definition at line 94 of file SENSSimulated.h.

◆ _commonSimStartTimePt

template<typename T >

SENSTimePt SENSSimulated< T >::_commonSimStartTimePt

protected

Definition at line 90 of file SENSSimulated.h.

◆ _condVar

template<typename T >

std::condition_variable SENSSimulated< T >::_condVar

protected

Definition at line 86 of file SENSSimulated.h.

◆ _data

template<typename T >

std::vector<std::pair<SENSTimePt, T> > SENSSimulated< T >::_data

protected

Definition at line 79 of file SENSSimulated.h.

◆ _errorMsg

template<typename T >

std::string SENSSimulated< T >::_errorMsg

protected

Definition at line 99 of file SENSSimulated.h.

◆ _msgMutex

template<typename T >

std::mutex SENSSimulated< T >::_msgMutex

protected

Definition at line 98 of file SENSSimulated.h.

◆ _mutex

template<typename T >

std::mutex SENSSimulated< T >::_mutex

protected

Definition at line 87 of file SENSSimulated.h.

◆ _name

template<typename T >

std::string SENSSimulated< T >::_name

protected

Definition at line 96 of file SENSSimulated.h.

◆ _sensorSimStoppedCB

template<typename T >

SensorSimStoppedCB SENSSimulated< T >::_sensorSimStoppedCB

protected

Definition at line 82 of file SENSSimulated.h.

◆ _startSimCB

template<typename T >

StartSimCB SENSSimulated< T >::_startSimCB

protected

Definition at line 83 of file SENSSimulated.h.

◆ _stop

template<typename T >

bool SENSSimulated< T >::_stop = false

protected

Definition at line 88 of file SENSSimulated.h.

◆ _thread

template<typename T >

std::thread SENSSimulated< T >::_thread

protected

Definition at line 85 of file SENSSimulated.h.

◆ _threadIsRunning

template<typename T >

bool SENSSimulated< T >::_threadIsRunning = false

protected

Definition at line 92 of file SENSSimulated.h.

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

Public Member Functions

Protected Types

Protected Member Functions

Protected Attributes

Private Member Functions

Detailed Description

template<typename T> class SENSSimulated< T >

Member Typedef Documentation

◆ SensorSimStoppedCB

◆ StartSimCB

Constructor & Destructor Documentation

◆ ~SENSSimulated()

◆ SENSSimulated()

Member Function Documentation

◆ feedSensor()

◆ feedSensorData()

◆ getErrorMsg()

◆ isThreadRunning()

◆ onLatencyProblem()

◆ prepareSensorData()

◆ startSim()

◆ stopSim()

Member Data Documentation

◆ _clock

◆ _commonSimStartTimePt

◆ _condVar

◆ _data

◆ _errorMsg

◆ _msgMutex

◆ _mutex

◆ _name

◆ _sensorSimStoppedCB

◆ _startSimCB

◆ _stop

◆ _thread

◆ _threadIsRunning

template<typename T>
class SENSSimulated< T >