diff --git a/rmw_zenoh_cpp/CMakeLists.txt b/rmw_zenoh_cpp/CMakeLists.txt index 85230a8f..b62cccc4 100644 --- a/rmw_zenoh_cpp/CMakeLists.txt +++ b/rmw_zenoh_cpp/CMakeLists.txt @@ -100,6 +100,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( diff --git a/rmw_zenoh_cpp/apps/init_rmw_zenoh_router.cpp b/rmw_zenoh_cpp/apps/init_rmw_zenoh_router.cpp index b664e423..df63e8db 100644 --- a/rmw_zenoh_cpp/apps/init_rmw_zenoh_router.cpp +++ b/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 -#include - -#include +#include +#include +#include +#include #include +#include + +#ifdef _WIN32 +#include +#else +#include +#include +#include +#endif + +#include #include -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; +} diff --git a/rmw_zenoh_cpp/package.xml b/rmw_zenoh_cpp/package.xml index 26b2a946..67b488be 100644 --- a/rmw_zenoh_cpp/package.xml +++ b/rmw_zenoh_cpp/package.xml @@ -10,7 +10,6 @@ ament_cmake zenoh_c_vendor - zenohd_vendor ament_index_cpp fastcdr diff --git a/zenohd_vendor/CMakeLists.txt b/zenohd_vendor/CMakeLists.txt deleted file mode 100644 index c748c7ab..00000000 --- a/zenohd_vendor/CMakeLists.txt +++ /dev/null @@ -1,33 +0,0 @@ -cmake_minimum_required(VERSION 3.16) -project(zenohd_vendor) - -# Default to C++17 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) - set(CMAKE_CXX_STANDARD_REQUIRED ON) -endif() - -find_package(ament_cmake REQUIRED) - -include(ExternalProject) - -ExternalProject_Add( - zenoh - PREFIX ${CMAKE_BINARY_DIR}/_cargo_deps - GIT_REPOSITORY https://github.com/eclipse-zenoh/zenoh.git - GIT_TAG 0.10.1-rc - BUILD_COMMAND cargo build --package zenohd --release - CONFIGURE_COMMAND "" # Disable the configure step - INSTALL_COMMAND "" - BUILD_IN_SOURCE 1 -) - -# Install the zenohd executable from the rust crate -install( - PROGRAMS ${CMAKE_BINARY_DIR}/_cargo_deps/src/zenoh/target/release/zenohd - DESTINATION bin -) - -# TODO(francocipollone): Install zenoh router plugins - -ament_package() diff --git a/zenohd_vendor/package.xml b/zenohd_vendor/package.xml deleted file mode 100644 index 8a4b1966..00000000 --- a/zenohd_vendor/package.xml +++ /dev/null @@ -1,17 +0,0 @@ - - - - zenohd_vendor - 0.0.1 - Vendor pkg to install zenoh router - FrancoCipollone - Apache License 2.0 - - ament_cmake - - - - - ament_cmake - -