diff --git a/rmw_zenoh_cpp/src/zenohd/main.cpp b/rmw_zenoh_cpp/src/zenohd/main.cpp
index fe6241be..8ae25809 100644
--- a/rmw_zenoh_cpp/src/zenohd/main.cpp
+++ b/rmw_zenoh_cpp/src/zenohd/main.cpp
@@ -12,19 +12,15 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include <chrono>
+#include <condition_variable>
 #include <cstdio>
-#include <cstring>
+#include <mutex>
 #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>
@@ -39,128 +35,15 @@
 #include "rcpputils/scope_exit.hpp"
 
 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
-  }
-
-private:
-#ifdef _WIN32
-  HANDLE hstdin_;
-  DWORD old_mode_;
-#else
-  struct termios cooked_;
-#endif
-};
+static std::mutex run_mutex;
+static std::condition_variable run_cv;
 
 #ifdef _WIN32
 BOOL WINAPI quit(DWORD ctrl_type)
 {
   (void)ctrl_type;
   running = false;
+  run_cv.notify_one();
   return true;
 }
 #else
@@ -168,6 +51,7 @@ void quit(int sig)
 {
   (void)sig;
   running = false;
+  run_cv.notify_one();
 }
 #endif
 
@@ -214,26 +98,9 @@ int main(int argc, char ** argv)
   signal(SIGTERM, 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));
-  }
+  // Wait until it's time to exit.
+  std::unique_lock lock(run_mutex);
+  run_cv.wait(lock, [] {return !running;});
 
   return 0;
 }