Skip to content

Commit

Permalink
change shared_ptr to move semantics
Browse files Browse the repository at this point in the history
  • Loading branch information
@zrzz-hq
zrzz-hq committed Feb 16, 2025
1 parent 9c93fc0 commit d6ee1f9
Show file tree
Hide file tree
Showing 4 changed files with 110 additions and 116 deletions.
68 changes: 7 additions & 61 deletions src/mil_common/electrical_protocol_cpp/include/electrical_protocol_cpp/driver.h
View file
Original file line number Diff line number Diff line change
@@ -1,8 +1,5 @@
#pragma once

#include <boost/asio/serial_port.hpp>
#include <boost/asio.hpp>

#include <string>
#include <queue>
#include <array>
Expand All @@ -12,6 +9,7 @@
#include <unistd.h>
#include <termios.h>
#include <pthread.h>
#include <fcntl.h>

#include <electrical_protocol_cpp/packet.h>

Expand All @@ -32,11 +30,11 @@ class SerialDevice
int open(const std::string& deviceName, speed_t baudrate);
void close();
bool isOpened() const;
void write(std::shared_ptr<Packet> packet);
void read(std::shared_ptr<Packet> packet);
void write(Packet&& packet);
void read(Packet&& packet);

virtual void onWrite(std::shared_ptr<Packet> packet, int errorCode ,size_t bytesWritten) = 0;
virtual void onRead(std::shared_ptr<Packet> packet, int errorCode ,size_t bytesRead) = 0;
virtual void onWrite(Packet&& packet, int errorCode ,size_t bytesWritten) = 0;
virtual void onRead(Packet&& packet, int errorCode ,size_t bytesRead) = 0;

private:

Expand All @@ -63,65 +61,13 @@ class SerialDevice

pthread_mutex_t readMutex_ = PTHREAD_MUTEX_INITIALIZER;

struct IdHash
{
size_t operator()(const std::pair<uint8_t, uint8_t>& p) const
{
return static_cast<size_t>((static_cast<uint16_t>(p.first) << 8) + p.second);
}
};

std::queue<std::shared_ptr<Packet>> writeQueue_;
std::unordered_map<std::pair<uint8_t, uint8_t>, std::queue<std::shared_ptr<Packet>>, IdHash> readQueues_;
std::queue<Packet> writeQueue_;
std::unordered_map<std::pair<uint8_t, uint8_t>, std::queue<Packet>, Packet::IdHash> readQueues_;

static void* readThreadFunc_(void* arg);
static void* writeThreadFunc_(void* arg);
static void readThreadCleanupFunc_(void* arg);
static void writeThreadCleanupFunc_(void* arg);

void readPacket_(std::shared_ptr<Packet> packet);
void readPacket_();

void writeData_(std::shared_ptr<Packet> data);
void readData_(std::shared_ptr<Packet> data);
};

// class Subscriber
// {
// public:
// friend class ;
// Subscriber();
// ~Subscriber();
// private:
// };

// class Publisher
// {
// public:
// friend class SerialTransfer;
// Publisher();
// ~Publisher();
// private:
// std::queue<std::shared_ptr<std::vector<uint8_t>>> dataQueue_;
// };

// class SerialTransfer: private SerialDevice
// {
// public:
// SerialTransfer() = delete;
// SerialTransfer(std::string& portName, unsigned baudrate=9600);
// ~SerialTransfer();

// int publish(, unsigned queueLen);
// int subscribe(, unsigned queueLen);

// private:
// pthread_t transferThread_;

// static void* transferThreadFunc_(void* arg);

