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

[driver] Added a wait_for_message method

parent d96026d2
......@@ -43,8 +43,8 @@ class SeatracDriver : public SeatracSerial
bool send_request(unsigned int cmdSize, const uint8_t* cmdData,
T* respData, int64_t timeout = 5000);
// bool wait_for_message(CID_E msgId, std::vector<uint8_t>& data,
// int64_t timeout = 5000);
template <typename T>
bool wait_for_message(CID_E msgId, T* data, int64_t timeout = 5000);
};
/**
......@@ -72,6 +72,22 @@ bool SeatracDriver::send_request(unsigned int cmdSize, const uint8_t* cmdData,
return true;
}
/**
* Waits for a specific message identified by its CID_E (with a timeout).
*/
template <typename T>
bool SeatracDriver::wait_for_message(CID_E msgId, T* data, int64_t timeout)
{
auto waiter = std::make_shared<DataWaiter<T>>(msgId, data);
{
std::unique_lock<std::mutex> lock(waitersMutex_);
waiters_.push_back(waiter);
}
// TODO delete waiter if timeout reached
return waiter->wait_for_data(timeout);
}
}; //namespace seatrac
}; //namespace narval
......
......@@ -9,22 +9,6 @@ SeatracDriver::SeatracDriver(const IoServicePtr& ioService,
{}
// /**
// * Waits for a specific message identified by its CID_E (with a timeout).
// */
// bool SeatracDriver::wait_for_message(CID_E msgId, std::vector<uint8_t>& data,
// int64_t timeout)
// {
// auto waiter = std::make_shared<Waiter>(msgId, &data);
// {
// std::unique_lock<std::mutex> lock(waitersMutex_);
// waiters_.push_back(waiter);
// }
//
// // TODO delete waiter if timeout reached
// return waiter->wait_for_data(timeout);
// }
void SeatracDriver::on_receive(const std::vector<uint8_t>& data)
{
// main dispatch function
......@@ -46,48 +30,6 @@ void SeatracDriver::on_receive(const std::vector<uint8_t>& data)
}
}
// std::cout << "Received : " << data.size() << " bytes." << std::endl;
// print_message(std::cout, data);
// return;
//
// messages::Status header = *reinterpret_cast<const messages::Status*>(data.data());
// if(header.cmdId != CID_STATUS) {
// return;
// }
// if(header.expected_size() != data.size()) {
// std::cerr << "Got status but wrong number of bytes (expected "
// << header.expected_size() << ", got " << data.size() << ")\n";
// return;
// }
// const uint8_t* p = data.data() + sizeof(messages::Status);
// if(header.contentType & ENVIRONMENT) {
// std::cout << *reinterpret_cast<const messages::StatusEnvironment*>(p) << std::endl;
// p += sizeof(messages::StatusEnvironment);
// }
// if(header.contentType & ATTITUDE) {
// std::cout << *reinterpret_cast<const messages::StatusAttitude*>(p) << std::endl;
// p += sizeof(messages::StatusAttitude);
// }
// if(header.contentType & MAG_CAL) {
// std::cout << *reinterpret_cast<const messages::StatusMagCalibration*>(p) << std::endl;
// p += sizeof(messages::StatusMagCalibration);
// }
// if(header.contentType & ACC_CAL) {
// std::cout << *reinterpret_cast<const messages::StatusAccCalibration*>(p) << std::endl;
// p += sizeof(messages::StatusAccCalibration);
// }
// if(header.contentType & AHRS_RAW_DATA) {
// std::cout << *reinterpret_cast<const messages::StatusRawAHRS*>(p) << std::endl;
// p += sizeof(messages::StatusRawAHRS);
// }
// if(header.contentType & AHRS_COMP_DATA) {
// std::cout << *reinterpret_cast<const messages::StatusCompensatedAHRS*>(p) << std::endl;
// p += sizeof(messages::StatusCompensatedAHRS);
// }
// std::cout << "expectedSize : " << header.expected_size()
// << ", got : " << data.size() << std::endl;
}
}; //namespace seatrac
......
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