From 93cf50ae72bc96d6e5562d007829d73bdaf415e1 Mon Sep 17 00:00:00 2001
From: Chris Lalancette <clalancette@gmail.com>
Date: Tue, 23 Jan 2024 12:14:03 -0500
Subject: [PATCH] Rewrite init_rmw_zenoh_router to handle keyboard properly.

This should work for both Linux and Windows, though only
tested on Linux so far.

Signed-off-by: Chris Lalancette <clalancette@gmail.com>
---
 rmw_zenoh_cpp/apps/init_rmw_zenoh_router.cpp | 181 +++++++++++++++++--
 1 file changed, 170 insertions(+), 11 deletions(-)

diff --git a/rmw_zenoh_cpp/apps/init_rmw_zenoh_router.cpp b/rmw_zenoh_cpp/apps/init_rmw_zenoh_router.cpp
index 672a8e80..df63e8db 100644
--- a/rmw_zenoh_cpp/apps/init_rmw_zenoh_router.cpp
+++ b/rmw_zenoh_cpp/apps/init_rmw_zenoh_router.cpp
@@ -14,18 +14,161 @@
 
 #include <chrono>
 #include <cstdio>
+#include <cstring>
+#include <stdexcept>
 #include <string>
 #include <thread>
 
+#ifdef _WIN32
+#include <windows.h>
+#else
+#include <signal.h>
+#include <termios.h>
+#include <unistd.h>
+#endif
+
 #include <zenoh.h>
 
 #include <ament_index_cpp/get_package_share_directory.hpp>
 
