Improve server socket handling and add signal handling for graceful shutdown

Author: iltlo

include/vive_ros2/server.hpp

@@ -7,6 +7,7 @@
 #include <sys/socket.h>
 #include <netinet/in.h>
 #include <signal.h>
+#include <csignal>
 
 #include "json.hpp"
 #include "VRUtils.hpp"
@@ -25,12 +26,21 @@ class Server {
 
     json prepareData();
 
+    static void signalHandler(int signum) {
+        std::cout << "Interrupt signal (" << signum << ") received.\n";
+        exit(signum);   // terminate program
+    }
+
 public:
     Server(int port, std::mutex &mutex, std::condition_variable &cv, VRControllerData &data);
     ~Server();
 
     void start();
     static std::string getCurrentTimeWithMilliseconds();
+    static void setupSignalHandlers() {
+        signal(SIGINT, signalHandler);
+        signal(SIGTERM, signalHandler);
+    }
 };
 
 #endif // SERVER_HPP

src/server.cpp

@@ -14,7 +14,8 @@ Server::Server(int port, std::mutex &mutex, std::condition_variable &cv, VRContr
         exit(EXIT_FAILURE);
     }
 
-    if (setsockopt(server_fd, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof(opt))) {
+    int reuse = 1;
+    if (setsockopt(server_fd, SOL_SOCKET, SO_REUSEADDR | SO_REUSEPORT, &reuse, sizeof(opt))) {
         perror("setsockopt");
         exit(EXIT_FAILURE);
     }
@@ -29,7 +30,11 @@ Server::Server(int port, std::mutex &mutex, std::condition_variable &cv, VRContr
     }
 }
 Server::~Server() {
-    close(server_fd);
+    if (server_fd != -1) {
+        shutdown(server_fd, SHUT_RDWR);
+        close(server_fd);
+        server_fd = -1;
+    }
 }
 
 void Server::start() {
@@ -58,7 +63,7 @@ void Server::start() {
                 close(new_socket);
                 break; // Exit the inner loop to wait for a new connection
             }
-            std::this_thread::sleep_for(std::chrono::milliseconds(10));
+            std::this_thread::sleep_for(std::chrono::milliseconds(2));  // 500Hz
         }
     }
 }

src/vive_input.cpp

@@ -32,7 +32,7 @@ class ViveInput {
     std::chrono::steady_clock::time_point prev_time;
     bool first_run = true;
     // const float velocity_threshold = 2.5f;
-    const float distance_threshold = 0.05f;
+    const float distance_threshold = 0.1f;
 
 };
 
@@ -133,6 +133,10 @@ void ViveInput::runVR() {
               VRUtils::HapticFeedback(pHMD, i, vibrationDuration);
               previousStep = currentStep;
           }
+          // haptic feedback after trigger value pass 0.6
+          // if (local_data.trigger == 0.6) {
+          //     VRUtils::HapticFeedback(pHMD, i, 100);
+          // }
 
           // Check if the input data is reasonable
           auto current_time = std::chrono::steady_clock::now();
@@ -192,7 +196,7 @@ void ViveInput::runVR() {
       }
       std::this_thread::sleep_for(std::chrono::milliseconds(50)); // ~20Hz
     } else {
-      std::this_thread::sleep_for(std::chrono::milliseconds(10)); // ~50Hz
+      std::this_thread::sleep_for(std::chrono::milliseconds(5)); // ~200Hz
       lastLogTime = currentTime;
     }
   }
@@ -222,6 +226,8 @@ bool ViveInput::shutdownVR() {
 }
 
 int main(int argc, char **argv) {
+    Server::setupSignalHandlers();
+
     std::mutex data_mutex;
     std::condition_variable data_cv;
     VRControllerData shared_data;
@@ -233,5 +239,7 @@ int main(int argc, char **argv) {
     ViveInput vive_input(data_mutex, data_cv, shared_data);
     vive_input.runVR();
 
+    serverThread.join(); // Wait for the server thread to finish
+
     return 0;
 }