84 lines
2.5 KiB
Python
84 lines
2.5 KiB
Python
from launch import LaunchDescription
|
|
from launch.actions import DeclareLaunchArgument
|
|
from launch.substitutions import LaunchConfiguration
|
|
from launch_ros.actions import Node
|
|
import os
|
|
from ament_index_python.packages import get_package_share_directory
|
|
|
|
def generate_launch_description():
|
|
# 获取包路径
|
|
package_dir = get_package_share_directory('dual_arm_leg_robot')
|
|
|
|
# 文件路径
|
|
urdf_path = os.path.join(package_dir, 'urdf', 'dual_arm_leg_robot.urdf')
|
|
srdf_path = os.path.join(package_dir, 'config', 'srdf', 'dual_arm_leg_robot.srdf')
|
|
kinematics_path = os.path.join(package_dir, 'config', 'kinematics.yaml')
|
|
rviz_config_path = os.path.join(package_dir, 'config', 'moveit.rviz')
|
|
|
|
# 声明参数
|
|
use_sim_time = LaunchConfiguration('use_sim_time', default='false')
|
|
|
|
# 机器人状态发布器
|
|
robot_state_publisher = Node(
|
|
package='robot_state_publisher',
|
|
executable='robot_state_publisher',
|
|
name='robot_state_publisher',
|
|
output='screen',
|
|
parameters=[{
|
|
'robot_description': open(urdf_path).read(),
|
|
'use_sim_time': use_sim_time
|
|
}]
|
|
)
|
|
|
|
# 关节状态发布器
|
|
joint_state_publisher = Node(
|
|
package='joint_state_publisher_gui',
|
|
executable='joint_state_publisher_gui',
|
|
name='joint_state_publisher_gui',
|
|
output='screen'
|
|
)
|
|
|
|
# MoveIt规划节点
|
|
move_group_node = Node(
|
|
package='moveit_ros_move_group',
|
|
executable='move_group',
|
|
name='move_group',
|
|
output='screen',
|
|
parameters=[{
|
|
'robot_description': open(urdf_path).read(),
|
|
'robot_description_semantic': open(srdf_path).read(),
|
|
'robot_description_kinematics': kinematics_path,
|
|
'use_sim_time': use_sim_time
|
|
}]
|
|
)
|
|
|
|
# 运动学演示节点
|
|
kinematics_demo = Node(
|
|
package='dual_arm_leg_robot',
|
|
executable='kinematics_demo',
|
|
name='kinematics_demo',
|
|
output='screen'
|
|
)
|
|
|
|
# RViz可视化
|
|
rviz_node = Node(
|
|
package='rviz2',
|
|
executable='rviz2',
|
|
name='rviz2',
|
|
output='screen',
|
|
arguments=['-d', rviz_config_path]
|
|
)
|
|
|
|
return LaunchDescription([
|
|
DeclareLaunchArgument(
|
|
'use_sim_time',
|
|
default_value='false',
|
|
description='是否使用仿真时间'
|
|
),
|
|
robot_state_publisher,
|
|
joint_state_publisher,
|
|
move_group_node,
|
|
kinematics_demo,
|
|
rviz_node
|
|
])
|