Issue #21: Add glog to all src files

Author: taketook34

src/kpi_rover_ecu/src/IMUController.cpp

@@ -7,6 +7,8 @@
 #include <iostream>
 #include <vector>
 
+#include "loggingIncludes.h"
+
 IMUController::IMUController() : data_{}, isStarted_(false), configuration_(rc_mpu_default_config()), actualData_({}) {
     configuration_.i2c_bus = kI2cBus;
     configuration_.gpio_interrupt_pin_chip = kGpioIntPinChip;
@@ -32,20 +34,23 @@ std::vector<float> IMUController::GetData() {
     if (!isStarted_) {
         return {};
     }
-
+    LOG_DEBUG << "Read data from accel and gyro";
     rc_mpu_read_accel(&data_);
     rc_mpu_read_gyro(&data_);
 
     actualData_.resize(kActualDataSize);
     const std::vector<float> kAccelData = GetAccel();
+    LOG_DEBUG << "Get data from accelerometr";
     const int kAccelDataSize = static_cast<int>(kAccelData.size());
     std::copy(kAccelData.begin(), kAccelData.begin() + kAccelDataSize, actualData_.begin());
 
     const std::vector<float> kGyroData = GetGyro();
+    LOG_DEBUG << "Get data from gyroscope";
     const int kGyroDataSize = static_cast<int>(kGyroData.size());
     std::copy(kGyroData.begin(), kGyroData.begin() + kGyroDataSize, actualData_.begin() + kAccelDataSize);
 
     const std::vector<float> kQaternionData = GetQaternion();
+    LOG_DEBUG << "Get data from magnetometer (qaternion)";
     const int kQaternionDataSize = static_cast<int>(kQaternionData.size());
     std::copy(kQaternionData.begin(), kQaternionData.begin() + kQaternionDataSize,
               actualData_.begin() + kAccelDataSize + kGyroDataSize);

src/kpi_rover_ecu/src/KPIRoverECU.cpp

@@ -11,6 +11,7 @@
 #include <vector>
 
 #include "IMUController.h"
+#include "loggingIncludes.h"
 #include "TCPTransport.h"
 #include "UDPClient.h"
 #include "protocolHandler.h"
@@ -27,12 +28,14 @@ KPIRoverECU::KPIRoverECU(ProtocolHanlder *_protocolHandler, TCPTransport *_tcpTr
 
 bool KPIRoverECU::Start() {
     tcp_transport_->Start();
+    LOG_DEBUG << "Starting all thread in KPIRoverECU";
     timerThread_ = std::thread([this] { TimerThreadFuction(this->protocol_handler_); });
     processingThread_ = std::thread([this] { ProcessingThreadFunction(); });
     imuThread_ = std::thread([this] { IMUThreadFucntion(this->imu_controller_); });
+    LOG_DEBUG << "All thread in KPIRoverECU started";
 
     if (!timerThread_.joinable() || !processingThread_.joinable() || !imuThread_.joinable()) {
-        std::cout << "Error creating thread" << '\n';
+        LOG_ERROR << "Error creating thread";
         return false;
     }
 
@@ -47,9 +50,11 @@ void KPIRoverECU::IMUThreadFucntion(IMUController *workClass) {
 
     while (runningProcess_) {
         if (destination_address.empty()) {
+            LOG_DEBUG << "Udp server address is unknown, get address from tcp server clients";
             destination_address = tcp_transport_->GetClientIp();
             destination_port = tcp_transport_->GetClientPort();
             if (!destination_address.empty()) {
+                LOG_DEBUG << "Get UDP server address, initializing UDPClient";
                 udp_client_->Init(destination_address, destination_port);
             }
 
@@ -58,9 +63,11 @@ void KPIRoverECU::IMUThreadFucntion(IMUController *workClass) {
             if (!kImuData.empty()) {
                 if (packet_number == k16MaxCount) {
                     packet_number = 0;
+                    LOG_DEBUG << "set UDP packet number to 0";
                 }
                 packet_number += 1;
 
+                LOG_DEBUG << "build UDP packet " << packet_number << " for UDP server";
                 std::vector<uint8_t> send_val;
                 send_val.push_back(workClass->GetId());
 
@@ -83,6 +90,8 @@ void KPIRoverECU::IMUThreadFucntion(IMUController *workClass) {
                         send_val.push_back(bytes[j]);
                     }
                 }
+                
+                LOG_DEBUG << "use UDP client to send message";
                 udp_client_->Send(send_val);
             }
         }
@@ -97,13 +106,15 @@ void KPIRoverECU::TimerThreadFuction(ProtocolHanlder *workClass) {
         if (counter_ > 0) {
             rc_usleep(kTimerPrecision);
             counter_--;
+            LOG_DEBUG << "counter decrement: " << counter_;
 
             if (counter_ == 0) {
-                std::cout << "[INFO] Motor set to stop" << '\n';
+                LOG_INFO << "Motor set to stop";
             }
 
         } else if (counter_ == 0) {
             // command to stop all motors
+            LOG_DEBUG << "Send command to stop all motors";
             workClass->HandleMessage(kStopVector);
             imu_controller_->SetDisable();
             rc_usleep(kTimeStop * kOneSecondMicro);
@@ -112,34 +123,42 @@ void KPIRoverECU::TimerThreadFuction(ProtocolHanlder *workClass) {
 }
 
 void KPIRoverECU::ProcessingThreadFunction() {
+    LOG_DEBUG << "start cycle in main thread";
     while (runningProcess_) {
         std::vector<uint8_t> message;
         if (tcp_transport_->Receive(message)) {
+            LOG_DEBUG << "TCP packet received, processing it";
             imu_controller_->SetEnable();
 
             counter_.store(GetCounter());
+            LOG_DEBUG << "set timer counter to GetCounter()";
             const std::vector<uint8_t> kReturnMessage = protocol_handler_->HandleMessage(message);
+            LOG_DEBUG << "get TCP response";
             tcp_transport_->Send(kReturnMessage);
+            LOG_DEBUG << "Send TCP response";
         }
     }
 }
 
 void KPIRoverECU::Stop() {
-    std::cout << "END of program" << '\n';
-    std::cout << "joining threads" << '\n';
+    LOG_INFO << "END of program";
+    LOG_INFO << "joining threads";
     runningState_ = true;
     if (timerThread_.joinable()) {
         timerThread_.join();
+        LOG_DEBUG << "timerThread_ joined";
     }
     runningProcess_ = false;
     if (processingThread_.joinable()) {
         processingThread_.join();
+        LOG_DEBUG << "main thread joined";
     }
     if (imuThread_.joinable()) {
         imuThread_.join();
+        LOG_DEBUG << "IMUThread joined";
     }
 
-    std::cout << "destroying drivers" << '\n';
+    LOG_INFO << "destroying drivers";
     // tcp_transport_->Destroy();
 }
 

src/kpi_rover_ecu/src/TCPTransport.cpp

@@ -13,6 +13,8 @@
 #include <string>
 #include <vector>
 
+#include "loggingIncludes.h"
+
 constexpr std::size_t kBufferSize = 1024;
 constexpr int kTimeoutMs = 50;
 
@@ -29,9 +31,9 @@ TCPTransport::TCPTransport(const char *ip_address, int port)
 TCPTransport::~TCPTransport() {
     running_ = false;
     shutdown(sockfd_, SHUT_RDWR);
-    std::cout << "joining thread ... " << '\n';
+    LOG_INFO << "joining thread ... ";
     acceptThread_.join();
-    std::cout << "closing socket" << '\n';
+    LOG_INFO << "closing socket";
     close(client_sockfd_);
     close(sockfd_);
     delete[] server_address_;
@@ -43,33 +45,38 @@ int TCPTransport::Init() {
     source_address_[0] = '\0';
 
     sockfd_ = socket(AF_INET, SOCK_STREAM, 0);
+    
 
     if (sockfd_ < 0) {
-        perror("[ERROR] Socket()");
+        LOG_ERROR << "Socket()";
         return -1;
     }
+    LOG_DEBUG << "Created socket";
 
     if (setsockopt(sockfd_, SOL_SOCKET, SO_REUSEADDR, &reuseaddr, sizeof(int)) != 0) {
-        perror("[ERROR] setsockopt()");
+        LOG_ERROR << "setsockopt()";
         return -1;
     }
 
     memset(&serv_addr, 0, sizeof(serv_addr));
     serv_addr.sin_family = AF_INET;
     serv_addr.sin_addr.s_addr = inet_addr(server_address_);
     serv_addr.sin_port = htons(server_portnum_);
+    LOG_DEBUG << "Set socket parameters";
 
     if (bind(sockfd_, reinterpret_cast<struct sockaddr *>(&serv_addr), sizeof(serv_addr)) != 0) {
-        perror("[ERROR] bind()");
+        LOG_ERROR << "bind()";
         return -1;
     }
+    LOG_DEBUG << "Socket binded";
 
     if (listen(sockfd_, TCPTransport::kNumSlots) != 0) {
-        perror("[ERROR] listen");
+        LOG_ERROR << "listen()";
         return -1;
     }
+    LOG_DEBUG << "Start listening";
 
-    std::cout << "[INFO] Started server on " << server_address_ << ":" << server_portnum_ << '\n';
+    LOG_INFO << " Started server on " << server_address_ << ":" << server_portnum_;
 
     return sockfd_;
 }
@@ -80,19 +87,21 @@ void TCPTransport::Start() {
         socklen_t client_add_size = sizeof(client_addr);
 
         while (running_) {
-            std::cout << "Waiting for connection..." << '\n';
+            LOG_INFO << "Waiting for connection...";
             client_sockfd_ = accept(sockfd_, reinterpret_cast<sockaddr *>(&client_addr), &client_add_size);
             if (client_sockfd_ >= 0) {
                 inet_ntop(AF_INET, &client_addr.sin_addr, source_address_, INET_ADDRSTRLEN);
                 source_port_ = static_cast<int>(ntohs(client_addr.sin_port));
+                LOG_INFO << "Client connected, source address: " << std::string(source_address_) << " source port: " << source_port_;
                 while (true) {  // Use true instead of 1
                     std::uint8_t buffer[kBufferSize];
                     const ssize_t kBytesReceived = recv(client_sockfd_, buffer, sizeof(buffer), 0);
                     if (kBytesReceived > 0) {
                         const std::vector<std::uint8_t> kMessage(buffer, buffer + kBytesReceived);
                         messageQueue_.Push(kMessage);
+                        LOG_DEBUG << "Get message, put in MessageQueue";
                     } else {
-                        std::cout << "Client disconnected." << '\n';
+                        LOG_INFO << "Client disconnected.";
                         close(client_sockfd_);
                         client_sockfd_ = -1;
                         break;
@@ -107,6 +116,8 @@ bool TCPTransport::Send(const std::vector<std::uint8_t> &data) {
     if (client_sockfd_ < 0) {
         return false;
     }
+    LOG_DEBUG << "Send TCP packet to TCP client";
+
     return ::send(client_sockfd_, data.data(), data.size(), 0) != -1;
 }
 
@@ -119,9 +130,9 @@ int TCPTransport::GetClientPort() { return source_port_; }
 void TCPTransport::Destroy() {
     running_ = false;
     shutdown(sockfd_, SHUT_RDWR);
-    std::cout << "joining thread ... " << '\n';
+    LOG_INFO << "joining thread ... ";
     acceptThread_.join();
-    std::cout << "closing socket" << '\n';
+    LOG_INFO << "closing socket";
     close(client_sockfd_);
     close(sockfd_);
     delete[] server_address_;

src/kpi_rover_ecu/src/UDPClient.cpp

@@ -13,6 +13,8 @@
 #include <string>
 #include <vector>
 
+#include "loggingIncludes.h"
+
 UDPClient::UDPClient() : sockfd_(-1), server_portnum_(0), server_address_{} {}
 
 int UDPClient::Init(std::string ip_address, int port) {
@@ -21,10 +23,10 @@ int UDPClient::Init(std::string ip_address, int port) {
 
     server_portnum_ = port;
 
-    std::cout << "initialize socket" << '\n';
+    LOG_DEBUG << "initialize socket";
     sockfd_ = socket(AF_INET, SOCK_DGRAM, 0);
     if (sockfd_ == -1) {
-        std::cout << "Error while creating socket" << '\n';
+        LOG_ERROR << "Error while creating socket";
         return -1;
     }
 
@@ -40,7 +42,7 @@ bool UDPClient::Send(const std::vector<std::uint8_t> &data) {
     if (sockfd_ < 0) {
         return false;
     }
-
+    LOG_DEBUG << "send UDP packet to UDP server , addr: " << server_address_ << "port: " << server_portnum_;
     return sendto(sockfd_, data.data(), data.size(), 0, reinterpret_cast<sockaddr *>(&serverStruct_),
                   sizeof(serverStruct_)) != -1;
 }
@@ -51,12 +53,12 @@ void UDPClient::Destroy() {
     shutdown(sockfd_, SHUT_WR);
 
     close(sockfd_);
-    std::cout << "joining udp client socket ..." << '\n';
+    LOG_INFO << "joining udp client socket ...";
 }
 
 UDPClient::~UDPClient() {
     shutdown(sockfd_, SHUT_WR);
 
     close(sockfd_);
-    std::cout << "joining udp client socket ..." << '\n';
+    LOG_INFO << "joining udp client socket ...";
 }

src/kpi_rover_ecu/src/main.cpp

@@ -52,10 +52,10 @@ int main(int argc, char* argv[]) {
                 log_level = strtol(optarg, nullptr, kBase);
                 break;
             default:
-                std::cout << "Usage: " << argv[0] << '\n';
-                std::cout << " [-a server_address] " << '\n';
-                std::cout << " [-p server_portnum]" << '\n';
-                std::cout << " [-l log level]" << '\n';
+                std::cout << "Usage: " << argv[0];
+                std::cout << " [-a server_address] ";
+                std::cout << " [-p server_portnum]";
+                std::cout << " [-l log level]";
                 return EXIT_FAILURE;
         }
     }
@@ -71,7 +71,7 @@ int main(int argc, char* argv[]) {
         FLAGS_stderrthreshold = 0;
     }
     FLAGS_log_dir = "./log";
-    LOG_INFO << "Logger was set up";
+    LOG_INFO << "Logger was set up." << "Directory to log: " << FLAGS_log_dir;
 
     MotorController motors_processor;
     const uint8_t kMotorNumber = 4;
@@ -88,7 +88,7 @@ int main(int argc, char* argv[]) {
     IMUController imu_controller;
 
     if (imu_controller.Init() == -1) {
-        std::cout << "[ERROR] Error initializing IMU controller" << '\n';
+        LOG_ERROR << "Error initializing IMU controller";
         imu_controller.Stop();
         return 1;
     }
@@ -97,28 +97,28 @@ int main(int argc, char* argv[]) {
     UDPClient udp_client;
 
     if (tcp_transport.Init() == -1) {
-        std::cout << "[ERROR] Error creating socket" << '\n';
+        LOG_ERROR << "Error creating socket";
         tcp_transport.Destroy();
         udp_client.Destroy();
         return 1;
     }
 
-    std::cout << "start ..." << '\n';
+    LOG_INFO << "start ...";
 
     KPIRoverECU kpi_rover_ecu(&protocol_handler, &tcp_transport, &udp_client, &imu_controller);
 
     if (!kpi_rover_ecu.Start()) {
-        std::cout << "Error In intitalizing main class" << '\n';
+        LOG_ERROR << "Error In intitalizing main class";
         return 1;
     }
 
     if (std::signal(SIGINT, InterruptSignalHandler) == SIG_ERR) {
-        std::cerr << "Error: Unable to set signal handler for SIGINT" << '\n';
+        LOG_ERROR << "Unable to set signal handler for SIGINT";
         return EXIT_FAILURE;
     }
 
     if (std::signal(SIGTERM, InterruptSignalHandler) == SIG_ERR) {
-        std::cerr << "Error: Unable to set signal handler for SIGTERM" << '\n';
+        LOG_ERROR << "Unable to set signal handler for SIGTERM";
         return EXIT_FAILURE;
     }