-namespace rmw_zenoh_cpp
+static bool running = true;
+
+class KeyboardReader final
 {
+public:
+  KeyboardReader()
+  {
+#ifdef _WIN32
+    hstdin_ = GetStdHandle(STD_INPUT_HANDLE);
+    if (hstdin_ == INVALID_HANDLE_VALUE) {
+      throw std::runtime_error("Failed to get stdin handle");
+    }
+    if (!GetConsoleMode(hstdin_, &old_mode_)) {
+      throw std::runtime_error("Failed to get old console mode");
+    }
+    DWORD new_mode = ENABLE_PROCESSED_INPUT;  // for Ctrl-C processing
+    if (!SetConsoleMode(hstdin_, new_mode)) {
+      throw std::runtime_error("Failed to set new console mode");
+    }
+#else
+    // get the console in raw mode
+    if (tcgetattr(0, &cooked_) < 0) {
+      throw std::runtime_error("Failed to get old console mode");
+    }
+    struct termios raw;
+    memcpy(&raw, &cooked_, sizeof(struct termios));
+    raw.c_lflag &= ~(ICANON | ECHO);
+    // Setting a new line, then end of file
+    raw.c_cc[VEOL] = 1;
+    raw.c_cc[VEOF] = 2;
+    raw.c_cc[VTIME] = 1;
+    raw.c_cc[VMIN] = 0;
+    if (tcsetattr(0, TCSANOW, &raw) < 0) {
+      throw std::runtime_error("Failed to set new console mode");
+    }
+#endif
+  }
+
+  char readOne()
+  {
+    char c = 0;
+
+#ifdef _WIN32
+    INPUT_RECORD record;
+    DWORD num_read;
+    switch (WaitForSingleObject(hstdin_, 100)) {
+      case WAIT_OBJECT_0:
+        if (!ReadConsoleInput(hstdin_, &record, 1, &num_read)) {
+          throw std::runtime_error("Read failed");
+        }
+
+        if (record.EventType != KEY_EVENT || !record.Event.KeyEvent.bKeyDown) {
+          break;
+        }
+
+        if (record.Event.KeyEvent.wVirtualKeyCode == VK_LEFT) {
+          c = KEYCODE_LEFT;
+        } else if (record.Event.KeyEvent.wVirtualKeyCode == VK_UP) {
+          c = KEYCODE_UP;
+        } else if (record.Event.KeyEvent.wVirtualKeyCode == VK_RIGHT) {
+          c = KEYCODE_RIGHT;
+        } else if (record.Event.KeyEvent.wVirtualKeyCode == VK_DOWN) {
+          c = KEYCODE_DOWN;
+        } else if (record.Event.KeyEvent.wVirtualKeyCode == 0x42) {
+          c = KEYCODE_B;
+        } else if (record.Event.KeyEvent.wVirtualKeyCode == 0x43) {
+          c = KEYCODE_C;
+        } else if (record.Event.KeyEvent.wVirtualKeyCode == 0x44) {
+          c = KEYCODE_D;
+        } else if (record.Event.KeyEvent.wVirtualKeyCode == 0x45) {
+          c = KEYCODE_E;
+        } else if (record.Event.KeyEvent.wVirtualKeyCode == 0x46) {
+          c = KEYCODE_F;
+        } else if (record.Event.KeyEvent.wVirtualKeyCode == 0x47) {
+          c = KEYCODE_G;
+        } else if (record.Event.KeyEvent.wVirtualKeyCode == 0x51) {
+          c = KEYCODE_Q;
+        } else if (record.Event.KeyEvent.wVirtualKeyCode == 0x52) {
+          c = KEYCODE_R;
+        } else if (record.Event.KeyEvent.wVirtualKeyCode == 0x54) {
+          c = KEYCODE_T;
+        } else if (record.Event.KeyEvent.wVirtualKeyCode == 0x56) {
+          c = KEYCODE_V;
+        }
+        break;
+
+      case WAIT_TIMEOUT:
+        break;
+    }
+
+#else
+    int rc = read(0, &c, 1);
+    if (rc < 0) {
+      throw std::runtime_error("read failed");
+    }
+#endif
+
+    return c;
+  }
+
+  ~KeyboardReader()
+  {
+#ifdef _WIN32
+    SetConsoleMode(hstdin_, old_mode_);
+#else
+    tcsetattr(0, TCSANOW, &cooked_);
+#endif
+  }
 
-int Main(int, char **)
+private:
+#ifdef _WIN32
+  HANDLE hstdin_;
+  DWORD old_mode_;
+#else
+  struct termios cooked_;
+#endif
+};
+
+#ifdef _WIN32
+BOOL WINAPI quit(DWORD ctrl_type)
+{
+  (void)ctrl_type;
+  running = false;
+  return true;
+}
+#else
+void quit(int sig)
+{
+  (void)sig;
+  running = false;
+}
+#endif
+
+int main(int argc, char ** argv)
 {
+  (void)argc;
+  (void)argv;
+
   static const char * RMW_ZENOH_IDENTIFIER = "rmw_zenoh_cpp";
   static const char * ZENOH_ROUTER_CONFIG_NAME = "DEFAULT_RMW_ZENOH_ROUTER_CONFIG.json5";
   const std::string zenoh_router_config_path =
@@ -39,18 +182,34 @@ int Main(int, char **)
     return 1;
   }
 
-  printf("Enter 'q' to quit...\n");
+#ifdef _WIN32
+  SetConsoleCtrlHandler(quit, TRUE);
+#else
+  signal(SIGINT, quit);
+#endif
+
+  KeyboardReader keyreader;
+
   char c = 0;
-  while (c != 'q') {
-    c = getchar();
-    if (c == -1) {
-      std::this_thread::sleep_for(std::chrono::seconds(1));
+
+  printf("Enter 'q' to quit...\n");
+  while (running) {
+    // get the next event from the keyboard
+    try {
+      c = keyreader.readOne();
+    } catch (const std::runtime_error &) {
+      perror("read():");
+      return -1;
     }
+
+    if (c == 'q') {
+      break;
+    }
+
+    std::this_thread::sleep_for(std::chrono::milliseconds(100));
   }
+
   z_close(z_move(s));
+
   return 0;
 }
-
-}  // namespace rmw_zenoh_cpp
-
-int main(int argc, char ** argv) {return rmw_zenoh_cpp::Main(argc, argv);}