diff --git a/ros2_socketcan/launch/socket_can_receiver.launch.py b/ros2_socketcan/launch/socket_can_receiver.launch.py index 080541c..2c411d0 100644 --- a/ros2_socketcan/launch/socket_can_receiver.launch.py +++ b/ros2_socketcan/launch/socket_can_receiver.launch.py @@ -18,7 +18,7 @@ from launch import LaunchDescription from launch.actions import (DeclareLaunchArgument, EmitEvent, RegisterEventHandler) -from launch.conditions import IfCondition +from launch.conditions import IfCondition, UnlessCondition from launch.event_handlers import OnProcessStart from launch.events import matches_action from launch.substitutions import LaunchConfiguration, TextSubstitution @@ -42,7 +42,8 @@ def generate_launch_description(): 'filters': LaunchConfiguration('filters'), 'use_bus_time': LaunchConfiguration('use_bus_time'), }], - remappings=[('from_can_bus', LaunchConfiguration('from_can_bus_topic'))], + remappings=[('from_can_bus', LaunchConfiguration('from_can_bus_topic')), + ('from_can_bus_fd', LaunchConfiguration('from_can_bus_topic'))], output='screen') socket_can_receiver_configure_event_handler = RegisterEventHandler( @@ -107,7 +108,10 @@ def generate_launch_description(): 'man1/candump.1.html'), DeclareLaunchArgument('auto_configure', default_value='true'), DeclareLaunchArgument('auto_activate', default_value='true'), - DeclareLaunchArgument('from_can_bus_topic', default_value='from_can_bus'), + DeclareLaunchArgument('from_can_bus_topic', default_value='from_can_bus_fd', + condition=IfCondition(LaunchConfiguration('enable_can_fd'))), + DeclareLaunchArgument('from_can_bus_topic', default_value='from_can_bus', + condition=UnlessCondition(LaunchConfiguration('enable_can_fd'))), socket_can_receiver_node, socket_can_receiver_configure_event_handler, socket_can_receiver_activate_event_handler, diff --git a/ros2_socketcan/launch/socket_can_sender.launch.py b/ros2_socketcan/launch/socket_can_sender.launch.py index 39829de..e72c228 100644 --- a/ros2_socketcan/launch/socket_can_sender.launch.py +++ b/ros2_socketcan/launch/socket_can_sender.launch.py @@ -18,7 +18,7 @@ from launch import LaunchDescription from launch.actions import (DeclareLaunchArgument, EmitEvent, RegisterEventHandler) -from launch.conditions import IfCondition +from launch.conditions import IfCondition, UnlessCondition from launch.event_handlers import OnProcessStart from launch.events import matches_action from launch.substitutions import LaunchConfiguration, TextSubstitution @@ -40,7 +40,8 @@ def generate_launch_description(): 'timeout_sec': LaunchConfiguration('timeout_sec'), }], - remappings=[('to_can_bus', LaunchConfiguration('to_can_bus_topic'))], + remappings=[('to_can_bus', LaunchConfiguration('to_can_bus_topic')), + ('to_can_bus_fd', LaunchConfiguration('to_can_bus_topic'))], output='screen') socket_can_sender_configure_event_handler = RegisterEventHandler( @@ -81,7 +82,10 @@ def generate_launch_description(): DeclareLaunchArgument('timeout_sec', default_value='0.01'), DeclareLaunchArgument('auto_configure', default_value='true'), DeclareLaunchArgument('auto_activate', default_value='true'), - DeclareLaunchArgument('to_can_bus_topic', default_value='to_can_bus'), + DeclareLaunchArgument('to_can_bus_topic', default_value='to_can_bus_fd', + condition=IfCondition(LaunchConfiguration('enable_can_fd'))), + DeclareLaunchArgument('to_can_bus_topic', default_value='to_can_bus', + condition=UnlessCondition(LaunchConfiguration('enable_can_fd'))), socket_can_sender_node, socket_can_sender_configure_event_handler, socket_can_sender_activate_event_handler,