Commit 5c51d292 authored by Pierre NARVOR's avatar Pierre NARVOR
Browse files

[driver] Added a dedicated more generic DataWaiter type, and moved it outside SeatracDriver

parent d85f5525
......@@ -12,6 +12,9 @@ list(APPEND seatrac_driver_headers
include/seatrac_driver/print_utils.h
include/seatrac_driver/SeatracSerial.h
include/seatrac_driver/SeatracDriver.h
include/seatrac_driver/DataWaiter.h
include/seatrac_driver/commands.h
include/seatrac_driver/messages/MessageBase.h
include/seatrac_driver/messages/Messages.h
......@@ -23,6 +26,7 @@ add_library(seatrac_driver SHARED
src/AsyncService.cpp
src/SeatracSerial.cpp
src/SeatracDriver.cpp
src/DataWaiter.cpp
)
set_target_properties(seatrac_driver PROPERTIES
NARVAL_PUBLIC_HEADERS "${seatrac_driver_headers}"
......
#ifndef _DEF_SEATRAC_DRIVER_DATA_WAITER_H_
#define _DEF_SEATRAC_DRIVER_DATA_WAITER_H_
#include <iostream>
#include <chrono>
#include <thread>
#include <mutex>
#include <condition_variable>
#include <vector>
#include <seatrac_driver/SeatracEnums.h>
namespace narval { namespace seatrac {
struct TimeoutReached : public std::exception
{
const char* what() const throw() {
return "Timeout reached while waiting for message.";
}
};
class DataWaiterBase
{
public:
//using Ptr = std::unique_ptr<Waiter>;
using Ptr = std::shared_ptr<DataWaiterBase>;
protected:
CID_E msgId_;
std::vector<uint8_t>* dataDestination_;
std::mutex mutex_;
std::condition_variable cv_;
bool wasCalled_;
public:
DataWaiterBase(CID_E msgId);
CID_E msg_id() const;
bool wait_for_data(int64_t timeout = 5000);
virtual void set_data(const std::vector<uint8_t>& data) = 0;
};
template <typename T>
class DataWaiter : public DataWaiterBase
{
public:
using Ptr = std::shared_ptr<DataWaiter<T>>;
protected:
T* dataDestination_;
public:
DataWaiter(CID_E msgId, T* dataDestination);
virtual void set_data(const std::vector<uint8_t>& data);
};
template <typename T>
DataWaiter<T>::DataWaiter(CID_E msgId, T* dataDestination) :
DataWaiterBase(msgId),
dataDestination_(dataDestination)
{
assert(dataDestination_ != nullptr);
}
template <typename T>
void DataWaiter<T>::set_data(const std::vector<uint8_t>& data)
{
{
std::unique_lock<std::mutex> lock(this->mutex_);
*dataDestination_ = data;
this->wasCalled_ = true;
}
this->cv_.notify_all();
}
}; //namespace seatrac
}; //namespace narval
#endif //_DEF_SEATRAC_DRIVER_DATA_WAITER_H_
......@@ -12,11 +12,10 @@
#include <seatrac_driver/SeatracEnums.h>
#include <seatrac_driver/SeatracTypes.h>
#include <seatrac_driver/SeatracSerial.h>
#include <seatrac_driver/messages/Messages.h>
#include <seatrac_driver/DataWaiter.h>
namespace narval { namespace seatrac {
class SeatracDriver : public SeatracSerial
{
public:
......@@ -25,32 +24,8 @@ class SeatracDriver : public SeatracSerial
using IoServicePtr = std::shared_ptr<boost::asio::io_service>;
using SerialPort = boost::asio::serial_port;
using ReadBuffer = boost::asio::streambuf;
class Waiter
{
public:
//using Ptr = std::unique_ptr<Waiter>;
using Ptr = std::shared_ptr<Waiter>;
protected:
CID_E msgId_;
std::vector<uint8_t>* dataDestination_;
std::mutex mutex_;
std::condition_variable cv_;
bool wasCalled_;
public:
Waiter(CID_E msgId, std::vector<uint8_t>* dataDestination);
CID_E msg_id() const;
bool wait_for_data(int64_t timeout = 5000);
void set_data(const std::vector<uint8_t>& data);
};
using Waiters = std::list<Waiter::Ptr>;
using Waiters = std::list<DataWaiterBase::Ptr>;
protected:
......@@ -64,14 +39,39 @@ class SeatracDriver : public SeatracSerial
SeatracDriver(const IoServicePtr& ioService,
const std::string& port = "/dev/narval_usbl");
template <typename T>
bool send_request(unsigned int cmdSize, const uint8_t* cmdData,
std::vector<uint8_t>& respData,
int64_t timeout = 5000);
T* respData, int64_t timeout = 5000);
bool wait_for_message(CID_E msgId, std::vector<uint8_t>& data,
int64_t timeout = 5000);
// bool wait_for_message(CID_E msgId, std::vector<uint8_t>& data,
// int64_t timeout = 5000);
};
/**
* Sends a command and waits for an answer (synchronous call)
*/
template <typename T>
bool SeatracDriver::send_request(unsigned int cmdSize, const uint8_t* cmdData,
T* respData, int64_t timeout)
{
CID_E msgId = (CID_E)cmdData[0]; // TODO check validity
auto waiter = std::make_shared<DataWaiter<T>>(msgId, respData);
{
std::unique_lock<std::mutex> lock(waitersMutex_);
waiters_.push_back(waiter);
}
// sending data when waiter is set, not before (to avoid missing the
// response)
this->send(cmdSize, cmdData);
if(!waiter->wait_for_data(timeout)) {
std::cerr << "Timeout reached while waiting for request response (cmd_id : "
<< msgId << ")" << std::endl;
return false;
}
return true;
}
}; //namespace seatrac
}; //namespace narval
......
#include <seatrac_driver/DataWaiter.h>
namespace narval { namespace seatrac {
DataWaiterBase::DataWaiterBase(CID_E msgId) :
msgId_(msgId),
wasCalled_(false)
{}
CID_E DataWaiterBase::msg_id() const
{
return msgId_;
}
bool DataWaiterBase::wait_for_data(int64_t timeout)
{
std::unique_lock<std::mutex> lock(mutex_);
if(wasCalled_)
return true;
if(timeout < 0) {
cv_.wait(lock, [&]{ return wasCalled_; });
}
else {
if(!cv_.wait_for(lock, std::chrono::milliseconds(timeout),
[&]{ return wasCalled_; })) {
// Timeout reached, data was not set
return false;
}
}
return true;
}
}; //namespace seatrac
}; //namespace narval
......@@ -3,87 +3,27 @@
namespace narval { namespace seatrac {
SeatracDriver::Waiter::Waiter(CID_E msgId, std::vector<uint8_t>* dataDestination) :
msgId_(msgId),
dataDestination_(dataDestination),
wasCalled_(false)
{}
CID_E SeatracDriver::Waiter::msg_id() const
{
return msgId_;
}
bool SeatracDriver::Waiter::wait_for_data(int64_t timeout)
{
std::unique_lock<std::mutex> lock(mutex_);
if(wasCalled_)
return true;
if(timeout < 0) {
cv_.wait(lock, [&]{ return wasCalled_; });
}
else {
if(!cv_.wait_for(lock, std::chrono::milliseconds(timeout),
[&]{ return wasCalled_; })) {
// Timeout reached, data was not set
return false;
}
}
return true;
}
void SeatracDriver::Waiter::set_data(const std::vector<uint8_t>& data)
{
{
std::unique_lock<std::mutex> lock(mutex_);
*dataDestination_ = data;
wasCalled_ = true;
}
cv_.notify_all();
}
SeatracDriver::SeatracDriver(const IoServicePtr& ioService,
const std::string& port) :
SeatracSerial(ioService, port)
{}
/**
* Sends a command and waits for an answer (synchronous call)
*/
bool SeatracDriver::send_request(unsigned int cmdSize, const uint8_t* cmdData,
std::vector<uint8_t>& respData,
int64_t timeout)
{
CID_E msgId = (CID_E)cmdData[0]; // TODO check validity
auto waiter = std::make_shared<Waiter>(msgId, &respData);
{
std::unique_lock<std::mutex> lock(waitersMutex_);
waiters_.push_back(waiter);
}
// sending data when waiter is set, not before (to avoid missing the
// response)
this->send(cmdSize, cmdData);
return waiter->wait_for_data(timeout);
}
/**
* 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);
}
// /**
// * 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)
{
......@@ -105,8 +45,8 @@ void SeatracDriver::on_receive(const std::vector<uint8_t>& data)
}
}
}
// std::cout << "Received : " << data.size() << " bytes." << std::endl;
std::cout << "Received : " << data.size() << " bytes." << std::endl;
// print_message(std::cout, data);
// return;
......
......@@ -235,9 +235,9 @@ void SeatracSerial::on_sent(const boost::system::error_code& err, size_t byteCou
//throw std::runtime_error(oss.str());
std::cerr << oss.str() << std::endl;
}
else {
std::cout << "Written " << byteCount << " bytes." << std::endl;
}
// else {
// std::cout << "Written " << byteCount << " bytes." << std::endl;
// }
}
SeatracSerial::IoServicePtr SeatracSerial::io_service() const
......
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