315 lines
12 KiB
Python
315 lines
12 KiB
Python
#!/usr/bin/env python3
|
||
# Copyright 2019 ROBOTIS CO., LTD.
|
||
# SPDX-License-Identifier: Apache-2.0
|
||
#
|
||
# Gazebo Sim 8 (Harmonic) + ROS 2 Humble — TurtleBot3 办公室 Task1 链路：
|
||
# 默认加载 office_gz_dartsim.sdf（已去除网格碰撞，dartsim 稳定）+ GPU 激光 + Nav2/SLAM 所需话题。
|
||
|
||
import os
|
||
from pathlib import Path
|
||
|
||
from ament_index_python.packages import get_package_share_directory
|
||
from launch import LaunchDescription
|
||
from launch.actions import (
|
||
DeclareLaunchArgument,
|
||
IncludeLaunchDescription,
|
||
LogInfo,
|
||
OpaqueFunction,
|
||
SetEnvironmentVariable,
|
||
TimerAction,
|
||
)
|
||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||
from launch_ros.actions import Node
|
||
|
||
|
||
def _turtlebot3_gazebo_share():
|
||
try:
|
||
return get_package_share_directory('turtlebot3_gazebo')
|
||
except LookupError:
|
||
return str(Path(__file__).resolve().parent.parent)
|
||
|
||
|
||
def _launch_setup(context, *args, **kwargs):
|
||
pkg_share = _turtlebot3_gazebo_share()
|
||
pkg_ros_gz_sim = get_package_share_directory('ros_gz_sim')
|
||
|
||
world = context.launch_configurations['world']
|
||
gz_partition = context.launch_configurations['gz_partition']
|
||
world_name = context.launch_configurations['gz_world_name']
|
||
physics = context.launch_configurations['physics_engine']
|
||
headless = context.launch_configurations['headless'].lower() == 'true'
|
||
x_pose = context.launch_configurations['x_pose']
|
||
y_pose = context.launch_configurations['y_pose']
|
||
use_sim_time = context.launch_configurations['use_sim_time']
|
||
robot_model = context.launch_configurations['robot_model']
|
||
|
||
model_gz = os.path.abspath(
|
||
os.path.join(pkg_share, 'models', f'turtlebot3_{robot_model}', 'model_gz.sdf'))
|
||
if not os.path.isfile(model_gz):
|
||
raise RuntimeError(
|
||
f'Missing Gazebo Sim model: {model_gz}. Supported robot_model: burger, waffle.'
|
||
)
|
||
|
||
world_abs = os.path.abspath(os.path.expanduser(world))
|
||
if not os.path.isfile(world_abs):
|
||
raise RuntimeError(
|
||
f'World file not found: {world_abs}. '
|
||
'Build/install turtlebot3_gazebo or pass world:=/absolute/path/office_gz_dartsim.sdf'
|
||
)
|
||
|
||
gz_entity = f'turtlebot3_{robot_model}'
|
||
gz_prefix = '-s ' if headless else ''
|
||
gz_args = f'{gz_prefix}-r --physics-engine {physics} {world_abs}'
|
||
|
||
gz_sim = IncludeLaunchDescription(
|
||
PythonLaunchDescriptionSource(
|
||
os.path.join(pkg_ros_gz_sim, 'launch', 'gz_sim.launch.py')),
|
||
launch_arguments={'gz_args': gz_args}.items(),
|
||
)
|
||
|
||
urdf_name = f'turtlebot3_{robot_model}.urdf'
|
||
urdf_path = os.path.join(pkg_share, 'urdf', urdf_name)
|
||
with open(urdf_path, 'r', encoding='utf-8') as urdf_file:
|
||
robot_desc = urdf_file.read()
|
||
|
||
robot_state_publisher = Node(
|
||
package='robot_state_publisher',
|
||
executable='robot_state_publisher',
|
||
name='robot_state_publisher',
|
||
output='screen',
|
||
parameters=[{
|
||
'use_sim_time': use_sim_time.lower() == 'true',
|
||
'robot_description': robot_desc,
|
||
}],
|
||
)
|
||
|
||
joint_gz = f'/world/{world_name}/model/{gz_entity}/joint_state'
|
||
cmd_topic = f'/model/{gz_entity}/cmd_vel'
|
||
odom_topic = f'/model/{gz_entity}/odometry'
|
||
# DiffDrive publishes odom->base_footprint as gz.msgs.Pose_V on this topic.
|
||
tf_topic = f'/model/{gz_entity}/tf'
|
||
|
||
# Gazebo sometimes publishes LaserScan frame_id as "<model>/base_scan/hls_lfcd_lds" even with
|
||
# gz_frame_id; SLAM/RViz expect "base_scan" (URDF). Identity tie fixes TF without changing hits.
|
||
gz_lidar_frame = f'{gz_entity}/base_scan/hls_lfcd_lds'
|
||
lidar_frame_broadcaster = Node(
|
||
package='tf2_ros',
|
||
executable='static_transform_publisher',
|
||
name='gz_lidar_frame_align',
|
||
arguments=[
|
||
'--frame-id', 'base_scan',
|
||
'--child-frame-id', gz_lidar_frame,
|
||
],
|
||
parameters=[{'use_sim_time': use_sim_time.lower() == 'true'}],
|
||
)
|
||
|
||
bridge = Node(
|
||
package='ros_gz_bridge',
|
||
executable='parameter_bridge',
|
||
arguments=[
|
||
'/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock',
|
||
f'{cmd_topic}@geometry_msgs/msg/Twist@gz.msgs.Twist',
|
||
f'{odom_topic}@nav_msgs/msg/Odometry@gz.msgs.Odometry',
|
||
f'{joint_gz}@sensor_msgs/msg/JointState[gz.msgs.Model',
|
||
f'{tf_topic}@tf2_msgs/msg/TFMessage[gz.msgs.Pose_V',
|
||
'/scan@sensor_msgs/msg/LaserScan@gz.msgs.LaserScan',
|
||
'/camera@sensor_msgs/msg/Image@gz.msgs.Image',
|
||
'/camera_info@sensor_msgs/msg/CameraInfo@gz.msgs.CameraInfo',
|
||
],
|
||
remappings=[
|
||
(joint_gz, '/joint_states'),
|
||
(tf_topic, '/tf'),
|
||
(cmd_topic, '/cmd_vel'),
|
||
(odom_topic, '/odom'),
|
||
('/camera', '/camera/image_raw'),
|
||
('/camera_info', '/camera/camera_info'),
|
||
],
|
||
parameters=[{
|
||
f'qos_overrides.{cmd_topic}.subscriber.reliability': 'reliable',
|
||
}],
|
||
output='screen',
|
||
)
|
||
|
||
spawn = Node(
|
||
package='ros_gz_sim',
|
||
executable='create',
|
||
arguments=[
|
||
'-world', world_name,
|
||
'-file', model_gz,
|
||
'-name', gz_entity,
|
||
'-x', x_pose,
|
||
'-y', y_pose,
|
||
'-z', '0.05',
|
||
],
|
||
output='screen',
|
||
)
|
||
|
||
delayed_spawn = TimerAction(period=12.0, actions=[spawn])
|
||
|
||
set_gz_partition = SetEnvironmentVariable(name='GZ_PARTITION', value=gz_partition)
|
||
|
||
set_gz_res = SetEnvironmentVariable(
|
||
name='GZ_SIM_RESOURCE_PATH',
|
||
value=os.pathsep.join([
|
||
os.path.join(pkg_share, 'models', 'turtlebot3_office'),
|
||
os.path.join(pkg_share, 'models'),
|
||
os.path.join(Path.home(), '.gazebo', 'models'),
|
||
]),
|
||
)
|
||
|
||
log_start = LogInfo(msg=[
|
||
'[turtlebot3_office_gz] Isolated GZ_PARTITION='
|
||
+ gz_partition
|
||
+ ' (close other gz sim windows or they stay on the default partition). World='
|
||
+ world_abs,
|
||
])
|
||
|
||
actions = [
|
||
set_gz_partition,
|
||
set_gz_res,
|
||
log_start,
|
||
robot_state_publisher,
|
||
lidar_frame_broadcaster,
|
||
gz_sim,
|
||
bridge,
|
||
delayed_spawn,
|
||
]
|
||
|
||
start_slam = context.launch_configurations['start_slam'].lower() == 'true'
|
||
if start_slam:
|
||
try:
|
||
slam_pkg = get_package_share_directory('slam_toolbox')
|
||
except LookupError:
|
||
slam_pkg = ''
|
||
if slam_pkg:
|
||
slam_launch = os.path.join(slam_pkg, 'launch', 'online_async_launch.py')
|
||
slam_params = context.launch_configurations['slam_params_file']
|
||
if not os.path.isfile(slam_params):
|
||
raise RuntimeError(
|
||
f'slam_params_file not found: {slam_params} '
|
||
'(install turtlebot3_gazebo + config, or pass slam_params_file:=...)'
|
||
)
|
||
actions.append(
|
||
TimerAction(
|
||
period=14.0,
|
||
actions=[
|
||
LogInfo(msg=[
|
||
'[turtlebot3_office_gz] Starting slam_toolbox (map frame will appear for RViz).',
|
||
]),
|
||
IncludeLaunchDescription(
|
||
PythonLaunchDescriptionSource(slam_launch),
|
||
launch_arguments={
|
||
'use_sim_time': context.launch_configurations['use_sim_time'],
|
||
'slam_params_file': slam_params,
|
||
}.items(),
|
||
),
|
||
],
|
||
))
|
||
else:
|
||
actions.append(
|
||
LogInfo(msg=[
|
||
'[turtlebot3_office_gz] start_slam=true but slam_toolbox package not found '
|
||
'(install ros-humble-slam-toolbox).',
|
||
]))
|
||
|
||
if context.launch_configurations['show_rviz'].lower() == 'true':
|
||
try:
|
||
nav2_share = get_package_share_directory('nav2_bringup')
|
||
rviz_cfg = os.path.join(nav2_share, 'rviz', 'nav2_default_view.rviz')
|
||
except LookupError:
|
||
rviz_cfg = ''
|
||
if rviz_cfg and os.path.isfile(rviz_cfg):
|
||
rviz_node = Node(
|
||
package='rviz2',
|
||
executable='rviz2',
|
||
name='rviz2',
|
||
arguments=['-d', rviz_cfg],
|
||
parameters=[{'use_sim_time': use_sim_time.lower() == 'true'}],
|
||
output='screen',
|
||
)
|
||
# RViz fixed frame is "map": wait for slam_toolbox; otherwise scans pile up and drop.
|
||
if start_slam:
|
||
actions.append(
|
||
TimerAction(
|
||
period=20.0,
|
||
actions=[
|
||
LogInfo(msg=[
|
||
'[turtlebot3_office_gz] Starting RViz (after SLAM). Drive the robot to map the office.',
|
||
]),
|
||
rviz_node,
|
||
],
|
||
))
|
||
else:
|
||
actions.append(rviz_node)
|
||
else:
|
||
actions.append(
|
||
LogInfo(msg=[
|
||
'[turtlebot3_office_gz] show_rviz=true but nav2_bringup not found or '
|
||
'missing nav2_default_view.rviz — install ros-humble-nav2-bringup or '
|
||
'run rviz2 manually.',
|
||
]))
|
||
|
||
return actions
|
||
|
||
|
||
def generate_launch_description():
|
||
pkg_share = _turtlebot3_gazebo_share()
|
||
default_world = os.path.join(pkg_share, 'worlds', 'office_gz_dartsim.sdf')
|
||
default_slam_params = os.path.join(pkg_share, 'config', 'mapper_params_office_gz_sim.yaml')
|
||
default_robot = os.environ.get('TURTLEBOT3_MODEL', 'burger')
|
||
|
||
return LaunchDescription([
|
||
DeclareLaunchArgument(
|
||
'world',
|
||
default_value=default_world,
|
||
description='SDF world file (must match gz_world_name in the file).',
|
||
),
|
||
DeclareLaunchArgument(
|
||
'gz_partition',
|
||
default_value='tb3_office_gz',
|
||
description='Unique Gazebo transport partition so this sim does not attach to another gz sim '
|
||
'(e.g. tutorial playground / gpu_lidar) on the same machine.',
|
||
),
|
||
DeclareLaunchArgument(
|
||
'gz_world_name',
|
||
default_value='default',
|
||
description='<world name="..."> (office_gz_dartsim.sdf → default).',
|
||
),
|
||
DeclareLaunchArgument(
|
||
'physics_engine',
|
||
default_value='gz-physics-dartsim-plugin',
|
||
description='Physics engine plugin for gz-sim.',
|
||
),
|
||
DeclareLaunchArgument(
|
||
'headless',
|
||
default_value='false',
|
||
description='If true, run gz sim server only (-s).',
|
||
),
|
||
DeclareLaunchArgument(
|
||
'robot_model',
|
||
default_value=default_robot,
|
||
description='TurtleBot3 variant: burger or waffle (must match TURTLEBOT3_MODEL).',
|
||
),
|
||
DeclareLaunchArgument('x_pose', default_value='0.0'),
|
||
DeclareLaunchArgument('y_pose', default_value='0.0'),
|
||
DeclareLaunchArgument('use_sim_time', default_value='true'),
|
||
DeclareLaunchArgument(
|
||
'show_rviz',
|
||
default_value='true',
|
||
description='If true, start RViz2 with nav2_bringup default config (needs ros-humble-nav2-bringup). '
|
||
'Set false when RViz is started separately (e.g. Docker run_task1.sh).',
|
||
),
|
||
DeclareLaunchArgument(
|
||
'start_slam',
|
||
default_value='true',
|
||
description='If true, start slam_toolbox online_async after the robot spawns (recommended for mapping). '
|
||
'Set false if you launch SLAM separately (e.g. Docker run_task1.sh).',
|
||
),
|
||
DeclareLaunchArgument(
|
||
'slam_params_file',
|
||
default_value=default_slam_params,
|
||
description='slam_toolbox YAML (office + gz-sim lidar 6 m).',
|
||
),
|
||
OpaqueFunction(function=_launch_setup),
|
||
])
|