a

src/gc_bringup/launch/slam_mapping.launch.py

@@ -0,0 +1,153 @@
+#!/usr/bin/env python3
+# =============================================================================
+# gc_bringup — SLAM 建图模式
+# 启动 Gazebo + 机器人 + slam_toolbox 在线异步建图 + RViz
+# =============================================================================
+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, UnlessCondition
+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'))
+
+    declare_use_sim_time = DeclareLaunchArgument(
+        'use_sim_time', default_value='true',
+        description='Use simulation (Gazebo) clock if true')
+    declare_use_gazebo = DeclareLaunchArgument(
+        'use_gazebo', default_value='true',
+        description='Start Gazebo simulation')
+    declare_use_rviz = DeclareLaunchArgument(
+        'use_rviz', default_value='false',
+        description='Start RViz2 (set true if you need local visualization)')
+    declare_world = DeclareLaunchArgument(
+        'world', default_value=os.path.join(origincar_dir, 'world', 'gc_world.world'),
+        description='Full path to Gazebo world file')
+
+    # ============================ 机器人模型 ============================
+    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'
+    )
+
+    # ============================ SLAM Toolbox (在线异步建图) ============================
+    slam_params_file = os.path.join(
+        bringup_dir, 'config', 'mapper_params_online_async.yaml')
+
+    slam_toolbox_node = Node(
+        package='slam_toolbox',
+        executable='async_slam_toolbox_node',
+        name='slam_toolbox',
+        output='screen',
+        parameters=[slam_params_file, {'use_sim_time': True}],
+        remappings=[('/scan', '/scan')]
+    )
+
+    # ============================ RViz2 ============================
+    rviz_config = os.path.join(bringup_dir, 'rviz', 'slam_mapping.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'
+    )
+
+    # ============================ 键盘遥控 (用于驱动机器人建图) ============================
+    use_teleop = LaunchConfiguration('use_teleop', default='true')
+    declare_use_teleop = DeclareLaunchArgument(
+        'use_teleop', default_value='true',
+        description='Start keyboard teleop for driving')
+
+    teleop_node = Node(
+        condition=IfCondition(use_teleop),
+        package='teleop_twist_keyboard',
+        executable='teleop_twist_keyboard',
+        name='teleop_twist_keyboard',
+        prefix='xterm -e',
+        output='screen',
+        remappings=[('/cmd_vel', '/cmd_vel')],
+        parameters=[{'use_sim_time': True}]
+    )
+
+    # ============================ 组装启动描述 ============================
+    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_use_teleop)
+
+    ld.add_action(start_gazebo)
+
+    # 等 Gazebo 启动后再生成机器人 (延迟 5 秒)
+    ld.add_action(TimerAction(
+        period=5.0,
+        actions=[spawn_robot]
+    ))
+
+    ld.add_action(robot_state_publisher)
+    ld.add_action(joint_state_publisher)
+
+    # 等机器人生成后再启动 SLAM (延迟 3 秒)
+    ld.add_action(TimerAction(
+        period=8.0,
+        actions=[slam_toolbox_node, teleop_node, rviz_node]
+    ))
+
+    return ld