toid_vision/CMakeLists.txt

@@ -14,7 +14,7 @@ set(PACKAGE_DEPS
   message_filters
   cv_bridge
   OpenCV
-  
+  rclcpp_components
   tf2
   tf2_ros
   tf2_geometry_msgs
@@ -38,6 +38,10 @@ set(SOURCES
     src/vision.cpp
 )
 
+set(SOURCES_COMPOSABLE
+    src/toid_vision.cpp
+)
+
 add_executable(toid_vision ${SOURCES})
 
 target_include_directories(
@@ -50,6 +54,39 @@ ament_target_dependencies(toid_vision ${PACKAGE_DEPS})
 
 install(TARGETS toid_vision DESTINATION lib/${PROJECT_NAME})
 
+# TOID_VISION COMPOSABLE NODE
+
+add_library(toid_vision_component SHARED ${SOURCES_COMPOSABLE})
+
+rclcpp_components_register_node(
+    toid_vision_component
+    PLUGIN "toid::NutDetector"
+    EXECUTABLE nut_detector 
+)
+
+target_include_directories(
+  toid_vision_component
+  PRIVATE
+  include
+)
+
+ament_target_dependencies(toid_vision_component ${PACKAGE_DEPS})
+
+ament_export_targets(export_toid_vision_component)
+install(TARGETS toid_vision_component
+        EXPORT export_toid_vision_component
+        ARCHIVE DESTINATION lib
+        LIBRARY DESTINATION lib
+        RUNTIME DESTINATION bin
+)
+
+install(
+  DIRECTORY 
+    launch 
+    config
+  DESTINATION share/${PROJECT_NAME}/
+)
+
 target_compile_features(
   toid_vision PUBLIC
   c_std_99

toid_vision/config/camera_info.yaml

@@ -0,0 +1,27 @@
+image_width: 1640
+image_height: 1232
+camera_name: imx219__base_axi_pcie_1000120000_rp1_i2c_88000_imx219_10_1640x1232
+camera_matrix:
+  rows: 3
+  cols: 3
+  data: [1281.9678810697528, 0.0, 792.9357401565763,
+         0.0, 1283.1760537785433, 615.1915708814056,
+         0.0, 0.0, 1.0]
+distortion_model: plumb_bob
+distortion_coefficients:
+  rows: 1
+  cols: 5
+  data: [0.19247275241900028, -0.5098314224324741, 6.710711247494001e-05,
+         -0.00018449989636263572, 0.40356075592228546]
+rectification_matrix:
+  rows: 3
+  cols: 3
+  data: [1.0, 0.0, 0.0,
+         0.0, 1.0, 0.0,
+         0.0, 0.0, 1.0]
+projection_matrix:
+  rows: 3
+  cols: 4
+  data: [1281.9678810697528, 0.0, 792.9357401565763, 0.0,
+         0.0, 1283.1760537785433, 615.1915708814056, 0.0,
+         0.0, 0.0, 1.0, 0.0]

toid_vision/include/toid_vision/toid_vision.hpp

@@ -27,7 +27,7 @@ class NutDetector : public rclcpp::Node
     sensor_msgs::msg::CompressedImage, sensor_msgs::msg::CameraInfo>;
 
 public:
-  NutDetector(rclcpp::NodeOptions &options);
+  NutDetector(const rclcpp::NodeOptions &options);
 
 public:
   void compressed_topic_callback(

toid_vision/launch/launch.py

@@ -0,0 +1,43 @@
+import launch
+from launch_ros.actions import ComposableNodeContainer
+from launch_ros.descriptions import ComposableNode
+from launch_ros.substitutions import FindPackageShare
+import os
+
+def generate_launch_description():
+    """Generate launch description with multiple components."""
+
+    vision_share = FindPackageShare("").find("toid_vision")
+    camera_info = os.path.join(vision_share, 'config/camera_info.yaml')
+
+
+    container = ComposableNodeContainer(
+            name='vision_container',
+            namespace='',
+            package='rclcpp_components',
+            executable='component_container_mt',
+            composable_node_descriptions=[
+                ComposableNode(
+                    package='toid_vision',
+                    plugin='toid::NutDetector',
+                    name='nut_detector',
+                    parameters= [{
+                        'is_blue': True,
+                    }]),
+                ComposableNode(
+                    package='camera_ros',
+                    plugin='camera::CameraNode',
+                    name='camera_driver',
+                    parameters= [{
+                        'width': 1640,
+                        'height': 1242,
+                        'orientation': 180,
+                        'camera': 0,
+                        'frame_id': 'camera',
+                        'camera_info_url': "file://" + camera_info
+                    }])
+            ],
+            output='screen',
+    )
+
+    return launch.LaunchDescription([container])

toid_vision/package.xml

@@ -9,9 +9,17 @@
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
-  <test_depend>ament_lint_auto</test_depend>
+  <depend>rclcpp</depend>
-  <test_depend>ament_lint_common</test_depend>
+  <depend>sensor_msgs</depend>
+  <depend>message_filters</depend>
+  <depend>cv_bridge</depend>
+  <depend>opencv</depend>
+  <depend>rclcpp_components</depend>
 
+  <depend>tf2</depend>
+  <depend>tf2_ros</depend>
+  <depend>tf2_geometry_msgs</depend>
+  
   <export>
     <build_type>ament_cmake</build_type>
   </export>

toid_vision/src/toid_vision.cpp

@@ -6,7 +6,7 @@
 namespace toid
 {
 
-NutDetector::NutDetector(rclcpp::NodeOptions & options)
+NutDetector::NutDetector(const rclcpp::NodeOptions & options)
 : Node("compressed_image_subscriber", options)
 {
   detectorParams_ = cv::aruco::DetectorParameters::create();
@@ -174,4 +174,7 @@ void NutDetector::get_pose(cv::Vec3d & tvec, cv::Vec3d & rvec, geometry_msgs::ms
   out_pose.position.z = tvec[2];
 }
 
-}  // namespace toid
+}  // namespace toid
+
+#include "rclcpp_components/register_node_macro.hpp"
+RCLCPP_COMPONENTS_REGISTER_NODE(toid::NutDetector)