Skip to content

Commit

Permalink
Merge commit '0225a59de4fe2f180291d94845071753269e2744'
Browse files Browse the repository at this point in the history
  • Loading branch information
yellowhatter committed Aug 27, 2024
2 parents cadd258 + 0225a59 commit 3b31861
Show file tree
Hide file tree
Showing 3 changed files with 45 additions and 161 deletions.
48 changes: 33 additions & 15 deletions .github/workflows/build.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -4,27 +4,45 @@ on:
push:
branches: [ rolling ]
workflow_dispatch:
schedule:
# Run every morning to detect flakiness and broken dependencies
- cron: '03 5 * * *'
defaults:
run:
shell: bash
jobs:
test:
runs-on: [self-hosted, ubuntu-24.04]
build_and_test:
name: build_and_test_${{ matrix.BUILD_TYPE }}_${{ matrix.ROS_DISTRO }}
strategy:
fail-fast: false
matrix:
distro: ['iron', 'jazzy', 'rolling']
include:
# Rolling (source)
- ROS_DISTRO: rolling
BUILD_TYPE: source
# Jazzy (binary)
- ROS_DISTRO: jazzy
BUILD_TYPE: binary
# Iron (binary)
- ROS_DISTRO: iron
BUILD_TYPE: binary
env:
ROS2_REPOS_FILE_URL: 'https://raw.githubusercontent.com/ros2/ros2/${{ matrix.ROS_DISTRO }}/ros2.repos'
runs-on: [self-hosted, ubuntu-24.04]
container:
image: ros:${{ matrix.distro }}-ros-base
timeout-minutes: 30
image: ${{ matrix.BUILD_TYPE == 'binary' && format('ros:{0}-ros-base', matrix.ROS_DISTRO) || 'ubuntu:noble' }}
steps:
- name: Deps
run: |
apt update && apt install -y curl
- uses: actions/checkout@v4
- name: rosdep
run: |
rosdep update
rosdep install --from-paths . -yir
- name: build
run: /ros_entrypoint.sh colcon build
- uses: ros-tooling/[email protected]
if: ${{ matrix.BUILD_TYPE == 'source' }}
- name: Install Coverage Tools
if: ${{ matrix.BUILD_TYPE == 'binary' }}
run: sudo apt update && sudo apt install -y python3-colcon-coveragepy-result python3-colcon-lcov-result lcov
- name: Build and run tests
id: action-ros-ci
uses: ros-tooling/[email protected]
with:
package-name: |
rmw_zenoh_cpp
zenoh_c_vendor
target-ros2-distro: ${{ matrix.ROS_DISTRO }}
vcs-repo-file-url: ${{ matrix.BUILD_TYPE == 'source' && env.ROS2_REPOS_FILE_URL || '' }}
7 changes: 3 additions & 4 deletions rmw_zenoh_cpp/src/rmw_init_options.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -13,6 +13,7 @@
// limitations under the License.

#include <zenoh.h>
#include <string.h>

#include "detail/identifier.hpp"
#include "detail/rmw_init_options_impl.hpp"
Expand Down Expand Up @@ -40,14 +41,14 @@ rmw_init_options_init(rmw_init_options_t * init_options, rcutils_allocator_t all
return RMW_RET_INVALID_ARGUMENT;
}

memset(init_options, 0, sizeof(rmw_init_options_t));
init_options->instance_id = 0;
init_options->implementation_identifier = rmw_zenoh_cpp::rmw_zenoh_identifier;
init_options->allocator = allocator;
init_options->impl = nullptr;
init_options->enclave = nullptr;
init_options->domain_id = RMW_DEFAULT_DOMAIN_ID;
init_options->security_options = rmw_get_default_security_options();
init_options->localhost_only = RMW_LOCALHOST_ONLY_DEFAULT;
init_options->discovery_options = rmw_get_zero_initialized_discovery_options();
return rmw_discovery_options_init(&(init_options->discovery_options), 0, &allocator);
}
Expand Down Expand Up @@ -76,9 +77,8 @@ rmw_init_options_copy(const rmw_init_options_t * src, rmw_init_options_t * dst)
RCUTILS_CHECK_ALLOCATOR(&allocator, return RMW_RET_INVALID_ARGUMENT);

rmw_init_options_t tmp;
tmp.instance_id = src->instance_id;
memcpy(&tmp, src, sizeof(rmw_init_options_t));
tmp.implementation_identifier = rmw_zenoh_cpp::rmw_zenoh_identifier;
tmp.domain_id = src->domain_id;
tmp.security_options = rmw_get_zero_initialized_security_options();
rmw_ret_t ret =
rmw_security_options_copy(&src->security_options, &allocator, &tmp.security_options);
Expand All @@ -89,7 +89,6 @@ rmw_init_options_copy(const rmw_init_options_t * src, rmw_init_options_t * dst)
[&tmp, allocator]() {
rmw_security_options_fini(&tmp.security_options, &allocator);
});
tmp.localhost_only = src->localhost_only;
tmp.discovery_options = rmw_get_zero_initialized_discovery_options();
ret = rmw_discovery_options_copy(
&src->discovery_options,
Expand Down
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 3b31861

Please sign in to comment.