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; +}