新的代码
#ifndef __ROS_SIM_H__
#define __ROS_SIM_H__
#include <iostream>
#include <stdint.h>
#include <string>
#include <functional>
#include <queue>
#include <unordered_map>
#include <thread>
#include <mutex>
#include <condition_variable>
#include <atomic>
#include <memory>
#include <utility>
#include <chrono>
enum RosSimMsgType {
ROS_SIM_MSG_UNKNOW,
ROS_SIM_MSG_TEST_A,
ROS_SIM_MSG_TEST_B,
ROS_SIM_MSG_TEST_C,
ROS_SIM_MSG_TEST_D,
ROS_SIM_MSG_TEST_E,
ROS_SIM_MSG_TEST_F
};
enum RosSimPackageType { ROS_SIM_PACKAGE_UNKNOW, ROS_SIM_PACKAGE_INS, ROS_SIM_PACKAGE_HDM };
class RosSimMsg {
public:
RosSimMsg() : _type(ROS_SIM_MSG_UNKNOW)
{}
RosSimMsg(RosSimMsgType type) : _type(type)
{}
virtual std::string msg_str() const
{
return std::string("Base Msg.");
}
RosSimMsgType _type;
};
typedef std::shared_ptr<RosSimMsg> RosSimMsgPtr;
typedef std::function<void(RosSimMsgPtr)> RosSimMsgCallback;
class RosSimPackage {
public:
RosSimPackage() : _type(ROS_SIM_PACKAGE_UNKNOW)
{}
RosSimPackage(RosSimPackageType type) : _type(type)
{}
virtual std::string package_str() const
{
return std::string("Base Package.");
}
RosSimPackageType _type;
};
typedef std::shared_ptr<RosSimPackage> RosSimPackagePtr;
typedef std::function<void(RosSimPackagePtr)> RosSimPackageCallback;
typedef std::function<void()> RosSimPackageHandler;
class RosSimPackageINS : public RosSimPackage {
public:
RosSimPackageINS() : RosSimPackage(ROS_SIM_PACKAGE_INS)
{}
RosSimPackageINS(RosSimMsgPtr msgA_start, RosSimMsgPtr msgB, RosSimMsgPtr msgC, RosSimMsgPtr msgA_end)
: _msgA_start(msgA_start), _msgB(msgB), _msgC(msgC), _msgA_end(msgA_end){};
RosSimMsgPtr _msgA_start;
RosSimMsgPtr _msgB;
RosSimMsgPtr _msgC;
RosSimMsgPtr _msgA_end;
};
typedef std::shared_ptr<RosSimPackageINS> RosSimPackageINSPtr;
typedef std::function<void(std::queue<RosSimPackageINS> &)> PackageINSCallback;
class RosSimPackageHDM : public RosSimPackage {
public:
RosSimPackageHDM() : RosSimPackage(ROS_SIM_PACKAGE_HDM)
{}
};
typedef std::shared_ptr<RosSimPackageHDM> RosSimPackageHDMPtr;
typedef std::function<void(std::queue<RosSimPackageHDM> &)> PackageHDMCallback;
template <typename Type>
class QueueWrapper {
public:
QueueWrapper() : _maxSize(DEFAULT_QUEUE_MAX_SIZE)
{}
QueueWrapper(uint64_t maxSize) : _maxSize(maxSize)
{}
bool empty()
{
std::unique_lock<std::mutex> lk(_mtx);
return _queue.empty();
}
bool push(const Type &item)
{
std::unique_lock<std::mutex> lk(_mtx);
_queue.push(item);
return true;
}
bool pop(Type &item)
{
bool ret = false;
std::unique_lock<std::mutex> lk(_mtx);
if (!_queue.empty()) {
item = _queue.front();
_queue.pop();
ret = true;
}
return ret;
}
static const uint64_t DEFAULT_QUEUE_MAX_SIZE = 100UL;
std::queue<Type> _queue;
std::mutex _mtx;
uint64_t _maxSize;
};
typedef QueueWrapper<RosSimMsgPtr> MsgQueue;
typedef std::shared_ptr<MsgQueue> MsgQueuePtr;
typedef std::unordered_map<unsigned int, MsgQueuePtr> MsgQueueMap;
struct ThreadWrapper {
ThreadWrapper() : _has_new_event(false)
{}
ThreadWrapper(RosSimMsgType triggerMsgType, RosSimPackageHandler handler, std::thread &&thd)
: _triggerMsgType(triggerMsgType), _packageHandler(handler), _runner(std::move(thd))
{}
void join()
{
if (_runner.joinable()) {
_runner.join();
}
}
bool _has_new_event;
std::condition_variable _event_cv;
std::mutex _cv_mtx;
std::thread _runner;
RosSimMsgType _triggerMsgType;
RosSimPackageHandler _packageHandler;
};
typedef std::shared_ptr<ThreadWrapper> ThreadPtr;
typedef std::unordered_map<unsigned int, ThreadPtr> PackageThreadMap;
class RosSim {
public:
static RosSim& getInstance();
RosSim();
~RosSim();
void register_ins_package_callback(PackageINSCallback cb);
void register_hdm_package_callback(PackageHDMCallback cb);
void set_ins_package_send_flag();
void set_hdm_package_send_flag();
int insert_msg(RosSimMsgPtr msg);
RosSimPackageType get_package_type(RosSimMsgType msgType);
private:
void run(RosSimPackageType type);
void wakeup(ThreadPtr thd);
void wait_event(ThreadPtr thd, uint32_t timeout_us);
void generate_package_ins();
void generate_package_hdm();
MsgQueueMap _msgQueueMap;
PackageThreadMap _pkgThdMap;
std::atomic<bool> _shutdown;
// following codes are package type specified
std::shared_ptr<QueueWrapper<RosSimPackageINS>> _insPkgQueue;
std::shared_ptr<QueueWrapper<RosSimPackageHDM>> _hdmPkgQueue;
PackageINSCallback _insPkgCallback;
PackageHDMCallback _hdmPkgCallback;
std::atomic<bool> _insPkgSendFlag;
std::atomic<bool> _hdmPkgSendFlag;
};
#endif
#include "ros_sim.h"
RosSim &RosSim::getInstance()
{
static RosSim rosSim;
return rosSim;
}
RosSim::RosSim() : _shutdown(false), _insPkgSendFlag(true), _hdmPkgSendFlag(true)
{
// init msg queue map
for (int i = ROS_SIM_MSG_TEST_A; i <= ROS_SIM_MSG_TEST_F; i++) {
_msgQueueMap.emplace(i, std::make_shared<MsgQueue>());
}
// ins package generator thread
_pkgThdMap.emplace(ROS_SIM_PACKAGE_INS, std::make_shared<ThreadWrapper>(
ROS_SIM_MSG_TEST_A, std::bind(&RosSim::generate_package_ins, this),
std::thread(&RosSim::run, this, ROS_SIM_PACKAGE_INS)));
// hdm package generator thread
_pkgThdMap.emplace(ROS_SIM_PACKAGE_HDM, std::make_shared<ThreadWrapper>(
ROS_SIM_MSG_TEST_D, std::bind(&RosSim::generate_package_ins, this),
std::thread(&RosSim::run, this, ROS_SIM_PACKAGE_HDM)));
}
RosSim::~RosSim()
{
_shutdown.store(true);
for (int i = ROS_SIM_PACKAGE_INS; i <= ROS_SIM_PACKAGE_HDM; i++) {
_pkgThdMap[i]->join();
}
}
RosSimPackageType RosSim::get_package_type(RosSimMsgType msgType)
{
RosSimPackageType type = ROS_SIM_PACKAGE_UNKNOW;
switch (msgType) {
case ROS_SIM_MSG_TEST_A:
case ROS_SIM_MSG_TEST_B:
case ROS_SIM_MSG_TEST_C:
type = ROS_SIM_PACKAGE_INS;
break;
case ROS_SIM_MSG_TEST_D:
case ROS_SIM_MSG_TEST_E:
case ROS_SIM_MSG_TEST_F:
type = ROS_SIM_PACKAGE_HDM;
break;
default:
break;
}
return type;
}
int RosSim::insert_msg(RosSimMsgPtr msg)
{
RosSimMsgType msgType = msg->_type;
RosSimPackageType pkgType = get_package_type(msg->_type);
if (_msgQueueMap.count(msgType)) {
auto &cur_queue = _msgQueueMap[msgType];
std::unique_lock<std::mutex> lk(cur_queue->_mtx);
if (cur_queue->_queue.size() >= cur_queue->_maxSize) {
cur_queue->_queue.pop();
}
cur_queue->_queue.push(msg);
// wakeup the thread according to msg type.
if (msgType == _pkgThdMap[pkgType]->_triggerMsgType) {
wakeup(_pkgThdMap[pkgType]);
}
}
return 0;
}
void RosSim::wakeup(ThreadPtr thd)
{
std::unique_lock<std::mutex> lk(thd->_cv_mtx);
thd->_has_new_event = true;
thd->_event_cv.notify_all();
}
void RosSim::wait_event(ThreadPtr thd, uint32_t timeout_us)
{
std::unique_lock<std::mutex> lk(thd->_cv_mtx);
thd->_event_cv.wait_for(lk, std::chrono::microseconds(timeout_us), [thd]() {
if (thd->_has_new_event) {
thd->_has_new_event = false;
return true;
} else {
return false;
}
});
}
void RosSim::generate_package_ins()
{
RosSimMsgPtr msgA_start;
RosSimMsgPtr msgB;
RosSimMsgPtr msgC;
RosSimMsgPtr msgA_end;
auto &triggerQueue = _msgQueueMap[_pkgThdMap[ROS_SIM_PACKAGE_INS]->_triggerMsgType];
while (!triggerQueue->empty()) {
triggerQueue->pop(msgA_start);
_msgQueueMap[ROS_SIM_MSG_TEST_B]->pop(msgB);
_msgQueueMap[ROS_SIM_MSG_TEST_C]->pop(msgC);
_msgQueueMap[ROS_SIM_MSG_TEST_A]->pop(msgA_end);
_insPkgQueue->push(RosSimPackageINS(msgA_start, msgB, msgC, msgA_end));
}
}
void RosSim::generate_package_hdm()
{}
void RosSim::register_ins_package_callback(PackageINSCallback cb)
{
_insPkgCallback = cb;
}
void RosSim::register_hdm_package_callback(PackageHDMCallback cb)
{
_hdmPkgCallback = cb;
}
void RosSim::set_ins_package_send_flag()
{
_insPkgSendFlag.store(true);
}
void RosSim::set_hdm_package_send_flag()
{
_hdmPkgSendFlag.store(true);
}
void RosSim::run(RosSimPackageType type)
{
while (!_shutdown) {
if (type == ROS_SIM_PACKAGE_INS) {
generate_package_ins();
} else if (type == ROS_SIM_PACKAGE_HDM) {
generate_package_hdm();
}
if (_msgQueueMap[_pkgThdMap[type]->_triggerMsgType]->empty()) {
wait_event(_pkgThdMap[type], 100);
}
}
}
老的代码
#ifndef __ROS_SIM_H__
#define __ROS_SIM_H__
#include <iostream>
#include <string>
#include <functional>
#include <queue>
#include <unordered_map>
#include <thread>
#include <mutex>
#include <condition_variable>
#include <atomic>
#include <utility>
#include <chrono>
enum RosSimMsgType
{
ROS_SIM_MSG_UNKNOW,
ROS_SIM_MSG_TEST
};
using MsgType = unsigned int;
using FrequencyType = unsigned int;
using RosSimClock = std::chrono::steady_clock;
using RosSimTimepoint = RosSimClock::time_point;
class RosSimMsg
{
public:
RosSimMsg() : _type(ROS_SIM_MSG_UNKNOW) {}
RosSimMsg(RosSimMsgType type) : _type(type) {}
RosSimMsgType get_msg_type() const { return _type; }
virtual RosSimTimepoint get_msg_timepoint() const = 0;
virtual std::string msg_str() const = 0;
private:
RosSimMsgType _type;
};
using RosSimCallback = std::function<void(RosSimMsg *)>;
struct RosSimCallbackInfo
{
RosSimCallback _callback;
RosSimTimepoint _last_send_timepoint;
FrequencyType _freq;
};
class RosSimMsgTest : public RosSimMsg
{
public:
RosSimMsgTest(int data) : RosSimMsg(ROS_SIM_MSG_TEST), _data(data), _timepoint(RosSimClock::now()) {}
std::string msg_str() const
{
return std::string("This message is just for testing. ") + std::to_string(_data);
}
RosSimTimepoint get_msg_timepoint() const
{
return _timepoint;
}
void set_msg_timepoint()
{
_timepoint = RosSimClock::now();
}
private:
RosSimTimepoint _timepoint;
int _data;
};
class RosSim
{
public:
RosSim() : _has_new_msg(false), _force_shutdown(false), _shutdown(false) {}
int init();
int exit(bool force = false);
void subscrible_msg(RosSimMsgType type, RosSimCallback &callback, unsigned int frequency = 0);
int publish_msg(RosSimMsg *msg);
private:
void run();
bool queue_is_empty();
bool queue_push(RosSimMsg *msg);
bool queue_pop(RosSimMsg **msg);
void wait_new_msg(unsigned int timeout_us);
void wakeup_msg_hander();
void invoke_callback(RosSimMsg *msg);
bool should_exit();
bool should_send_msg(RosSimMsg *msg);
std::queue<RosSimMsg *> _msg_queue;
std::mutex _msg_queue_mtx;
std::unordered_map<MsgType, RosSimCallbackInfo> _msg_handler_map;
std::mutex _msg_handler_map_mtx;
std::atomic<bool> _force_shutdown;
std::atomic<bool> _shutdown;
bool _has_new_msg;
std::mutex _msg_cv_mtx;
std::condition_variable _msg_cv;
std::thread _msg_handler;
};
#endif
#include "ros_sim.h"
bool RosSim::queue_is_empty()
{
std::unique_lock<std::mutex> queue_lk(_msg_queue_mtx);
return _msg_queue.empty();
}
bool RosSim::queue_push(RosSimMsg *msg)
{
if (msg == nullptr)
{
return false;
}
std::unique_lock<std::mutex> queue_lk(_msg_queue_mtx);
_msg_queue.push(msg);
return true;
}
bool RosSim::queue_pop(RosSimMsg **msg)
{
bool ret = false;
std::unique_lock<std::mutex> queue_lk(_msg_queue_mtx);
if (!_msg_queue.empty())
{
*msg = _msg_queue.front();
_msg_queue.pop();
ret = true;
}
return ret;
}
void RosSim::wait_new_msg(unsigned int timeout_us)
{
std::unique_lock<std::mutex> cv_lk(_msg_cv_mtx);
_msg_cv.wait_for(cv_lk, std::chrono::microseconds(timeout_us), [this]()
{
if (_has_new_msg)
{
_has_new_msg = false;
return true;
}
else
{
return false;
} });
}
bool RosSim::should_send_msg(RosSimMsg *msg)
{
RosSimMsgType type = msg->get_msg_type();
RosSimTimepoint timp = msg->get_msg_timepoint();
RosSimTimepoint last_send_timp = _msg_handler_map[type]._last_send_timepoint;
FrequencyType freq = _msg_handler_map[type]._freq;
if (freq == 0)
{
return true;
}
if (last_send_timp == RosSimTimepoint::min())
{
return true;
}
long long duration_thresh = static_cast<long long>(1.0 * 1000 * 1000 / freq);
long long duration = std::chrono::duration_cast<std::chrono::microseconds>(timp - last_send_timp).count();
return duration >= duration_thresh;
}
void RosSim::invoke_callback(RosSimMsg *msg)
{
if (msg == nullptr)
{
return;
}
RosSimMsgType type = msg->get_msg_type();
std::unique_lock<std::mutex> map_lk(_msg_handler_map_mtx);
if (_msg_handler_map.count(type))
{
if (should_send_msg(msg))
{
_msg_handler_map[type]._callback(msg);
_msg_handler_map[type]._last_send_timepoint = msg->get_msg_timepoint();
}
}
}
void RosSim::subscrible_msg(RosSimMsgType type, RosSimCallback &callback, unsigned int frequency)
{
std::unique_lock<std::mutex> map_lk(_msg_handler_map_mtx);
if (!_msg_handler_map.count(type))
{
RosSimCallbackInfo cb_info;
cb_info._callback = callback;
cb_info._freq = frequency;
cb_info._last_send_timepoint = RosSimTimepoint::min();
_msg_handler_map.emplace(type, cb_info);
}
}
int RosSim::publish_msg(RosSimMsg *msg)
{
if (msg == nullptr)
{
return -1;
}
queue_push(msg);
wakeup_msg_hander();
}
void RosSim::wakeup_msg_hander()
{
std::unique_lock<std::mutex> cv_lk(_msg_cv_mtx);
_has_new_msg = true;
_msg_cv.notify_all();
}
bool RosSim::should_exit()
{
return _force_shutdown || (_shutdown && queue_is_empty());
}
void RosSim::run()
{
while (!should_exit())
{
RosSimMsg *msg = nullptr;
while (queue_pop(&msg))
{
invoke_callback(msg);
msg = nullptr;
}
if (queue_is_empty())
{
wait_new_msg(100);
}
}
}
int RosSim::init()
{
_msg_handler = std::thread(&RosSim::run, this);
return 0;
}
int RosSim::exit(bool force)
{
if (force)
{
_force_shutdown.store(true);
}
else
{
_shutdown.store(true);
}
if (_msg_handler.joinable())
{
_msg_handler.join();
}
_force_shutdown.store(false);
_shutdown.store(false);
return 0;
}
#include <iostream>
#include <vector>
#include "ros_sim.h"
int main()
{
RosSimCallback msg1_callback = [](RosSimMsg *msg) -> void
{
std::cout << "Msg1 Callback. " << msg->msg_str() << std::endl;
};
// test1: single thread
RosSim ros_sim1;
ros_sim1.init();
ros_sim1.subscrible_msg(ROS_SIM_MSG_TEST, msg1_callback);
RosSimMsgTest msg_test1(100);
ros_sim1.publish_msg(&msg_test1);
ros_sim1.exit();
// test2: multi thread
RosSim ros_sim2;
ros_sim2.init();
ros_sim2.subscrible_msg(ROS_SIM_MSG_TEST, msg1_callback);
RosSimMsgTest msg_test2(200);
auto test_thd = [&ros_sim2, &msg_test2]() -> void
{
ros_sim2.publish_msg(&msg_test2);
};
std::thread t(test_thd);
t.join();
ros_sim2.exit();
// test3: single thread with frequency
RosSim ros_sim3;
ros_sim3.init();
int nmsgs = 100;
std::vector<RosSimMsgTest> msgs(nmsgs, RosSimMsgTest(300));
ros_sim3.subscrible_msg(ROS_SIM_MSG_TEST, msg1_callback, 100);
for (size_t i = 0; i < nmsgs; i++)
{
msgs[i].set_msg_timepoint();
ros_sim3.publish_msg(&msgs[i]);
std::this_thread::sleep_for(std::chrono::milliseconds(1));
}
ros_sim3.exit();
return 0;
}
g++ -std=c++11 -pthread -g3 -o ros_sim_test ros_sim_test.cpp ros_sim.cpp
Msg1 Callback. This message is just for testing. 100
Msg1 Callback. This message is just for testing. 200
Msg1 Callback. This message is just for testing. 300
Msg1 Callback. This message is just for testing. 300
Msg1 Callback. This message is just for testing. 300
Msg1 Callback. This message is just for testing. 300
Msg1 Callback. This message is just for testing. 300
Msg1 Callback. This message is just for testing. 300
Msg1 Callback. This message is just for testing. 300
Msg1 Callback. This message is just for testing. 300
Msg1 Callback. This message is just for testing. 300
Msg1 Callback. This message is just for testing. 300
