From d6ee1f98afac6d2ae3fd73888193334c5b1a2667 Mon Sep 17 00:00:00 2001 From: "Zhongzheng R. Zhang" Date: Sat, 15 Feb 2025 21:50:37 -0500 Subject: [PATCH] change shared_ptr to move semantics --- .../include/electrical_protocol_cpp/driver.h | 68 ++---------------- .../include/electrical_protocol_cpp/packet.h | 72 ++++++++++++++++--- .../electrical_protocol_cpp/src/driver.cpp | 57 ++++++--------- .../electrical_protocol_cpp/test/driver.cpp | 29 +++++--- 4 files changed, 110 insertions(+), 116 deletions(-) diff --git a/src/mil_common/electrical_protocol_cpp/include/electrical_protocol_cpp/driver.h b/src/mil_common/electrical_protocol_cpp/include/electrical_protocol_cpp/driver.h index 7f222bc..2fdfdda 100644 --- a/src/mil_common/electrical_protocol_cpp/include/electrical_protocol_cpp/driver.h +++ b/src/mil_common/electrical_protocol_cpp/include/electrical_protocol_cpp/driver.h @@ -1,8 +1,5 @@ #pragma once -#include -#include - #include #include #include @@ -12,6 +9,7 @@ #include #include #include +#include #include @@ -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); - void read(std::shared_ptr packet); + void write(Packet&& packet); + void read(Packet&& packet); - virtual void onWrite(std::shared_ptr packet, int errorCode ,size_t bytesWritten) = 0; - virtual void onRead(std::shared_ptr 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: @@ -63,65 +61,13 @@ class SerialDevice pthread_mutex_t readMutex_ = PTHREAD_MUTEX_INITIALIZER; - struct IdHash - { - size_t operator()(const std::pair& p) const - { - return static_cast((static_cast(p.first) << 8) + p.second); - } - }; - - std::queue> writeQueue_; - std::unordered_map, std::queue>, IdHash> readQueues_; + std::queue writeQueue_; + std::unordered_map, std::queue, 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); - void readPacket_(); - - void writeData_(std::shared_ptr data); - void readData_(std::shared_ptr data); }; -// class Subscriber -// { -// public: -// friend class ; -// Subscriber(); -// ~Subscriber(); -// private: -// }; - -// class Publisher -// { -// public: -// friend class SerialTransfer; -// Publisher(); -// ~Publisher(); -// private: -// std::queue>> 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_; -// }; - } \ No newline at end of file diff --git a/src/mil_common/electrical_protocol_cpp/include/electrical_protocol_cpp/packet.h b/src/mil_common/electrical_protocol_cpp/include/electrical_protocol_cpp/packet.h index a12ba3c..aecff44 100644 --- a/src/mil_common/electrical_protocol_cpp/include/electrical_protocol_cpp/packet.h +++ b/src/mil_common/electrical_protocol_cpp/include/electrical_protocol_cpp/packet.h @@ -17,14 +17,46 @@ 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(classId) << 8) + subClassId; + + } + + Packet(uint8_t classId, uint8_t subClassId) + { + data_[2] = classId; + data_[3] = subClassId; + } + + Packet(const std::pair& Id) + { + data_[2] = Id.first; + data_[3] = Id.second; } - Packet(const std::pair& 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 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() @@ -32,14 +64,33 @@ namespace electrical_protocol } + struct IdHash + { + size_t operator()(const std::pair& p) const + { + return static_cast((static_cast(p.first) << 8) + p.second); + } + }; + + inline void setId(const std::pair& Id) + { + data_[2] = Id.first; + data_[3] = Id.second; + } + inline std::pair 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 @@ -55,8 +106,10 @@ namespace electrical_protocol template 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); @@ -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 data_; - // uint16_t id_; - + std::vector data_ = {SYNC_CHAR_1, SYNC_CHAR_2, 0, 0, 0, 0, 0, 0}; + template constexpr void pack_(std::index_sequence, Args&&... args) { diff --git a/src/mil_common/electrical_protocol_cpp/src/driver.cpp b/src/mil_common/electrical_protocol_cpp/src/driver.cpp index 53a95d5..21e8821 100644 --- a/src/mil_common/electrical_protocol_cpp/src/driver.cpp +++ b/src/mil_common/electrical_protocol_cpp/src/driver.cpp @@ -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); } @@ -102,24 +94,18 @@ namespace electrical_protocol return opened_; } - void SerialDevice::write(std::shared_ptr 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) + 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_); } @@ -150,22 +136,22 @@ namespace electrical_protocol pthread_cond_wait(&device->writeCond_, &device->writeMutex_); } - std::shared_ptr 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(); } @@ -194,8 +180,8 @@ namespace electrical_protocol pthread_cleanup_push(readThreadCleanupFunc_, device); ReadState state = ReadState::SYNC_1; - std::shared_ptr packet = nullptr; size_t bytesRead = 0; + Packet packet; while(1) { @@ -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(&idndataLen[2]); bytesToRead += (dataLen + Packet::TRAILER_LEN); - if(packet == nullptr) + if(!findPacket) { constexpr size_t bufferLen = 100; uint8_t buffer[bufferLen]; @@ -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(&packet->data_[4]) = dataLen; + packet.data_.resize(Packet::HEADER_LEN + dataLen + Packet::TRAILER_LEN); + *reinterpret_cast(&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; diff --git a/src/mil_common/electrical_protocol_cpp/test/driver.cpp b/src/mil_common/electrical_protocol_cpp/test/driver.cpp index 7f3452f..59c1f7b 100644 --- a/src/mil_common/electrical_protocol_cpp/test/driver.cpp +++ b/src/mil_common/electrical_protocol_cpp/test/driver.cpp @@ -41,35 +41,41 @@ class SerialTest: public electrical_protocol::SerialDevice sem_destroy(&writeSem); } - int readSync(std::shared_ptr packet) + int readSync(electrical_protocol::Packet& packet) { - read(packet); + read(std::move(packet)); if(sem_wait(&readSem) == -1) return errno; + + packet = std::move(readPacket); return readErrno; } - int writeSync(std::shared_ptr packet) + int writeSync(electrical_protocol::Packet& packet) { - write(packet); + write(std::move(packet)); if(sem_wait(&writeSem) == -1) return errno; + + packet = std::move(writePacket); return writeErrno; } - void onWrite([[maybe_unused]]std::shared_ptr packet, + void onWrite([[maybe_unused]]electrical_protocol::Packet&& packet, int errorCode , [[maybe_unused]]size_t bytesWritten) { writeErrno = errorCode; + writePacket = std::move(packet); sem_post(&writeSem); } - void onRead([[maybe_unused]]std::shared_ptr packet, + void onRead([[maybe_unused]]electrical_protocol::Packet&& packet, int errorCode , [[maybe_unused]]size_t bytesRead) { readErrno = errorCode; + readPacket = std::move(packet); sem_post(&readSem); } @@ -78,7 +84,8 @@ class SerialTest: public electrical_protocol::SerialDevice int writeErrno = 0; sem_t readSem; sem_t writeSem; - + electrical_protocol::Packet readPacket; + electrical_protocol::Packet writePacket; }; void* writeThreadFunc(void* arg) @@ -87,8 +94,8 @@ void* writeThreadFunc(void* arg) SerialTest test(*serialName); while(1) { - auto packet = std::make_shared(1,1); - packet->pack(PY_STRING("831s"), testString); + electrical_protocol::Packet packet(1,1); + packet.pack(PY_STRING("831s"), testString); EXPECT_EQ(test.writeSync(packet), 0); usleep(100000); pthread_testcancel(); @@ -102,9 +109,9 @@ void * readThreadFunc(void* arg) std::string* serialName = reinterpret_cast(arg); SerialTest test(*serialName); - auto packet = std::make_shared(1,1); + electrical_protocol::Packet packet(1,1); test.readSync(packet); - auto [readString] = packet->unpack(PY_STRING("831s")); + auto [readString] = packet.unpack(PY_STRING("831s")); EXPECT_EQ(readString, testString); return 0; }