Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- import os
- from ament_index_python.packages import get_package_share_directory
- from launch import LaunchDescription
- from launch.substitutions import LaunchConfiguration
- from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument
- from launch.launch_description_sources import PythonLaunchDescriptionSource
- from launch.conditions import IfCondition
- from nav2_common.launch import RewrittenYaml
- from launch.actions import ExecuteProcess
- from launch_ros.actions import Node
- def generate_launch_description():
- # Get the launch directory
- bringup_dir = get_package_share_directory('nav2_bringup')
- launch_dir = os.path.join(bringup_dir, 'launch')
- params_dir = os.path.join(bringup_dir, "params")
- nav2_params = os.path.join(params_dir, "real_truck_nav2_params.yaml")
- configured_params = RewrittenYaml(
- source_file=nav2_params, root_key="", param_rewrites="", convert_types=True
- )
- # Keep truck static by publishing to /cmd_vel in a loop
- keep_truck_static_cmd = ExecuteProcess(
- cmd=['bash', '-c', "while true; do ros2 topic pub /cmd_vel geometry_msgs/msg/Twist '{linear: {x: -0.8, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.0}}' -1; sleep 0.1; done"],
- output='screen'
- )
- use_rviz = LaunchConfiguration('use_rviz')
- use_mapviz = LaunchConfiguration('use_mapviz')
- declare_use_rviz_cmd = DeclareLaunchArgument(
- 'use_rviz',
- default_value='False',
- description='Whether to start RVIZ')
- declare_use_mapviz_cmd = DeclareLaunchArgument(
- 'use_mapviz',
- default_value='True',
- description='Whether to start mapviz')
- robot_localization_cmd = IncludeLaunchDescription(
- PythonLaunchDescriptionSource(
- os.path.join(launch_dir, 'real_bot_dual_ekf_navsat.launch.py'))
- )
- navigation2_cmd = IncludeLaunchDescription(
- PythonLaunchDescriptionSource(
- os.path.join(bringup_dir, "launch", "real_navigation_launch.py")
- ),
- launch_arguments={
- "use_sim_time": "False",
- "params_file": configured_params,
- "autostart": "True",
- }.items(),
- )
- rviz_cmd = IncludeLaunchDescription(
- PythonLaunchDescriptionSource(
- os.path.join(bringup_dir, "launch", 'rviz_launch.py')),
- condition=IfCondition(use_rviz)
- )
- mapviz_cmd = IncludeLaunchDescription(
- PythonLaunchDescriptionSource(
- os.path.join(launch_dir, 'mapviz.launch.py')),
- condition=IfCondition(use_mapviz)
- )
- robot_cmd = IncludeLaunchDescription(
- PythonLaunchDescriptionSource(
- os.path.join(launch_dir, 'real_truck_environment_launch.py'))
- )
- # Node to run broadcast_odom_base_footprint.py
- broadcast_odom_base_footprint_node = Node(
- package='nav2_bringup',
- executable='broadcast_odom_base_footprint.py',
- name='broadcast_odom_base_footprint',
- output='screen'
- )
- # Node to relay GPS from CAN to proper topic
- nmea_to_navsat_converter_node = Node(
- package='nav2_bringup',
- executable='nmea_to_navsat_converter.py',
- name='nmea_to_navsat_converter',
- output='screen'
- )
- # Relay command to publish GPS data to the correct topic
- relay_gps_fix_cmd = ExecuteProcess(
- cmd=['ros2', 'run', 'topic_tools', 'relay', '/real_gps/fix', '/gps/fix'],
- output='screen'
- )
- # Create the launch description and populate
- ld = LaunchDescription()
- # Add launch arguments
- ld.add_action(declare_use_rviz_cmd)
- ld.add_action(declare_use_mapviz_cmd)
- # Add robot launch
- ld.add_action(robot_cmd)
- # Add robot localization launch
- ld.add_action(robot_localization_cmd)
- # Add navigation2 launch
- ld.add_action(navigation2_cmd)
- # Add viz launch
- ld.add_action(rviz_cmd)
- ld.add_action(mapviz_cmd)
- # Add the broadcast_odom_base_footprint node
- ld.add_action(broadcast_odom_base_footprint_node)
- # Add nmea relay
- # ld.add_action(nmea_to_navsat_converter_node)
- # Add the relay GPS fix command
- ld.add_action(relay_gps_fix_cmd)
- # Uncomment when the terrain does not move the truck backwards (plane terrains don't need it)
- # ld.add_action(keep_truck_static_cmd) #FAKE BRAKE
- return ld
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement