Skip to content

Commit

Permalink
Merge branch 'rolling' into yadu/qos-durability
Browse files Browse the repository at this point in the history
  • Loading branch information
Yadunund committed Jan 26, 2024
2 parents a664ac2 + f2a1432 commit 5c2b179
Show file tree
Hide file tree
Showing 5 changed files with 185 additions and 72 deletions.
1 change: 1 addition & 0 deletions rmw_zenoh_cpp/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -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(
Expand Down
205 changes: 184 additions & 21 deletions rmw_zenoh_cpp/apps/init_rmw_zenoh_router.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}
1 change: 0 additions & 1 deletion rmw_zenoh_cpp/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -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>
Expand Down
33 changes: 0 additions & 33 deletions zenohd_vendor/CMakeLists.txt

This file was deleted.

17 changes: 0 additions & 17 deletions zenohd_vendor/package.xml

This file was deleted.

0 comments on commit 5c2b179

Please sign in to comment.