forked from zbw/yiliao2026
208 lines
8.0 KiB
Python
208 lines
8.0 KiB
Python
#!/usr/bin/env python3
|
|
# =============================================================================
|
|
# gc_bringup — AMCL 导航模式 (直接启动 Nav2 节点)
|
|
# 启动 Gazebo + 机器人 + Nav2 (AMCL 定位)
|
|
# =============================================================================
|
|
import os
|
|
from ament_index_python.packages import get_package_share_directory
|
|
from launch import LaunchDescription
|
|
from launch.actions import (DeclareLaunchArgument, ExecuteProcess,
|
|
TimerAction)
|
|
from launch.conditions import IfCondition
|
|
from launch.substitutions import LaunchConfiguration, Command
|
|
from launch_ros.actions import Node
|
|
from launch_ros.parameter_descriptions import ParameterValue
|
|
|
|
|
|
def generate_launch_description():
|
|
bringup_dir = get_package_share_directory('gc_bringup')
|
|
origincar_dir = get_package_share_directory('origincar_description')
|
|
|
|
# ==================== 参数 ====================
|
|
use_sim_time = LaunchConfiguration('use_sim_time', default='true')
|
|
use_gazebo = LaunchConfiguration('use_gazebo', default='true')
|
|
use_rviz = LaunchConfiguration('use_rviz', default='false')
|
|
world = LaunchConfiguration('world', default=os.path.join(
|
|
origincar_dir, 'world', 'gc_world.world'))
|
|
map_yaml = LaunchConfiguration('map', default=os.path.join(
|
|
bringup_dir, 'maps', 'my_map.yaml'))
|
|
|
|
declare_use_sim_time = DeclareLaunchArgument('use_sim_time', default_value='true')
|
|
declare_use_gazebo = DeclareLaunchArgument('use_gazebo', default_value='true')
|
|
declare_use_rviz = DeclareLaunchArgument('use_rviz', default_value='false')
|
|
declare_world = DeclareLaunchArgument(
|
|
'world', default_value=os.path.join(origincar_dir, 'world', 'gc_world.world'))
|
|
declare_map = DeclareLaunchArgument(
|
|
'map', default_value=os.path.join(bringup_dir, 'maps', 'my_map.yaml'))
|
|
|
|
# ==================== 机器人模型 ====================
|
|
default_model_path = os.path.join(origincar_dir, 'urdf', 'origincar.urdf')
|
|
robot_description = ParameterValue(
|
|
Command(['xacro ', default_model_path]), value_type=str)
|
|
|
|
# ==================== Gazebo ====================
|
|
start_gazebo = ExecuteProcess(
|
|
condition=IfCondition(use_gazebo),
|
|
cmd=['gazebo', '--verbose', '-s', 'libgazebo_ros_init.so',
|
|
'-s', 'libgazebo_ros_factory.so', world],
|
|
output='screen')
|
|
|
|
spawn_robot = Node(
|
|
condition=IfCondition(use_gazebo),
|
|
package='gazebo_ros',
|
|
executable='spawn_entity.py',
|
|
arguments=['-entity', 'mycar', '-file', default_model_path,
|
|
'-x', '0.0', '-y', '0.0', '-z', '0.1'],
|
|
output='screen')
|
|
|
|
# ==================== TF & 机器人状态 ====================
|
|
robot_state_publisher = Node(
|
|
package='robot_state_publisher',
|
|
executable='robot_state_publisher',
|
|
name='robot_state_publisher',
|
|
parameters=[{'robot_description': robot_description,
|
|
'use_sim_time': True, 'publish_frequency': 30.0}],
|
|
output='screen')
|
|
|
|
joint_state_publisher = Node(
|
|
package='joint_state_publisher',
|
|
executable='joint_state_publisher',
|
|
name='joint_state_publisher',
|
|
parameters=[{'use_sim_time': True}], output='screen')
|
|
|
|
# ==================== Nav2 参数文件路径 ====================
|
|
nav2_params = os.path.join(bringup_dir, 'config', 'nav2_params.yaml')
|
|
|
|
# ==================== Map Server ====================
|
|
map_server = Node(
|
|
package='nav2_map_server',
|
|
executable='map_server',
|
|
name='map_server',
|
|
output='screen',
|
|
parameters=[nav2_params,
|
|
{'use_sim_time': True, 'yaml_filename': map_yaml}],
|
|
ros_arguments=['-p', 'use_sim_time:=True'],
|
|
)
|
|
|
|
# ==================== AMCL ====================
|
|
amcl = Node(
|
|
package='nav2_amcl',
|
|
executable='amcl',
|
|
name='amcl',
|
|
output='screen',
|
|
parameters=[nav2_params, {'use_sim_time': True}],
|
|
remappings=[('/tf', 'tf'), ('/tf_static', 'tf_static')],
|
|
ros_arguments=['-p', 'use_sim_time:=True'],
|
|
)
|
|
|
|
# ==================== Planner Server ====================
|
|
planner_server = Node(
|
|
package='nav2_planner',
|
|
executable='planner_server',
|
|
name='planner_server',
|
|
output='screen',
|
|
parameters=[nav2_params, {'use_sim_time': True}],
|
|
remappings=[('/tf', 'tf'), ('/tf_static', 'tf_static')],
|
|
ros_arguments=['-p', 'use_sim_time:=True'],
|
|
)
|
|
|
|
# ==================== Controller Server ====================
|
|
controller_server = Node(
|
|
package='nav2_controller',
|
|
executable='controller_server',
|
|
name='controller_server',
|
|
output='screen',
|
|
parameters=[nav2_params, {'use_sim_time': True}],
|
|
remappings=[('/tf', 'tf'), ('/tf_static', 'tf_static')],
|
|
ros_arguments=['-p', 'use_sim_time:=True'],
|
|
)
|
|
|
|
# ==================== Behavior Server ====================
|
|
behavior_server = Node(
|
|
package='nav2_behaviors',
|
|
executable='behavior_server',
|
|
name='behavior_server',
|
|
output='screen',
|
|
parameters=[nav2_params, {'use_sim_time': True}],
|
|
remappings=[('/tf', 'tf'), ('/tf_static', 'tf_static')],
|
|
ros_arguments=['-p', 'use_sim_time:=True'],
|
|
)
|
|
|
|
# ==================== BT Navigator ====================
|
|
bt_navigator = Node(
|
|
package='nav2_bt_navigator',
|
|
executable='bt_navigator',
|
|
name='bt_navigator',
|
|
output='screen',
|
|
parameters=[nav2_params, {'use_sim_time': True}],
|
|
remappings=[('/tf', 'tf'), ('/tf_static', 'tf_static')],
|
|
ros_arguments=['-p', 'use_sim_time:=True'],
|
|
)
|
|
|
|
# ==================== Waypoint Follower ====================
|
|
waypoint_follower = Node(
|
|
package='nav2_waypoint_follower',
|
|
executable='waypoint_follower',
|
|
name='waypoint_follower',
|
|
output='screen',
|
|
parameters=[nav2_params, {'use_sim_time': True}],
|
|
remappings=[('/tf', 'tf'), ('/tf_static', 'tf_static')],
|
|
ros_arguments=['-p', 'use_sim_time:=True'],
|
|
)
|
|
|
|
# ==================== Lifecycle Manager (map + amcl) ====================
|
|
lifecycle_localization = Node(
|
|
package='nav2_lifecycle_manager',
|
|
executable='lifecycle_manager',
|
|
name='lifecycle_manager_localization',
|
|
output='screen',
|
|
parameters=[{'use_sim_time': True, 'autostart': True,
|
|
'node_names': ['map_server', 'amcl']}],
|
|
)
|
|
|
|
# ==================== Lifecycle Manager (nav nodes) ====================
|
|
lifecycle_navigation = Node(
|
|
package='nav2_lifecycle_manager',
|
|
executable='lifecycle_manager',
|
|
name='lifecycle_manager_navigation',
|
|
output='screen',
|
|
parameters=[{'use_sim_time': True, 'autostart': True,
|
|
'node_names': ['controller_server', 'planner_server',
|
|
'behavior_server', 'bt_navigator',
|
|
'waypoint_follower']}],
|
|
)
|
|
|
|
# ==================== RViz2 (可选) ====================
|
|
rviz_config = os.path.join(bringup_dir, 'rviz', 'navigation.rviz')
|
|
rviz_node = Node(
|
|
condition=IfCondition(use_rviz),
|
|
package='rviz2',
|
|
executable='rviz2',
|
|
name='rviz2',
|
|
arguments=['-d', rviz_config],
|
|
parameters=[{'use_sim_time': True}],
|
|
output='screen')
|
|
|
|
# ==================== 组装 ====================
|
|
ld = LaunchDescription()
|
|
ld.add_action(declare_use_sim_time)
|
|
ld.add_action(declare_use_gazebo)
|
|
ld.add_action(declare_use_rviz)
|
|
ld.add_action(declare_world)
|
|
ld.add_action(declare_map)
|
|
|
|
ld.add_action(start_gazebo)
|
|
ld.add_action(TimerAction(period=5.0, actions=[spawn_robot]))
|
|
ld.add_action(robot_state_publisher)
|
|
ld.add_action(joint_state_publisher)
|
|
|
|
ld.add_action(TimerAction(period=8.0, actions=[
|
|
map_server, amcl,
|
|
planner_server, controller_server, behavior_server, bt_navigator,
|
|
waypoint_follower,
|
|
lifecycle_localization, lifecycle_navigation,
|
|
rviz_node,
|
|
]))
|
|
|
|
return ld
|