diff --git a/raspimouse_navigation/launch/pc_navigation.launch.py b/raspimouse_navigation/launch/pc_navigation.launch.py index 7744eaf8..c0551419 100644 --- a/raspimouse_navigation/launch/pc_navigation.launch.py +++ b/raspimouse_navigation/launch/pc_navigation.launch.py @@ -16,10 +16,10 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch_ros.actions import Node from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node def generate_launch_description(): @@ -32,12 +32,11 @@ def generate_launch_description(): declare_arg_use_sim_time = DeclareLaunchArgument( 'use_sim_tim', default_value='True', - description='True: Simulate, False: Machine' + description='True: Simulate, False: Machine', ) declare_arg_map = DeclareLaunchArgument( - 'map', - description='The full path to the map yaml file.' + 'map', description='The full path to the map yaml file.' ) declare_arg_params_file = DeclareLaunchArgument( @@ -45,38 +44,42 @@ def generate_launch_description(): default_value=os.path.join( get_package_share_directory('raspimouse_navigation'), 'params', - 'raspimouse.yaml'), - description='The full path to the param file.' + 'raspimouse.yaml', + ), + description='The full path to the param file.', ) declare_arg_rviz2_config_path = DeclareLaunchArgument( - 'rviz2_file', default_value=os.path.join( + 'rviz2_file', + default_value=os.path.join( get_package_share_directory('raspimouse_navigation'), 'rviz', - 'nav2_view.rviz'), - description='The full path to the rviz file' + 'nav2_view.rviz', + ), + description='The full path to the rviz file', ) nav2_launch_file_dir = os.path.join( - get_package_share_directory('nav2_bringup'), - 'launch' + get_package_share_directory('nav2_bringup'), 'launch' ) # Launch files and Nodes # nav2_node = IncludeLaunchDescription( - PythonLaunchDescriptionSource([ - nav2_launch_file_dir, '/bringup_launch.py']), + PythonLaunchDescriptionSource([nav2_launch_file_dir, '/bringup_launch.py']), launch_arguments={ 'map': map_yaml_file, 'params_file': params_file, - 'use_sim_time': use_sim_time}.items(), + 'use_sim_time': use_sim_time, + }.items(), ) rviz2_node = Node( name='rviz2', - package='rviz2', executable='rviz2', output='screen', + package='rviz2', + executable='rviz2', + output='screen', arguments=['-d', rviz2_file], - parameters=[{'use_sim_time': use_sim_time}] + parameters=[{'use_sim_time': use_sim_time}], ) ld = LaunchDescription() diff --git a/raspimouse_navigation/launch/robot_navigation.launch.py b/raspimouse_navigation/launch/robot_navigation.launch.py index 9c73cf46..eab8bb18 100644 --- a/raspimouse_navigation/launch/robot_navigation.launch.py +++ b/raspimouse_navigation/launch/robot_navigation.launch.py @@ -17,82 +17,99 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription -from launch.actions import RegisterEventHandler, EmitEvent +from launch.actions import EmitEvent, RegisterEventHandler from launch.conditions import LaunchConfigurationEquals from launch.events import matches_action, Shutdown -from launch.substitutions import LaunchConfiguration from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration from launch_ros.actions import LifecycleNode, Node -from launch_ros.events import lifecycle from launch_ros.event_handlers import OnStateTransition +from launch_ros.events import lifecycle from lifecycle_msgs.msg import Transition def generate_launch_description(): # Launch arguments # - lidar_port = LaunchConfiguration( - 'lidar_port', default='/dev/ttyUSB0') + lidar_port = LaunchConfiguration('lidar_port', default='/dev/ttyUSB0') declare_arg_lidar = DeclareLaunchArgument( 'lidar', default_value='none', - description='Set "none", "urg", "lds", or "rplidar".') + description='Set "none", "urg", "lds", or "rplidar".', + ) declare_arg_lidar_frame = DeclareLaunchArgument( - 'lidar_frame', - default_value='laser', - description='Set lidar frame name.') + 'lidar_frame', default_value='laser', description='Set lidar frame name.' + ) declare_arg_namespace = DeclareLaunchArgument( - 'namespace', - default_value='', - description='Set namespace for tf tree.') + 'namespace', default_value='', description='Set namespace for tf tree.' + ) # Launch files and Nodes # lds_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource([os.path.join( - get_package_share_directory('hls_lfcd_lds_driver'), - 'launch'), - '/hlds_laser.launch.py']), - condition=LaunchConfigurationEquals('lidar', 'lds') + PythonLaunchDescriptionSource( + [ + os.path.join( + get_package_share_directory('hls_lfcd_lds_driver'), 'launch' + ), + '/hlds_laser.launch.py', + ] + ), + condition=LaunchConfigurationEquals('lidar', 'lds'), ) urg_launch = Node( name='urg_node_driver', - package='urg_node', executable='urg_node_driver', output='screen', + package='urg_node', + executable='urg_node_driver', + output='screen', parameters=[{'serial_port': lidar_port}], - condition=LaunchConfigurationEquals('lidar', 'urg') + condition=LaunchConfigurationEquals('lidar', 'urg'), ) rplidar_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource([os.path.join( - get_package_share_directory('rplidar_ros'), - 'launch'), - '/rplidar_a1_launch.py']), - launch_arguments={'serial_port': lidar_port, - 'frame_id': LaunchConfiguration('lidar_frame') - }.items(), - condition=LaunchConfigurationEquals('lidar', 'rplidar') + PythonLaunchDescriptionSource( + [ + os.path.join(get_package_share_directory('rplidar_ros'), 'launch'), + '/rplidar_a1_launch.py', + ] + ), + launch_arguments={ + 'serial_port': lidar_port, + 'frame_id': LaunchConfiguration('lidar_frame'), + }.items(), + condition=LaunchConfigurationEquals('lidar', 'rplidar'), ) - description_params = {'lidar': LaunchConfiguration('lidar'), - 'lidar_frame': LaunchConfiguration('lidar_frame'), - 'namespace': LaunchConfiguration('namespace') - }.items() + description_params = { + 'lidar': LaunchConfiguration('lidar'), + 'lidar_frame': LaunchConfiguration('lidar_frame'), + 'namespace': LaunchConfiguration('namespace'), + }.items() # Launch files and Nodes # robot_description_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource([os.path.join( - get_package_share_directory('raspimouse_slam'), 'launch/'), - 'description.launch.py']), - launch_arguments=description_params + PythonLaunchDescriptionSource( + [ + os.path.join(get_package_share_directory('raspimouse_slam'), 'launch/'), + 'description.launch.py', + ] + ), + launch_arguments=description_params, ) mouse_node = LifecycleNode( - name='raspimouse', namespace='', - package='raspimouse', executable='raspimouse', output='screen', - parameters=[os.path.join(get_package_share_directory( - 'raspimouse_slam'), 'config', 'mouse.yaml')] + name='raspimouse', + namespace='', + package='raspimouse', + executable='raspimouse', + output='screen', + parameters=[ + os.path.join( + get_package_share_directory('raspimouse_slam'), 'config', 'mouse.yaml' + ) + ], ) emit_configuring_event = EmitEvent( @@ -109,17 +126,13 @@ def generate_launch_description(): ) ) - emit_shutdown_event = EmitEvent( - event=Shutdown() - ) + emit_shutdown_event = EmitEvent(event=Shutdown()) register_activating_transition = RegisterEventHandler( OnStateTransition( target_lifecycle_node=mouse_node, goal_state='inactive', - entities=[ - emit_activating_event - ], + entities=[emit_activating_event], ) ) @@ -127,9 +140,7 @@ def generate_launch_description(): OnStateTransition( target_lifecycle_node=mouse_node, goal_state='finalized', - entities=[ - emit_shutdown_event - ], + entities=[emit_shutdown_event], ) ) diff --git a/raspimouse_slam/launch/description.launch.py b/raspimouse_slam/launch/description.launch.py index 21b14bfb..b84f2e53 100644 --- a/raspimouse_slam/launch/description.launch.py +++ b/raspimouse_slam/launch/description.launch.py @@ -17,10 +17,10 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import DeclareLaunchArgument -from launch_ros.actions import Node -from launch_ros.actions import PushRosNamespace from launch.substitutions import Command from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from launch_ros.actions import PushRosNamespace def generate_launch_description(): @@ -28,41 +28,52 @@ def generate_launch_description(): declare_arg_lidar = DeclareLaunchArgument( 'lidar', default_value='none', - description='Set "none", "urg", "lds", or "rplidar".') + description='Set "none", "urg", "lds", or "rplidar".', + ) declare_arg_lidar_frame = DeclareLaunchArgument( - 'lidar_frame', - default_value='laser', - description='Set lidar link name.') + 'lidar_frame', default_value='laser', description='Set lidar link name.' + ) declare_arg_namespace = DeclareLaunchArgument( - 'namespace', - default_value='', - description='Set namespace for tf tree.') + 'namespace', default_value='', description='Set namespace for tf tree.' + ) xacro_file = os.path.join( get_package_share_directory('raspimouse_description'), 'urdf', - 'raspimouse.urdf.xacro') + 'raspimouse.urdf.xacro', + ) - params = {'robot_description': Command( - ['xacro ', xacro_file, - ' lidar:=', LaunchConfiguration('lidar'), - ' lidar_frame:=', LaunchConfiguration('lidar_frame')]), - 'frame_prefix': [LaunchConfiguration('namespace'), '/']} + params = { + 'robot_description': Command( + [ + 'xacro ', + xacro_file, + ' lidar:=', + LaunchConfiguration('lidar'), + ' lidar_frame:=', + LaunchConfiguration('lidar_frame'), + ] + ), + 'frame_prefix': [LaunchConfiguration('namespace'), '/'], + } push_ns = PushRosNamespace([LaunchConfiguration('namespace')]) # Nodes # - robot_state_pub_node = Node(package='robot_state_publisher', - executable='robot_state_publisher', - output='both', - parameters=[params]) + robot_state_pub_node = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + output='both', + parameters=[params], + ) joint_state_pub_node = Node( package='joint_state_publisher', executable='joint_state_publisher', - output='screen') + output='screen', + ) ld = LaunchDescription() diff --git a/raspimouse_slam/launch/pc_slam.launch.py b/raspimouse_slam/launch/pc_slam.launch.py index d00255e0..bcfd5bc7 100644 --- a/raspimouse_slam/launch/pc_slam.launch.py +++ b/raspimouse_slam/launch/pc_slam.launch.py @@ -16,19 +16,20 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument -from launch.substitutions import LaunchConfiguration -from launch_ros.actions import Node -from launch.actions import (DeclareLaunchArgument, EmitEvent, LogInfo, - RegisterEventHandler) +from launch.actions import ( + DeclareLaunchArgument, + EmitEvent, + LogInfo, + RegisterEventHandler, +) from launch.conditions import IfCondition from launch.events import matches_action +from launch.substitutions import AndSubstitution, LaunchConfiguration, NotSubstitution from launch_ros.actions import LifecycleNode +from launch_ros.actions import Node from launch_ros.event_handlers import OnStateTransition from launch_ros.events.lifecycle import ChangeState from lifecycle_msgs.msg import Transition -from launch.substitutions import LaunchConfiguration, NotSubstitution, AndSubstitution - def generate_launch_description(): @@ -36,30 +37,36 @@ def generate_launch_description(): slam_config_file = LaunchConfiguration('slam_config_file') rviz2_file = LaunchConfiguration('rviz2_file') autostart = LaunchConfiguration('autostart') - use_lifecycle_manager = LaunchConfiguration("use_lifecycle_manager") + use_lifecycle_manager = LaunchConfiguration('use_lifecycle_manager') declare_autostart_cmd = DeclareLaunchArgument( - 'autostart', default_value='true', + 'autostart', + default_value='true', description='Automatically startup the slamtoolbox. ' - 'Ignored when use_lifecycle_manager is true.') + 'Ignored when use_lifecycle_manager is true.', + ) declare_use_lifecycle_manager = DeclareLaunchArgument( - 'use_lifecycle_manager', default_value='false', - description='Enable bond connection during node activation') + 'use_lifecycle_manager', + default_value='false', + description='Enable bond connection during node activation', + ) declare_arg_slam_config_file = DeclareLaunchArgument( 'slam_config_file', default_value=os.path.join( get_package_share_directory('raspimouse_slam'), 'config', - 'mapper_params_online_sync.yaml'), - description='The full path to the config file for SLAM') + 'mapper_params_online_sync.yaml', + ), + description='The full path to the config file for SLAM', + ) declare_arg_rviz2_config_path = DeclareLaunchArgument( - 'rviz2_file', default_value=os.path.join( - get_package_share_directory('raspimouse_slam'), - 'rviz', - 'default.rviz'), - description='The full path to the rviz file' + 'rviz2_file', + default_value=os.path.join( + get_package_share_directory('raspimouse_slam'), 'rviz', 'default.rviz' + ), + description='The full path to the rviz file', ) # Nodes @@ -69,36 +76,44 @@ def generate_launch_description(): name='slam_toolbox', output='screen', parameters=[slam_config_file, {'use_lifecycle_manager': use_lifecycle_manager}], - namespace='' + namespace='', ) configure_event = EmitEvent( event=ChangeState( lifecycle_node_matcher=matches_action(slam_node), - transition_id=Transition.TRANSITION_CONFIGURE + transition_id=Transition.TRANSITION_CONFIGURE, + ), + condition=IfCondition( + AndSubstitution(autostart, NotSubstitution(use_lifecycle_manager)) ), - condition=IfCondition(AndSubstitution(autostart, NotSubstitution(use_lifecycle_manager))) ) activate_event = RegisterEventHandler( OnStateTransition( target_lifecycle_node=slam_node, - start_state="configuring", - goal_state="inactive", + start_state='configuring', + goal_state='inactive', entities=[ - LogInfo(msg="[LifecycleLaunch] Slamtoolbox node is activating."), - EmitEvent(event=ChangeState( - lifecycle_node_matcher=matches_action(slam_node), - transition_id=Transition.TRANSITION_ACTIVATE - )) - ] + LogInfo(msg='[LifecycleLaunch] Slamtoolbox node is activating.'), + EmitEvent( + event=ChangeState( + lifecycle_node_matcher=matches_action(slam_node), + transition_id=Transition.TRANSITION_ACTIVATE, + ) + ), + ], + ), + condition=IfCondition( + AndSubstitution(autostart, NotSubstitution(use_lifecycle_manager)) ), - condition=IfCondition(AndSubstitution(autostart, NotSubstitution(use_lifecycle_manager))) ) rviz2_node = Node( name='rviz2', - package='rviz2', executable='rviz2', output='screen', + package='rviz2', + executable='rviz2', + output='screen', arguments=['-d', rviz2_file], ) diff --git a/raspimouse_slam/launch/robot_bringup.launch.py b/raspimouse_slam/launch/robot_bringup.launch.py index 399945ab..1e99ffc6 100644 --- a/raspimouse_slam/launch/robot_bringup.launch.py +++ b/raspimouse_slam/launch/robot_bringup.launch.py @@ -18,93 +18,120 @@ from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription from launch.conditions import LaunchConfigurationEquals -from launch.substitutions import LaunchConfiguration from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration from launch_ros.actions import LifecycleNode, Node def generate_launch_description(): # Declare arguments # declare_arg_namespace = DeclareLaunchArgument( - 'namespace', default_value='', - description='Set namespace for tf tree.') + 'namespace', default_value='', description='Set namespace for tf tree.' + ) - lidar_port = LaunchConfiguration( - 'lidar_port', default='/dev/ttyUSB0') + lidar_port = LaunchConfiguration('lidar_port', default='/dev/ttyUSB0') declare_arg_lidar = DeclareLaunchArgument( - 'lidar', default_value='none', - description='Set "none", "urg", "lds", or "rplidar".') + 'lidar', + default_value='none', + description='Set "none", "urg", "lds", or "rplidar".', + ) declare_arg_lidar_frame = DeclareLaunchArgument( - 'lidar_frame', default_value='laser', - description='Set lidar frame name.') + 'lidar_frame', default_value='laser', description='Set lidar frame name.' + ) declare_arg_joydev = DeclareLaunchArgument( - 'joydev', default_value='/dev/input/js0', - description='Device file for JoyStick Controller' + 'joydev', + default_value='/dev/input/js0', + description='Device file for JoyStick Controller', ) declare_arg_joyconfig = DeclareLaunchArgument( - 'joyconfig', default_value='f710', + 'joyconfig', + default_value='f710', description='Keyconfig of joystick controllers: \ - supported: f710, dualshock3, dualshock4' + supported: f710, dualshock3, dualshock4', ) # Launch files and Nodes # mouse_node = LifecycleNode( - name='raspimouse', namespace='', - package='raspimouse', executable='raspimouse', output='screen', - parameters=[os.path.join(get_package_share_directory( - 'raspimouse_slam'), 'config', 'mouse.yaml')] + name='raspimouse', + namespace='', + package='raspimouse', + executable='raspimouse', + output='screen', + parameters=[ + os.path.join( + get_package_share_directory('raspimouse_slam'), 'config', 'mouse.yaml' + ) + ], ) lds_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource([os.path.join( - get_package_share_directory('hls_lfcd_lds_driver'), - 'launch'), - '/hlds_laser.launch.py']), - condition=LaunchConfigurationEquals('lidar', 'lds') + PythonLaunchDescriptionSource( + [ + os.path.join( + get_package_share_directory('hls_lfcd_lds_driver'), 'launch' + ), + '/hlds_laser.launch.py', + ] + ), + condition=LaunchConfigurationEquals('lidar', 'lds'), ) urg_launch = Node( name='urg_node_driver', - package='urg_node', executable='urg_node_driver', output='screen', + package='urg_node', + executable='urg_node_driver', + output='screen', parameters=[{'serial_port': lidar_port}], - condition=LaunchConfigurationEquals('lidar', 'urg') + condition=LaunchConfigurationEquals('lidar', 'urg'), ) rplidar_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource([os.path.join( - get_package_share_directory('rplidar_ros'), - 'launch'), - '/rplidar_a1_launch.py']), - launch_arguments={'serial_port': lidar_port, - 'frame_id': LaunchConfiguration('lidar_frame') - }.items(), - condition=LaunchConfigurationEquals('lidar', 'rplidar') + PythonLaunchDescriptionSource( + [ + os.path.join(get_package_share_directory('rplidar_ros'), 'launch'), + '/rplidar_a1_launch.py', + ] + ), + launch_arguments={ + 'serial_port': lidar_port, + 'frame_id': LaunchConfiguration('lidar_frame'), + }.items(), + condition=LaunchConfigurationEquals('lidar', 'rplidar'), ) - description_params = {'lidar': LaunchConfiguration('lidar'), - 'lidar_frame': LaunchConfiguration('lidar_frame'), - 'namespace': LaunchConfiguration('namespace') - }.items() + description_params = { + 'lidar': LaunchConfiguration('lidar'), + 'lidar_frame': LaunchConfiguration('lidar_frame'), + 'namespace': LaunchConfiguration('namespace'), + }.items() display_robot_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource([os.path.join( - get_package_share_directory('raspimouse_slam'), 'launch/'), - 'description.launch.py']), - launch_arguments=description_params + PythonLaunchDescriptionSource( + [ + os.path.join(get_package_share_directory('raspimouse_slam'), 'launch/'), + 'description.launch.py', + ] + ), + launch_arguments=description_params, ) - teleop_params = {'joydev': LaunchConfiguration('joydev'), - 'joyconfig': LaunchConfiguration('joyconfig')}.items() + teleop_params = { + 'joydev': LaunchConfiguration('joydev'), + 'joyconfig': LaunchConfiguration('joyconfig'), + }.items() teleop_joy_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource([os.path.join( - get_package_share_directory('raspimouse_slam'), 'launch/'), - 'teleop.launch.py']), - launch_arguments=teleop_params + PythonLaunchDescriptionSource( + [ + os.path.join(get_package_share_directory('raspimouse_slam'), 'launch/'), + 'teleop.launch.py', + ] + ), + launch_arguments=teleop_params, ) ld = LaunchDescription() diff --git a/raspimouse_slam/launch/teleop.launch.py b/raspimouse_slam/launch/teleop.launch.py index 229b29b5..a820d76c 100644 --- a/raspimouse_slam/launch/teleop.launch.py +++ b/raspimouse_slam/launch/teleop.launch.py @@ -19,53 +19,64 @@ from launch.actions import DeclareLaunchArgument from launch.conditions import IfCondition from launch.substitutions import LaunchConfiguration -from launch_ros.actions import Node, LifecycleNode +from launch_ros.actions import LifecycleNode, Node def generate_launch_description(): # Declare arguments # declare_arg_joydev = DeclareLaunchArgument( - 'joydev', default_value='/dev/input/js0', - description='Device file for JoyStick Controller' + 'joydev', + default_value='/dev/input/js0', + description='Device file for JoyStick Controller', ) declare_arg_joyconfig = DeclareLaunchArgument( - 'joyconfig', default_value='f710', + 'joyconfig', + default_value='f710', description='Keyconfig of joystick controllers. \ These are the supported controllers and value names: \ F710 -> f710 \ DUALSHOCK3 -> dualshock3 \ - DUALSHOCK4 -> dualshock4' + DUALSHOCK4 -> dualshock4', ) declare_arg_mouse = DeclareLaunchArgument( - 'mouse', default_value="false", - description='Launch raspimouse node') + 'mouse', default_value='false', description='Launch raspimouse node' + ) - joycon_param = [os.path.join( - get_package_share_directory('raspimouse_ros2_examples')), - '/config', - '/joy_', LaunchConfiguration('joyconfig'), '.yml'] + joycon_param = [ + os.path.join(get_package_share_directory('raspimouse_ros2_examples')), + '/config', + '/joy_', + LaunchConfiguration('joyconfig'), + '.yml', + ] # Nodes # joy_node = Node( package='joy_linux', executable='joy_linux_node', - parameters=[{'dev': LaunchConfiguration('joydev')}] + parameters=[{'dev': LaunchConfiguration('joydev')}], ) joystick_control_node = Node( package='raspimouse_ros2_examples', executable='joystick_control.py', - parameters=[joycon_param] + parameters=[joycon_param], ) mouse_node = LifecycleNode( - name='raspimouse', namespace='', - package='raspimouse', executable='raspimouse', output='screen', - parameters=[os.path.join(get_package_share_directory( - 'raspimouse_slam'), 'config', 'mouse.yaml')], - condition=IfCondition(LaunchConfiguration('mouse')) + name='raspimouse', + namespace='', + package='raspimouse', + executable='raspimouse', + output='screen', + parameters=[ + os.path.join( + get_package_share_directory('raspimouse_slam'), 'config', 'mouse.yaml' + ) + ], + condition=IfCondition(LaunchConfiguration('mouse')), ) ld = LaunchDescription()