Finished Node Composition

CMakeLists.txt

@@ -256,9 +256,8 @@ if(ZED_FOUND AND CUDA_FOUND)
     # set(node_plugins "${node_plugins}mrover::ZedWrapper;$<TARGET_FILE:zed_wrapper>\n")
 
     mrover_add_component(zed perception/zed_wrapper/*.c* perception/zed_wrapper/pch.hpp)
-	# target_compile_options(zed PRIVATE $<$<COMPILE_LANGUAGE:CXX>:-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 $<$<COMPILE_LANGUAGE:CXX>:-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;$<TARGET_FILE:gst_websocket_streamer>\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 ()
 

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()

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<mrover::GstWebsocketStreamer>());
-//     rclcpp::shutdown();
-//     return EXIT_SUCCESS;
-// }

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

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<mrover::GstWebsocketStreamer>());
+    rclcpp::shutdown();
+    return EXIT_SUCCESS;
+}

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])

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<mrover::ImageObjectDetector>();
+    auto stereoOD = std::make_shared<mrover::StereoObjectDetector>();
+
+    rclcpp::executors::SingleThreadedExecutor executor;
+    executor.add_node(imgOD);
+    executor.add_node(stereoOD);
+    executor.spin();
+
+    rclcpp::shutdown();
+    return EXIT_SUCCESS;
+}

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<sensor_msgs::msg::Image>("/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<ParameterWrapper> 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<mrover::ImageObjectDetector>();
-    auto stereoOD = std::make_shared<mrover::StereoObjectDetector>();
-
-    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)

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<sensor_msgs::msg::PointCloud2>::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;
 

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<mrover::ZedWrapper>());
+    rclcpp::shutdown();
+    return 0;
+}

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)
+RCLCPP_COMPONENTS_REGISTER_NODE(mrover::ZedWrapper)

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