From ff9a25aef35db56e1b7b8c7f04293f2101096e34 Mon Sep 17 00:00:00 2001 From: Johannes Schrimpf Date: Fri, 6 Oct 2023 23:34:21 +0200 Subject: [PATCH 01/12] Convert to ros2 --- CMakeLists.txt | 42 ++-- README.md | 18 +- examples/ros_context/call.cpp | 24 +-- examples/ros_context/serve.cpp | 30 +-- tyndall/ros_context.cpp | 14 +- tyndall/ros_context.h | 342 ++++++++++++++++----------------- 6 files changed, 231 insertions(+), 239 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index afbd37a..3a5d7c0 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,5 +1,6 @@ +cmake_minimum_required(VERSION 3.5) + project(tyndall) -cmake_minimum_required(VERSION 2.8.3) set(CMAKE_CXX_STANDARD 20) set(CMAKE_CXX_STANDARD_REQUIRED ON) @@ -15,17 +16,12 @@ add_definitions(-DDEBUG) include_directories(${CMAKE_SOURCE_DIR}) set(LD_FLAGS "-lpthread -lzmq -lprotobuf -lfmt -lrt -latomic") -find_package(roscpp) - -if ("${roscpp_FOUND}") - if ("${roscpp_LINK_DIRECTORIES}" STREQUAL "") - # quickfix using the path of the first library in roscpp_LIBRARIES - execute_process(COMMAND bash "-c" "dirname -z $(echo \"${roscpp_LIBRARIES}\" | cut -f1 -d ';')" OUTPUT_VARIABLE roscpp_LINK_DIRECTORIES) - endif() +find_package(ament_cmake) +find_package(rclcpp) +find_package(builtin_interfaces ) - include_directories(${roscpp_INCLUDE_DIRS}) - link_directories(${roscpp_LINK_DIRECTORIES}) - set(LD_FLAGS "${LD_FLAGS} -lpthread -lroscpp -lrosconsole -lrosconsole_backend_interface -lrosconsole_log4cxx -lroscpp_serialization -lxmlrpcpp -lrostime -lcpp_common") +if ("${rclcpp_FOUND}") + set(LD_FLAGS "${LD_FLAGS} -lpthread -lcpp_common") else() add_definitions("-DNO_ROS") endif() @@ -35,13 +31,19 @@ add_library(tyndall SHARED ${CFILES}) target_link_libraries(tyndall ${LD_FLAGS}) install(TARGETS tyndall DESTINATION lib) +if ("${rclcpp_FOUND}") + ament_target_dependencies(tyndall rclcpp builtin_interfaces) +endif() + + + file(GLOB HEADERS "tyndall/*.h*" "tyndall/*/*.h*") foreach(HEADER ${HEADERS}) execute_process(COMMAND bash "-c" "dirname -z $(realpath --relative-to=${CMAKE_SOURCE_DIR} ${HEADER})" OUTPUT_VARIABLE HEADER_DIR) install(FILES ${HEADER} DESTINATION include/${HEADER_DIR}) endforeach(HEADER) -execute_process(COMMAND bash "-c" "echo -n \"${roscpp_LINK_DIRECTORIES}\" | sed -n -e 's/^.*\\(\\/opt\\)/\\1/p'" OUTPUT_VARIABLE PKGCONF_ROS_LIBLINK_PATH) +execute_process(COMMAND bash "-c" "echo -n \"${rclcpp_LINK_DIRECTORIES}\" | sed -n -e 's/^.*\\(\\/opt\\)/\\1/p'" OUTPUT_VARIABLE PKGCONF_ROS_LIBLINK_PATH) set(PKGCONF_ROS_LIBLINK "-L${PKGCONF_ROS_LIBLINK_PATH}") set(PKGCONF_LD_FLAGS ${LD_FLAGS}) configure_file(tyndall.pc.in tyndall.pc @ONLY) @@ -97,3 +99,19 @@ foreach(TOOL ${TOOLS}) endforeach(TOOL) add_custom_target(tools) add_dependencies(tools ${TOOL_TARGETS}) + +function(dump_cmake_variables) + get_cmake_property(_variableNames VARIABLES) + list (SORT _variableNames) + foreach (_variableName ${_variableNames}) + if (ARGV0) + unset(MATCHED) + string(REGEX MATCH ${ARGV0} MATCHED ${_variableName}) + if (NOT MATCHED) + continue() + endif() + endif() + message(STATUS "${_variableName}=${${_variableName}}") + endforeach() +endfunction() +#dump_cmake_variables() \ No newline at end of file diff --git a/README.md b/README.md index 4ab00cf..a75cc54 100644 --- a/README.md +++ b/README.md @@ -146,11 +146,11 @@ Pub / sub example: ```cpp #include -#include +#include ros_context::init(argc, argv, std::chrono::milliseconds{3}, "ex_ros_context_write"); -std_msgs::Int32 msg; +std_msgs::msg::Int32 msg; msg.data = 42; ros_context_write(msg, "/ex_ros_context"); @@ -160,13 +160,13 @@ ros_context_write(msg, "/ex_ros_context"); ```cpp #include -#include +#include ros_context::init(argc, argv, std::chrono::milliseconds{3}, "ex_ros_context_read"); while(1) { - std_msgs::Int32 msg; + std_msgs::msg::Int32 msg; int rc = ros_context_read(msg, "/ex_ros_context"); @@ -181,12 +181,12 @@ Serve / call example: ```cpp #include -#include +#include ros_context::init(argc, argv, std::chrono::milliseconds{3}, "ex_ros_context_serve"); while(1) { - std_srvs::SetBool srv; + std_srvs::srv::SetBool srv; srv.response.success = true; int rc = ros_context_serve(srv, "ex_ros_context"); @@ -200,15 +200,15 @@ while(1) ```cpp #include -#include +#include ros_context::init(argc, argv, std::chrono::milliseconds{3}, "ex_ros_context_serve"); while(1) { - std_srvs::SetBool srv; + std_srvs::srv::SetBool srv; srv.response.success = true; - std_srvs::SetBool srv; + std_srvs::srv::SetBool srv; srv.request.data = true; int rc = ros_context_call(srv, "/ex_ros_context_serve/ex_ros_context"); diff --git a/examples/ros_context/call.cpp b/examples/ros_context/call.cpp index 090788a..2994f0d 100644 --- a/examples/ros_context/call.cpp +++ b/examples/ros_context/call.cpp @@ -1,29 +1,25 @@ #ifdef NO_ROS #include -int main(){ printf("no ros\n"); } +int main() { printf("no ros\n"); } #else -#include -#include +#include #include +#include +#include #include -#include -#include +#include sig_atomic_t run = 1; -void signal_handler(int sig) -{ - run = 0; -} +void signal_handler(int sig) { run = 0; } -int main(int argc, char** argv) -{ - ros_context::init(argc, argv, std::chrono::milliseconds{3}, "ex_ros_context_call"); +int main(int argc, char **argv) { + ros_context::init(argc, argv, std::chrono::milliseconds{3}, + "ex_ros_context_call"); signal(SIGINT, signal_handler); - while(run) - { + while (run) { std_srvs::SetBool srv; srv.request.data = (bool)(run++ % 3); diff --git a/examples/ros_context/serve.cpp b/examples/ros_context/serve.cpp index ba13554..bd21639 100644 --- a/examples/ros_context/serve.cpp +++ b/examples/ros_context/serve.cpp @@ -1,31 +1,31 @@ #ifdef NO_ROS #include -int main(){ printf("no ros\n"); } +int main() { printf("no ros\n"); } #else -#include -#include +#include #include +#include +#include #include -#include -#include +#include sig_atomic_t run = 1; -void signal_handler(int sig) -{ - run = 0; -} +void signal_handler(int sig) { run = 0; } -int main(int argc, char** argv) -{ - ros_context::init(argc, argv, std::chrono::milliseconds{3}, "ex_ros_context_serve"); +int main(int argc, char **argv) { + ros_context::init(argc, argv, std::chrono::milliseconds{3}, + "ex_ros_context_serve"); signal(SIGINT, signal_handler); - while(run) - { + while (run) { std_srvs::SetBool srv; - srv.response.success = (bool)(std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count() % 3); + srv.response.success = + (bool)(std::chrono::duration_cast( + std::chrono::system_clock::now().time_since_epoch()) + .count() % + 3); int rc = ros_context_serve(srv, "ex_ros_context"); diff --git a/tyndall/ros_context.cpp b/tyndall/ros_context.cpp index 91b8636..0aa1b81 100644 --- a/tyndall/ros_context.cpp +++ b/tyndall/ros_context.cpp @@ -1,12 +1,10 @@ #include "ros_context.h" - #ifndef NO_ROS -namespace ros_context -{ - ros::NodeHandle *nh = NULL; - std::mutex ros_mutex; - std::thread ros_thread; - int run_ros = 1; -} +namespace ros_context { +rclcpp::Node::SharedPtr nh; +std::mutex ros_mutex; +std::thread ros_thread; +int run_ros = 1; +} // namespace ros_context #endif diff --git a/tyndall/ros_context.h b/tyndall/ros_context.h index c98e77c..06ba5ed 100644 --- a/tyndall/ros_context.h +++ b/tyndall/ros_context.h @@ -1,230 +1,210 @@ #pragma once +#include #include -#include -#include #include +#include +#include #include -#include #ifdef NO_ROS // mock -namespace ros_context -{ - static inline int shutdown() - { return 0; } - static inline int init(int argc, char** argv, std::chrono::milliseconds loop_sleep = std::chrono::milliseconds{3}, const char *node_name = "default_node_name") - { return 0;} +namespace ros_context { +static inline int shutdown() { return 0; } +static inline int +init(int argc, char **argv, + std::chrono::milliseconds loop_sleep = std::chrono::milliseconds{3}, + const char *node_name = "default_node_name") { + return 0; } +} // namespace ros_context #include #define ros_context_read(...) ({ ipc_read(__VA_ARGS__); }) #define ros_context_write(...) ({ ipc_write(__VA_ARGS__); }) -#define ros_context_serve(...) ({ (void)sizeof(msg); 0; }) -#define ros_context_call(...) ({ (void)sizeof(msg); 0; }) +#define ros_context_serve(...) \ + ({ \ + (void)sizeof(msg); \ + 0; \ + }) +#define ros_context_call(...) \ + ({ \ + (void)sizeof(msg); \ + 0; \ + }) #else -#include +#include #include "tyndall/meta/strval.h" -// ros_context wraps ros initialization, destruction and pub sub pattern in a thread safe interface. -// lazy initialization of ros communication objects is used for ease of use -namespace ros_context -{ - extern ros::NodeHandle *nh; - extern std::mutex ros_mutex; - extern std::thread ros_thread; - extern int run_ros; - - // methods - - static inline int shutdown() - { - run_ros = 0; - ros_thread.join(); - - ros::shutdown(); - return 0; - } +// ros_context wraps ros initialization, destruction and pub sub pattern in a +// thread safe interface. lazy initialization of ros communication objects is +// used for ease of use +namespace ros_context { +extern rclcpp::Node::SharedPtr nh; +extern std::mutex ros_mutex; +extern std::thread ros_thread; +extern int run_ros; - static inline int init(int argc, char** argv, std::chrono::milliseconds loop_sleep = std::chrono::milliseconds{3}, const char *node_name = "default_node_name") - { - assert(nh == NULL); // enforce single initialization per process +// methods - ros::init(argc, argv, node_name); // node_name is usually overridden by launch file +static inline int shutdown() { + run_ros = 0; + ros_thread.join(); - // start roscore if it's not running - if (!ros::master::check() && (fork() == 0)) - { - int rc = system("roscore"); - assert(rc != -1); - } + rclcpp::shutdown(); + return 0; +} - static ros::NodeHandle nh_instance("~"); - nh = &nh_instance; +static inline int +init(int argc, char **argv, + std::chrono::milliseconds loop_sleep = std::chrono::milliseconds{3}, + const char *node_name = "default_node_name") { + assert(nh == NULL); // enforce single initialization per process - static struct lifetime_t - { - ~lifetime_t() - { shutdown(); } - } lifetime; + rclcpp::init(argc, argv); - ros_thread = std::thread([loop_sleep](){ - while (run_ros) - { - { - std::lock_guard guard(ros_mutex); - ros::spinOnce(); - } - std::this_thread::sleep_for(loop_sleep); - } - }); - return 0; - } + static rclcpp::Node::SharedPtr nh_new = + std::make_shared(node_name); + nh = nh_new; - template - int lazy_read(Message& msg, Id) - { - static Message save; - static std::mutex save_mutex; - static bool new_save = false; - static bool valid_save = false; - - // register ros callback - static bool must_initialize_callback = true; - if (must_initialize_callback) - { - must_initialize_callback = false; - std::lock_guard guard(ros_mutex); - - static auto sub = nh->subscribe(Id::c_str(), 1, - boost::function - ([](const Message& sub_msg) - -> void { - std::lock_guard guard(save_mutex); - save = sub_msg; - new_save = true; - valid_save = true; - }) - ); - } + static struct lifetime_t { + ~lifetime_t() { shutdown(); } + } lifetime; - // get saved ros message - int rc; - { - std::lock_guard guard(save_mutex); - if (new_save) - { - new_save = false; - msg = save; - rc = 0; - } - else if (valid_save) - { - msg = save; - rc = -1; - errno = EAGAIN; - } - else + ros_thread = std::thread([loop_sleep]() { + while (run_ros) { { - rc = -1; - errno = ENOMSG; + std::lock_guard guard(ros_mutex); + rclcpp::spin_some(nh); } + std::this_thread::sleep_for(loop_sleep); } + }); + return 0; +} - return rc; +template int lazy_read(Message &msg, Id) { + static Message save; + static std::mutex save_mutex; + static bool new_save = false; + static bool valid_save = false; + + // register ros callback + static bool must_initialize_callback = true; + if (must_initialize_callback) { + must_initialize_callback = false; + std::lock_guard guard(ros_mutex); + + static auto sub = nh->create_subscription( + Id::c_str(), 1, + std::function( + [](const typename Message::ConstSharedPtr sub_msg) -> void { + std::lock_guard guard(save_mutex); + save = *sub_msg; + new_save = true; + valid_save = true; + })); } - template - void lazy_write(const Message& msg, Id) + // get saved ros message + int rc; { - static ros::Publisher pub = nh->advertise(Id::c_str(), 1, true); // queue size 1 and latching - - pub.publish(msg); + std::lock_guard guard(save_mutex); + if (new_save) { + new_save = false; + msg = save; + rc = 0; + } else if (valid_save) { + msg = save; + rc = -1; + errno = EAGAIN; + } else { + rc = -1; + errno = ENOMSG; + } } + return rc; +} - // Ros service based methods - - template - int lazy_serve(Service& srv, Id) - { - static Service save; - static std::mutex save_mutex; - static bool new_save = false; - static bool valid_save = false; - - // handle service data - int rc; - { - std::lock_guard guard(save_mutex); +template +void lazy_write(const Message &msg, Id) { + static rclcpp::QoS qos_profile(1); + qos_profile.transient_local(); + static rclcpp::PublisherOptions pub_options; + static auto pub = + nh->create_publisher(Id::c_str(), qos_profile, pub_options); + pub->publish(msg); +} - save.response = srv.response; // set new response +// Ros service based methods - if (new_save) - { - new_save = false; - srv = save; - rc = 0; - } - else if (valid_save) - { - srv = save; - rc = -1; - errno = EAGAIN; - } - else - { - rc = -1; - errno = ENOMSG; - } - } +template int lazy_serve(Service &srv, Id) { + static Service save; + static std::mutex save_mutex; + static bool new_save = false; + static bool valid_save = false; - // register ros callback - static bool must_initialize_callback = true; - if (must_initialize_callback) - { - must_initialize_callback = false; - std::lock_guard guard(ros_mutex); - - static auto server = nh->advertiseService(Id::c_str(), - boost::function - ([](typename Service::Request& req, typename Service::Response& rep) - -> bool { - std::lock_guard guard(save_mutex); - rep = save.response; - save.request = req; - new_save = true; - valid_save = true; - return true; - }) - ); + // handle service data + int rc; + { + std::lock_guard guard(save_mutex); + + save.response = srv.response; // set new response + + if (new_save) { + new_save = false; + srv = save; + rc = 0; + } else if (valid_save) { + srv = save; + rc = -1; + errno = EAGAIN; + } else { + rc = -1; + errno = ENOMSG; } + } - return rc; + // register ros callback + static bool must_initialize_callback = true; + if (must_initialize_callback) { + must_initialize_callback = false; + std::lock_guard guard(ros_mutex); + + static auto server = nh->create_service( + Id::c_str(), + std::function( + [](typename Service::Request::SharedPtr req, + typename Service::Response::SharedPtr rep) -> bool { + std::lock_guard guard(save_mutex); + rep = save.response; + save.request = req; + new_save = true; + valid_save = true; + return true; + })); } - template - int lazy_call(Service& srv, Id) - { - static ros::ServiceClient client = nh->serviceClient(Id::c_str(), true); + return rc; +} - if (!client.isValid()) - { - client = nh->serviceClient(Id::c_str(), true); - } +template int lazy_call(Service &srv, Id) { + static auto client = nh->create_client(Id::c_str()); - return client.call(srv) ? 0 : -1; + if (!client.isValid()) { + client = nh->create_client(Id::c_str()); } + + return client.call(srv) ? 0 : -1; } +} // namespace ros_context -#define ros_context_read(msg, id) \ - ros_context::lazy_read(msg, id ## _strval) +#define ros_context_read(msg, id) ros_context::lazy_read(msg, id##_strval) -#define ros_context_write(msg, id) \ - ros_context::lazy_write(msg, id ## _strval) +#define ros_context_write(msg, id) ros_context::lazy_write(msg, id##_strval) -#define ros_context_serve(srv, id) \ - ros_context::lazy_serve(srv, id ## _strval) +#define ros_context_serve(srv, id) ros_context::lazy_serve(srv, id##_strval) -#define ros_context_call(srv, id) \ - ros_context::lazy_call(srv, id ## _strval) +#define ros_context_call(srv, id) ros_context::lazy_call(srv, id##_strval) #endif From 9fa41183f270fa64fd813ed9feebf6e935187e09 Mon Sep 17 00:00:00 2001 From: Johannes Schrimpf Date: Mon, 27 Nov 2023 13:31:11 +0100 Subject: [PATCH 02/12] Remove cpp_common --- CMakeLists.txt | 53 +++++++++++++++++++++++++++++--------------------- 1 file changed, 31 insertions(+), 22 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 3a5d7c0..e3d4dc0 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -18,10 +18,10 @@ set(LD_FLAGS "-lpthread -lzmq -lprotobuf -lfmt -lrt -latomic") find_package(ament_cmake) find_package(rclcpp) -find_package(builtin_interfaces ) +find_package(builtin_interfaces) -if ("${rclcpp_FOUND}") - set(LD_FLAGS "${LD_FLAGS} -lpthread -lcpp_common") +if("${rclcpp_FOUND}") + set(LD_FLAGS "${LD_FLAGS} -lpthread") else() add_definitions("-DNO_ROS") endif() @@ -31,13 +31,12 @@ add_library(tyndall SHARED ${CFILES}) target_link_libraries(tyndall ${LD_FLAGS}) install(TARGETS tyndall DESTINATION lib) -if ("${rclcpp_FOUND}") +if("${rclcpp_FOUND}") ament_target_dependencies(tyndall rclcpp builtin_interfaces) endif() - - file(GLOB HEADERS "tyndall/*.h*" "tyndall/*/*.h*") + foreach(HEADER ${HEADERS}) execute_process(COMMAND bash "-c" "dirname -z $(realpath --relative-to=${CMAKE_SOURCE_DIR} ${HEADER})" OUTPUT_VARIABLE HEADER_DIR) install(FILES ${HEADER} DESTINATION include/${HEADER_DIR}) @@ -51,6 +50,7 @@ install(FILES ${CMAKE_CURRENT_BINARY_DIR}/tyndall.pc DESTINATION lib/pkgconfig) set(EXAMPLE_PREFIX "${CMAKE_PROJECT_NAME}_ex_") file(GLOB EXAMPLES "examples/*.c*") + foreach(EXAMPLE ${EXAMPLES}) execute_process(COMMAND bash "-c" "printf ${EXAMPLE_PREFIX}$(basename ${EXAMPLE} | cut -f 1 -d '.')" OUTPUT_VARIABLE EXAMPLE_TARGET) add_executable(${EXAMPLE_TARGET} EXCLUDE_FROM_ALL ${EXAMPLE}) @@ -59,8 +59,8 @@ foreach(EXAMPLE ${EXAMPLES}) set(EXAMPLE_TARGETS "${EXAMPLE_TARGETS}${EXAMPLE_TARGET};") endforeach(EXAMPLE) - file(GLOB SUBEXAMPLES "examples/*/*.c*") + foreach(EXAMPLE ${SUBEXAMPLES}) execute_process(COMMAND bash "-c" "printf ${EXAMPLE_PREFIX}$(dirname ${EXAMPLE} | xargs basename)_$(basename ${EXAMPLE} | cut -f 1 -d '.')" OUTPUT_VARIABLE EXAMPLE_TARGET) add_executable(${EXAMPLE_TARGET} EXCLUDE_FROM_ALL ${EXAMPLE}) @@ -68,11 +68,13 @@ foreach(EXAMPLE ${SUBEXAMPLES}) install(TARGETS ${EXAMPLE_TARGET} DESTINATION bin OPTIONAL) set(EXAMPLE_TARGETS "${EXAMPLE_TARGETS}${EXAMPLE_TARGET};") endforeach(EXAMPLE) + add_custom_target(examples) add_dependencies(examples ${EXAMPLE_TARGETS}) set(TEST_PREFIX "${CMAKE_PROJECT_NAME}_test_") file(GLOB CPP_TESTS "tests/*/*.cpp") + foreach(CPP_TEST ${CPP_TESTS}) execute_process(COMMAND bash "-c" "printf ${TEST_PREFIX}$(dirname ${CPP_TEST} | xargs basename)_$(basename ${CPP_TEST} | cut -f 1 -d '.')" OUTPUT_VARIABLE CPP_TEST_TARGET) add_executable(${CPP_TEST_TARGET} EXCLUDE_FROM_ALL ${CPP_TEST}) @@ -82,14 +84,16 @@ foreach(CPP_TEST ${CPP_TESTS}) endforeach(CPP_TEST) IF(CMAKE_CROSSCOMPILING) -set(TEST_CMD "echo WARNING: Tests are not executed when cross compiling") + set(TEST_CMD "echo WARNING: Tests are not executed when cross compiling") ELSE() -set(TEST_CMD "set -e; for t in ${CMAKE_BINARY_DIR}/${TEST_PREFIX}*; do $t; done") + set(TEST_CMD "set -e; for t in ${CMAKE_BINARY_DIR}/${TEST_PREFIX}*; do $t; done") ENDIF() + add_custom_target(tests COMMAND /bin/bash -c "${TEST_CMD}" VERBATIM DEPENDS ${CPP_TEST_TARGETS}) set(TOOL_PREFIX "${CMAKE_PROJECT_NAME}_tool_") file(GLOB TOOLS "tools/*.c*") + foreach(TOOL ${TOOLS}) execute_process(COMMAND bash "-c" "printf ${TOOL_PREFIX}$(basename ${TOOL} | cut -f 1 -d '.')" OUTPUT_VARIABLE TOOL_TARGET) add_executable(${TOOL_TARGET} EXCLUDE_FROM_ALL ${TOOL}) @@ -97,21 +101,26 @@ foreach(TOOL ${TOOLS}) install(TARGETS ${TOOL_TARGET} DESTINATION bin OPTIONAL) set(TOOL_TARGETS "${TOOL_TARGETS}${TOOL_TARGET};") endforeach(TOOL) + add_custom_target(tools) add_dependencies(tools ${TOOL_TARGETS}) function(dump_cmake_variables) - get_cmake_property(_variableNames VARIABLES) - list (SORT _variableNames) - foreach (_variableName ${_variableNames}) - if (ARGV0) - unset(MATCHED) - string(REGEX MATCH ${ARGV0} MATCHED ${_variableName}) - if (NOT MATCHED) - continue() - endif() - endif() - message(STATUS "${_variableName}=${${_variableName}}") - endforeach() + get_cmake_property(_variableNames VARIABLES) + list(SORT _variableNames) + + foreach(_variableName ${_variableNames}) + if(ARGV0) + unset(MATCHED) + string(REGEX MATCH ${ARGV0} MATCHED ${_variableName}) + + if(NOT MATCHED) + continue() + endif() + endif() + + message(STATUS "${_variableName}=${${_variableName}}") + endforeach() endfunction() -#dump_cmake_variables() \ No newline at end of file + +# dump_cmake_variables() \ No newline at end of file From ab866d06d9020fa0b042ed41453e164e62357ad1 Mon Sep 17 00:00:00 2001 From: Johannes Schrimpf Date: Mon, 4 Dec 2023 10:05:36 +0100 Subject: [PATCH 03/12] Update to use Response and Request --- tyndall/ros_context.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/tyndall/ros_context.h b/tyndall/ros_context.h index 06ba5ed..35e7245 100644 --- a/tyndall/ros_context.h +++ b/tyndall/ros_context.h @@ -148,7 +148,7 @@ template int lazy_serve(Service &srv, Id) { { std::lock_guard guard(save_mutex); - save.response = srv.response; // set new response + save.Response = srv.Response; // set new response if (new_save) { new_save = false; @@ -177,8 +177,8 @@ template int lazy_serve(Service &srv, Id) { [](typename Service::Request::SharedPtr req, typename Service::Response::SharedPtr rep) -> bool { std::lock_guard guard(save_mutex); - rep = save.response; - save.request = req; + rep = save.Response; + save.Request = req; new_save = true; valid_save = true; return true; From 192fd4a45ef301fc2e6e28cb002c9b87af1aa5d5 Mon Sep 17 00:00:00 2001 From: Johannes Schrimpf Date: Thu, 18 Jan 2024 12:19:32 +0100 Subject: [PATCH 04/12] Convert functions to be usable with ros2 --- CMakeLists.txt | 4 ++- README.md | 2 +- examples/ros_context/call.cpp | 10 +++--- examples/ros_context/read.cpp | 29 ++++++++------- examples/ros_context/serve.cpp | 9 ++--- examples/ros_context/write.cpp | 29 ++++++++------- tests/ros_context/ros_context.cpp | 38 +++++++++++++------- tyndall/ros_context.cpp | 1 + tyndall/ros_context.h | 59 +++++++++++++++++++++++-------- 9 files changed, 114 insertions(+), 67 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index e3d4dc0..bb59815 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -18,6 +18,8 @@ set(LD_FLAGS "-lpthread -lzmq -lprotobuf -lfmt -lrt -latomic") find_package(ament_cmake) find_package(rclcpp) +find_package(std_msgs) +find_package(std_srvs) find_package(builtin_interfaces) if("${rclcpp_FOUND}") @@ -32,7 +34,7 @@ target_link_libraries(tyndall ${LD_FLAGS}) install(TARGETS tyndall DESTINATION lib) if("${rclcpp_FOUND}") - ament_target_dependencies(tyndall rclcpp builtin_interfaces) + ament_target_dependencies(tyndall rclcpp builtin_interfaces std_msgs std_srvs) endif() file(GLOB HEADERS "tyndall/*.h*" "tyndall/*/*.h*") diff --git a/README.md b/README.md index a75cc54..29c165e 100644 --- a/README.md +++ b/README.md @@ -192,7 +192,7 @@ while(1) int rc = ros_context_serve(srv, "ex_ros_context"); if (rc == 0) - printf("got: %d\n", srv.request.data); + printf("got: %d\n", srv_req.data); } ``` diff --git a/examples/ros_context/call.cpp b/examples/ros_context/call.cpp index 2994f0d..9040463 100644 --- a/examples/ros_context/call.cpp +++ b/examples/ros_context/call.cpp @@ -20,13 +20,15 @@ int main(int argc, char **argv) { signal(SIGINT, signal_handler); while (run) { - std_srvs::SetBool srv; - srv.request.data = (bool)(run++ % 3); + std_srvs::srv::SetBool::Request srv_req; + std_srvs::srv::SetBool::Response srv_resp; + srv_req.data = (bool)(run++ % 3); - int rc = ros_context_call(srv, "/ex_ros_context_serve/ex_ros_context"); + int rc = ros_context_call(srv_req, srv_resp, + "/ex_ros_context_serve/ex_ros_context"); if (rc == 0) - printf("req rep: %d %d\n", srv.request.data, srv.response.success); + printf("req rep: %d %d\n", srv_req.data, srv_resp.success); else printf("response error\n"); diff --git a/examples/ros_context/read.cpp b/examples/ros_context/read.cpp index 61fb40a..f121afe 100644 --- a/examples/ros_context/read.cpp +++ b/examples/ros_context/read.cpp @@ -1,31 +1,30 @@ -#include #include +#include #ifdef NO_ROS - namespace std_msgs { struct Int32 { int data; }; } +namespace std_msgs::msg { +struct Int32 { + int data; +}; +} // namespace std_msgs::msg #else - #include +#include #endif +#include #include #include -#include sig_atomic_t run = 1; -void signal_handler(int sig) -{ - run = 0; -} +void signal_handler(int sig) { run = 0; } -int main(int argc, char** argv) -{ - ros_context::init(argc, argv, std::chrono::milliseconds{3}, "ex_ros_context_read"); +int main(int argc, char **argv) { + ros_context::init(argc, argv, std::chrono::milliseconds{3}, + "ex_ros_context_read"); signal(SIGINT, signal_handler); - while(run) - { - std_msgs::Int32 msg{}; - + while (run) { + std_msgs::msg::Int32 msg{}; int rc = ros_context_read(msg, "/ex_ros_context"); diff --git a/examples/ros_context/serve.cpp b/examples/ros_context/serve.cpp index bd21639..0758a92 100644 --- a/examples/ros_context/serve.cpp +++ b/examples/ros_context/serve.cpp @@ -20,17 +20,18 @@ int main(int argc, char **argv) { signal(SIGINT, signal_handler); while (run) { - std_srvs::SetBool srv; - srv.response.success = + std_srvs::srv::SetBool::Request srv_req; + std_srvs::srv::SetBool::Response srv_resp; + srv_resp.success = (bool)(std::chrono::duration_cast( std::chrono::system_clock::now().time_since_epoch()) .count() % 3); - int rc = ros_context_serve(srv, "ex_ros_context"); + int rc = ros_context_serve(srv_req, srv_resp, "ex_ros_context"); if (rc == 0) - printf("read: %d\n", srv.request.data); + printf("read: %d\n", srv_req.data); std::this_thread::sleep_for(std::chrono::milliseconds{3}); } diff --git a/examples/ros_context/write.cpp b/examples/ros_context/write.cpp index de00600..8d30888 100644 --- a/examples/ros_context/write.cpp +++ b/examples/ros_context/write.cpp @@ -1,35 +1,34 @@ -#include #include +#include #ifdef NO_ROS - namespace std_msgs { struct Int32 { int data; }; } +namespace std_msgs { +struct Int32 { + int data; +}; +} // namespace std_msgs #else - #include +#include #endif +#include #include #include -#include sig_atomic_t run = 1; -void signal_handler(int sig) -{ - run = 0; -} +void signal_handler(int sig) { run = 0; } -int main(int argc, char** argv) -{ - ros_context::init(argc, argv, std::chrono::milliseconds{3}, "ex_ros_context_write"); +int main(int argc, char **argv) { + ros_context::init(argc, argv, std::chrono::milliseconds{3}, + "ex_ros_context_write"); signal(SIGINT, signal_handler); - for(int i=0; run; ++i) - { - std_msgs::Int32 msg; + for (int i = 0; run; ++i) { + std_msgs::msg::Int32 msg; msg.data = i; printf("writing: %d\n", msg.data); - ros_context_write(msg, "/ex_ros_context"); std::this_thread::sleep_for(std::chrono::milliseconds{3}); diff --git a/tests/ros_context/ros_context.cpp b/tests/ros_context/ros_context.cpp index c140037..c232099 100644 --- a/tests/ros_context/ros_context.cpp +++ b/tests/ros_context/ros_context.cpp @@ -1,27 +1,39 @@ #include -#include +#include #include +#include #include -#include #ifdef NO_ROS -namespace std_msgs { struct Int32 { int data; }; } +namespace std_msgs { +struct Int32 { + int data; +}; +} // namespace std_msgs #else -#include +#include #endif -using i32 = std_msgs::Int32; +using i32 = std_msgs::msg::Int32; -bool operator==(const i32& lhs, const i32& rhs) -{ - return lhs.data == rhs.data; -} +bool operator==(const i32 &lhs, const i32 &rhs) { return lhs.data == rhs.data; } -const i32 ref = [](){i32 ret; ret.data = 42; return ret; }(); +const i32 ref = []() { + i32 ret; + ret.data = 42; + return ret; +}(); -#define check(cond) do { if (!(cond)){ printf( __FILE__ ":" M_STRINGIFY(__LINE__) " " "Assertion failed: " #cond "\n"); exit(1); }} while(0) +#define check(cond) \ + do { \ + if (!(cond)) { \ + printf(__FILE__ ":" M_STRINGIFY(__LINE__) " " \ + "Assertion failed: " #cond \ + "\n"); \ + exit(1); \ + } \ + } while (0) -int main() -{ +int main() { ros_context::init(0, NULL, std::chrono::milliseconds{3}, "test_ros_context"); { diff --git a/tyndall/ros_context.cpp b/tyndall/ros_context.cpp index 0aa1b81..3427f77 100644 --- a/tyndall/ros_context.cpp +++ b/tyndall/ros_context.cpp @@ -1,3 +1,4 @@ + #include "ros_context.h" #ifndef NO_ROS diff --git a/tyndall/ros_context.h b/tyndall/ros_context.h index 35e7245..c4b1391 100644 --- a/tyndall/ros_context.h +++ b/tyndall/ros_context.h @@ -5,6 +5,7 @@ #include #include #include + #ifdef NO_ROS // mock namespace ros_context { @@ -137,8 +138,11 @@ void lazy_write(const Message &msg, Id) { // Ros service based methods -template int lazy_serve(Service &srv, Id) { - static Service save; +template +int lazy_serve(typename Service::Request &srv_req, + typename Service::Response &srv_resp) { + static typename Service::Request save_req; + static typename Service::Response save_resp; static std::mutex save_mutex; static bool new_save = false; static bool valid_save = false; @@ -148,14 +152,17 @@ template int lazy_serve(Service &srv, Id) { { std::lock_guard guard(save_mutex); - save.Response = srv.Response; // set new response + save_req = srv_req; + save_resp = srv_resp; // set new response if (new_save) { new_save = false; - srv = save; + srv_req = save_req; + srv_resp = save_resp; rc = 0; } else if (valid_save) { - srv = save; + srv_req = save_req; + srv_resp = save_resp; rc = -1; errno = EAGAIN; } else { @@ -172,13 +179,13 @@ template int lazy_serve(Service &srv, Id) { static auto server = nh->create_service( Id::c_str(), - std::function( + std::function( [](typename Service::Request::SharedPtr req, typename Service::Response::SharedPtr rep) -> bool { std::lock_guard guard(save_mutex); - rep = save.Response; - save.Request = req; + *rep = save_resp; + save_req = *req; new_save = true; valid_save = true; return true; @@ -188,14 +195,21 @@ template int lazy_serve(Service &srv, Id) { return rc; } -template int lazy_call(Service &srv, Id) { +template +int lazy_call(typename Service::Request &srv_req, + typename Service::Response &srv_resp, Id) { static auto client = nh->create_client(Id::c_str()); if (!client.isValid()) { client = nh->create_client(Id::c_str()); } - - return client.call(srv) ? 0 : -1; + auto result = client->async_send_request(srv_req); + if (rclcpp::spin_until_future_complete(nh, result) == + rclcpp::FutureReturnCode::SUCCESS) { + srv_resp = result.get(); + return 0; + } + return 1; } } // namespace ros_context @@ -203,8 +217,25 @@ template int lazy_call(Service &srv, Id) { #define ros_context_write(msg, id) ros_context::lazy_write(msg, id##_strval) -#define ros_context_serve(srv, id) ros_context::lazy_serve(srv, id##_strval) +template +int ros_context_serve(typename Service::Request &srv_req, + typename Service::Response &srv_resp) { + return ros_context::lazy_serve(srv_req, srv_resp); +} + +template +int ros_context_serve(typename Service::Request &srv_req, + typename Service::Response &srv_resp, Id) { + return ros_context::lazy_serve(srv_req, srv_resp); +} + +template +int ros_context_call(Request &srv_req, Response &srv_resp, const char *id) { + using Service = typename std::decay::type; + return ros_context::lazy_call(srv_req, srv_resp, strval(id)); +} -#define ros_context_call(srv, id) ros_context::lazy_call(srv, id##_strval) +#define ros_context_call(srv_req, srv_resp, id) \ + ros_context::lazy_call(srv_req, srv_resp, id##_strval) #endif From d25c70fb396493da8e2a853eca1d5f925ac91021 Mon Sep 17 00:00:00 2001 From: Johannes Schrimpf Date: Wed, 15 May 2024 14:25:12 +0200 Subject: [PATCH 05/12] Remove ros service functions --- .gitignore | 3 ++ examples/ros_context/call.cpp | 40 -------------- examples/ros_context/serve.cpp | 41 --------------- tyndall/ros_context.h | 96 ---------------------------------- 4 files changed, 3 insertions(+), 177 deletions(-) delete mode 100644 examples/ros_context/call.cpp delete mode 100644 examples/ros_context/serve.cpp diff --git a/.gitignore b/.gitignore index 3a520cc..9a04c33 100644 --- a/.gitignore +++ b/.gitignore @@ -1,3 +1,6 @@ *.swp *.swo /build +/build_imx +.DS_Store +.vscode diff --git a/examples/ros_context/call.cpp b/examples/ros_context/call.cpp deleted file mode 100644 index 9040463..0000000 --- a/examples/ros_context/call.cpp +++ /dev/null @@ -1,40 +0,0 @@ -#ifdef NO_ROS -#include -int main() { printf("no ros\n"); } -#else -#include -#include -#include -#include -#include -#include - -sig_atomic_t run = 1; - -void signal_handler(int sig) { run = 0; } - -int main(int argc, char **argv) { - ros_context::init(argc, argv, std::chrono::milliseconds{3}, - "ex_ros_context_call"); - - signal(SIGINT, signal_handler); - - while (run) { - std_srvs::srv::SetBool::Request srv_req; - std_srvs::srv::SetBool::Response srv_resp; - srv_req.data = (bool)(run++ % 3); - - int rc = ros_context_call(srv_req, srv_resp, - "/ex_ros_context_serve/ex_ros_context"); - - if (rc == 0) - printf("req rep: %d %d\n", srv_req.data, srv_resp.success); - else - printf("response error\n"); - - std::this_thread::sleep_for(std::chrono::milliseconds{3}); - } - - return 0; -} -#endif diff --git a/examples/ros_context/serve.cpp b/examples/ros_context/serve.cpp deleted file mode 100644 index 0758a92..0000000 --- a/examples/ros_context/serve.cpp +++ /dev/null @@ -1,41 +0,0 @@ -#ifdef NO_ROS -#include -int main() { printf("no ros\n"); } -#else -#include -#include -#include -#include -#include -#include - -sig_atomic_t run = 1; - -void signal_handler(int sig) { run = 0; } - -int main(int argc, char **argv) { - ros_context::init(argc, argv, std::chrono::milliseconds{3}, - "ex_ros_context_serve"); - - signal(SIGINT, signal_handler); - - while (run) { - std_srvs::srv::SetBool::Request srv_req; - std_srvs::srv::SetBool::Response srv_resp; - srv_resp.success = - (bool)(std::chrono::duration_cast( - std::chrono::system_clock::now().time_since_epoch()) - .count() % - 3); - - int rc = ros_context_serve(srv_req, srv_resp, "ex_ros_context"); - - if (rc == 0) - printf("read: %d\n", srv_req.data); - - std::this_thread::sleep_for(std::chrono::milliseconds{3}); - } - - return 0; -} -#endif diff --git a/tyndall/ros_context.h b/tyndall/ros_context.h index c4b1391..2390511 100644 --- a/tyndall/ros_context.h +++ b/tyndall/ros_context.h @@ -136,106 +136,10 @@ void lazy_write(const Message &msg, Id) { pub->publish(msg); } -// Ros service based methods - -template -int lazy_serve(typename Service::Request &srv_req, - typename Service::Response &srv_resp) { - static typename Service::Request save_req; - static typename Service::Response save_resp; - static std::mutex save_mutex; - static bool new_save = false; - static bool valid_save = false; - - // handle service data - int rc; - { - std::lock_guard guard(save_mutex); - - save_req = srv_req; - save_resp = srv_resp; // set new response - - if (new_save) { - new_save = false; - srv_req = save_req; - srv_resp = save_resp; - rc = 0; - } else if (valid_save) { - srv_req = save_req; - srv_resp = save_resp; - rc = -1; - errno = EAGAIN; - } else { - rc = -1; - errno = ENOMSG; - } - } - - // register ros callback - static bool must_initialize_callback = true; - if (must_initialize_callback) { - must_initialize_callback = false; - std::lock_guard guard(ros_mutex); - - static auto server = nh->create_service( - Id::c_str(), - std::function( - [](typename Service::Request::SharedPtr req, - typename Service::Response::SharedPtr rep) -> bool { - std::lock_guard guard(save_mutex); - *rep = save_resp; - save_req = *req; - new_save = true; - valid_save = true; - return true; - })); - } - - return rc; -} - -template -int lazy_call(typename Service::Request &srv_req, - typename Service::Response &srv_resp, Id) { - static auto client = nh->create_client(Id::c_str()); - - if (!client.isValid()) { - client = nh->create_client(Id::c_str()); - } - auto result = client->async_send_request(srv_req); - if (rclcpp::spin_until_future_complete(nh, result) == - rclcpp::FutureReturnCode::SUCCESS) { - srv_resp = result.get(); - return 0; - } - return 1; -} } // namespace ros_context #define ros_context_read(msg, id) ros_context::lazy_read(msg, id##_strval) #define ros_context_write(msg, id) ros_context::lazy_write(msg, id##_strval) -template -int ros_context_serve(typename Service::Request &srv_req, - typename Service::Response &srv_resp) { - return ros_context::lazy_serve(srv_req, srv_resp); -} - -template -int ros_context_serve(typename Service::Request &srv_req, - typename Service::Response &srv_resp, Id) { - return ros_context::lazy_serve(srv_req, srv_resp); -} - -template -int ros_context_call(Request &srv_req, Response &srv_resp, const char *id) { - using Service = typename std::decay::type; - return ros_context::lazy_call(srv_req, srv_resp, strval(id)); -} - -#define ros_context_call(srv_req, srv_resp, id) \ - ros_context::lazy_call(srv_req, srv_resp, id##_strval) - #endif From dce8b45bdc9a2ed3e26d0338223d7fea1986efcd Mon Sep 17 00:00:00 2001 From: Johannes Schrimpf Date: Tue, 28 May 2024 00:48:41 +0200 Subject: [PATCH 06/12] Add spin --- tyndall/ros_context.cpp | 2 +- tyndall/ros_context.h | 45 +++++++++++------------------------------ 2 files changed, 13 insertions(+), 34 deletions(-) diff --git a/tyndall/ros_context.cpp b/tyndall/ros_context.cpp index 3427f77..88fb895 100644 --- a/tyndall/ros_context.cpp +++ b/tyndall/ros_context.cpp @@ -3,7 +3,7 @@ #ifndef NO_ROS namespace ros_context { -rclcpp::Node::SharedPtr nh; +std::weak_ptr nh; std::mutex ros_mutex; std::thread ros_thread; int run_ros = 1; diff --git a/tyndall/ros_context.h b/tyndall/ros_context.h index 2390511..f07864f 100644 --- a/tyndall/ros_context.h +++ b/tyndall/ros_context.h @@ -9,7 +9,7 @@ #ifdef NO_ROS // mock namespace ros_context { -static inline int shutdown() { return 0; } +static inline void spin() {} static inline int init(int argc, char **argv, std::chrono::milliseconds loop_sleep = std::chrono::milliseconds{3}, @@ -39,26 +39,14 @@ init(int argc, char **argv, // thread safe interface. lazy initialization of ros communication objects is // used for ease of use namespace ros_context { -extern rclcpp::Node::SharedPtr nh; +extern std::weak_ptr nh; extern std::mutex ros_mutex; -extern std::thread ros_thread; -extern int run_ros; - -// methods - -static inline int shutdown() { - run_ros = 0; - ros_thread.join(); - - rclcpp::shutdown(); - return 0; -} static inline int init(int argc, char **argv, std::chrono::milliseconds loop_sleep = std::chrono::milliseconds{3}, const char *node_name = "default_node_name") { - assert(nh == NULL); // enforce single initialization per process + // assert(nh == NULL); // enforce single initialization per process rclcpp::init(argc, argv); @@ -66,22 +54,14 @@ init(int argc, char **argv, std::make_shared(node_name); nh = nh_new; - static struct lifetime_t { - ~lifetime_t() { shutdown(); } - } lifetime; - - ros_thread = std::thread([loop_sleep]() { - while (run_ros) { - { - std::lock_guard guard(ros_mutex); - rclcpp::spin_some(nh); - } - std::this_thread::sleep_for(loop_sleep); - } - }); return 0; } +static inline void spin() { + // std::lock_guard guard(ros_mutex); + rclcpp::spin(nh.lock()); +} + template int lazy_read(Message &msg, Id) { static Message save; static std::mutex save_mutex; @@ -92,9 +72,8 @@ template int lazy_read(Message &msg, Id) { static bool must_initialize_callback = true; if (must_initialize_callback) { must_initialize_callback = false; - std::lock_guard guard(ros_mutex); - - static auto sub = nh->create_subscription( + // std::lock_guard guard(ros_mutex); + static auto sub = nh.lock()->create_subscription( Id::c_str(), 1, std::function( [](const typename Message::ConstSharedPtr sub_msg) -> void { @@ -131,8 +110,8 @@ void lazy_write(const Message &msg, Id) { static rclcpp::QoS qos_profile(1); qos_profile.transient_local(); static rclcpp::PublisherOptions pub_options; - static auto pub = - nh->create_publisher(Id::c_str(), qos_profile, pub_options); + static auto pub = nh.lock()->create_publisher( + Id::c_str(), qos_profile, pub_options); pub->publish(msg); } From ac3e03922f8dfa3247d4ff6fc669e757af07a542 Mon Sep 17 00:00:00 2001 From: Johannes Schrimpf Date: Mon, 2 Sep 2024 13:59:42 +0200 Subject: [PATCH 07/12] Use ros2 EventsExecutor --- tyndall/ros_context.h | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/tyndall/ros_context.h b/tyndall/ros_context.h index f07864f..66a1de9 100644 --- a/tyndall/ros_context.h +++ b/tyndall/ros_context.h @@ -58,8 +58,16 @@ init(int argc, char **argv, } static inline void spin() { - // std::lock_guard guard(ros_mutex); - rclcpp::spin(nh.lock()); + rclcpp::experimental::executors::EventsExecutor executor; + + // Add the node to the executor + executor.add_node(nh.lock()); + + // Spin the executor + executor.spin(); + + // Remove the node from the executor after spinning + executor.remove_node(nh.lock()); } template int lazy_read(Message &msg, Id) { From f2c4ae0620a40d8444e3073fd0b603cc15a3e2e0 Mon Sep 17 00:00:00 2001 From: Johannes Schrimpf Date: Mon, 2 Sep 2024 14:05:38 +0200 Subject: [PATCH 08/12] Update ros version in tests --- .github/workflows/tests.yml | 51 ++++++++++++++++++------------------- 1 file changed, 25 insertions(+), 26 deletions(-) diff --git a/.github/workflows/tests.yml b/.github/workflows/tests.yml index e5088e9..82bc431 100644 --- a/.github/workflows/tests.yml +++ b/.github/workflows/tests.yml @@ -2,9 +2,9 @@ name: tests on: push: - branches: [ master ] + branches: [master] pull_request: - branches: [ master ] + branches: [master] env: # Customize the CMake build type here (Release, Debug, RelWithDebInfo, etc.) @@ -15,27 +15,26 @@ jobs: runs-on: ubuntu-latest steps: - - uses: actions/checkout@v2 - - uses: ros-tooling/setup-ros@v0.2 - with: - required-ros-distributions: noetic - - - name: Install Dependendies - run: | - sudo apt-get update - sudo apt-get install gcc-10 g++-10 - sudo apt-get install libboost-all-dev libprotobuf-dev libzmq3-dev libfmt-dev - - - name: Configure CMake - run: cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} - env: - CC: gcc-10 - CXX: g++-10 - - - name: Build - run: cmake --build ${{github.workspace}}/build --config ${{env.BUILD_TYPE}} - - - name: Test - working-directory: ${{github.workspace}}/build - run: make -j$(nproc) && make -j$(nproc) tests - + - uses: actions/checkout@v2 + - uses: ros-tooling/setup-ros@v0.7 + with: + required-ros-distributions: jazzy + + - name: Install Dependendies + run: | + sudo apt-get update + sudo apt-get install gcc-10 g++-10 + sudo apt-get install libboost-all-dev libprotobuf-dev libzmq3-dev libfmt-dev + + - name: Configure CMake + run: cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} + env: + CC: gcc-10 + CXX: g++-10 + + - name: Build + run: cmake --build ${{github.workspace}}/build --config ${{env.BUILD_TYPE}} + + - name: Test + working-directory: ${{github.workspace}}/build + run: make -j$(nproc) && make -j$(nproc) tests From 72dd1d1a7eb61ed0c66958e54222da9f41f886db Mon Sep 17 00:00:00 2001 From: Johannes Schrimpf Date: Mon, 2 Sep 2024 15:25:52 +0200 Subject: [PATCH 09/12] Use Ubuntu 24.04 --- .github/workflows/tests.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/tests.yml b/.github/workflows/tests.yml index 82bc431..5d1dae8 100644 --- a/.github/workflows/tests.yml +++ b/.github/workflows/tests.yml @@ -12,7 +12,7 @@ env: jobs: build: - runs-on: ubuntu-latest + runs-on: ubuntu-24.04 steps: - uses: actions/checkout@v2 From 15d48ec0dc66262e73f307a9fb6160e08dd0856d Mon Sep 17 00:00:00 2001 From: Johannes Schrimpf Date: Mon, 2 Sep 2024 15:45:29 +0200 Subject: [PATCH 10/12] Add messages to CMakeLists.txt --- CMakeLists.txt | 2 ++ 1 file changed, 2 insertions(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index bb59815..edef84f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -23,8 +23,10 @@ find_package(std_srvs) find_package(builtin_interfaces) if("${rclcpp_FOUND}") + message(STATUS "ROS2 found") set(LD_FLAGS "${LD_FLAGS} -lpthread") else() + message(STATUS "ROS2 not found") add_definitions("-DNO_ROS") endif() From cbc68ff006e47e2b0410aa4357820b4d36081181 Mon Sep 17 00:00:00 2001 From: Johannes Schrimpf Date: Mon, 2 Sep 2024 17:15:16 +0200 Subject: [PATCH 11/12] Source environment in github action --- .github/workflows/tests.yml | 8 ++++++-- tests/ros_context/ros_context.cpp | 4 ++-- 2 files changed, 8 insertions(+), 4 deletions(-) diff --git a/.github/workflows/tests.yml b/.github/workflows/tests.yml index 5d1dae8..d4a4736 100644 --- a/.github/workflows/tests.yml +++ b/.github/workflows/tests.yml @@ -27,7 +27,9 @@ jobs: sudo apt-get install libboost-all-dev libprotobuf-dev libzmq3-dev libfmt-dev - name: Configure CMake - run: cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} + run: | + source /opt/ros/jazzy/setup.bash + cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} env: CC: gcc-10 CXX: g++-10 @@ -37,4 +39,6 @@ jobs: - name: Test working-directory: ${{github.workspace}}/build - run: make -j$(nproc) && make -j$(nproc) tests + run: | + source /opt/ros/jazzy/setup.bash + make -j$(nproc) && make -j$(nproc) tests diff --git a/tests/ros_context/ros_context.cpp b/tests/ros_context/ros_context.cpp index c232099..61cd219 100644 --- a/tests/ros_context/ros_context.cpp +++ b/tests/ros_context/ros_context.cpp @@ -4,11 +4,11 @@ #include #include #ifdef NO_ROS -namespace std_msgs { +namespace std_msgs::msg { struct Int32 { int data; }; -} // namespace std_msgs +} // namespace std_msgs::msg #else #include #endif From 4a526ec94deec38e23f7a0be40aef3dc9d5ee6dd Mon Sep 17 00:00:00 2001 From: Johannes Schrimpf Date: Mon, 2 Sep 2024 17:41:01 +0200 Subject: [PATCH 12/12] Add spin thread to test --- tests/ros_context/ros_context.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/tests/ros_context/ros_context.cpp b/tests/ros_context/ros_context.cpp index 61cd219..b22d68e 100644 --- a/tests/ros_context/ros_context.cpp +++ b/tests/ros_context/ros_context.cpp @@ -36,6 +36,9 @@ const i32 ref = []() { int main() { ros_context::init(0, NULL, std::chrono::milliseconds{3}, "test_ros_context"); + // Create a thread to run ros_context::spin + std::thread spin_thread([]() { ros_context::spin(); }); + { { i32 entry = ref; @@ -43,7 +46,7 @@ int main() { } { - i32 entry = {}, tmp; + i32 entry, tmp; ros_context_read(tmp, "/test/standard"); sleep(1); int rc = ros_context_read(entry, "/test/standard"); @@ -51,6 +54,8 @@ int main() { check(entry == ref); } } + rclcpp::shutdown(); + spin_thread.join(); #ifdef NO_ROS ipc_cleanup();