52 lines
2.1 KiB
Python
52 lines
2.1 KiB
Python
import os
|
|
import yaml
|
|
|
|
from ament_index_python.packages import get_package_share_directory
|
|
from launch import LaunchDescription
|
|
from launch.actions import DeclareLaunchArgument, OpaqueFunction, SetEnvironmentVariable
|
|
from launch.substitutions import LaunchConfiguration
|
|
from launch_ros.actions import Node
|
|
|
|
|
|
def create_nodes(context):
|
|
package_dir = get_package_share_directory('odin_ros_driver')
|
|
config_file = LaunchConfiguration('config_file').perform(context)
|
|
|
|
with open(config_file, 'r', encoding='utf-8') as stream:
|
|
reprojection_params = yaml.safe_load(stream) or {}
|
|
|
|
reprojection_params.setdefault('synced_cloud_topic', '/odin1/sync/cloud_in_cam')
|
|
reprojection_params.setdefault('synced_odometry_topic', '/odin1/sync/odometry')
|
|
reprojection_params.setdefault('synced_wiwc_topic', '/odin1/sync/wiwc')
|
|
reprojection_params.setdefault('synced_image_topic', '/odin1/sync/image')
|
|
sync_camera_compressed = int(reprojection_params.get('register_keys.sync_camera_compressed', 0))
|
|
reprojection_params['synced_image_compressed'] = sync_camera_compressed
|
|
reprojection_params.setdefault('register_keys.sync_topic_prefix', '/odin1/sync')
|
|
reprojection_params['calib_file_path'] = os.path.join(package_dir, 'config', 'calib.yaml')
|
|
|
|
synced_target_observation_node = Node(
|
|
package='odin_ros_driver',
|
|
executable='cloud_reprojection_synced_ros2_node',
|
|
name='cloud_reprojection_synced_ros2_node',
|
|
output='screen',
|
|
parameters=[reprojection_params]
|
|
)
|
|
|
|
return [synced_target_observation_node]
|
|
|
|
|
|
def generate_launch_description():
|
|
package_dir = get_package_share_directory('odin_ros_driver')
|
|
|
|
config_file_arg = DeclareLaunchArgument(
|
|
'config_file',
|
|
default_value=os.path.join(package_dir, 'config', 'control_command.yaml'),
|
|
description='Path to the control config YAML file'
|
|
)
|
|
|
|
ld = LaunchDescription()
|
|
ld.add_action(SetEnvironmentVariable('RCUTILS_COLORIZED_OUTPUT', '1'))
|
|
ld.add_action(config_file_arg)
|
|
ld.add_action(OpaqueFunction(function=lambda context: create_nodes(context)))
|
|
return ld
|