一、写gazebo.launch.py直接复制下列文件并且修改对应的文件名称import os from launch import LaunchDescription from launch.actions import ExecuteProcess, RegisterEventHandler, TimerAction from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare from launch.event_handlers import OnProcessExit from launch.substitutions import PathJoinSubstitution import xacro import re def remove_comments(text): pattern r!--(.*?)-- return re.sub(pattern, , text, flagsre.DOTALL) def generate_launch_description(): robot_name_in_model lerobot package_name description_lerobot urdf_name lerobot_camera.urdf pkg_share FindPackageShare(packagepackage_name).find(package_name) urdf_model_path os.path.join(pkg_share, furdf/{urdf_name}) # gazebo world 路径 gazebo_world os.path.join(pkg_share, gazebo_world, cabinet.world) # 控制器yaml路径 moveit_config/config/ros2_controllers.yaml controller_config_path PathJoinSubstitution([ FindPackageShare(moveit_config), config, ros2_controllers.yaml ]) # 1. 启动Gazebo仿真环境 start_gazebo_cmd ExecuteProcess( cmd[gazebo, --verbose, gazebo_world, -s, libgazebo_ros_init.so, -s, libgazebo_ros_factory.so], outputscreen ) # 2. 独立controller_manager进程解决找不到服务的核心 controller_manager_node Node( packagecontroller_manager, executableros2_control_node, parameters[controller_config_path], outputscreen ) # 解析xacro/urdf清除注释避免解析异常 xacro_file urdf_model_path doc xacro.parse(open(xacro_file)) xacro.process_doc(doc) params {robot_description: remove_comments(doc.toxml())} # 3. robot_state_publisher 发布机器人描述与tf node_robot_state_publisher Node( packagerobot_state_publisher, executablerobot_state_publisher, parameters[{use_sim_time: True}, params, {publish_frequency: 15.0}], outputscreen ) # 4. 在Gazebo中生成机械臂模型 spawn_entity_cmd Node( packagegazebo_ros, executablespawn_entity.py, arguments[-entity, robot_name_in_model, -topic, robot_description], outputscreen ) # 自动加载控制器 # 关节状态广播器 load_joint_state_broadcaster ExecuteProcess( cmd[ros2, control, load_controller, --set-state, active, joint_state_broadcaster], outputscreen, ) # 手臂轨迹控制器 load_group_controller ExecuteProcess( cmd[ros2, control, load_controller, --set-state, active, lerobot_group_controller], outputscreen, ) # 腕部夹爪控制器 load_gripper_controller ExecuteProcess( cmd[ros2, control, load_controller, --set-state, active, lerobot_gripper_controller], outputscreen, ) # 时序延时模型生成完成后等待8秒再加载控制器 delay_after_spawn TimerAction( period8.0, actions[load_joint_state_broadcaster] ) # 广播器启动2秒后同时启动两组轨迹控制器 delay_after_broadcaster TimerAction( period2.0, actions[load_group_controller, load_gripper_controller] ) # 事件绑定 spawn_done_handler RegisterEventHandler( event_handlerOnProcessExit( target_actionspawn_entity_cmd, on_exit[delay_after_spawn], ) ) broadcaster_done_handler RegisterEventHandler( event_handlerOnProcessExit( target_actionload_joint_state_broadcaster, on_exit[delay_after_broadcaster], ) ) ld LaunchDescription() ld.add_action(spawn_done_handler) ld.add_action(broadcaster_done_handler) # 固定启动顺序Gazebo → controller_manager → robot_state_publisher → spawn模型 ld.add_action(start_gazebo_cmd) ld.add_action(controller_manager_node) ld.add_action(node_robot_state_publisher) ld.add_action(spawn_entity_cmd) return ld二、ros2_controllers.yaml# This config file is used by ros2_control controller_manager: ros__parameters: update_rate: 100 # Hz hardware_interface: type: gazebo_ros2_control/GazeboSystem lerobot_group_controller: type: joint_trajectory_controller/JointTrajectoryController lerobot_gripper_controller: type: joint_trajectory_controller/JointTrajectoryController joint_state_broadcaster: type: joint_state_broadcaster/JointStateBroadcaster lerobot_group_controller: ros__parameters: joints: - Shoulder_Rotation - Shoulder_Pitch - Elbow - Wrist_Pitch command_interfaces: - position state_interfaces: - position - velocity lerobot_gripper_controller: ros__parameters: joints: - Wrist_Roll - Gripper command_interfaces: - position state_interfaces: - position - velocity三、urdfrobot nameso_arm_100_5dof link namebase_link gazebo materialGazebo/Grey/material /gazebo /link joint namebase_link_joint typefixed parent linkbase_link/ child linkBase/ origin rpy0 0 0 xyz0 0 0/ /joint link nameBase inertial origin rpy0 0 0 xyz-2.45960666746703E-07 0.0311418169687909 0.0175746661003382/ mass value0.193184127927598/ inertia ixx0.000137030709467877 ixy2.10136126944992E-08 ixz4.24087422551286E-09 iyy0.000169089551209259 iyz2.26514711036514E-05 izz0.000145097720857224/ /inertial visual origin rpy0 0 0 xyz0 0 0/ geometry mesh filenamefile://$(find description_lerobot)/models/so_arm_100_5dof/meshes/Base.STL/ /geometry material namebrightyellow color rgba1.0 0.9921568627450981 0.00392156862745098 1.0/ /material /visual collision origin rpy0 0 0 xyz0 0 0/ geometry mesh filenamefile://$(find description_lerobot)/models/so_arm_100_5dof/meshes/Base.STL scale0.9 0.9 0.9/ /geometry /collision /link link nameShoulder_Rotation_Pitch inertial origin rpy0 0 0 xyz-9.07886224712597E-05 0.0590971820568318 0.031089016892169/ mass value0.119226314127197/ inertia ixx5.90408775624429E-05 ixy4.90800532852998E-07 ixz-5.90451772654387E-08 iyy3.21498601038881E-05 iyz-4.58026206663885E-06 izz5.86058514263952E-05/ /inertial visual origin rpy0 0 0 xyz0 0 0/ geometry mesh filenamefile://$(find description_lerobot)/models/so_arm_100_5dof/meshes/Shoulder_Rotation_Pitch.STL/ /geometry material namebrightyellow color rgba1.0 0.9921568627450981 0.00392156862745098 1.0/ /material /visual collision origin rpy0 0 0 xyz0 0 0/ geometry mesh filenamefile://$(find description_lerobot)/models/so_arm_100_5dof/meshes/Shoulder_Rotation_Pitch.STL scale0.9 0.9 0.9/ /geometry /collision /link joint nameShoulder_Rotation typerevolute origin rpy1.5708 0 0 xyz0 -0.0452 0.0165/ parent linkBase/ child linkShoulder_Rotation_Pitch/ axis xyz0 -1 0/ limit effort1000 lower-1.96 upper1.96 velocity5.0/ dynamics damping0.1/ /joint link nameUpper_Arm inertial origin rpy0 0 0 xyz-1.7205170190925E-05 0.0701802156327694 0.00310545118155671/ mass value0.162409284599177/ inertia ixx0.000167153146617081 ixy1.03902689187701E-06 ixz-1.20161820645189E-08 iyy7.01946992214245E-05 iyz2.11884806298698E-06 izz0.000213280241160769/ /inertial visual origin rpy0 0 0 xyz0 0 0/ geometry mesh filenamefile://$(find description_lerobot)/models/so_arm_100_5dof/meshes/Upper_Arm.STL/ /geometry material namebrightyellow color rgba1.0 0.9921568627450981 0.00392156862745098 1.0/ /material /visual collision origin rpy0 0 0 xyz0 0 0/ geometry mesh filenamefile://$(find description_lerobot)/models/so_arm_100_5dof/meshes/Upper_Arm.STL scale0.9 0.9 0.9/ /geometry /collision /link joint nameShoulder_Pitch typerevolute origin rpy0 0 0 xyz0 0.1025 0.0306/ parent linkShoulder_Rotation_Pitch/ child linkUpper_Arm/ axis xyz1 0 0/ limit effort1000 lower-1.745 upper1.745 velocity5.0/ dynamics damping0.1/ /joint link nameLower_Arm inertial origin rpy0 0 0 xyz-0.00339603710186651 0.00137796353960074 0.0768006751156044/ mass value0.147967774582291/ inertia ixx0.000105333995841409 ixy1.73059237226499E-07 ixz-1.1720305455211E-05 iyy0.000138766654485212 iyz1.77429964684103E-06 izz5.08741652515214E-05/ /inertial visual origin rpy0 0 0 xyz0 0 0/ geometry mesh filenamefile://$(find description_lerobot)/models/so_arm_100_5dof/meshes/Lower_Arm.STL/ /geometry material namebrightyellow color rgba1.0 0.9921568627450981 0.00392156862745098 1.0/ /material /visual collision origin rpy0 0 0 xyz0 0 0/ geometry mesh filenamefile://$(find description_lerobot)/models/so_arm_100_5dof/meshes/Lower_Arm.STL scale0.9 0.9 0.9/ /geometry /collision /link joint nameElbow typerevolute origin rpy0 0 0 xyz0 0.11257 0.028/ parent linkUpper_Arm/ child linkLower_Arm/ axis xyz1 0 0/ limit effort1000 lower-1.5 upper1.5 velocity5.0/ dynamics damping0.1/ /joint link nameWrist_Pitch_Roll inertial origin rpy0 0 0 xyz-0.00852653127372418 -0.0352278997897927 -2.34622481569413E-05/ mass value0.066132067097723/ inertia ixx1.95717492443445E-05 ixy-6.62714374412293E-07 ixz5.20089016442066E-09 iyy2.38028417569933E-05 iyz4.09549055863776E-08 izz3.4540143384536E-05/ /inertial visual origin rpy0 0 0 xyz0 0 0/ geometry mesh filenamefile://$(find description_lerobot)/models/so_arm_100_5dof/meshes/Wrist_Pitch_Roll.STL/ /geometry material namebrightyellow color rgba1.0 0.9921568627450981 0.00392156862745098 1.0/ /material /visual collision origin rpy0 0 0 xyz0 0 0/ geometry mesh filenamefile://$(find description_lerobot)/models/so_arm_100_5dof/meshes/Wrist_Pitch_Roll.STL scale0.9 0.9 0.9/ /geometry /collision /link joint nameWrist_Pitch typerevolute origin rpy-1.57079 0 0 xyz0 0.0052 0.1349/ parent linkLower_Arm/ child linkWrist_Pitch_Roll/ axis xyz1 0 0/ limit effort1000 lower-1.658 upper1.658 velocity5.0/ dynamics damping0.1/ /joint link nameFixed_Gripper inertial origin rpy0 0 0 xyz0.00552376906426563 -0.0280167153359021 0.000483582592841092/ mass value0.0929859131176897/ inertia ixx4.3328249304211E-05 ixy7.09654328670947E-06 ixz5.99838530879484E-07 iyy3.04451747368212E-05 iyz-1.58743247545413E-07 izz5.02460913506734E-05/ /inertial visual origin rpy0 0 0 xyz0 0 0/ geometry mesh filenamefile://$(find description_lerobot)/models/so_arm_100_5dof/meshes/Fixed_Gripper.STL/ /geometry material namebrightyellow color rgba1.0 0.9921568627450981 0.00392156862745098 1.0/ /material /visual collision origin rpy0 0 0 xyz0 0 0/ geometry mesh filenamefile://$(find description_lerobot)/models/so_arm_100_5dof/meshes/Fixed_Gripper.STL scale0.9 0.9 0.9/ /geometry /collision /link joint nameWrist_Roll typerevolute origin rpy0 1.57079 0 xyz0 -0.0601 0/ parent linkWrist_Pitch_Roll/ child linkFixed_Gripper/ axis xyz0 1 0/ limit effort1000 lower-2.75 upper2.75 velocity5.0/ dynamics damping0.1/ /joint link nameMoving_Jaw inertial origin rpy0 0 0 xyz-0.00161744605468241 -0.0303472584046471 0.000449645961853651/ mass value0.0202443794940372/ inertia ixx1.10911325081525E-05 ixy-5.35076503033314E-07 ixz-9.46105662101403E-09 iyy3.03576451001973E-06 iyz-1.71146075110632E-07 izz8.9916083370498E-06/ /inertial visual origin rpy0 0 0 xyz0 0 0/ geometry mesh filenamefile://$(find description_lerobot)/models/so_arm_100_5dof/meshes/Moving_Jaw.STL/ /geometry material namebrightyellow color rgba1.0 0.9921568627450981 0.00392156862745098 1.0/ /material /visual collision origin rpy0 0 0 xyz0 0 0/ geometry mesh filenamefile://$(find description_lerobot)/models/so_arm_100_5dof/meshes/Moving_Jaw.STL scale0.9 0.9 0.9/ /geometry /collision /link joint nameGripper typerevolute origin rpy3.1416 0 3.1416 xyz-0.0202 -0.0244 0/ parent linkFixed_Gripper/ child linkMoving_Jaw/ axis xyz0 0 1/ limit effort1000 lower-0.1792 upper1.5708 velocity5.0/ dynamics damping0.1/ /joint link nameEnd_Effector visual origin rpy0 0 0 xyz0 0 0/ geometry box size0.01 0.01 0.01/ /geometry material namered color rgba1.0 0.0 0.0 1/ /material /visual /link joint nameEnd_Effector_Joint typefixed parent linkFixed_Gripper/ child linkEnd_Effector/ origin rpy0 0 0 xyz0.0 -0.09 0.0/ /joint gazebo referencebase_link materialGazebo/Yellow/material gravitytrue/gravity selfCollidefalse/selfCollide /gazebo gazebo referenceBase materialGazebo/Yellow/material selfCollidefalse/selfCollide /gazebo gazebo referenceShoulder_Rotation_Pitch materialGazebo/Yellow/material selfCollidefalse/selfCollide /gazebo gazebo referenceUpper_Arm materialGazebo/Yellow/material /gazebo gazebo referenceLower_Arm materialGazebo/Yellow/material /gazebo gazebo referenceWrist_Pitch_Roll materialGazebo/Yellow/material /gazebo gazebo referenceFixed_Gripper materialGazebo/Yellow/material /gazebo gazebo referenceMoving_Jaw materialGazebo/Yellow/material /gazebo gazebo referencedepth_camera_link materialGazebo/Yellow/material /gazebo ros2_control nameGazeboSystem typesystem hardware plugingazebo_ros2_control/GazeboSystem/plugin /hardware joint nameShoulder_Rotation command_interface nameposition/ state_interface nameposition param nameinitial_value0.0/param /state_interface state_interface namevelocity/ /joint joint nameShoulder_Pitch command_interface nameposition/ state_interface nameposition param nameinitial_value0.0/param /state_interface state_interface namevelocity/ /joint joint nameElbow command_interface nameposition/ state_interface nameposition param nameinitial_value0.0/param /state_interface state_interface namevelocity/ /joint joint nameWrist_Pitch command_interface nameposition/ state_interface nameposition param nameinitial_value0.0/param /state_interface state_interface namevelocity/ /joint joint nameWrist_Roll command_interface nameposition/ state_interface nameposition param nameinitial_value0.0/param /state_interface state_interface namevelocity/ /joint joint nameGripper command_interface nameposition/ state_interface nameposition param nameinitial_value0.0/param /state_interface state_interface namevelocity/ /joint /ros2_control gazebo plugin filenamelibgazebo_ros2_control.so namegazebo_ros2_control parameters$(find moveit_config)/config/ros2_controllers.yaml/parameters robot_param_noderobot_state_publisher/robot_param_node /plugin /gazebo /robot四、最后效果图