Skip to content

Commit

Permalink
Support running zenohd without console access (ros2#261)
Browse files Browse the repository at this point in the history
* Support running zenohd without console access

Signed-off-by: Nate Koenig <[email protected]>
Co-authored-by: Chris Lalancette <[email protected]>
  • Loading branch information
2 people authored and imstevenpmwork committed Aug 27, 2024
1 parent 62c3f54 commit 001f490
Showing 1 changed file with 9 additions and 142 deletions.
151 changes: 9 additions & 142 deletions rmw_zenoh_cpp/src/zenohd/main.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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>
Expand All @@ -39,135 +35,23 @@
#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
void quit(int sig)
{
(void)sig;
running = false;
run_cv.notify_one();
}
#endif

Expand Down Expand Up @@ -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;
}

0 comments on commit 001f490

Please sign in to comment.