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