HiveCoreRD/hivecore_robot_motion
6
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

interface modification

Browse Source
This commit is contained in:
NuoDaJia
2026-01-28 19:36:48 +08:00
parent efe2f9f87c
commit a721198fe0
2 changed files with 69 additions and 64 deletions
Download Patch File Download Diff File Expand all files Collapse all files

37
src/robot_control/scripts/dual_arm_action_client.py
View File

@@ -35,17 +35,13 @@ class DualArmActionClient(Node):
self.get_logger().info('Action server已就绪')
return True
def send_goal(self, arm_type, arm_motion_params_list, velocity_scaling=0.5,
acceleration_scaling=0.5, cartesian_step=0.01):
def send_goal(self, arm_motion_params_list, velocity_scaling=0.5):
"""
发送运动目标
Args:
arm_type: 运动臂类型 (DualArm.Goal.ARM_LEFT=0, ARM_RIGHT=1, ARM_DUAL=2)
arm_motion_params_list: ArmMotionParams消息列表
velocity_scaling: 速度缩放因子 [0,1]
acceleration_scaling: 加速度缩放因子 [0,1]
cartesian_step: MOVEL步长 [0,0.5] (单位: 米)
"""
# reset per-goal futures
self._goal_handle = None
@@ -53,16 +49,11 @@ class DualArmActionClient(Node):
self._get_result_future = None
goal_msg = DualArm.Goal()
goal_msg.arm_type = arm_type
goal_msg.arm_motion_params = arm_motion_params_list
goal_msg.velocity_scaling = velocity_scaling
goal_msg.acceleration_scaling = acceleration_scaling
goal_msg.cartesian_step = cartesian_step
self.get_logger().info(f'发送目标: arm_type={arm_type}, '
f'arm_count={len(arm_motion_params_list)}, '
f'velocity_scaling={velocity_scaling}, '
f'acceleration_scaling={acceleration_scaling}')
self.get_logger().info(f'发送目标: arm_count={len(arm_motion_params_list)}, '
f'velocity_scaling={velocity_scaling}')
# 发送goal（带 feedback 回调）
self._send_goal_future = self._action_client.send_goal_async(
@@ -130,11 +121,8 @@ class DualArmActionClient(Node):
def send_goal_and_wait(
self,
arm_type,
arm_motion_params_list,
velocity_scaling=0.5,
acceleration_scaling=0.5,
cartesian_step=0.01,
result_timeout_sec=30.0,
):
"""
@@ -142,11 +130,8 @@ class DualArmActionClient(Node):
返回 (success: bool, result_message: str)
"""
future = self.send_goal(
arm_type=arm_type,
arm_motion_params_list=arm_motion_params_list,
velocity_scaling=velocity_scaling,
acceleration_scaling=acceleration_scaling,
cartesian_step=cartesian_step,
)
rclpy.spin_until_future_complete(self, future)
@@ -173,7 +158,7 @@ class DualArmActionClient(Node):
return bool(result.success), str(result.message)
def create_arm_motion_param(arm_id, motion_type, target_joints=None, target_pose=None):
def create_arm_motion_param(arm_id, motion_type, target_joints=None, target_pose=None, blend_radius=0.0):
"""
创建ArmMotionParams消息
@@ -182,6 +167,7 @@ def create_arm_motion_param(arm_id, motion_type, target_joints=None, target_pose
motion_type: 运动模式 (ArmMotionParams.MOVEJ=0, MOVEL=1)
target_joints: 目标关节角列表（弧度），用于MOVEJ
target_pose: 目标位姿（geometry_msgs.Pose），用于MOVEL
blend_radius: 轨迹交融半径 [m]，默认 0.0
Returns:
ArmMotionParams消息对象
@@ -189,6 +175,7 @@ def create_arm_motion_param(arm_id, motion_type, target_joints=None, target_pose
param = ArmMotionParams()
param.arm_id = arm_id
param.motion_type = motion_type
param.blend_radius = blend_radius
if motion_type == ArmMotionParams.MOVEJ:
if target_joints is not None:
@@ -219,7 +206,6 @@ def example_left_and_right_arm_joint_motion():
try:
# ========= 用例 1：左臂单独运动（简单 up/down 测试）=========
arm_type_left = DualArm.Goal.ARM_LEFT
arm_id_left = ArmMotionParams.ARM_LEFT
home_left = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
@@ -236,10 +222,8 @@ def example_left_and_right_arm_joint_motion():
target_joints=target_left,
)
ok, msg = client.send_goal_and_wait(
arm_type=arm_type_left,
arm_motion_params_list=[left_param_up],
velocity_scaling=0.5,
acceleration_scaling=0.5,
result_timeout_sec=30.0,
)
client.get_logger().info(f'左臂 move-to 结果: ok={ok}, msg={msg}')
@@ -253,16 +237,13 @@ def example_left_and_right_arm_joint_motion():
target_joints=home_left,
)
ok, msg = client.send_goal_and_wait(
arm_type=arm_type_left,
arm_motion_params_list=[left_param_home],
velocity_scaling=0.5,
acceleration_scaling=0.5,
result_timeout_sec=30.0,
)
client.get_logger().info(f'左臂回零结果: ok={ok}, msg={msg}')
# ========= 用例 2：右臂单独运动 =========
arm_type_right = DualArm.Goal.ARM_RIGHT
arm_id_right = ArmMotionParams.ARM_RIGHT
home_right = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
@@ -278,10 +259,8 @@ def example_left_and_right_arm_joint_motion():
target_joints=target_right,
)
ok, msg = client.send_goal_and_wait(
arm_type=arm_type_right,
arm_motion_params_list=[right_param_up],
velocity_scaling=0.5,
acceleration_scaling=0.5,
result_timeout_sec=30.0,
)
client.get_logger().info(f'右臂 move-to 结果: ok={ok}, msg={msg}')
@@ -294,10 +273,8 @@ def example_left_and_right_arm_joint_motion():
target_joints=home_right,
)
ok, msg = client.send_goal_and_wait(
arm_type=arm_type_right,
arm_motion_params_list=[right_param_home],
velocity_scaling=0.5,
acceleration_scaling=0.5,
result_timeout_sec=30.0,
)
client.get_logger().info(f'右臂回零结果: ok={ok}, msg={msg}')
@@ -323,10 +300,8 @@ def example_left_and_right_arm_joint_motion():
)
ok, msg = client.send_goal_and_wait(
arm_type=DualArm.Goal.ARM_DUAL,
arm_motion_params_list=[left_param_dual, right_param_dual],
velocity_scaling=0.5,
acceleration_scaling=0.5,
result_timeout_sec=30.0,
)
client.get_logger().info(f'双臂同时运动结果: ok={ok}, msg={msg}')

96
src/robot_control/src/actions/dual_arm_action.cpp
View File

@@ -43,8 +43,8 @@ rclcpp_action::GoalResponse DualArmAction::handle_goal_(
if (!ctx_.robot_control_manager) return rclcpp_action::GoalResponse::REJECT;
RCLCPP_INFO(ctx_.node->get_logger(),
"Received DualArm goal: arm_type=%d, velocity_scaling=%.2f, acceleration_scaling=%.2f, arm_motion_params_count=%zu",
goal->arm_type, goal->velocity_scaling, goal->acceleration_scaling, goal->arm_motion_params.size());
"Received DualArm goal: velocity_scaling=%.2f, arm_motion_params_count=%zu",
goal->velocity_scaling, goal->arm_motion_params.size());
// 统一检查 MoveIt 规划器占用：同一时刻只能进行一组规划
if (planner_busy_.load()) {
@@ -52,13 +52,20 @@ rclcpp_action::GoalResponse DualArmAction::handle_goal_(
return rclcpp_action::GoalResponse::REJECT;
}
const uint8_t ARM_LEFT = goal->ARM_LEFT;
const uint8_t ARM_RIGHT = goal->ARM_RIGHT;
const uint8_t ARM_DUAL = goal->ARM_DUAL;
const uint8_t ARM_LEFT = ArmMotionParams::ARM_LEFT;
const uint8_t ARM_RIGHT = ArmMotionParams::ARM_RIGHT;
const size_t arm_count = goal->arm_motion_params.size();
if (goal->arm_type == ARM_LEFT || goal->arm_type == ARM_RIGHT) {
if (arm_count == 1) {
const auto& arm_param = goal->arm_motion_params[0];
if (arm_param.arm_id != ARM_LEFT && arm_param.arm_id != ARM_RIGHT) {
RCLCPP_ERROR(ctx_.node->get_logger(),
"DualArm goal rejected: invalid arm_id=%d (expected 0=left, 1=right)",
static_cast<int>(arm_param.arm_id));
return rclcpp_action::GoalResponse::REJECT;
}
// 单臂模式：只检查对应手臂状态
const uint8_t arm_id = (goal->arm_type == ARM_LEFT) ? 0 : 1;
const uint8_t arm_id = arm_param.arm_id;
// 不允许在同一只手臂正在执行时再次下发该臂的指令，但允许另一只手在动
bool arm_executing = (arm_id == 0) ? executing_left_.load() : executing_right_.load();
@@ -68,22 +75,6 @@ rclcpp_action::GoalResponse DualArmAction::handle_goal_(
return rclcpp_action::GoalResponse::REJECT;
}
// 目前仅支持一条 ArmMotionParams
if (goal->arm_motion_params.size() != 1) {
RCLCPP_ERROR(ctx_.node->get_logger(),
"DualArm goal rejected: single-arm mode expects exactly one ArmMotionParams (got %zu)",
goal->arm_motion_params.size());
return rclcpp_action::GoalResponse::REJECT;
}
const auto& arm_param = goal->arm_motion_params[0];
if (arm_param.arm_id != arm_id) {
RCLCPP_ERROR(ctx_.node->get_logger(),
"DualArm goal rejected: arm_type=%d but arm_motion_params[0].arm_id=%d",
static_cast<int>(goal->arm_type), static_cast<int>(arm_param.arm_id));
return rclcpp_action::GoalResponse::REJECT;
}
// 目前只支持 MOVEJ
if (arm_param.motion_type != arm_param.MOVEJ) {
RCLCPP_ERROR(ctx_.node->get_logger(),
@@ -104,7 +95,27 @@ rclcpp_action::GoalResponse DualArmAction::handle_goal_(
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
}
if (goal->arm_type == ARM_DUAL) {
if (arm_count == 2) {
bool has_left = false;
bool has_right = false;
for (const auto& arm_param : goal->arm_motion_params) {
if (arm_param.arm_id == ARM_LEFT) {
has_left = true;
} else if (arm_param.arm_id == ARM_RIGHT) {
has_right = true;
} else {
RCLCPP_ERROR(ctx_.node->get_logger(),
"DualArm goal rejected: invalid arm_id=%d (expected 0=left, 1=right)",
static_cast<int>(arm_param.arm_id));
return rclcpp_action::GoalResponse::REJECT;
}
}
if (!has_left || !has_right) {
RCLCPP_ERROR(ctx_.node->get_logger(),
"DualArm goal rejected: dual-arm requires both left and right arm_id");
return rclcpp_action::GoalResponse::REJECT;
}
// 双臂模式：两只手都不能在执行，且只允许 MOVEJ
if (executing_left_.load() || executing_right_.load()) {
RCLCPP_ERROR(ctx_.node->get_logger(),
@@ -131,7 +142,7 @@ rclcpp_action::GoalResponse DualArmAction::handle_goal_(
}
RCLCPP_ERROR(ctx_.node->get_logger(),
"DualArm goal rejected: invalid arm_type=%d", static_cast<int>(goal->arm_type));
"DualArm goal rejected: arm_motion_params must contain 1 or 2 items (got %zu)", arm_count);
return rclcpp_action::GoalResponse::REJECT;
}
@@ -501,8 +512,7 @@ void DualArmAction::execute_(const std::shared_ptr<GoalHandleDualArm> goal_handl
const auto& motionParams = ctx_.robot_control_manager->GetMotionParameters();
const size_t ARM_DOF = motionParams.arm_dof_;
const uint8_t ARM_LEFT = goal->ARM_LEFT;
const uint8_t ARM_RIGHT = goal->ARM_RIGHT;
const double acceleration_scaling = 0.5;
// planning feedback
feedback->status = 0;
@@ -512,8 +522,16 @@ void DualArmAction::execute_(const std::shared_ptr<GoalHandleDualArm> goal_handl
goal_handle->publish_feedback(feedback);
// ==================== 单臂模式 ====================
if (goal->arm_type == ARM_LEFT || goal->arm_type == ARM_RIGHT) {
const uint8_t arm_id = (goal->arm_type == ARM_LEFT) ? 0 : 1;
const size_t arm_count = goal->arm_motion_params.size();
if (arm_count == 1) {
const auto& arm_param = goal->arm_motion_params[0];
const uint8_t arm_id = arm_param.arm_id;
if (arm_id != ArmMotionParams::ARM_LEFT && arm_id != ArmMotionParams::ARM_RIGHT) {
result->success = false;
result->message = "Invalid arm_id for single-arm goal";
goal_handle->abort(result);
return;
}
if ((arm_id == 0 && !left_fjt_client_) || (arm_id == 1 && !right_fjt_client_)) {
result->success = false;
@@ -532,13 +550,12 @@ void DualArmAction::execute_(const std::shared_ptr<GoalHandleDualArm> goal_handl
}
// 使用单臂规划接口
auto arm_params_msg = goal->arm_motion_params.at(0);
ArmMotionParams arm_params = arm_params_msg;
ArmMotionParams arm_params = arm_param;
if (!ctx_.robot_control_manager->PlanArmMotion(
arm_params,
goal->velocity_scaling,
goal->acceleration_scaling)) {
acceleration_scaling)) {
planner_busy_.store(false);
result->success = false;
result->message = "PlanArmMotion failed for single arm";
@@ -662,6 +679,12 @@ void DualArmAction::execute_(const std::shared_ptr<GoalHandleDualArm> goal_handl
}
// ==================== 双臂模式 ====================
if (arm_count != 2) {
result->success = false;
result->message = "Dual-arm mode requires two ArmMotionParams";
goal_handle->abort(result);
return;
}
if (!left_fjt_client_ || !right_fjt_client_) {
result->success = false;
result->message = "FJT clients for left/right arm are null";
@@ -690,10 +713,17 @@ void DualArmAction::execute_(const std::shared_ptr<GoalHandleDualArm> goal_handl
goal_handle->abort(result);
return;
}
if (!need_left || !need_right) {
planner_busy_.store(false);
result->success = false;
result->message = "DualArm goal invalid: dual-arm requires both left and right targets";
goal_handle->abort(result);
return;
}
if (!ctx_.robot_control_manager->PlanDualArmJointMotion(
need_left, left_target, need_right, right_target,
goal->velocity_scaling, goal->acceleration_scaling))
goal->velocity_scaling, acceleration_scaling))
{
planner_busy_.store(false);
result->success = false;