// unsigned baudrate_;
// std::string portName_;
// };

}
72 changes: 62 additions & 10 deletions src/mil_common/electrical_protocol_cpp/include/electrical_protocol_cpp/packet.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,29 +17,80 @@ namespace electrical_protocol
{
public:
friend class SerialDevice;
Packet(uint8_t classId, uint8_t subClassId):data_{SYNC_CHAR_1, SYNC_CHAR_2, classId, subClassId}
Packet()
{
// id_ = (static_cast<uint16_t>(classId) << 8) + subClassId;

}

Packet(uint8_t classId, uint8_t subClassId)
{
data_[2] = classId;
data_[3] = subClassId;
}

Packet(const std::pair<uint8_t, uint8_t>& Id)
{
data_[2] = Id.first;
data_[3] = Id.second;
}

Packet(const std::pair<uint8_t, uint8_t>& Id):data_{SYNC_CHAR_1, SYNC_CHAR_2, Id.first, Id.second}
Packet(Packet&& packet)
{
data_ = std::move(packet.data_);
}

Packet(Packet& packet)
{
data_ = packet.data_;
}

Packet& operator= (Packet&& packet)
{
std::vector<uint8_t> temp(std::move(data_));
data_ = std::move(packet.data_);
packet.data_ = std::move(temp);
packet.data_.resize(HEADER_LEN + TRAILER_LEN);
return *this;
}

Packet& operator= (Packet& packet)
{
data_ = packet.data_;
return *this;
}

~Packet()
{

}

struct IdHash
{
size_t operator()(const std::pair<uint8_t, uint8_t>& p) const
{
return static_cast<size_t>((static_cast<uint16_t>(p.first) << 8) + p.second);
}
};

inline void setId(const std::pair<uint8_t, uint8_t>& Id)
{
data_[2] = Id.first;
data_[3] = Id.second;
}

inline std::pair<uint8_t, uint8_t> getId() const
{
return {data_[2], data_[3]};
}

inline size_t getTotalSize() const
inline size_t size() const
{
return data_.size()- HEADER_LEN - TRAILER_LEN;
}

inline uint8_t* data()
{
return data_.size();
return &data_[HEADER_LEN];
}

template <typename Fmt, typename... Args>
Expand All @@ -55,8 +106,10 @@ namespace electrical_protocol
template<typename Fmt>
auto unpack(Fmt) const
{
if(pystruct::calcsize(Fmt{}) + HEADER_LEN + TRAILER_LEN > data_.size())
throw std::out_of_range("No enough data");
constexpr size_t expectSize = pystruct::calcsize(Fmt{}) + HEADER_LEN + TRAILER_LEN;
if(expectSize > data_.size())
throw std::out_of_range("No enough data expect " + std::to_string(expectSize)
+ " get " + std::to_string(data_.size()));

uint8_t sum[2];
calcCheckSum_(sum);
Expand All @@ -72,9 +125,8 @@ namespace electrical_protocol
static constexpr size_t HEADER_LEN = 6;
static constexpr size_t TRAILER_LEN = 2;
static constexpr size_t SYNC_LEN = 2;
std::vector<uint8_t> data_;
// uint16_t id_;

std::vector<uint8_t> data_ = {SYNC_CHAR_1, SYNC_CHAR_2, 0, 0, 0, 0, 0, 0};

template <typename Fmt, size_t... Items, typename... Args>
constexpr void pack_(std::index_sequence<Items...>, Args&&... args)
{
Expand Down
57 changes: 23 additions & 34 deletions src/mil_common/electrical_protocol_cpp/src/driver.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -41,14 +41,6 @@ namespace electrical_protocol
cfsetospeed(&options, baudrate);
options.c_cc[VMIN] = 0;
options.c_cc[VTIME] = 1;
// options.c_cflag |= (CLOCAL | CREAD);
// options.c_cflag &= ~CSIZE;
// options.c_cflag |= CS8;
// options.c_cflag &= ~PARENB;
// options.c_cflag &= ~CSTOPB;
// options.c_iflag &= ~(IXON | IXOFF | IXANY);
// options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
// options.c_oflag &= ~OPOST;
tcsetattr(serialFd_, TCSANOW, &options);
}

Expand Down Expand Up @@ -102,24 +94,18 @@ namespace electrical_protocol
return opened_;
}

void SerialDevice::write(std::shared_ptr<Packet> packet)
{
if(packet == nullptr || packet->data_.size() == 0)
onWrite(packet, EINVAL, 0);

void SerialDevice::write(Packet&& packet)
{
pthread_mutex_lock(&writeMutex_);
writeQueue_.push(packet);
writeQueue_.push(std::move(packet));
pthread_cond_signal(&writeCond_);
pthread_mutex_unlock(&writeMutex_);
}

void SerialDevice::read(std::shared_ptr<Packet> packet)
void SerialDevice::read(Packet&& packet)
{
if(packet == nullptr || packet->data_.size() == 0)
onRead(packet, EINVAL, 0);

pthread_mutex_lock(&readMutex_);
readQueues_[packet->getId()].push(packet);
readQueues_[packet.getId()].push(std::move(packet));
pthread_mutex_unlock(&readMutex_);
}

Expand Down Expand Up @@ -150,22 +136,22 @@ namespace electrical_protocol
pthread_cond_wait(&device->writeCond_, &device->writeMutex_);
}

std::shared_ptr<Packet> packet = device->writeQueue_.front();
Packet packet = std::move(device->writeQueue_.front());
device->writeQueue_.pop();

pthread_mutex_unlock(&device->writeMutex_);

size_t bytesToWrite = packet->data_.size();
size_t bytesToWrite = packet.data_.size();
size_t bytesWritten = 0;
int ret = 0;
while(bytesWritten < bytesToWrite && ret != -1)
{
bytesWritten += ret;
ret = ::write(device->serialFd_, packet->data_.data() + bytesWritten, bytesToWrite - bytesWritten);
ret = ::write(device->serialFd_, packet.data_.data() + bytesWritten, bytesToWrite - bytesWritten);
}

size_t dataLen = bytesToWrite - Packet::HEADER_LEN - Packet::TRAILER_LEN;
device->onWrite(packet, errno, bytesWritten);
device->onWrite(std::move(packet), errno, bytesWritten);

pthread_testcancel();
}
Expand Down Expand Up @@ -194,8 +180,8 @@ namespace electrical_protocol
pthread_cleanup_push(readThreadCleanupFunc_, device);
ReadState state = ReadState::SYNC_1;

