Issue #29. Fixed abortion bug when stop

Author: taketook34

src/kpi_rover_ecu/include/IClient.h

@@ -14,7 +14,7 @@ class IClient {
     virtual bool Receive(std::vector<std::uint8_t>& data) = 0;
     virtual int Init(std::string ip_address, int port) = 0;
     virtual void Destroy() = 0;
-    virtual ~IClient() = default;
+    // virtual ~IClient() = default;
 
     IClient(const IClient&) = delete;
     IClient& operator=(const IClient&) = delete;

src/kpi_rover_ecu/include/ITransport.h

@@ -14,7 +14,7 @@ class ITransport {
     virtual int Init() = 0;
     virtual void Start() = 0;
     virtual void Destroy() = 0;
-    virtual ~ITransport() = default;
+    // virtual ~ITransport() = default;
 
     ITransport(const ITransport&) = delete;
     ITransport& operator=(const ITransport&) = delete;

src/kpi_rover_ecu/include/TCPTransport.h

@@ -22,7 +22,7 @@ class TCPTransport : public ITransport {
     TCPTransport& operator=(const TCPTransport&) = delete;
     TCPTransport(TCPTransport&&) = delete;
     TCPTransport& operator=(TCPTransport&&) = delete;
-    ~TCPTransport() override;
+    // ~TCPTransport() override;
 
     bool Send(const std::vector<std::uint8_t>& data) override;
     bool Receive(std::vector<std::uint8_t>& data) override;

src/kpi_rover_ecu/include/UDPClient.h

@@ -23,7 +23,7 @@ class UDPClient : public IClient {
     bool Send(const std::vector<std::uint8_t>& data) override;
     bool Receive(std::vector<std::uint8_t>& data) override;
     void Destroy() override;
-    ~UDPClient() override;
+    // ~UDPClient() override;
 
    private:
     int sockfd_;

src/kpi_rover_ecu/src/KPIRoverECU.cpp

@@ -31,7 +31,7 @@ bool KPIRoverECU::Start() {
     processingThread_ = std::thread([this] { ProcessingThreadFunction(); });
     imuThread_ = std::thread([this] { IMUThreadFucntion(this->imu_controller_); });
 
-    if (!timerThread_.joinable() || !processingThread_.joinable()) {
+    if (!timerThread_.joinable() || !processingThread_.joinable() || !imuThread_.joinable() ) {
         std::cout << "Error creating thread" << '\n';
         return false;
     }
@@ -45,45 +45,45 @@ void KPIRoverECU::IMUThreadFucntion(IMUController *workClass) {
     std::string destination_address;
     int destination_port = 0;
 
-    while (destination_address.empty()) {
-        destination_address = tcp_transport_->GetClientIp();
-        destination_port = tcp_transport_->GetClientPort();
-    }
-
-    udp_client_->Init(destination_address, destination_port);
-
     while (runningProcess_) {
-        const std::vector<float> kImuData = workClass->GetData();
-        if (!kImuData.empty()) {
-            if (packet_number == k16MaxCount) {
-                packet_number = 0;
-            }
-            packet_number += 1;
+        if (destination_address.empty()) {
+            destination_address = tcp_transport_->GetClientIp();
+            destination_port = tcp_transport_->GetClientPort();
+            if (!destination_address.empty()) { udp_client_->Init(destination_address, destination_port); }
+
+        } else {
+            const std::vector<float> kImuData = workClass->GetData();
+            if (!kImuData.empty()) {
+                if (packet_number == k16MaxCount) {
+                    packet_number = 0;
+                }
+                packet_number += 1;
 
-            std::vector<uint8_t> send_val;
-            send_val.push_back(workClass->GetId());
+                std::vector<uint8_t> send_val;
+                send_val.push_back(workClass->GetId());
 
-            uint16_t send_packet_number = htons(packet_number);
-            auto *bytes = reinterpret_cast<uint8_t *>(&send_packet_number);
-            for (size_t i = 0; i < 2; ++i) {
-                send_val.push_back(bytes[i]);
-            }
+                uint16_t send_packet_number = htons(packet_number);
+                auto *bytes = reinterpret_cast<uint8_t *>(&send_packet_number);
+                for (size_t i = 0; i < 2; ++i) {
+                    send_val.push_back(bytes[i]);
+                }
 
-            float insert_value = 0;
-            uint32_t value = 0;
+                float insert_value = 0;
+                uint32_t value = 0;
 
-            for (const float kImuValue : kImuData) {
-                insert_value = kImuValue;
-                std::memcpy(&value, &insert_value, sizeof(float));
-                value = ntohl(value);
-                bytes = reinterpret_cast<uint8_t *>(&value);
+                for (const float kImuValue : kImuData) {
+                    insert_value = kImuValue;
+                    std::memcpy(&value, &insert_value, sizeof(float));
+                    value = ntohl(value);
+                    bytes = reinterpret_cast<uint8_t *>(&value);
 
-                for (size_t j = 0; j < sizeof(uint32_t); ++j) {
-                    send_val.push_back(bytes[j]);
+                    for (size_t j = 0; j < sizeof(uint32_t); ++j) {
+                        send_val.push_back(bytes[j]);
+                    }
                 }
+                udp_client_->Send(send_val);
+                
             }
-
-            udp_client_->Send(send_val);
         }
         rc_usleep(kTimerPrecision);
     }
@@ -134,6 +134,10 @@ void KPIRoverECU::Stop() {
     if (processingThread_.joinable()) {
         processingThread_.join();
     }
+    if (imuThread_.joinable()) {
+        imuThread_.join();
+    }
+
     std::cout << "destroying drivers" << '\n';
     // tcp_transport_->Destroy();
 }

src/kpi_rover_ecu/src/TCPTransport.cpp

@@ -26,16 +26,16 @@ TCPTransport::TCPTransport(const char *ip_address, int port)
     strncpy(server_address_, ip_address, strlen(ip_address) + 1);
 }
 
-TCPTransport::~TCPTransport() {
-    running_ = false;
-    shutdown(sockfd_, SHUT_RDWR);
-    std::cout << "joining thread ... " << '\n';
-    acceptThread_.join();
-    std::cout << "closing socket" << '\n';
-    close(client_sockfd_);
-    close(sockfd_);
-    delete[] server_address_;
-}
+// TCPTransport::~TCPTransport() {
+//     running_ = false;
+//     shutdown(sockfd_, SHUT_RDWR);
+//     std::cout << "joining thread ... " << '\n';
+//     acceptThread_.join();
+//     std::cout << "closing socket" << '\n';
+//     close(client_sockfd_);
+//     close(sockfd_);
+//     delete[] server_address_;
+// }
 
 int TCPTransport::Init() {
     struct sockaddr_in serv_addr = {};
@@ -124,4 +124,5 @@ void TCPTransport::Destroy() {
     std::cout << "closing socket" << '\n';
     close(client_sockfd_);
     close(sockfd_);
+    delete[] server_address_;
 }

src/kpi_rover_ecu/src/UDPClient.cpp

@@ -47,10 +47,15 @@ bool UDPClient::Send(const std::vector<std::uint8_t> &data) {
 
 bool UDPClient::Receive(std::vector<std::uint8_t> &data) { return false; }
 
-void UDPClient::Destroy() {}
-
-UDPClient::~UDPClient() {
+void UDPClient::Destroy() {
     shutdown(sockfd_, SHUT_WR);
 
     close(sockfd_);
+    std::cout << "joining udp client socket ..." << '\n';
 }
+
+// UDPClient::~UDPClient() {
+//     shutdown(sockfd_, SHUT_WR);
+
+//     close(sockfd_);
+// }

src/kpi_rover_ecu/src/main.cpp

@@ -109,6 +109,8 @@ int main(int argc, char* argv[]) {
 
     kpi_rover_ecu.Stop();
     motors_processor.Destroy();
+    tcp_transport.Destroy();
+    udp_client.Destroy();
 
     return 0;
 }