Commit d96026d2 authored by Pierre NARVOR's avatar Pierre NARVOR
Browse files

[messages/commands] More generic message system (dynamic data assignment)

parent 5c51d292
#ifndef _DEF_SEATRAC_DRIVER_COMMANDS_H_
#define _DEF_SEATRAC_DRIVER_COMMANDS_H_
#include <seatrac_driver/SeatracDriver.h>
#include <seatrac_driver/messages/Messages.h>
namespace narval { namespace seatrac { namespace command {
inline messages::SysAlive sys_alive(SeatracDriver& seatrac, int64_t timeout=1000)
{
messages::SysAlive response;
CID_E msgId = CID_SYS_ALIVE;
if(!seatrac.send_request(1, (const uint8_t*)&msgId, &response, timeout)) {
throw TimeoutReached();
}
return response;
}
inline messages::SysInfo sys_info(SeatracDriver& seatrac, int64_t timeout=1000)
{
messages::SysInfo response;
CID_E msgId = CID_SYS_INFO;
if(!seatrac.send_request(1, (const uint8_t*)&msgId, &response, timeout)) {
throw TimeoutReached();
}
return response;
}
}; //namespace command
}; //namespace seatrac
}; //namespace narval
#endif //_DEF_SEATRAC_DRIVER_COMMANDS_H_
#ifndef _DEF_SEATRAC_DRIVER_MESSAGES_MESSAGE_BASE_H_
#define _DEF_SEATRAC_DRIVER_MESSAGES_MESSAGE_BASE_H_
#include <iostream>
#include <type_traits>
#include <seatrac_driver/SeatracTypes.h>
namespace narval { namespace seatrac { namespace messages {
// implementation involve the Curiously Recursive Template Pattern.
template <class T>
struct Message
{
CID_E cmdId;
Message<T>& operator=(const std::vector<uint8_t>& other) {
// This checks that the type T inherit from A<T>, i.e. is a Curriously
// Recurring Template Pattern.
static_assert(std::is_base_of<Message<T>,T>::value,
"T is not derived from Message<T> (search CRTP).");
if(other.size() != sizeof(T)) {
throw std::runtime_error("Wrong size to copy message from raw data");
}
memcpy(this, other.data(), sizeof(T));
return *this;
}
}__attribute__((packed));
}; //namespace messages
}; //namespace seatrac
}; //namespace narval
inline std::ostream& operator<<(std::ostream& os, const narval::seatrac::messages::Message& msg)
template <typename T>
std::ostream& operator<<(std::ostream& os, const narval::seatrac::messages::Message<T>& msg)
{
os << "Message :\n- cmdId : " << msg.cmdId;
return os;
......
......@@ -2,8 +2,9 @@
#define _DEF_SEATRAC_DRIVER_MESSAGES_MESSAGES_H_
#include <seatrac_driver/messages/MessageBase.h>
#include <seatrac_driver/messages/Status.h>
#include <seatrac_driver/messages/SysAlive.h>
#include <seatrac_driver/messages/SysInfo.h>
// #include <seatrac_driver/messages/Status.h>
#endif //_DEF_SEATRAC_DRIVER_MESSAGES_MESSAGES_H_
......
#ifndef _DEF_SEATRAC_DRIVER_MESSAGES_SYSALIVE_H_
#define _DEF_SEATRAC_DRIVER_MESSAGES_SYSALIVE_H_
#include <seatrac_driver/SeatracTypes.h>
#include <seatrac_driver/messages/MessageBase.h>
namespace narval { namespace seatrac { namespace messages {
struct SysAlive : public Message<SysAlive>
{
using Request = Message;
using Message<SysAlive>::operator=;
uint32_t alivedFor;
}__attribute__((packed));
}; //namespace messages
}; //namespace seatrac
}; //namespace narval
inline std::ostream& operator<<(std::ostream& os,
const narval::seatrac::messages::SysAlive& msg)
{
static const char* prefix = "\n- ";
os << "SysAlive :"
<< prefix << "cmdId : " << (uint32_t)msg.cmdId
<< prefix << "alivedFor : " << (uint32_t)msg.alivedFor;
return os;
}
#endif //_DEF_SEATRAC_DRIVER_MESSAGES_SYSAlive_H_
......@@ -6,8 +6,11 @@
namespace narval { namespace seatrac { namespace messages {
struct SysInfo : public Message
struct SysInfo : public Message<SysInfo>
{
using Request = Message;
using Message<SysInfo>::operator=;
uint32_t alivedFor;
uint8_t currentApp;
HARDWARE_T hardware;
......
......@@ -2,20 +2,21 @@
#define _DEF_SEATRAC_DRIVER_PRINT_UTILS_H_
#include <iostream>
#include <vector>
#include <seatrac_driver/SeatracTypes.h>
#include <seatrac_driver/messages/Messages.h>
inline void print_message(std::ostream& os, const std::vector<uint8_t>& msg)
{
using namespace narval::seatrac;
using namespace narval::seatrac::messages;
switch(msg[0]) {
default:
//os << "Unknown message (CID : " << (uint32_t)msg[0] << ")" << std::endl;
break;
case CID_STATUS: os << *reinterpret_cast<const Status*>(msg.data()) << std::endl; break;
case CID_SYS_INFO: os << *reinterpret_cast<const SysInfo*>(msg.data()) << std::endl; break;
}
}
// inline void print_message(std::ostream& os, const std::vector<uint8_t>& msg)
// {
// using namespace narval::seatrac;
// using namespace narval::seatrac::messages;
// switch(msg[0]) {
// default:
// //os << "Unknown message (CID : " << (uint32_t)msg[0] << ")" << std::endl;
// break;
// case CID_STATUS: os << *reinterpret_cast<const Status*>(msg.data()) << std::endl; break;
// case CID_SYS_INFO: os << *reinterpret_cast<const SysInfo*>(msg.data()) << std::endl; break;
// }
// }
#endif //_DEF_SEATRAC_DRIVER_PRINT_UTILS_H_
......@@ -3,6 +3,7 @@ using namespace std;
#include <seatrac_driver/AsyncService.h>
#include <seatrac_driver/SeatracDriver.h>
#include <seatrac_driver/commands.h>
using namespace narval::seatrac;
void request_sys_info(boost::asio::steady_timer* timer,
......@@ -17,7 +18,7 @@ void request_sys_info(boost::asio::steady_timer* timer,
uint8_t cid = 0x02;
std::vector<uint8_t> respData;
if(!seatrac->send_request(1, &cid, respData, 5000)) {
if(!seatrac->send_request(1, &cid, &respData, 5000)) {
cout << "Timeout reached !" << endl;
}
else {
......@@ -41,13 +42,21 @@ int main()
service.start();
for(int i = 0; i < 10; i++) {
uint8_t cid = 0x02;
std::vector<uint8_t> respData;
if(!seatrac.send_request(1, &cid, respData, 1000)) {
cout << "Timeout reached !" << endl;
// uint8_t cid = 0x02;
// std::vector<uint8_t> respData;
// if(!seatrac.send_request(1, &cid, &respData, 1000)) {
// cout << "Timeout reached !" << endl;
// }
// else {
// cout << "Response ok" << endl;
// }
try {
// cout << command::sys_alive(seatrac) << endl;
cout << command::sys_info(seatrac) << endl;
}
else {
cout << "Response ok" << endl;
catch(const TimeoutReached& e) {
cout << "Timeout reached" << endl;
}
getchar();
}
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment