trying this
This commit is contained in:
@@ -1,13 +1,13 @@
|
||||
import os
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
from launch import LaunchDescription
|
||||
from launch_ros.actions import Node
|
||||
from launch_ros.actions import ComposableNodeContainer
|
||||
from launch_ros.descriptions import ComposableNode
|
||||
|
||||
#from perspective of cameras
|
||||
# From perspective of cameras
|
||||
LEFT_CAMERA_NAME = 'left'
|
||||
RIGHT_CAMERA_NAME = 'right'
|
||||
LEFT_CAMERA_NAMESPACE = 'stereo'
|
||||
RIGHT_CAMERA_NAMESPACE = 'stereo'
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
config_path = os.path.join(
|
||||
@@ -16,57 +16,175 @@ def generate_launch_description():
|
||||
'blackfly_s.yaml'
|
||||
)
|
||||
|
||||
# ── Container 1: Camera Drivers (component_container_mt) ─────────────
|
||||
#
|
||||
# Both cameras share a single process because the FLIR Spinnaker SDK
|
||||
# requires USB3 cameras to negotiate bandwidth within one process.
|
||||
# Using _mt so both capture callbacks can fire on separate threads.
|
||||
#
|
||||
# Topics published (per camera):
|
||||
# /stereo/{left,right}/image_raw (sensor_msgs/Image)
|
||||
# /stereo/{left,right}/camera_info (sensor_msgs/CameraInfo)
|
||||
camera_container = ComposableNodeContainer(
|
||||
name='camera_container',
|
||||
namespace='',
|
||||
package='rclcpp_components',
|
||||
executable='component_container_mt',
|
||||
composable_node_descriptions=[
|
||||
# ── Left Blackfly S (serial 25282106) ──
|
||||
# namespace='stereo/left' → publishes /stereo/left/image_raw
|
||||
# /stereo/left/camera_info
|
||||
ComposableNode(
|
||||
package='spinnaker_camera_driver',
|
||||
plugin='spinnaker_camera_driver::CameraDriver',
|
||||
name=LEFT_CAMERA_NAME,
|
||||
namespace='stereo/left',
|
||||
parameters=[{
|
||||
'parameter_file': config_path,
|
||||
'serial_number': '25282106',
|
||||
'camera_info_url': 'file:///home/sentry/camera_ws/stereoCal/left.yaml',
|
||||
}],
|
||||
extra_arguments=[{'use_intra_process_comms': True}],
|
||||
),
|
||||
# ── Right Blackfly S (serial 25235293) ──
|
||||
# namespace='stereo/right' → publishes /stereo/right/image_raw
|
||||
# /stereo/right/camera_info
|
||||
ComposableNode(
|
||||
package='spinnaker_camera_driver',
|
||||
plugin='spinnaker_camera_driver::CameraDriver',
|
||||
name=RIGHT_CAMERA_NAME,
|
||||
namespace='stereo/right',
|
||||
parameters=[{
|
||||
'parameter_file': config_path,
|
||||
'serial_number': '25235293',
|
||||
'camera_info_url': 'file:///home/sentry/camera_ws/stereoCal/right.yaml',
|
||||
}],
|
||||
extra_arguments=[{'use_intra_process_comms': True}],
|
||||
),
|
||||
],
|
||||
output='screen',
|
||||
)
|
||||
|
||||
# ── Container 2: Image Processing (component_container_mt) ───────────
|
||||
#
|
||||
# Debayer + Rectify for both cameras. Grouped together so that
|
||||
# debayer→rectify is zero-copy (intra-process) per side, while _mt
|
||||
# lets left and right pipelines run concurrently on different threads.
|
||||
#
|
||||
# Per-side data flow:
|
||||
# image_raw → DebayerNode → image_mono → RectifyNode → image_rect
|
||||
# ↘ image_color (available for other consumers)
|
||||
image_proc_container = ComposableNodeContainer(
|
||||
name='image_proc_container',
|
||||
namespace='',
|
||||
package='rclcpp_components',
|
||||
executable='component_container_mt',
|
||||
composable_node_descriptions=[
|
||||
# ── Left Debayer ──
|
||||
# Subscribes: /stereo/left/image_raw (default topic, matches camera)
|
||||
# Publishes: /stereo/left/image_mono (grayscale debayered)
|
||||
# /stereo/left/image_color (color debayered)
|
||||
ComposableNode(
|
||||
package='image_proc',
|
||||
plugin='image_proc::DebayerNode',
|
||||
name='debayer',
|
||||
namespace='stereo/left',
|
||||
extra_arguments=[{'use_intra_process_comms': True}],
|
||||
),
|
||||
# ── Left Rectify ──
|
||||
# Remap: 'image' → 'image_mono' so it subscribes to debayer output.
|
||||
# image_transport auto-derives camera_info from the image topic's
|
||||
# namespace: /stereo/left/image_mono → /stereo/left/camera_info ✓
|
||||
# Publishes: /stereo/left/image_rect
|
||||
ComposableNode(
|
||||
package='image_proc',
|
||||
plugin='image_proc::RectifyNode',
|
||||
name='rectify_mono',
|
||||
namespace='stereo/left',
|
||||
remappings=[('image', 'image_mono')],
|
||||
extra_arguments=[{'use_intra_process_comms': True}],
|
||||
),
|
||||
# ── Right Debayer ──
|
||||
# Subscribes: /stereo/right/image_raw
|
||||
# Publishes: /stereo/right/image_mono, /stereo/right/image_color
|
||||
ComposableNode(
|
||||
package='image_proc',
|
||||
plugin='image_proc::DebayerNode',
|
||||
name='debayer',
|
||||
namespace='stereo/right',
|
||||
extra_arguments=[{'use_intra_process_comms': True}],
|
||||
),
|
||||
# ── Right Rectify ──
|
||||
# Remap: 'image' → 'image_mono'
|
||||
# Subscribes: /stereo/right/image_mono + /stereo/right/camera_info
|
||||
# Publishes: /stereo/right/image_rect
|
||||
ComposableNode(
|
||||
package='image_proc',
|
||||
plugin='image_proc::RectifyNode',
|
||||
name='rectify_mono',
|
||||
namespace='stereo/right',
|
||||
remappings=[('image', 'image_mono')],
|
||||
extra_arguments=[{'use_intra_process_comms': True}],
|
||||
),
|
||||
],
|
||||
output='screen',
|
||||
)
|
||||
|
||||
# ── Container 3: Stereo Processing (component_container_mt) ──────────
|
||||
#
|
||||
# DisparityNode + PointCloudNode isolated from image processing so the
|
||||
# heavy stereo matching doesn't starve debayer/rectify callbacks.
|
||||
# Zero-copy between disparity → point_cloud within this container.
|
||||
#
|
||||
# Data flow:
|
||||
# left/image_rect + right/image_rect → DisparityNode → disparity
|
||||
# disparity + left/image_rect → PointCloudNode → points2
|
||||
stereo_proc_container = ComposableNodeContainer(
|
||||
name='stereo_proc_container',
|
||||
namespace='',
|
||||
package='rclcpp_components',
|
||||
executable='component_container_mt',
|
||||
composable_node_descriptions=[
|
||||
# ── Disparity ──
|
||||
# Subscribes: /stereo/left/image_rect (from left RectifyNode)
|
||||
# /stereo/left/camera_info (from left CameraDriver)
|
||||
# /stereo/right/image_rect (from right RectifyNode)
|
||||
# /stereo/right/camera_info (from right CameraDriver)
|
||||
# Publishes: /stereo/disparity (stereo_msgs/DisparityImage)
|
||||
ComposableNode(
|
||||
package='stereo_image_proc',
|
||||
plugin='stereo_image_proc::DisparityNode',
|
||||
name='disparity_node',
|
||||
namespace='stereo',
|
||||
parameters=[{'approximate_sync': True}],
|
||||
extra_arguments=[{'use_intra_process_comms': True}],
|
||||
),
|
||||
# ── Point Cloud ──
|
||||
# Remap: left/image_rect_color → left/image_rect because
|
||||
# RectifyNode publishes 'image_rect', not 'image_rect_color'.
|
||||
# Subscribes: /stereo/left/image_rect (remapped from left/image_rect_color)
|
||||
# /stereo/left/camera_info
|
||||
# /stereo/right/camera_info
|
||||
# /stereo/disparity (from DisparityNode — zero-copy)
|
||||
# Publishes: /stereo/points2 (sensor_msgs/PointCloud2)
|
||||
ComposableNode(
|
||||
package='stereo_image_proc',
|
||||
plugin='stereo_image_proc::PointCloudNode',
|
||||
name='point_cloud_node',
|
||||
namespace='stereo',
|
||||
parameters=[{'approximate_sync': True}],
|
||||
remappings=[
|
||||
('left/image_rect_color', 'left/image_rect'),
|
||||
('right/image_rect_color', 'right/image_rect'),
|
||||
],
|
||||
extra_arguments=[{'use_intra_process_comms': True}],
|
||||
),
|
||||
],
|
||||
output='screen',
|
||||
)
|
||||
|
||||
return LaunchDescription([
|
||||
Node(
|
||||
package='spinnaker_camera_driver',
|
||||
executable='camera_driver_node',
|
||||
name=LEFT_CAMERA_NAME,
|
||||
namespace=LEFT_CAMERA_NAMESPACE,
|
||||
parameters=[{
|
||||
'parameter_file': config_path,
|
||||
'serial_number': '25282106',
|
||||
'camera_info_url': 'file:///home/sentry/camera_ws/stereoCal/left.yaml'
|
||||
}]
|
||||
),
|
||||
|
||||
Node(
|
||||
package='spinnaker_camera_driver',
|
||||
executable='camera_driver_node',
|
||||
name=RIGHT_CAMERA_NAME,
|
||||
namespace=RIGHT_CAMERA_NAMESPACE,
|
||||
parameters=[{
|
||||
'parameter_file': config_path,
|
||||
'serial_number': '25235293',
|
||||
'camera_info_url': 'file:///home/sentry/camera_ws/stereoCal/right.yaml'
|
||||
}]
|
||||
),
|
||||
Node(
|
||||
package='image_proc',
|
||||
executable='image_proc',
|
||||
namespace='stereo/left',
|
||||
remappings=[
|
||||
('image', 'image_raw')
|
||||
]
|
||||
),
|
||||
Node(
|
||||
package='image_proc',
|
||||
executable='image_proc',
|
||||
namespace='stereo/right',
|
||||
remappings=[
|
||||
('image', 'image_raw')
|
||||
]
|
||||
),
|
||||
|
||||
Node(
|
||||
package='stereo_image_proc',
|
||||
executable='point_cloud_node',
|
||||
namespace='stereo',
|
||||
parameters=[{'approximate_sync': True}],
|
||||
remappings=[
|
||||
('left/image_rect_color', 'left/image_rect'),
|
||||
('right/image_rect_color', 'right/image_rect')
|
||||
],
|
||||
output='screen'
|
||||
)
|
||||
|
||||
])
|
||||
camera_container,
|
||||
image_proc_container,
|
||||
stereo_proc_container,
|
||||
])
|
||||
|
||||
Reference in New Issue
Block a user