std::shared_ptr<Packet> packet = nullptr;
size_t bytesRead = 0;
Packet packet;

while(1)
{
Expand Down Expand Up @@ -252,19 +238,21 @@ namespace electrical_protocol
continue;
}

bool findPacket = false;
pthread_mutex_lock(&device->readMutex_);
auto it = device->readQueues_.find({idndataLen[0], idndataLen[1]});
if(it != device->readQueues_.end() && it->second.size() > 0)
{
packet = it->second.front();
packet = std::move(it->second.front());
it->second.pop();
findPacket = true;
}
pthread_mutex_unlock(&device->readMutex_);

size_t dataLen = *reinterpret_cast<uint16_t*>(&idndataLen[2]);
bytesToRead += (dataLen + Packet::TRAILER_LEN);

if(packet == nullptr)
if(!findPacket)
{
constexpr size_t bufferLen = 100;
uint8_t buffer[bufferLen];
Expand All @@ -275,29 +263,30 @@ namespace electrical_protocol
bytesRead += ret;
ret = ::read(device->serialFd_, buffer + (bytesRead - Packet::HEADER_LEN) % bufferLen, std::min(bufferLen, bytesToRead - bytesRead));
}


bytesRead = 0;
errno = 0;
state = ReadState::SYNC_1;
}
else
{
packet->data_.resize(Packet::HEADER_LEN + dataLen + Packet::TRAILER_LEN);
*reinterpret_cast<uint16_t*>(&packet->data_[4]) = dataLen;
packet.data_.resize(Packet::HEADER_LEN + dataLen + Packet::TRAILER_LEN);
*reinterpret_cast<uint16_t*>(&packet.data_[4]) = dataLen;

ret = 0;
while(bytesRead < bytesToRead && ret != -1)
{
bytesRead += ret;
ret = ::read(device->serialFd_, packet->data_.data() + bytesRead, bytesToRead - bytesRead);
ret = ::read(device->serialFd_, packet.data_.data() + bytesRead, bytesToRead - bytesRead);
}

state = ReadState::CALLBACK;
}

state = ReadState::CALLBACK;

}
else if(state == ReadState::CALLBACK)
{
device->onRead(packet, errno, bytesRead);
packet.reset();
device->onRead(std::move(packet), errno, bytesRead);
bytesRead = 0;
errno = 0;
state = ReadState::SYNC_1;
Expand Down
Loading

0 comments on commit d6ee1f9

Please sign in to comment.