# Copyright 2020 ros2_control Development Team # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. # You may obtain a copy of the License at # # http://www.apache.org/licenses/LICENSE-2.0 # # Unless required by applicable law or agreed to in writing, software # distributed under the License is distributed on an "AS IS" BASIS, # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. # See the License for the specific language governing permissions and # limitations under the License. from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, RegisterEventHandler from launch.conditions import IfCondition from launch.event_handlers import OnProcessExit from launch.substitutions import Command, FindExecutable, PathJoinSubstitution, LaunchConfiguration from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare def generate_launch_description(): # Declare arguments declared_arguments = [] declared_arguments.append( DeclareLaunchArgument( "gui", default_value="false", description="Start RViz2 automatically with this launch file.", ) ) declared_arguments.append( DeclareLaunchArgument( "joy", default_value="false", description="Start joy node with this launch file.", ) ) declared_arguments.append( DeclareLaunchArgument( "use_mock_hardware", default_value="false", description="Start robot with mock hardware mirroring command to its states.", ) ) lidar_serial_baudrate = LaunchConfiguration('serial_baudrate', default='115200') lidar_channel_type = LaunchConfiguration('channel_type', default='serial') lidar_frame_id = LaunchConfiguration('frame_id', default='laser') lidar_serial_port = LaunchConfiguration('serial_port', default='/dev/ttyUSB0') lidar_inverted = LaunchConfiguration('inverted', default='false') lidar_angle_compensate = LaunchConfiguration('angle_compensate', default='true') lidar_scan_mode = LaunchConfiguration('scan_mode', default='Standard') joy_dev = LaunchConfiguration('joy_dev', default='/dev/input/js0') # Initialize Arguments gui = LaunchConfiguration("gui") joy = LaunchConfiguration("joy") use_mock_hardware = LaunchConfiguration("use_mock_hardware") # Get URDF via xacro robot_description_content = Command( [ PathJoinSubstitution([FindExecutable(name="xacro")]), " ", PathJoinSubstitution( [FindPackageShare("cyphal_hardware_interface"), "urdf", "staubi.urdf.xacro"] ), " ", "use_mock_hardware:=", use_mock_hardware, ] ) robot_description = {"robot_description": robot_description_content} robot_controllers = PathJoinSubstitution( [ FindPackageShare("cyphal_hardware_interface"), "config", "staubi_controllers.yaml", ] ) rviz_config_file = PathJoinSubstitution( [FindPackageShare("cyphal_hardware_interface"), "staubi/rviz", "staubi.rviz"] ) lidar_ld = [ DeclareLaunchArgument( 'channel_type', default_value=lidar_channel_type, description='Specifying channel type of lidar'), DeclareLaunchArgument( 'serial_port', default_value=lidar_serial_port, description='Specifying usb port to connected lidar'), DeclareLaunchArgument( 'serial_baudrate', default_value=lidar_serial_baudrate, description='Specifying usb port baudrate to connected lidar'), DeclareLaunchArgument( 'frame_id', default_value=lidar_frame_id, description='Specifying frame_id of lidar'), DeclareLaunchArgument( 'inverted', default_value=lidar_inverted, description='Specifying whether or not to invert scan data'), DeclareLaunchArgument( 'angle_compensate', default_value=lidar_angle_compensate, description='Specifying whether or not to enable angle_compensate of scan data'), DeclareLaunchArgument( 'scan_mode', default_value=lidar_scan_mode, description='Specifying scan mode of lidar'), Node( package='rplidar_ros', executable='rplidar_node', name='rplidar_node', parameters=[{'channel_type':lidar_channel_type, 'serial_port': lidar_serial_port, 'serial_baudrate': lidar_serial_baudrate, 'frame_id': lidar_frame_id, 'inverted': lidar_inverted, 'angle_compensate': lidar_angle_compensate, 'scan_mode': lidar_scan_mode }], output='screen'),] control_node = Node( package="controller_manager", executable="ros2_control_node", parameters=[robot_description, robot_controllers], output="both", remappings=[ ("/staubi_base_controller/cmd_vel", "/cmd_vel_nav"), ], #prefix=['gdbserver localhost:3000'], ) robot_state_pub_node = Node( package="robot_state_publisher", executable="robot_state_publisher", output="both", parameters=[robot_description], # remappings=[ # ("/diff_drive_controller/cmd_vel_unstamped", "/cmd_vel"), # ], ) rviz_node = Node( package="rviz2", executable="rviz2", name="rviz2", output="log", arguments=["-d", rviz_config_file], condition=IfCondition(gui), ) joint_state_broadcaster_spawner = Node( package="controller_manager", executable="spawner", arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"], ) robot_controller_spawner = Node( package="controller_manager", executable="spawner", arguments=["staubi_base_controller", "--controller-manager", "/controller_manager"], ) # Delay rviz start after `joint_state_broadcaster` delay_rviz_after_joint_state_broadcaster_spawner = RegisterEventHandler( event_handler=OnProcessExit( target_action=joint_state_broadcaster_spawner, on_exit=[rviz_node], ) ) # Delay start of robot_controller after `joint_state_broadcaster` delay_robot_controller_spawner_after_joint_state_broadcaster_spawner = RegisterEventHandler( event_handler=OnProcessExit( target_action=joint_state_broadcaster_spawner, on_exit=[robot_controller_spawner], ) ) joy_ld = [ DeclareLaunchArgument('joy_vel', default_value='/cmd_vel_nav'), DeclareLaunchArgument('joy_config', default_value='ps3'), DeclareLaunchArgument('joy_dev', default_value=joy_dev), Node( package='joy_linux', executable='joy_linux_node', name='joy_linux_node', parameters=[{ 'dev': joy_dev, # 'deadzone': 0.3, # 'autorepeat_rate': 20.0, }], condition=IfCondition(joy), ), Node( package='teleop_twist_joy', executable='teleop_node', name='teleop_twist_joy_node', parameters=[ { "publish_stamped_twist": True, # "require_enable_button": False, "enable_button": 10, # "scale_linear.x":3.0, "axis_angular.yaw": 2, "axis_linear.x": 3, "scale_linear.x": 0.5, "scale_angular.yaw": 1.6, "scale_linear_turbo.x": 1.8, "scale_angular_turbo.yaw": 2.6, "enable_turbo_button": 7, "frame": "base_link", }], # condition=IfCondition(joystick), remappings={('/cmd_vel', LaunchConfiguration('joy_vel'))}, ), ] nodes = [ control_node, robot_state_pub_node, joint_state_broadcaster_spawner, delay_rviz_after_joint_state_broadcaster_spawner, #delay_robot_controller_spawner_after_joint_state_broadcaster_spawner, ] + joy_ld #+ lidar_ld return LaunchDescription(declared_arguments + nodes)