trying this
This commit is contained in:
@@ -1,13 +1,13 @@
|
|||||||
import os
|
import os
|
||||||
from ament_index_python.packages import get_package_share_directory
|
from ament_index_python.packages import get_package_share_directory
|
||||||
from launch import LaunchDescription
|
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'
|
LEFT_CAMERA_NAME = 'left'
|
||||||
RIGHT_CAMERA_NAME = 'right'
|
RIGHT_CAMERA_NAME = 'right'
|
||||||
LEFT_CAMERA_NAMESPACE = 'stereo'
|
|
||||||
RIGHT_CAMERA_NAMESPACE = 'stereo'
|
|
||||||
|
|
||||||
def generate_launch_description():
|
def generate_launch_description():
|
||||||
config_path = os.path.join(
|
config_path = os.path.join(
|
||||||
@@ -16,57 +16,175 @@ def generate_launch_description():
|
|||||||
'blackfly_s.yaml'
|
'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([
|
return LaunchDescription([
|
||||||
Node(
|
camera_container,
|
||||||
package='spinnaker_camera_driver',
|
image_proc_container,
|
||||||
executable='camera_driver_node',
|
stereo_proc_container,
|
||||||
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'
|
|
||||||
)
|
|
||||||
|
|
||||||
])
|
])
|
||||||
Reference in New Issue
Block a user