From 85353d06b2e3ec07f355183a864e89cc66c38404 Mon Sep 17 00:00:00 2001
From: Pimpest <82343504+pimpest@users.noreply.github.com>
Date: Fri, 10 Apr 2026 20:02:35 +0200
Subject: [PATCH] Made toid_vision into composable node
---
toid_vision/CMakeLists.txt | 39 ++++++++++++++++-
toid_vision/config/camera_info.yaml | 27 ++++++++++++
.../include/toid_vision/toid_vision.hpp | 2 +-
toid_vision/launch/launch.py | 43 +++++++++++++++++++
toid_vision/package.xml | 12 +++++-
toid_vision/src/toid_vision.cpp | 7 ++-
6 files changed, 124 insertions(+), 6 deletions(-)
create mode 100644 toid_vision/config/camera_info.yaml
create mode 100644 toid_vision/launch/launch.py
diff --git a/toid_vision/CMakeLists.txt b/toid_vision/CMakeLists.txt
index c0e59f6..d09cf28 100644
--- a/toid_vision/CMakeLists.txt
+++ b/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
diff --git a/toid_vision/config/camera_info.yaml b/toid_vision/config/camera_info.yaml
new file mode 100644
index 0000000..bc031db
--- /dev/null
+++ b/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]
\ No newline at end of file
diff --git a/toid_vision/include/toid_vision/toid_vision.hpp b/toid_vision/include/toid_vision/toid_vision.hpp
index 85072b4..ade3112 100644
--- a/toid_vision/include/toid_vision/toid_vision.hpp
+++ b/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(
diff --git a/toid_vision/launch/launch.py b/toid_vision/launch/launch.py
new file mode 100644
index 0000000..843c0a7
--- /dev/null
+++ b/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])
\ No newline at end of file
diff --git a/toid_vision/package.xml b/toid_vision/package.xml
index 4ef3ce6..4893019 100644
--- a/toid_vision/package.xml
+++ b/toid_vision/package.xml
@@ -9,9 +9,17 @@
ament_cmake
- ament_lint_auto
- ament_lint_common
+ rclcpp
+ sensor_msgs
+ message_filters
+ cv_bridge
+ opencv
+ rclcpp_components
+ tf2
+ tf2_ros
+ tf2_geometry_msgs
+
ament_cmake
diff --git a/toid_vision/src/toid_vision.cpp b/toid_vision/src/toid_vision.cpp
index 398faee..5092944 100644
--- a/toid_vision/src/toid_vision.cpp
+++ b/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
\ No newline at end of file
+} // namespace toid
+
+#include "rclcpp_components/register_node_macro.hpp"
+RCLCPP_COMPONENTS_REGISTER_NODE(toid::NutDetector)
\ No newline at end of file