Skip to content

Commit

Permalink
Merge remote-tracking branch 'origin/rolling' into feature/modify_bui…
Browse files Browse the repository at this point in the history 
…ld_ci_with_industrial_ci
  • Loading branch information
@ahcorde
ahcorde committed Aug 22, 2024
2 parents 3938e8b + 90a16ec commit 325a9cf
Show file tree
Hide file tree
Showing 3 changed files with 24 additions and 151 deletions.
6 changes: 3 additions & 3 deletions rmw_zenoh_cpp/src/detail/graph_cache.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1202,13 +1202,13 @@ rmw_ret_t GraphCache::get_entities_info_by_topic(
rmw_ret_t GraphCache::service_server_is_available(
const char * service_name,
const char * service_type,
bool * is_available)
bool * is_available) const
{
*is_available = false;
std::lock_guard<std::mutex> lock(graph_mutex_);
GraphNode::TopicMap::iterator service_it = graph_services_.find(service_name);
GraphNode::TopicMap::const_iterator service_it = graph_services_.find(service_name);
if (service_it != graph_services_.end()) {
GraphNode::TopicTypeMap::iterator type_it = service_it.value().find(service_type);
GraphNode::TopicTypeMap::const_iterator type_it = service_it->second.find(service_type);
if (type_it != service_it->second.end()) {
for (const auto & [_, topic_data] : type_it->second) {
if (topic_data->subs_.size() > 0) {
Expand Down
2 changes: 1 addition & 1 deletion rmw_zenoh_cpp/src/detail/graph_cache.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -167,7 +167,7 @@ class GraphCache final
rmw_ret_t service_server_is_available(
const char * service_name,
const char * service_type,
bool * is_available);
bool * is_available) const;

/// @brief Signature for a function that will be invoked by the GraphCache when a QoS
/// event is detected.
Expand Down
167 changes: 20 additions & 147 deletions rmw_zenoh_cpp/src/zenohd/main.cpp
View file
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 @@ -36,136 +32,26 @@

#include "rmw/error_handling.h"

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;
}
#include "rcpputils/scope_exit.hpp"

~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 bool running = true;
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 All @@ -192,42 +78,29 @@ int main(int argc, char ** argv)
return 1;
}

z_owned_session_t s = z_open(z_move(config));
if (!z_check(s)) {
z_owned_session_t session = z_open(z_move(config));
if (!z_check(session)) {
printf("Unable to open router session!\n");
return 1;
}
auto always_close_session = rcpputils::make_scope_exit(
[&session]() {
z_close(z_move(session));
});

printf(
"Started Zenoh router with id %s.\n",
rmw_zenoh_cpp::liveliness::zid_to_str(z_info_zid(z_session_loan(&s))).c_str());
rmw_zenoh_cpp::liveliness::zid_to_str(z_info_zid(z_session_loan(&session))).c_str());
#ifdef _WIN32
SetConsoleCtrlHandler(quit, TRUE);
#else
signal(SIGINT, quit);
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));
}

z_close(z_move(s));
// 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 325a9cf

Please sign in to comment.