diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index f24403f..f29b6ea 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -28,6 +28,7 @@ set(msg_files msg/MotorCmd.msg msg/SkillCall.msg msg/MetaKey.msg + msg/ArmMotionParams.msg ) set(action_files action/MoveToPosition.action @@ -37,6 +38,7 @@ set(action_files action/VisionGraspObject.action action/SlamMode.action action/Arm.action + action/DualArm.action action/CameraTakePhoto.action action/MoveHome.action action/MoveLeg.action diff --git a/src/action/DualArm.action b/src/action/DualArm.action new file mode 100644 index 0000000..ae938b9 --- /dev/null +++ b/src/action/DualArm.action @@ -0,0 +1,29 @@ +# DualArm.action + +uint8 STATUS_PLANNING = 0 # 规划中 +uint8 STATUS_EXECUTING = 1 # 执行中 +uint8 STATUS_DONE = 2 # 完成 + +uint8 ARM_LEFT = 0 # 左臂 +uint8 ARM_RIGHT = 1 # 右臂 +uint8 ARM_DUAL = 2 # 双臂 + +uint8 arm_type # 运动臂类型 | ARM_LEFT/ARM_RIGHT/ARM_DUAL +interfaces/ArmMotionParams[] arm_motion_params # 每条手臂的运动参数 +float64 velocity_scaling # 速度缩放因子 [0,1] +float64 acceleration_scaling # 加速度缩放因子 [0,1] +float64 cartesian_step # MOVEL步长 m [0,0.5] + +--- + +bool success # 执行是否成功 +string message # 结果描述 +float64 final_progress # 最终进度 [0,1] + +--- + +float64 progress # 实时进度 [0,1] +uint8 status # 实时状态 STATUS_PLANNING/STATUS_EXECUTING/STATUS_DONE +float64[] joints_left # 左臂当前关节角(弧度) +float64[] joints_right # 右臂当前关节角(弧度) + diff --git a/src/msg/ArmMotionParams.msg b/src/msg/ArmMotionParams.msg new file mode 100644 index 0000000..0096703 --- /dev/null +++ b/src/msg/ArmMotionParams.msg @@ -0,0 +1,13 @@ +# ArmMotionParams.msg - 单臂运动参数 + +uint8 MOVEJ = 0 +uint8 MOVEL = 1 + +uint8 ARM_LEFT = 0 +uint8 ARM_RIGHT = 1 + +uint8 arm_id # 机械臂ID 0: 左臂, 1: 右臂 +uint8 motion_type # 运动模式: MOVEJ / MOVEL +geometry_msgs/Pose target_pose # 笛卡尔目标位姿 (仅 MOVEL 使用) +float64[] target_joints # 目标关节角(rad) (仅 MOVEJ 使用) +