Added image_pipeline

Robotic-Decision-Making-Lab · Oct 15, 2024 · cc81b83 · cc81b83
1 parent c6d87ac
commit cc81b83
Show file tree

Hide file tree

Showing 4 changed files with 26 additions and 5 deletions.
diff --git a/barlus_gstreamer_proxy/config/camera_info.yaml b/barlus_gstreamer_proxy/config/camera_info.yaml
@@ -1,6 +1,6 @@
-image_width: 640
-image_height: 480
-camera_name: test_camera
+image_width: 3072
+image_height: 2048
+camera_name: barlus
 camera_matrix:
   rows: 3
   cols: 3

diff --git a/barlus_gstreamer_proxy/launch/gstreamer_proxy.launch.py b/barlus_gstreamer_proxy/launch/gstreamer_proxy.launch.py
@@ -49,12 +49,28 @@ def generate_launch_description():
         extra_arguments=[{"use_intra_process_comms": True}],
     )
 
+    rectify_node = ComposableNode(
+        package="image_proc",
+        plugin="image_proc::RectifyNode",
+        name="rectify_node",
+        namespace=LaunchConfiguration("ns"),
+        remappings=[("image", "image_raw")],
+    )
+
+    debayer_node = ComposableNode(
+        package="image_proc",
+        plugin="image_proc::DebayerNode",
+        namespace=LaunchConfiguration("ns"),
+        name="debayer_node",
+        remappings=[("image_raw", "image_raw")],
+    )
+
     container = ComposableNodeContainer(
         name="barlus_container",
         namespace="",
         package="rclcpp_components",
         executable="component_container",
-        composable_node_descriptions=[gstreamer_proxy_node],
+        composable_node_descriptions=[gstreamer_proxy_node, rectify_node, debayer_node],
     )
 
     return LaunchDescription([declare_parameters_file, declare_ns, container])
diff --git a/barlus_gstreamer_proxy/package.xml b/barlus_gstreamer_proxy/package.xml
@@ -20,6 +20,7 @@
   <depend>cv_bridge</depend>
   <depend>camera_info_manager</depend>
   <depend>image_transport</depend>
+  <depend>image_proc</depend>
 
   <export>
     <build_type>ament_cmake</build_type>

diff --git a/barlus_gstreamer_proxy/src/gstreamer_proxy.cpp b/barlus_gstreamer_proxy/src/gstreamer_proxy.cpp
@@ -144,7 +144,11 @@ auto GStreamerProxy::configure_stream() -> bool
 
       *self->camera_info_ = self->camera_info_manager_->getCameraInfo();
       self->camera_info_->header.stamp = self->now();
-      self->camera_pub_->publish(cv_mat_to_ros_image(image), *self->camera_info_);
+
+      sensor_msgs::msg::Image image_msg = cv_mat_to_ros_image(image);
+      image_msg.header = self->camera_info_->header;
+
+      self->camera_pub_->publish(image_msg, *self->camera_info_);
 
       gst_sample_unref(sample);