Skip to content

Instantly share code, notes, and snippets.

@Micrified
Created April 21, 2024 09:59
Show Gist options
  • Save Micrified/d58859ca62737251a95f7adcc6c09ddc to your computer and use it in GitHub Desktop.
Save Micrified/d58859ca62737251a95f7adcc6c09ddc to your computer and use it in GitHub Desktop.
Safe sensor example
#include <chrono>
#include <cstdlib>
#include <iostream>
#include <ratio>
#include <string>
#include <thread>
#include <vector>
#include <mutex>
// [ Written to C++11 standard ]
// Note: So, the way the code is structured is explained here as briefly as
// possible.
//
// We have a sensor class which takes a template parameter <T>. This means
// you can generate templated types of sensors. That's great. But do note
// that that generated sensor types aren't the same as each other. They don't
// have a common base class. So if you want to organize sensors at a higher
// level, you need to add in some base class that isn't template derived.
//
// For that, we have the ReadableDevice class. This is the core base of all
// derived sensor types. All it stipulates is that their derivates must
// allow you to read() them. This lets you combine sensors together and
// provide them to threads which can read them without the thread needing
// to know anything about the derived type (it just has to call read()).
//
// For thread safe access, the goal of this code is to wrap that all up in
// the Sensor pure virtual class. That templated class takes your template
// data type and makes it private. Then, the setters and getters are mutex
// guarded. This makes all use of derivative sensor classes thread safe
//
// Of course, maybe you don't really want a mutex for every sensor. But to
// do away with that, you'll need to organize sensors into runtime entities
// that themselves control access to N+ sensors with a single mutex. That's
// more complicated and I don't know the use cases, so I think this is
// good enough for an example for now :)
/*\
*******************************************************************************
* Type Definition: Abstract Classes *
*******************************************************************************
\*/
/* A readable device is a pure virtual class which simply requires
* that the derivative support a read method on itself */
class ReadableDevice
{
public:
virtual void read() = 0;
};
/* A sensor is a readable device which is also a pure virtual class. Any
* instance of a sensor has its own dedicated type (Sensor<T>). However,
* these types have an underlying thread-safe data storage */
template<typename T> class Sensor : public ReadableDevice
{
public:
Sensor(std::chrono::microseconds delay):
d_data(),
d_delay(delay),
d_mutex()
{}
virtual void read() = 0;
T data()
{
T data;
d_mutex.lock();
data = d_data;
d_mutex.unlock();
return data;
}
const std::chrono::microseconds delay() const
{
return d_delay;
}
protected:
void set(const T data)
{
d_mutex.lock();
d_data = data;
d_mutex.unlock();
}
private:
T d_data;
std::chrono::microseconds d_delay;
std::mutex d_mutex;
};
/*\
*******************************************************************************
* Type Definition: Sensors *
*******************************************************************************
\*/
/* Structure of: Gyro data */
struct gyro_data_t
{
public:
gyro_data_t():
d_rx(0.0), d_ry(0.0), d_rz(0.0)
{}
virtual void tick()
{
d_rx += 1.0; d_ry += 1.0; d_rz += 1.0;
}
double d_rx, d_ry, d_rz;
};
std::ostream& operator<<(std::ostream& out, const gyro_data_t& d)
{
return out << "{ .rx = " << d.d_rx
<< ", .ry = " << d.d_ry
<< ", .rz = " << d.d_rz
<< " }";
}
/* Gyro sensor class */
class Gyro : public Sensor<gyro_data_t>
{
public:
Gyro(): Sensor(std::chrono::milliseconds(100))
{}
virtual void read() override
{
std::this_thread::sleep_for(delay());
gyro_data_t d = data();
d.tick();
set(d);
}
};
/* Structure of: Radar data */
struct radar_data_t
{
public:
radar_data_t():
d_value(0)
{}
virtual void tick()
{
d_value += 1.0;
}
double d_value;
};
std::ostream& operator<<(std::ostream& out, const radar_data_t& d)
{
return out << " { .value = " << d.d_value << " }";
}
/* Radar sensor class */
class Radar : public Sensor<radar_data_t>
{
public:
Radar() : Sensor(std::chrono::milliseconds(300))
{}
virtual void read() override
{
std::this_thread::sleep_for(delay());
radar_data_t d = data();
d.tick();
set(d);
}
};
// Add your additional sensor definitions here ...
/*\
*******************************************************************************
* Type Definition: TaskGroup *
*******************************************************************************
\*/
/* A task group is a dedicated thread which takes readable devices
* and calls read on them in a loop. It's basically type agnostic so
* various kinds of sensors can be put in the input array and read
*/
class TaskGroup
{
public:
TaskGroup() = delete;
TaskGroup(std::vector<ReadableDevice*>& devices):
d_devices(devices)
{}
std::thread thread()
{
return std::thread(&TaskGroup::run, this);
}
protected:
void run()
{
while (1)
{
for (auto i = d_devices.begin(); i != d_devices.end(); i++)
{
(*i)->read();
}
}
}
private:
std::vector<ReadableDevice*>& d_devices;
};
/*\
*******************************************************************************
* Main *
*******************************************************************************
\*/
/* Here we have our main loop that loops through the sensors and reads
* them. Notice it doesn't care about mutexes or anything, and nor should
* it. The integrity of the underlying data is guaranteed by the sensor
* class */
void sensor_read_loop(std::vector<Gyro*> gyros, std::vector<Radar*> radars)
{
while (1)
{
// Read Gyros
std::cout << "Reading gyros ..." << std::endl;
for (auto i = gyros.begin(); i != gyros.end(); i++)
{
std::cout << (*i)->data() << std::endl;
}
// Read Radars
std::cout << "Reading radars ..." << std::endl;
for (auto i = radars.begin(); i != radars.end(); i++)
{
std::cout << (*i)->data() << std::endl;
}
// Loop delay
std::this_thread::sleep_for(std::chrono::milliseconds(500));
}
}
/* Main loops setting everything up */
int main()
{
Gyro g1, g2, g3;
Radar r1, r2;
// Sensor arrangement
std::vector<Gyro*> gyros({&g1, &g2, &g3});
std::vector<Radar*> radars({&r1, &r2});
// Runtime organization
std::vector<ReadableDevice*> group_1({&g1, &g2});
std::vector<ReadableDevice*> group_2({&g3, &r1, &r2});
TaskGroup task1(group_1), task2(group_2);
// Launch all threads
std::thread t_main(&sensor_read_loop, gyros, radars);
std::thread t_task1 = task1.thread();
std::thread t_task2 = task2.thread();
// Join all threads
t_main.join();
t_task1.join();
t_task2.join();
return EXIT_SUCCESS;
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment