Use a zenoh session to run the router

rmw_zenoh_cpp/CMakeLists.txt

@@ -98,6 +98,7 @@ add_executable(init_rmw_zenoh_router apps/init_rmw_zenoh_router.cpp)
 target_link_libraries(init_rmw_zenoh_router
   PRIVATE
     ament_index_cpp::ament_index_cpp
+    zenohc::lib
 )
 
 install(

rmw_zenoh_cpp/apps/init_rmw_zenoh_router.cpp

@@ -12,41 +12,204 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include <stdlib.h>
-#include <sys/wait.h>
-
-#include <iostream>
+#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;
 
-int Main(int, char **)
+#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
+  }
+
+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 =
     ament_index_cpp::get_package_share_directory(RMW_ZENOH_IDENTIFIER) +
     "/config/" + std::string(ZENOH_ROUTER_CONFIG_NAME);
 
-  // Execute zenohd command
-  const std::string zenohd_cmd = "zenohd -c " + zenoh_router_config_path;
-  const int ret = system(zenohd_cmd.c_str());
-  if (ret < 0) {
-    std::cerr << "Error running zenoh router via command: " << zenohd_cmd << std::endl;
-    return ret;
-  } else {
-    if (WIFEXITED(ret)) {
-      std::cout << "Zenoh router exited normally." << std::endl;
-    } else {
-      std::cout << "Zenoh router exited abnormally with error code [" << ret << "]" << std::endl;
+  z_owned_config_t config = zc_config_from_file(zenoh_router_config_path.c_str());
+  z_owned_session_t s = z_open(z_move(config));
+  if (!z_check(s)) {
+    printf("Unable to open router session!\n");
+    return 1;
+  }
+
+#ifdef _WIN32
+  SetConsoleCtrlHandler(quit, TRUE);
+#else
+  signal(SIGINT, quit);
+#endif
+
+  KeyboardReader keyreader;
+
+  char c = 0;
+
+  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));
   }
-  return 0;
-}
 
-}  // namespace rmw_zenoh_cpp
+  z_close(z_move(s));
 
-int main(int argc, char ** argv) {return rmw_zenoh_cpp::Main(argc, argv);}
+  return 0;
+}

rmw_zenoh_cpp/package.xml

@@ -10,7 +10,6 @@
   <buildtool_depend>ament_cmake</buildtool_depend>
 
   <build_depend>zenoh_c_vendor</build_depend>
-  <build_depend>zenohd_vendor</build_depend>
 
   <depend>ament_index_cpp</depend>
   <depend>fastcdr</depend>