From 6b2570d443a3647641a6cd74de360e43957be524 Mon Sep 17 00:00:00 2001 From: Hans-Joachim Krauch Date: Mon, 21 Nov 2022 18:18:47 -0600 Subject: [PATCH] Refactor ROS 2 node as a component (#63) **Public-Facing Changes** Refactor ROS 2 node as a component **Description** Allows to run the ROS2 version both as standalone node or as a component. For the standalone version we simply create a `ComponentManager` and load the foxglove bridge component. I'm not sure if this is best practice, but it has the advantage that it does not additional header files to be included which would require an additional refactoring. Fixes #28 --- CMakeLists.txt | 22 ++++++++-- package.xml | 1 + .../src/ros2_foxglove_bridge.cpp | 41 +++++------------- .../src/ros2_foxglove_bridge_node.cpp | 42 +++++++++++++++++++ 4 files changed, 71 insertions(+), 35 deletions(-) create mode 100644 ros2_foxglove_bridge/src/ros2_foxglove_bridge_node.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 9f32b66..9b9ec89 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -111,13 +111,27 @@ elseif("$ENV{ROS_VERSION}" STREQUAL "2") find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) + find_package(rclcpp_components REQUIRED) - add_executable(foxglove_bridge + add_library(foxglove_bridge_component SHARED ros2_foxglove_bridge/src/message_definition_cache.cpp ros2_foxglove_bridge/src/ros2_foxglove_bridge.cpp ) + target_include_directories(foxglove_bridge_component + PUBLIC + $ + $ + $ + ) + ament_target_dependencies(foxglove_bridge_component rclcpp rclcpp_components) + target_link_libraries(foxglove_bridge_component foxglove_bridge_base) + rclcpp_components_register_nodes(foxglove_bridge_component "foxglove_bridge::FoxgloveBridge") + + add_executable(foxglove_bridge + ros2_foxglove_bridge/src/ros2_foxglove_bridge_node.cpp + ) target_include_directories(foxglove_bridge SYSTEM PRIVATE ${rclcpp_INCLUDE_DIRS}) - target_link_libraries(foxglove_bridge foxglove_bridge_base ${rclcpp_LIBRARIES}) + ament_target_dependencies(foxglove_bridge rclcpp rclcpp_components) else() message(FATAL_ERROR "Could not find ament_cmake") endif() @@ -177,11 +191,11 @@ elseif(ROS_BUILD_TYPE STREQUAL "ament_cmake") install(TARGETS foxglove_bridge DESTINATION lib/${PROJECT_NAME} ) - install(TARGETS foxglove_bridge_base + install(TARGETS foxglove_bridge_base foxglove_bridge_component ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin ) - ament_export_libraries(foxglove_bridge_base) + ament_export_libraries(foxglove_bridge_base foxglove_bridge_component) ament_package() endif() diff --git a/package.xml b/package.xml index e472fd9..222faf7 100644 --- a/package.xml +++ b/package.xml @@ -21,6 +21,7 @@ ament_index_cpp rclcpp + rclcpp_components rostest diff --git a/ros2_foxglove_bridge/src/ros2_foxglove_bridge.cpp b/ros2_foxglove_bridge/src/ros2_foxglove_bridge.cpp index 7893cac..73260b3 100644 --- a/ros2_foxglove_bridge/src/ros2_foxglove_bridge.cpp +++ b/ros2_foxglove_bridge/src/ros2_foxglove_bridge.cpp @@ -15,7 +15,6 @@ constexpr uint16_t DEFAULT_PORT = 8765; constexpr char DEFAULT_ADDRESS[] = "0.0.0.0"; -constexpr int DEFAULT_NUM_THREADS = 0; constexpr size_t DEFAULT_MAX_QOS_DEPTH = 10; using namespace std::chrono_literals; @@ -24,6 +23,8 @@ using LogLevel = foxglove::WebSocketLogLevel; using Subscription = std::pair; using SubscriptionsByClient = std::map>; +namespace foxglove_bridge { + class FoxgloveBridge : public rclcpp::Node { public: using TopicAndDatatype = std::pair; @@ -74,19 +75,6 @@ class FoxgloveBridge : public rclcpp::Node { keyfileDescription.read_only = true; this->declare_parameter("keyfile", "", keyfileDescription); - auto numThreadsDescription = rcl_interfaces::msg::ParameterDescriptor{}; - numThreadsDescription.name = "num_threads"; - numThreadsDescription.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - numThreadsDescription.description = - "The number of threads to use for the ROS node executor. 0 means one thread per CPU core."; - numThreadsDescription.read_only = true; - numThreadsDescription.additional_constraints = "Must be a non-negative integer"; - numThreadsDescription.integer_range.resize(1); - numThreadsDescription.integer_range[0].from_value = 0; - numThreadsDescription.integer_range[0].to_value = INT32_MAX; - numThreadsDescription.integer_range[0].step = 1; - this->declare_parameter("num_threads", DEFAULT_NUM_THREADS, numThreadsDescription); - auto maxQosDepthDescription = rcl_interfaces::msg::ParameterDescriptor{}; maxQosDepthDescription.name = "max_qos_depth"; maxQosDepthDescription.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; @@ -143,13 +131,6 @@ class FoxgloveBridge : public rclcpp::Node { RCLCPP_INFO(this->get_logger(), "Shutdown complete"); } - size_t numThreads() { - int numThreads; - rclcpp::spin_some(this->get_node_base_interface()); - this->get_parameter_or("num_threads", numThreads, DEFAULT_NUM_THREADS); - return size_t(numThreads); - } - void rosgraphPollThread() { updateAdvertisedTopics(); @@ -504,13 +485,11 @@ class FoxgloveBridge : public rclcpp::Node { } }; -int main(int argc, char* argv[]) { - rclcpp::init(argc, argv); - auto bridgeNode = std::make_shared(); - rclcpp::executors::MultiThreadedExecutor executor{rclcpp::ExecutorOptions{}, - bridgeNode->numThreads()}; - executor.add_node(bridgeNode); - executor.spin(); - rclcpp::shutdown(); - return 0; -} +} // namespace foxglove_bridge + +#include + +// Register the component with class_loader. +// This acts as a sort of entry point, allowing the component to be discoverable when its library +// is being loaded into a running process. +RCLCPP_COMPONENTS_REGISTER_NODE(foxglove_bridge::FoxgloveBridge) diff --git a/ros2_foxglove_bridge/src/ros2_foxglove_bridge_node.cpp b/ros2_foxglove_bridge/src/ros2_foxglove_bridge_node.cpp new file mode 100644 index 0000000..d62d63c --- /dev/null +++ b/ros2_foxglove_bridge/src/ros2_foxglove_bridge_node.cpp @@ -0,0 +1,42 @@ +#include + +int main(int argc, char* argv[]) { + rclcpp::init(argc, argv); + + auto dummyNode = std::make_shared("dummy"); + auto numThreadsDescription = rcl_interfaces::msg::ParameterDescriptor{}; + numThreadsDescription.name = "num_threads"; + numThreadsDescription.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; + numThreadsDescription.description = + "The number of threads to use for the ROS node executor. 0 means one thread per CPU core."; + numThreadsDescription.read_only = true; + numThreadsDescription.additional_constraints = "Must be a non-negative integer"; + numThreadsDescription.integer_range.resize(1); + numThreadsDescription.integer_range[0].from_value = 0; + numThreadsDescription.integer_range[0].to_value = INT32_MAX; + numThreadsDescription.integer_range[0].step = 1; + constexpr int DEFAULT_NUM_THREADS = 0; + dummyNode->declare_parameter("num_threads", DEFAULT_NUM_THREADS, numThreadsDescription); + + const auto numThreads = dummyNode->get_parameter("num_threads").as_int(); + + auto executor = + rclcpp::executors::MultiThreadedExecutor::make_shared(rclcpp::ExecutorOptions{}, numThreads); + + rclcpp_components::ComponentManager componentManager(executor, "ComponentManager"); + const auto componentResources = componentManager.get_component_resources("foxglove_bridge"); + RCLCPP_INFO(componentManager.get_logger(), std::to_string(numThreads).c_str()); + + if (componentResources.empty()) { + RCLCPP_INFO(componentManager.get_logger(), "No loadable resources found"); + return EXIT_FAILURE; + } + + auto componentFactory = componentManager.create_component_factory(componentResources.front()); + auto node = componentFactory->create_node_instance(rclcpp::NodeOptions()); + + executor->spin(); + rclcpp::shutdown(); + + return EXIT_SUCCESS; +}