From a6ceabc2efe9bbf9b2bc706123f54f34c2b81c64 Mon Sep 17 00:00:00 2001 From: jbrhm Date: Wed, 16 Oct 2024 23:34:30 -0400 Subject: [PATCH] Finished Node Composition --- CMakeLists.txt | 20 +++++------- cmake/macros.cmake | 31 ++++++++++++++----- .../gst_websocket_streamer.cpp | 8 ----- .../gst_websocket_streamer.hpp | 2 +- esw/gst_websocket_streamer/main.cpp | 8 +++++ launch/perception.launch.py | 28 +++++++++++++++-- perception/object_detector/main.cpp | 17 ++++++++++ .../object_detector/object_detector.cpp | 23 ++++---------- .../object_detector/object_detector.hpp | 6 ++-- perception/zed_wrapper/main.cpp | 8 +++++ perception/zed_wrapper/zed_wrapper.cpp | 4 +-- perception/zed_wrapper/zed_wrapper.hpp | 2 +- 12 files changed, 103 insertions(+), 54 deletions(-) create mode 100644 esw/gst_websocket_streamer/main.cpp create mode 100644 perception/object_detector/main.cpp create mode 100644 perception/zed_wrapper/main.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index df798efd..82318f15 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -256,9 +256,8 @@ if(ZED_FOUND AND CUDA_FOUND) # set(node_plugins "${node_plugins}mrover::ZedWrapper;$\n") mrover_add_component(zed perception/zed_wrapper/*.c* perception/zed_wrapper/pch.hpp) - # target_compile_options(zed PRIVATE $<$:-std=c++20>) - target_link_libraries(zed parameter_utils lie MANIF::manif ${CUDA_LIBRARIES} loop_profiler cuda_compiler_flags) - ament_target_dependencies(zed rclcpp rclcpp_components sensor_msgs ZED CUDA tf2 tf2_ros) + mrover_link_component(zed parameter_utils lie MANIF::manif ${CUDA_LIBRARIES} loop_profiler cuda_compiler_flags) + mrover_ament_component(zed rclcpp rclcpp_components sensor_msgs ZED CUDA tf2 tf2_ros) # Learning Library # TODO(john): Update to use the new API @@ -268,9 +267,10 @@ if(ZED_FOUND AND CUDA_FOUND) target_link_libraries(tensorrt PRIVATE opencv_core opencv_dnn opencv_imgproc lie nvinfer nvonnxparser tbb cuda_compiler_flags) # Object Detector - mrover_add_node(object_detector perception/object_detector/*.c* src/perception/object_detector/pch.hpp) - target_link_libraries(object_detector opencv_core opencv_dnn opencv_imgproc lie tbb tensorrt opencv_imgcodecs opencv_highgui loop_profiler parameter_utils cuda_compiler_flags) - ament_target_dependencies(object_detector rclcpp sensor_msgs CUDA tf2 tf2_ros) + mrover_add_component(object_detector perception/object_detector/*.c* src/perception/object_detector/pch.hpp) + target_compile_options(zed PRIVATE $<$:-std=c++20>) + mrover_link_component(object_detector opencv_core opencv_dnn opencv_imgproc lie tbb tensorrt opencv_imgcodecs opencv_highgui loop_profiler parameter_utils cuda_compiler_flags) + mrover_ament_component(object_detector rclcpp sensor_msgs rclcpp_components CUDA tf2 tf2_ros) else() message("ZED not found...") endif() @@ -300,12 +300,8 @@ if (Gst_FOUND AND GstApp_FOUND) ament_target_dependencies(websocket_server rclcpp) mrover_add_component(gst_websocket_streamer esw/gst_websocket_streamer/*.c* esw/gst_websocket_streamer/pch.hpp) - target_link_libraries(gst_websocket_streamer PkgConfig::Gst PkgConfig::GstApp PkgConfig::LibUdev opencv_core opencv_imgcodecs websocket_server) - ament_target_dependencies(gst_websocket_streamer rclcpp rclcpp_components sensor_msgs) - - rclcpp_components_register_nodes(gst_websocket_streamer "mrover::GstWebsocketStreamer") - set(node_plugins "${node_plugins}mrover::GstWebsocketStreamer;$\n") - + mrover_link_component(gst_websocket_streamer PkgConfig::Gst PkgConfig::GstApp PkgConfig::LibUdev opencv_core opencv_imgcodecs websocket_server) + mrover_ament_component(gst_websocket_streamer rclcpp rclcpp_components sensor_msgs) endif () endif () diff --git a/cmake/macros.cmake b/cmake/macros.cmake index 2c63afee..78f77379 100644 --- a/cmake/macros.cmake +++ b/cmake/macros.cmake @@ -27,14 +27,6 @@ macro(mrover_add_library name sources includes) endif () endmacro() -macro(mrover_add_component name sources includes) - file(GLOB_RECURSE LIBRARY_SOURCES CONFIGURE_DEPENDS ${sources}) - add_library(${name} SHARED ${ARGV3} ${LIBRARY_SOURCES}) - rclcpp_components_register_nodes(${name} "mrover::${name}") - mrover_target(${name}) - target_compile_definitions(${name} PRIVATE "COMPOSITION_BUILDING_DLL") -endmacro() - macro(mrover_add_node name sources) file(GLOB_RECURSE NODE_SOURCES CONFIGURE_DEPENDS ${sources}) add_executable(${name} ${NODE_SOURCES}) @@ -52,3 +44,26 @@ macro(mrover_add_node name sources) target_link_libraries(${name} /opt/homebrew/Caskroom/miniforge/base/envs/ros2_env/lib/libpython3.11.dylib) endif () endmacro() + +macro(mrover_add_component name sources includes) + # Create Executable + mrover_add_node(${name} ${sources}) + rosidl_target_interfaces(${name} ${PROJECT_NAME} "rosidl_typesupport_cpp") + + # Create Composition Library + set(component_name ${name}_component) + mrover_add_library(${component_name} ${sources} ${includes} SHARED) + rosidl_target_interfaces(${name}_component ${PROJECT_NAME} "rosidl_typesupport_cpp") + rclcpp_components_register_nodes(${component_name} "mrover::${component_name}") + target_compile_definitions(${component_name} PRIVATE "COMPOSITION_BUILDING_DLL") +endmacro() + +macro(mrover_link_component name) + target_link_libraries(${name} ${ARGN}) + target_link_libraries(${name}_component ${ARGN}) +endmacro() + +macro(mrover_ament_component name) + ament_target_dependencies(${name} ${ARGN}) + ament_target_dependencies(${name}_component ${ARGN}) +endmacro() diff --git a/esw/gst_websocket_streamer/gst_websocket_streamer.cpp b/esw/gst_websocket_streamer/gst_websocket_streamer.cpp index 6fb764fe..3feb2fc8 100644 --- a/esw/gst_websocket_streamer/gst_websocket_streamer.cpp +++ b/esw/gst_websocket_streamer/gst_websocket_streamer.cpp @@ -360,11 +360,3 @@ namespace mrover { // 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(mrover::GstWebsocketStreamer) - - -// auto main(int argc, char** argv) -> int { -// rclcpp::init(argc, argv); -// rclcpp::spin(std::make_shared()); -// rclcpp::shutdown(); -// return EXIT_SUCCESS; -// } \ No newline at end of file diff --git a/esw/gst_websocket_streamer/gst_websocket_streamer.hpp b/esw/gst_websocket_streamer/gst_websocket_streamer.hpp index 5796d940..3e4443ce 100644 --- a/esw/gst_websocket_streamer/gst_websocket_streamer.hpp +++ b/esw/gst_websocket_streamer/gst_websocket_streamer.hpp @@ -49,7 +49,7 @@ namespace mrover { public: // __attribute__ ((visibility("default"))) - explicit GstWebsocketStreamer(rclcpp::NodeOptions const& options); + explicit GstWebsocketStreamer(rclcpp::NodeOptions const& options = rclcpp::NodeOptions()); ~GstWebsocketStreamer() override; }; diff --git a/esw/gst_websocket_streamer/main.cpp b/esw/gst_websocket_streamer/main.cpp new file mode 100644 index 00000000..7cc13e3c --- /dev/null +++ b/esw/gst_websocket_streamer/main.cpp @@ -0,0 +1,8 @@ +#include "gst_websocket_streamer.hpp" + +auto main(int argc, char** argv) -> int { + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return EXIT_SUCCESS; +} diff --git a/launch/perception.launch.py b/launch/perception.launch.py index 81f87e9c..62a23359 100644 --- a/launch/perception.launch.py +++ b/launch/perception.launch.py @@ -2,6 +2,7 @@ from ament_index_python import get_package_share_directory +import launch from launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.substitutions import LaunchConfiguration @@ -11,18 +12,41 @@ def generate_launch_description(): + + container = ComposableNodeContainer( + name='perception', + namespace='', + package='mrover', + executable='component_container', + composable_node_descriptions=[ + ComposableNode( + package='mrover', + plugin='mrover::ObjectDetector', + name='object_detector', + parameters=[Path(get_package_share_directory("mrover"), "config", "object_detector.yaml")], + ), + ComposableNode( + package='mrover', + plugin='mrover::ZedWrapper', + name='zed_wrapper', + parameters=[Path(get_package_share_directory("mrover"), "config", "zed.yaml")], + ) + ], + output='screen', + ) + + return launch.LaunchDescription([container]) zed_node = Node( package="mrover", executable="zed", name="zed_wrapper", - parameters=[Path(get_package_share_directory("mrover"), "config", "zed.yaml")], + ) object_detector_node = Node( package="mrover", executable="object_detector", name="object_detector", - parameters=[Path(get_package_share_directory("mrover"), "config", "object_detector.yaml")], ) return LaunchDescription([zed_node, object_detector_node]) diff --git a/perception/object_detector/main.cpp b/perception/object_detector/main.cpp new file mode 100644 index 00000000..f4412f22 --- /dev/null +++ b/perception/object_detector/main.cpp @@ -0,0 +1,17 @@ +#include "object_detector.hpp" + +auto main(int argc, char** argv) -> int { + rclcpp::init(argc, argv); + + // DO NOT REMOVE OR ELSE REF COUNT WILL GO TO ZERO + auto imgOD = std::make_shared(); + auto stereoOD = std::make_shared(); + + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(imgOD); + executor.add_node(stereoOD); + executor.spin(); + + rclcpp::shutdown(); + return EXIT_SUCCESS; +} diff --git a/perception/object_detector/object_detector.cpp b/perception/object_detector/object_detector.cpp index 8ed72b0d..37135a7c 100644 --- a/perception/object_detector/object_detector.cpp +++ b/perception/object_detector/object_detector.cpp @@ -2,7 +2,7 @@ namespace mrover { - ObjectDetectorBase::ObjectDetectorBase() : rclcpp::Node(NODE_NAME), mLoopProfiler{get_logger()} { + ObjectDetectorBase::ObjectDetectorBase(rclcpp::NodeOptions const& options) : rclcpp::Node(NODE_NAME, options), mLoopProfiler{get_logger()} { std::string modelName; float modelScoreThreshold{}; float modelNMSThreshold{}; @@ -36,7 +36,7 @@ namespace mrover { RCLCPP_INFO_STREAM(get_logger(), std::format("Object detector initialized with model: {} and thresholds: {} and {}", mModel.modelName, modelScoreThreshold, modelNMSThreshold)); } - StereoObjectDetector::StereoObjectDetector() { + StereoObjectDetector::StereoObjectDetector(rclcpp::NodeOptions const& options) : ObjectDetectorBase(options) { RCLCPP_INFO_STREAM(get_logger(), "Creating Stereo Object Detector..."); mDebugImgPub = create_publisher("/stereo_object_detector/debug_img", 1); @@ -46,7 +46,7 @@ namespace mrover { }); } - ImageObjectDetector::ImageObjectDetector() { + ImageObjectDetector::ImageObjectDetector(rclcpp::NodeOptions const& options) : ObjectDetectorBase(options) { RCLCPP_INFO_STREAM(get_logger(), "Creating Image Object Detector..."); std::vector params{ @@ -65,18 +65,7 @@ namespace mrover { } // namespace mrover -auto main(int argc, char** argv) -> int { - rclcpp::init(argc, argv); - // DO NOT REMOVE OR ELSE REF COUNT WILL GO TO ZERO - auto imgOD = std::make_shared(); - auto stereoOD = std::make_shared(); - - rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(imgOD); - executor.add_node(stereoOD); - executor.spin(); - - rclcpp::shutdown(); - return EXIT_SUCCESS; -} +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(mrover::StereoObjectDetector) +RCLCPP_COMPONENTS_REGISTER_NODE(mrover::ImageObjectDetector) diff --git a/perception/object_detector/object_detector.hpp b/perception/object_detector/object_detector.hpp index 1e266d74..ae0beda3 100644 --- a/perception/object_detector/object_detector.hpp +++ b/perception/object_detector/object_detector.hpp @@ -77,7 +77,7 @@ namespace mrover { auto static preprocessYOLOv8Input(Model const& model, cv::Mat& rgbImage, cv::Mat& blobSizedImage, cv::Mat& blob) -> void; public: - explicit ObjectDetectorBase(); + explicit ObjectDetectorBase(rclcpp::NodeOptions const& options = rclcpp::NodeOptions()); ~ObjectDetectorBase() override = default; }; @@ -87,7 +87,7 @@ namespace mrover { rclcpp::Subscription::SharedPtr mSensorSub; public: - explicit StereoObjectDetector(); + explicit StereoObjectDetector(rclcpp::NodeOptions const& options = rclcpp::NodeOptions()); static auto convertPointCloudToRGB(sensor_msgs::msg::PointCloud2::UniquePtr const& msg, cv::Mat const& image) -> void; @@ -103,7 +103,7 @@ namespace mrover { float mCameraHorizontalFov{}; public: - explicit ImageObjectDetector(); + explicit ImageObjectDetector(rclcpp::NodeOptions const& options = rclcpp::NodeOptions()); auto getTagBearing(cv::InputArray image, cv::Rect const& box) const -> float; diff --git a/perception/zed_wrapper/main.cpp b/perception/zed_wrapper/main.cpp new file mode 100644 index 00000000..04c16440 --- /dev/null +++ b/perception/zed_wrapper/main.cpp @@ -0,0 +1,8 @@ +#include "zed_wrapper.hpp" + +auto main(int argc, char* argv[]) -> int { + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/perception/zed_wrapper/zed_wrapper.cpp b/perception/zed_wrapper/zed_wrapper.cpp index a5060dde..f82e4c50 100644 --- a/perception/zed_wrapper/zed_wrapper.cpp +++ b/perception/zed_wrapper/zed_wrapper.cpp @@ -13,7 +13,7 @@ namespace mrover { } - ZedWrapper::ZedWrapper() : Node(NODE_NAME, rclcpp::NodeOptions().use_intra_process_comms(true)), mLoopProfilerGrab{get_logger()}, mLoopProfilerUpdate{get_logger()} { + ZedWrapper::ZedWrapper(rclcpp::NodeOptions const& options) : Node(NODE_NAME, options), mLoopProfilerGrab{get_logger()}, mLoopProfilerUpdate{get_logger()} { try { RCLCPP_INFO(this->get_logger(), "Created Zed Wrapper Node, %s", NODE_NAME); @@ -316,4 +316,4 @@ namespace mrover { }; // namespace mrover #include "rclcpp_components/register_node_macro.hpp" -RCLCPP_COMPONENTS_REGISTER_NODE(mrover::ZedWrapper) \ No newline at end of file +RCLCPP_COMPONENTS_REGISTER_NODE(mrover::ZedWrapper) diff --git a/perception/zed_wrapper/zed_wrapper.hpp b/perception/zed_wrapper/zed_wrapper.hpp index f9b1f6d0..47fbb672 100644 --- a/perception/zed_wrapper/zed_wrapper.hpp +++ b/perception/zed_wrapper/zed_wrapper.hpp @@ -81,7 +81,7 @@ namespace mrover { auto pointCloudUpdateThread() -> void; public: - ZedWrapper(); + explicit ZedWrapper(rclcpp::NodeOptions const& options = rclcpp::NodeOptions()); ~ZedWrapper() override; };