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

adapt to two controllers

Browse Source
This commit is contained in:
NuoDaJia
2026-01-27 19:00:54 +08:00
parent 3b3bdc77a5
commit efe2f9f87c
14 changed files with 639 additions and 249 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
src/robot_control/CMakeLists.txt
View File

@@ -54,6 +54,7 @@ set(SOURCES
src/core/robot_control_manager.cpp
src/core/controller_factory.cpp
src/core/remote_controller.cpp
src/services/motor_service.cpp
src/core/motion_parameters.cpp
src/actions/action_manager.cpp
src/actions/follow_jt_executor.cpp

13
src/robot_control/include/actions/action_manager.hpp
View File

@@ -13,9 +13,6 @@
#include "control_msgs/action/follow_joint_trajectory.hpp"
#include "interfaces/msg/motor_cmd.hpp"
#include "interfaces/srv/motor_param.hpp"
namespace robot_control
{
class RobotControlManager;
@@ -25,19 +22,16 @@ class MoveLegAction;
class MoveWaistAction;
class MoveWheelAction;
class DualArmAction;
class MotorService;
class ActionManager
{
public:
using MotorCmd = interfaces::msg::MotorCmd;
using MotorParam = interfaces::srv::MotorParam;
using FollowJTA = control_msgs::action::FollowJointTrajectory;
ActionManager(
rclcpp::Node* node,
RobotControlManager& robot_control_manager,
rclcpp::Publisher<MotorCmd>::SharedPtr motor_cmd_pub,
rclcpp::Client<MotorParam>::SharedPtr motor_param_client);
RobotControlManager& robot_control_manager);
~ActionManager();
@@ -46,8 +40,7 @@ public:
private:
rclcpp::Node* node_{nullptr};
RobotControlManager& robot_control_manager_;
rclcpp::Publisher<MotorCmd>::SharedPtr motor_cmd_pub_;
rclcpp::Client<MotorParam>::SharedPtr motor_param_client_;
std::shared_ptr<MotorService> motor_service_;
rclcpp_action::Client<FollowJTA>::SharedPtr follow_jt_client_;

12
src/robot_control/include/actions/dual_arm_action.hpp
View File

@@ -69,9 +69,21 @@ private:
DualArm::Feedback* out) const;
ActionContext ctx_;
// 整体 DualArm 执行标志（用于双臂协同运动场景）
std::atomic<bool> executing_{false};
// 单臂执行标志：允许一只手在运动时，下发另一只手的指令
std::atomic<bool> executing_left_{false};
std::atomic<bool> executing_right_{false};
// MoveIt 规划器占用标志：同一时刻只允许一组规划（单臂或双臂）
std::atomic<bool> planner_busy_{false};
rclcpp_action::Server<DualArm>::SharedPtr server_;
// 通过左右臂各自的 FollowJointTrajectory action 下发轨迹
rclcpp_action::Client<FollowJTA>::SharedPtr left_fjt_client_;
rclcpp_action::Client<FollowJTA>::SharedPtr right_fjt_client_;
// 如需更复杂的执行逻辑（软停止等），可以复用 FollowJTExecutor；当前实现未使用。
std::unique_ptr<FollowJTExecutor> follow_executor_;
};
} // namespace robot_control

13
src/robot_control/include/actions/move_wheel_action.hpp
View File

@@ -5,25 +5,23 @@
#include "rclcpp_action/rclcpp_action.hpp"
#include "interfaces/action/move_wheel.hpp"
#include "interfaces/msg/motor_cmd.hpp"
#include "interfaces/srv/motor_param.hpp"
#include "actions/action_context.hpp"
namespace robot_control
{
class MotorService;
class MoveWheelAction
{
public:
using MoveWheel = interfaces::action::MoveWheel;
using GoalHandleMoveWheel = rclcpp_action::ServerGoalHandle<MoveWheel>;
using MotorCmd = interfaces::msg::MotorCmd;
using MotorParam = interfaces::srv::MotorParam;
MoveWheelAction(
const ActionContext& ctx,
rclcpp::Publisher<MotorCmd>::SharedPtr motor_cmd_pub,
rclcpp::Client<MotorParam>::SharedPtr motor_param_client);
std::shared_ptr<MotorService> motor_service);
void initialize();
@@ -42,8 +40,7 @@ private:
void execute_(const std::shared_ptr<GoalHandleMoveWheel> goal_handle);
ActionContext ctx_;
rclcpp::Publisher<MotorCmd>::SharedPtr motor_cmd_pub_;
rclcpp::Client<MotorParam>::SharedPtr motor_param_client_;
std::shared_ptr<MotorService> motor_service_;
std::atomic<bool> executing_{false};
rclcpp_action::Server<MoveWheel>::SharedPtr server_;
};

6
src/robot_control/include/core/robot_control_manager.hpp
View File

@@ -160,6 +160,12 @@ namespace robot_control
*/
bool ExportDualArmPlannedTrajectory(trajectory_msgs::msg::JointTrajectory& out) const;
/**
* @brief 导出最近一次单臂规划轨迹（6关节，包含 time/pos/vel/acc）
* @param arm_id 手臂ID（0=左臂，1=右臂）
*/
bool ExportArmPlannedTrajectory(uint8_t arm_id, trajectory_msgs::msg::JointTrajectory& out) const;
// ==================== 状态查询接口 ====================
/**

23
src/robot_control/include/core/robot_control_node.hpp
View File

@@ -21,30 +21,9 @@
#include "core/robot_control_manager.hpp"
#include "core/remote_controller.hpp"
#include "interfaces/action/move_home.hpp"
#include "interfaces/action/move_leg.hpp"
#include "interfaces/action/move_waist.hpp"
#include "interfaces/action/move_wheel.hpp"
#include "interfaces/msg/motor_cmd.hpp"
#include "interfaces/srv/motor_param.hpp"
#include "control_msgs/msg/dynamic_interface_group_values.hpp"
#include <controller_manager_msgs/srv/switch_controller.hpp>
using MoveHome = interfaces::action::MoveHome;
using GoalHandleMoveHome = rclcpp_action::ServerGoalHandle<MoveHome>;
using MoveLeg = interfaces::action::MoveLeg;
using GoalHandleMoveLeg = rclcpp_action::ServerGoalHandle<MoveLeg>;
using MoveWaist = interfaces::action::MoveWaist;
using GoalHandleMoveWaist = rclcpp_action::ServerGoalHandle<MoveWaist>;
using MoveWheel = interfaces::action::MoveWheel;
using GoalHandleMoveWheel = rclcpp_action::ServerGoalHandle<MoveWheel>;
using MotorCmd = interfaces::msg::MotorCmd;
using MotorParam = interfaces::srv::MotorParam;
namespace robot_control
{
// 前向声明
@@ -79,10 +58,8 @@ namespace robot_control
rclcpp::Publisher<control_msgs::msg::DynamicInterfaceGroupValues>::SharedPtr gpioPub_; ///< GPIO发布器
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr joyCommandSub_; ///< 手柄命令订阅器
rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr jointStatesSub_; ///< 关节状态订阅器
rclcpp::Publisher<MotorCmd>::SharedPtr motorCmdPub_; ///< 电机命令发布器
rclcpp::Subscription<MotorPos>::SharedPtr wheelStatesSub_; ///< 轮子状态订阅器
rclcpp::Subscription<ImuMsg>::SharedPtr imuMsgSub_; ///< IMU消息订阅器
rclcpp::Client<MotorParam>::SharedPtr motorParamClient_; ///< 电机参数客户端
rclcpp::Client<controller_manager_msgs::srv::SwitchController>::SharedPtr switch_client_; ///< 控制器切换客户端
// ==================== 核心组件 ====================

55
src/robot_control/include/services/motor_service.hpp Normal file
View File

@@ -0,0 +1,55 @@
/**
* @file motor_service.hpp
* @brief 封装 MotorCmd 发布与 MotorParam 服务调用的服务类
*/
#pragma once
#include <memory>
#include <vector>
#include "rclcpp/rclcpp.hpp"
#include "interfaces/msg/motor_cmd.hpp"
#include "interfaces/srv/motor_param.hpp"
namespace robot_control
{
/**
* @class MotorService
* @brief 负责底层电机相关 ROS 通信（MotorCmd 发布与 MotorParam 服务调用）
*
* 上层（例如 ActionManager / MoveWheelAction）只和本类交互，
* 不直接持有 publisher/client，便于集中管理和今后扩展。
*/
class MotorService
{
public:
using MotorCmd = interfaces::msg::MotorCmd;
using MotorParam = interfaces::srv::MotorParam;
explicit MotorService(rclcpp::Node* node);
/**
* @brief 根据运动距离与比例配置轮子速度（封装 MotorParam 调用逻辑）
* @param move_distance 轮子移动距离
* @param ratio 速度比例（由 RobotControlManager 提供）
* @param is_jog_mode 当前是否处于点动模式
*/
void configure_wheel_speed(double move_distance, double ratio, bool is_jog_mode);
/**
* @brief 发布轮子角度命令（封装 MotorCmd 发布逻辑）
* @param wheel_commands 轮子目标角度（至少包含两个元素）
*/
void publish_wheel_command(const std::vector<double>& wheel_commands);
private:
rclcpp::Node* node_{nullptr};
rclcpp::Publisher<MotorCmd>::SharedPtr motor_cmd_pub_;
rclcpp::Client<MotorParam>::SharedPtr motor_param_client_;
};
} // namespace robot_control

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

@@ -205,7 +205,7 @@ def create_arm_motion_param(arm_id, motion_type, target_joints=None, target_pose
def example_left_and_right_arm_joint_motion():
"""示例：循环测试（类似 right_arm_joint_test.cpp 的关节逐个 up/down 测试）"""
"""示例：测试左臂单独运动、右臂单独运动、双臂同时运动"""
rclpy.init()
client = DualArmActionClient()
@@ -218,148 +218,118 @@ def example_left_and_right_arm_joint_motion():
return
try:
# ========= 配置测试参数（按需修改）=========
arm_type = DualArm.Goal.ARM_LEFT
arm_id = ArmMotionParams.ARM_LEFT
# ========= 用例 1：左臂单独运动（简单 up/down 测试）=========
arm_type_left = DualArm.Goal.ARM_LEFT
arm_id_left = ArmMotionParams.ARM_LEFT
# 6关节 home（rad）
home = [0.0, 1.0, 0.0, 0.0, 0.0, 0.0]
home2 = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
amplitude = 1.0 # 单关节摆动幅度（rad）
cycles = 3 # 每个关节 up/down 循环次数
velocity_scaling = 0.8
acceleration_scaling = 0.8
result_timeout_sec = 30.0
sleep_between_sec = 2.0
dof = 6
home_left = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
target_left = [0.5, -0.3, 0.4, -0.2, 0.1, 0.0]
client.get_logger().info('=' * 60)
client.get_logger().info(f'循环关节测试开始: arm_type={arm_type}, dof={dof}, cycles={cycles}, amplitude={amplitude}rad')
client.get_logger().info('用例 1：左臂单独运动（MOVEJ）')
client.get_logger().info('=' * 60)
for j in range(dof):
client.get_logger().info(f'测试关节 {j} ...')
# move to target
left_param_up = create_arm_motion_param(
arm_id=arm_id_left,
motion_type=ArmMotionParams.MOVEJ,
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}')
for c in range(cycles):
# Move up
if j == 0:
up = list(home)
else:
up = list(home2)
time.sleep(2.0)
up[j] += amplitude / 2.0
up_param = create_arm_motion_param(
arm_id=arm_id,
motion_type=ArmMotionParams.MOVEJ,
target_joints=up
)
# move back home
left_param_home = create_arm_motion_param(
arm_id=arm_id_left,
motion_type=ArmMotionParams.MOVEJ,
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}')
ok, msg = client.send_goal_and_wait(
arm_type=arm_type,
arm_motion_params_list=[up_param],
velocity_scaling=velocity_scaling,
acceleration_scaling=acceleration_scaling,
result_timeout_sec=result_timeout_sec,
)
if not ok:
client.get_logger().error(f'UP 执行失败: joint={j}, cycle={c}, msg={msg}')
break
# ========= 用例 2：右臂单独运动 =========
arm_type_right = DualArm.Goal.ARM_RIGHT
arm_id_right = ArmMotionParams.ARM_RIGHT
time.sleep(sleep_between_sec)
home_right = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
target_right = [-0.4, 0.3, -0.5, 0.2, -0.1, 0.0]
# Move down
if j == 0:
down = list(home)
else:
down = list(home2)
down[j] -= amplitude / 2.0
down_param = create_arm_motion_param(
arm_id=arm_id,
motion_type=ArmMotionParams.MOVEJ,
target_joints=down
)
client.get_logger().info('=' * 60)
client.get_logger().info('用例 2：右臂单独运动（MOVEJ）')
client.get_logger().info('=' * 60)
ok, msg = client.send_goal_and_wait(
arm_type=arm_type,
arm_motion_params_list=[down_param],
velocity_scaling=velocity_scaling,
acceleration_scaling=acceleration_scaling,
result_timeout_sec=result_timeout_sec,
)
if not ok:
client.get_logger().error(f'DOWN 执行失败: joint={j}, cycle={c}, msg={msg}')
break
right_param_up = create_arm_motion_param(
arm_id=arm_id_right,
motion_type=ArmMotionParams.MOVEJ,
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}')
time.sleep(sleep_between_sec)
time.sleep(2.0)
client.get_logger().info('循环关节测试结束')
# 等待一段时间后执行下一个目标
# import time
# time.sleep(2.0)
# # 示例2: 右臂关节运动
# right_target_joints = [-0.1, -0.2, 0.3, -0.4, 0.2, -0.1] # 弧度
# right_param = create_arm_motion_param(
# arm_id=ArmMotionParams.ARM_RIGHT,
# motion_type=ArmMotionParams.MOVEJ,
# target_joints=right_target_joints
# )
# client.get_logger().info('=' * 60)
# client.get_logger().info('示例2: 发送右臂关节运动指令')
# client.get_logger().info('=' * 60)
# future = client.send_goal(
# arm_type=DualArm.Goal.ARM_RIGHT,
# arm_motion_params_list=[right_param],
# velocity_scaling=0.5,
# acceleration_scaling=0.5
# )
# # 等待goal响应
# rclpy.spin_until_future_complete(client, future)
# # 等待执行完成
# if client._get_result_future is not None:
# rclpy.spin_until_future_complete(client, client._get_result_future, timeout_sec=30.0)
# # 示例3: 双臂同时运动
# client.get_logger().info('=' * 60)
# client.get_logger().info('示例3: 发送双臂同时运动指令')
# client.get_logger().info('=' * 60)
# # 重新设置目标关节角度
# left_target_joints_dual = [0.2, 0.3, -0.4, 0.5, -0.3, 0.2]
# right_target_joints_dual = [-0.2, -0.3, 0.4, -0.5, 0.3, -0.2]
# left_param_dual = create_arm_motion_param(
# arm_id=ArmMotionParams.ARM_LEFT,
# motion_type=ArmMotionParams.MOVEJ,
# target_joints=left_target_joints_dual
# )
# right_param_dual = create_arm_motion_param(
# arm_id=ArmMotionParams.ARM_RIGHT,
# motion_type=ArmMotionParams.MOVEJ,
# target_joints=right_target_joints_dual
# )
# future = client.send_goal(
# arm_type=DualArm.Goal.ARM_DUAL,
# arm_motion_params_list=[left_param_dual, right_param_dual],
# velocity_scaling=0.5,
# acceleration_scaling=0.5
# )
# # 等待goal响应
# rclpy.spin_until_future_complete(client, future)
# # 等待执行完成
# if client._get_result_future is not None:
# rclpy.spin_until_future_complete(client, client._get_result_future, timeout_sec=30.0)
right_param_home = create_arm_motion_param(
arm_id=arm_id_right,
motion_type=ArmMotionParams.MOVEJ,
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}')
# ========= 用例 3：双臂同时运动（一起规划，拆分下发）=========
client.get_logger().info('=' * 60)
client.get_logger().info('用例 3：双臂同时运动（ARM_DUAL，MOVEJ）')
client.get_logger().info('=' * 60)
left_target_joints_dual = [0.3, 0.2, -0.3, 0.4, -0.2, 0.1]
right_target_joints_dual = [-0.3, -0.2, 0.3, -0.4, 0.2, -0.1]
left_param_dual = create_arm_motion_param(
arm_id=ArmMotionParams.ARM_LEFT,
motion_type=ArmMotionParams.MOVEJ,
target_joints=left_target_joints_dual,
)
right_param_dual = create_arm_motion_param(
arm_id=ArmMotionParams.ARM_RIGHT,
motion_type=ArmMotionParams.MOVEJ,
target_joints=right_target_joints_dual,
)
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}')
except KeyboardInterrupt:
client.get_logger().info('收到中断信号，取消目标...')

10
src/robot_control/src/actions/action_manager.cpp
View File

@@ -13,19 +13,17 @@
#include "actions/dual_arm_action.hpp"
#include "core/robot_control_manager.hpp"
#include "services/motor_service.hpp"
namespace robot_control
{
ActionManager::ActionManager(
rclcpp::Node* node,
RobotControlManager& robot_control_manager,
rclcpp::Publisher<MotorCmd>::SharedPtr motor_cmd_pub,
rclcpp::Client<MotorParam>::SharedPtr motor_param_client)
RobotControlManager& robot_control_manager)
: node_(node)
, robot_control_manager_(robot_control_manager)
, motor_cmd_pub_(std::move(motor_cmd_pub))
, motor_param_client_(std::move(motor_param_client))
{
motor_service_ = std::make_shared<MotorService>(node_);
}
ActionManager::~ActionManager() = default;
@@ -44,7 +42,7 @@ void ActionManager::initialize()
move_home_ = std::make_unique<MoveHomeAction>(ctx);
move_leg_ = std::make_unique<MoveLegAction>(ctx);
move_waist_ = std::make_unique<MoveWaistAction>(ctx);
move_wheel_ = std::make_unique<MoveWheelAction>(ctx, motor_cmd_pub_, motor_param_client_);
move_wheel_ = std::make_unique<MoveWheelAction>(ctx, motor_service_);
dual_arm_ = std::make_unique<DualArmAction>(ctx);
move_home_->initialize();

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

@@ -17,10 +17,12 @@ void DualArmAction::initialize()
{
using namespace std::placeholders;
// FJT executor (shared client created by ActionManager)
if (ctx_.node && ctx_.robot_control_manager && ctx_.follow_jt_client) {
follow_executor_ = std::make_unique<FollowJTExecutor>(
ctx_.node, *ctx_.robot_control_manager, ctx_.follow_jt_client);
// 通过左右臂各自的 FollowJointTrajectory action 下发轨迹
if (ctx_.node) {
left_fjt_client_ = rclcpp_action::create_client<FollowJTA>(
ctx_.node, "/left_arm_controller/follow_joint_trajectory");
right_fjt_client_ = rclcpp_action::create_client<FollowJTA>(
ctx_.node, "/right_arm_controller/follow_joint_trajectory");
}
server_ = rclcpp_action::create_server<DualArm>(
@@ -44,23 +46,53 @@ rclcpp_action::GoalResponse DualArmAction::handle_goal_(
"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());
if (ctx_.robot_control_manager->IsMoving(MovementPart::ALL)) {
RCLCPP_ERROR(ctx_.node->get_logger(), "Robot is moving");
return rclcpp_action::GoalResponse::REJECT;
}
if (executing_.load()) {
RCLCPP_ERROR(ctx_.node->get_logger(), "Another dual arm goal is executing");
// 统一检查 MoveIt 规划器占用：同一时刻只能进行一组规划
if (planner_busy_.load()) {
RCLCPP_ERROR(ctx_.node->get_logger(), "DualArm goal rejected: MoveIt planner is busy");
return rclcpp_action::GoalResponse::REJECT;
}
// MOVEJ-only for now (dual_arm group planning + single FJT)
for (const auto& arm_param : goal->arm_motion_params) {
const uint8_t ARM_LEFT = goal->ARM_LEFT;
const uint8_t ARM_RIGHT = goal->ARM_RIGHT;
const uint8_t ARM_DUAL = goal->ARM_DUAL;
if (goal->arm_type == ARM_LEFT || goal->arm_type == ARM_RIGHT) {
// 单臂模式：只检查对应手臂状态
const uint8_t arm_id = (goal->arm_type == ARM_LEFT) ? 0 : 1;
// 不允许在同一只手臂正在执行时再次下发该臂的指令，但允许另一只手在动
bool arm_executing = (arm_id == 0) ? executing_left_.load() : executing_right_.load();
if (arm_executing) {
RCLCPP_ERROR(ctx_.node->get_logger(),
"DualArm goal rejected: arm %d is already executing another goal", static_cast<int>(arm_id));
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(),
"DualArm goal rejected: only MOVEJ is supported for now (got motion_type=%d for arm_id=%d)",
arm_param.motion_type, arm_param.arm_id);
return rclcpp_action::GoalResponse::REJECT;
}
// 关节限位检查
std::string err;
if (!ctx_.robot_control_manager->ValidateArmJointTarget(arm_param.arm_id, arm_param.target_joints, &err)) {
RCLCPP_ERROR(ctx_.node->get_logger(),
@@ -68,9 +100,39 @@ rclcpp_action::GoalResponse DualArmAction::handle_goal_(
arm_param.arm_id, err.c_str());
return rclcpp_action::GoalResponse::REJECT;
}
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
}
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
if (goal->arm_type == ARM_DUAL) {
// 双臂模式：两只手都不能在执行，且只允许 MOVEJ
if (executing_left_.load() || executing_right_.load()) {
RCLCPP_ERROR(ctx_.node->get_logger(),
"DualArm goal rejected: left or right arm is executing another goal");
return rclcpp_action::GoalResponse::REJECT;
}
for (const auto& arm_param : goal->arm_motion_params) {
if (arm_param.motion_type != arm_param.MOVEJ) {
RCLCPP_ERROR(ctx_.node->get_logger(),
"DualArm goal rejected: only MOVEJ is supported for now (got motion_type=%d for arm_id=%d)",
arm_param.motion_type, arm_param.arm_id);
return rclcpp_action::GoalResponse::REJECT;
}
std::string err;
if (!ctx_.robot_control_manager->ValidateArmJointTarget(arm_param.arm_id, arm_param.target_joints, &err)) {
RCLCPP_ERROR(ctx_.node->get_logger(),
"DualArm goal rejected: MOVEJ target_joints exceed limits for arm_id %d: %s",
arm_param.arm_id, err.c_str());
return rclcpp_action::GoalResponse::REJECT;
}
}
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
}
RCLCPP_ERROR(ctx_.node->get_logger(),
"DualArm goal rejected: invalid arm_type=%d", static_cast<int>(goal->arm_type));
return rclcpp_action::GoalResponse::REJECT;
}
rclcpp_action::CancelResponse DualArmAction::handle_cancel_(
@@ -423,10 +485,10 @@ void DualArmAction::fill_feedback_from_fjt_(
void DualArmAction::execute_(const std::shared_ptr<GoalHandleDualArm> goal_handle)
{
if (!ctx_.robot_control_manager || !follow_executor_) {
if (!ctx_.robot_control_manager) {
auto result = std::make_shared<DualArm::Result>();
result->success = false;
result->message = "DualArm not initialized (missing robot_control_manager or follow_executor)";
result->message = "DualArm not initialized (missing robot_control_manager)";
goal_handle->abort(result);
return;
}
@@ -439,6 +501,8 @@ 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;
// planning feedback
feedback->status = 0;
@@ -447,12 +511,180 @@ void DualArmAction::execute_(const std::shared_ptr<GoalHandleDualArm> goal_handl
feedback->joints_right.clear();
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;
if ((arm_id == 0 && !left_fjt_client_) || (arm_id == 1 && !right_fjt_client_)) {
result->success = false;
result->message = "FJT client for target arm is null";
goal_handle->abort(result);
return;
}
// 规划器占用检查（非阻塞）
bool expected = false;
if (!planner_busy_.compare_exchange_strong(expected, true)) {
result->success = false;
result->message = "MoveIt planner is busy, cannot plan another arm motion";
goal_handle->abort(result);
return;
}
// 使用单臂规划接口
auto arm_params_msg = goal->arm_motion_params.at(0);
ArmMotionParams arm_params = arm_params_msg;
if (!ctx_.robot_control_manager->PlanArmMotion(
arm_params,
goal->velocity_scaling,
goal->acceleration_scaling)) {
planner_busy_.store(false);
result->success = false;
result->message = "PlanArmMotion failed for single arm";
goal_handle->abort(result);
return;
}
planner_busy_.store(false);
trajectory_msgs::msg::JointTrajectory traj_single;
if (!ctx_.robot_control_manager->ExportArmPlannedTrajectory(arm_id, traj_single)) {
result->success = false;
result->message = "ExportArmPlannedTrajectory failed";
goal_handle->abort(result);
return;
}
auto& fjt_client = (arm_id == 0) ? left_fjt_client_ : right_fjt_client_;
if (!fjt_client->wait_for_action_server(std::chrono::seconds(3))) {
result->success = false;
result->message = "FollowJointTrajectory action server not available for target arm";
goal_handle->abort(result);
return;
}
FollowJTA::Goal goal_msg;
goal_msg.trajectory = traj_single;
auto send_future = fjt_client->async_send_goal(goal_msg);
if (send_future.wait_for(std::chrono::seconds(5)) != std::future_status::ready) {
result->success = false;
result->message = "Timeout waiting FollowJointTrajectory goal acceptance for target arm";
goal_handle->abort(result);
return;
}
auto goal_handle_arm = send_future.get();
if (!goal_handle_arm) {
result->success = false;
result->message = "FollowJointTrajectory goal rejected for target arm";
goal_handle->abort(result);
return;
}
auto res_future = fjt_client->async_get_result(goal_handle_arm);
// 标记对应手臂正在执行
if (arm_id == 0) executing_left_.store(true);
else executing_right_.store(true);
// 使用时间进度估计执行完成
double total_time = 0.0;
if (!traj_single.points.empty()) {
const auto& last_point = traj_single.points.back();
total_time =
static_cast<double>(last_point.time_from_start.sec) +
static_cast<double>(last_point.time_from_start.nanosec) * 1e-9;
}
double total_time_sec = total_time;
rclcpp::Rate loop_rate(50.0);
auto start_time = ctx_.node->now();
while (rclcpp::ok()) {
if (goal_handle->is_canceling()) {
(void)fjt_client->async_cancel_goal(goal_handle_arm);
if (arm_id == 0) executing_left_.store(false);
else executing_right_.store(false);
result->success = false;
result->message = "Single-arm goal canceled";
goal_handle->canceled(result);
return;
}
auto now = ctx_.node->now();
const double elapsed =
(now.seconds() - start_time.seconds());
double prog = 1.0;
if (total_time_sec > 1e-6) {
prog = std::min(1.0, std::max(0.0, elapsed / total_time_sec));
}
feedback->status = 1;
feedback->progress = prog;
goal_handle->publish_feedback(feedback);
bool done = (res_future.wait_for(std::chrono::milliseconds(1)) == std::future_status::ready);
if (elapsed >= total_time_sec && done) {
break;
}
loop_rate.sleep();
}
if (arm_id == 0) executing_left_.store(false);
else executing_right_.store(false);
if (!rclcpp::ok()) {
result->success = false;
result->message = "ROS shutdown during single-arm execution";
goal_handle->abort(result);
return;
}
auto wrapped = res_future.get();
if (wrapped.code != rclcpp_action::ResultCode::SUCCEEDED) {
result->success = false;
result->message = "FollowJointTrajectory failed for target arm";
goal_handle->abort(result);
return;
}
feedback->status = 2;
feedback->progress = 1.0;
goal_handle->publish_feedback(feedback);
result->success = true;
result->message = "Single-arm motion succeeded";
result->final_progress = 1.0;
goal_handle->succeed(result);
return;
}
// ==================== 双臂模式 ====================
if (!left_fjt_client_ || !right_fjt_client_) {
result->success = false;
result->message = "FJT clients for left/right arm are null";
goal_handle->abort(result);
return;
}
// 规划器占用检查（非阻塞）
bool expected = false;
if (!planner_busy_.compare_exchange_strong(expected, true)) {
result->success = false;
result->message = "MoveIt planner is busy, cannot plan dual-arm motion";
goal_handle->abort(result);
return;
}
bool need_left = false;
bool need_right = false;
std::vector<double> left_target;
std::vector<double> right_target;
std::string err;
if (!collect_movej_targets_(*goal, ARM_DOF, &need_left, &left_target, &need_right, &right_target, &err)) {
planner_busy_.store(false);
result->success = false;
result->message = std::string("DualArm goal invalid: ") + err;
goal_handle->abort(result);
@@ -463,12 +695,15 @@ void DualArmAction::execute_(const std::shared_ptr<GoalHandleDualArm> goal_handl
need_left, left_target, need_right, right_target,
goal->velocity_scaling, goal->acceleration_scaling))
{
planner_busy_.store(false);
result->success = false;
result->message = "DualArm MoveIt planning failed (dual_arm group)";
goal_handle->abort(result);
return;
}
planner_busy_.store(false);
feedback->status = 1;
trajectory_msgs::msg::JointTrajectory traj;
@@ -480,47 +715,147 @@ void DualArmAction::execute_(const std::shared_ptr<GoalHandleDualArm> goal_handl
return;
}
// Smooth trajectory start and end to reduce impact
//smooth_trajectory_start_end_(&traj);
// 将 12 关节轨迹拆分为左右臂两条 6 关节轨迹
trajectory_msgs::msg::JointTrajectory traj_left;
trajectory_msgs::msg::JointTrajectory traj_right;
// Recalculate total_time after smoothing
if (!traj.points.empty()) {
const auto& last_point = traj.points.back();
total_time =
static_cast<double>(last_point.time_from_start.sec) +
static_cast<double>(last_point.time_from_start.nanosec) * 1e-9;
traj_left.header = traj.header;
traj_right.header = traj.header;
traj_left.joint_names.assign(
motionParams.dual_arm_joint_names_.begin(),
motionParams.dual_arm_joint_names_.begin() + ARM_DOF);
traj_right.joint_names.assign(
motionParams.dual_arm_joint_names_.begin() + ARM_DOF,
motionParams.dual_arm_joint_names_.begin() + 2 * ARM_DOF);
for (const auto& p : traj.points) {
trajectory_msgs::msg::JointTrajectoryPoint lp;
trajectory_msgs::msg::JointTrajectoryPoint rp;
if (p.positions.size() >= 2 * ARM_DOF) {
lp.positions.assign(p.positions.begin(), p.positions.begin() + ARM_DOF);
rp.positions.assign(p.positions.begin() + ARM_DOF, p.positions.begin() + 2 * ARM_DOF);
}
if (p.velocities.size() >= 2 * ARM_DOF) {
lp.velocities.assign(p.velocities.begin(), p.velocities.begin() + ARM_DOF);
rp.velocities.assign(p.velocities.begin() + ARM_DOF, p.velocities.begin() + 2 * ARM_DOF);
}
if (p.accelerations.size() >= 2 * ARM_DOF) {
lp.accelerations.assign(p.accelerations.begin(), p.accelerations.begin() + ARM_DOF);
rp.accelerations.assign(p.accelerations.begin() + ARM_DOF, p.accelerations.begin() + 2 * ARM_DOF);
}
lp.time_from_start = p.time_from_start;
rp.time_from_start = p.time_from_start;
traj_left.points.push_back(std::move(lp));
traj_right.points.push_back(std::move(rp));
}
// 通过左右臂各自的 FollowJointTrajectory action 下发
if (!left_fjt_client_->wait_for_action_server(std::chrono::seconds(3)) ||
!right_fjt_client_->wait_for_action_server(std::chrono::seconds(3))) {
result->success = false;
result->message = "FollowJointTrajectory action server not available for left/right arm controller";
goal_handle->abort(result);
return;
}
FollowJTA::Goal left_goal;
left_goal.trajectory = traj_left;
FollowJTA::Goal right_goal;
right_goal.trajectory = traj_right;
auto left_future = left_fjt_client_->async_send_goal(left_goal);
auto right_future = right_fjt_client_->async_send_goal(right_goal);
if (left_future.wait_for(std::chrono::seconds(5)) != std::future_status::ready ||
right_future.wait_for(std::chrono::seconds(5)) != std::future_status::ready) {
result->success = false;
result->message = "Timeout waiting FollowJointTrajectory goal acceptance for left/right arm";
goal_handle->abort(result);
return;
}
auto left_handle = left_future.get();
auto right_handle = right_future.get();
if (!left_handle || !right_handle) {
result->success = false;
result->message = "FollowJointTrajectory goal rejected by left/right arm controller";
goal_handle->abort(result);
return;
}
auto left_res_future = left_fjt_client_->async_get_result(left_handle);
auto right_res_future = right_fjt_client_->async_get_result(right_handle);
// 使用时间进度简单估计执行完成，同时轮询 action 结果与取消请求
feedback->progress = 0.0;
feedback->joints_left.clear();
feedback->joints_right.clear();
goal_handle->publish_feedback(feedback);
executing_.store(true);
executing_left_.store(true);
executing_right_.store(true);
auto on_fjt_feedback =
[this, motionParams, ARM_DOF, total_time, feedback, goal_handle](const FollowJTA::Feedback& fjt_fb) {
fill_feedback_from_fjt_(fjt_fb, motionParams, ARM_DOF, total_time, feedback.get());
goal_handle->publish_feedback(feedback);
};
const double total_time_sec = total_time;
rclcpp::Rate loop_rate(50.0);
auto start_time = ctx_.node->now();
auto exec_res = follow_executor_->send_and_wait(
traj,
[&]() { return goal_handle->is_canceling(); },
on_fjt_feedback);
while (rclcpp::ok()) {
if (goal_handle->is_canceling()) {
(void)left_fjt_client_->async_cancel_goal(left_handle);
(void)right_fjt_client_->async_cancel_goal(right_handle);
executing_.store(false);
executing_left_.store(false);
executing_right_.store(false);
result->success = false;
result->message = "DualArm goal canceled";
goal_handle->canceled(result);
return;
}
auto now = ctx_.node->now();
const double elapsed =
(now.seconds() - start_time.seconds());
double prog = 1.0;
if (total_time_sec > 1e-6) {
prog = std::min(1.0, std::max(0.0, elapsed / total_time_sec));
}
feedback->status = 1;
feedback->progress = prog;
goal_handle->publish_feedback(feedback);
// 同时检查左右臂 action 是否都完成
bool left_done = (left_res_future.wait_for(std::chrono::milliseconds(1)) == std::future_status::ready);
bool right_done = (right_res_future.wait_for(std::chrono::milliseconds(1)) == std::future_status::ready);
if (elapsed >= total_time_sec && left_done && right_done) {
break;
}
loop_rate.sleep();
}
executing_.store(false);
executing_left_.store(false);
executing_right_.store(false);
if (goal_handle->is_canceling()) {
// 获取左右臂执行结果
if (!rclcpp::ok()) {
result->success = false;
result->message = "DualArm goal canceled";
goal_handle->canceled(result);
result->message = "ROS shutdown during DualArm execution";
goal_handle->abort(result);
return;
}
if (!exec_res.ok) {
auto left_wrapped = left_res_future.get();
auto right_wrapped = right_res_future.get();
if (left_wrapped.code != rclcpp_action::ResultCode::SUCCEEDED ||
right_wrapped.code != rclcpp_action::ResultCode::SUCCEEDED) {
result->success = false;
result->message = std::string("FollowJointTrajectory failed: ") + exec_res.message;
result->message = "FollowJointTrajectory failed on left/right arm controller";
goal_handle->abort(result);
return;
}

41
src/robot_control/src/actions/move_wheel_action.cpp
View File

@@ -5,16 +5,15 @@
#include "core/robot_control_manager.hpp"
#include "core/common_enum.hpp"
#include "services/motor_service.hpp"
namespace robot_control
{
MoveWheelAction::MoveWheelAction(
const ActionContext& ctx,
rclcpp::Publisher<MotorCmd>::SharedPtr motor_cmd_pub,
rclcpp::Client<MotorParam>::SharedPtr motor_param_client)
std::shared_ptr<MotorService> motor_service)
: ctx_(ctx)
, motor_cmd_pub_(std::move(motor_cmd_pub))
, motor_param_client_(std::move(motor_param_client))
, motor_service_(std::move(motor_service))
{
}
@@ -93,33 +92,19 @@ void MoveWheelAction::execute_(const std::shared_ptr<GoalHandleMoveWheel> goal_h
ctx_.robot_control_manager->SetMoveWheelParameters(goal->move_distance, wheelAngle);
double ratio = ctx_.robot_control_manager->GetWheelRatio();
bool is_jog_mode = ctx_.robot_control_manager->GetJogWheel();
auto request = std::make_shared<MotorParam::Request>();
if ((goal->move_distance > 0.1) && !ctx_.robot_control_manager->GetJogWheel()) {
request->motor_id = 1;
request->max_speed = static_cast<uint16_t>(std::round((ratio) * 51));
request->add_acc = 8;
request->dec_acc = 8;
if (motor_param_client_) motor_param_client_->async_send_request(request);
rclcpp::sleep_for(std::chrono::milliseconds(50));
if (motor_service_) {
motor_service_->configure_wheel_speed(goal->move_distance, ratio, is_jog_mode);
}
ctx_.robot_control_manager->MoveWheel();
const auto& wheel_commands = ctx_.robot_control_manager->GetWheelCommandPositions();
MotorCmd wheel_commands_msg;
wheel_commands_msg.target = "rs485";
wheel_commands_msg.type = "bm";
wheel_commands_msg.position = "";
wheel_commands_msg.motor_id = {1, 2};
if (wheel_commands.size() >= 2) {
wheel_commands_msg.motor_angle = {(float)wheel_commands[0], (float)wheel_commands[1]};
if (motor_service_) {
motor_service_->publish_wheel_command(wheel_commands);
}
if (motor_cmd_pub_) motor_cmd_pub_->publish(wheel_commands_msg);
while (executing_.load() && rclcpp::ok()) {
if (goal_handle->is_canceling()) {
result->success = false;
@@ -142,12 +127,12 @@ void MoveWheelAction::execute_(const std::shared_ptr<GoalHandleMoveWheel> goal_h
loop_rate.sleep();
}
// 保持与原逻辑一致：在非点动模式下运动完成后，将速度恢复默认值
if ((goal->move_distance > 0.0) && !ctx_.robot_control_manager->GetJogWheel()) {
request->motor_id = 1;
request->max_speed = 51;
request->add_acc = 8;
request->dec_acc = 8;
if (motor_param_client_) motor_param_client_->async_send_request(request);
double default_ratio = 1.0;
if (motor_service_) {
motor_service_->configure_wheel_speed(0.2, default_ratio, false);
}
}
if (rclcpp::ok()) {

7
src/robot_control/src/core/robot_control_manager.cpp
View File

@@ -256,6 +256,13 @@ bool RobotControlManager::ExportDualArmPlannedTrajectory(trajectory_msgs::msg::J
return arm_controller_->ExportPlannedJointTrajectory(2, out);
}
bool RobotControlManager::ExportArmPlannedTrajectory(uint8_t arm_id, trajectory_msgs::msg::JointTrajectory& out) const
{
if (!arm_controller_enabled_ || !arm_controller_) return false;
if (arm_id != 0 && arm_id != 1) return false;
return arm_controller_->ExportPlannedJointTrajectory(arm_id, out);
}
const std::vector<double>& RobotControlManager::GetJointPositions() const
{
return jointPositions_;

10
src/robot_control/src/core/robot_control_node.cpp
View File

@@ -13,9 +13,7 @@ RobotControlNode::RobotControlNode() : Node("robot_control_node")
RCLCPP_INFO(this->get_logger(), "robot_control_node Node is creating");
is_robot_enabled_ = false;
// 创建发布器和客户端（ActionManager 需要）
motorCmdPub_ = this->create_publisher<MotorCmd>("/motor_cmd", 10);
motorParamClient_ = this->create_client<MotorParam>("/motor_param");
// 创建发布器和客户端
gpioPub_ = this->create_publisher<control_msgs::msg::DynamicInterfaceGroupValues>("/gpio_command_controller/commands", 10);
switch_client_ = create_client<controller_manager_msgs::srv::SwitchController>("/controller_manager/switch_controller");
imuMsgSub_ = this->create_subscription<ImuMsg>("/openzen/imu_msg", 10,std::bind(&RobotControlNode::ImuMsgCallback, this, std::placeholders::_1));
@@ -28,12 +26,10 @@ RobotControlNode::RobotControlNode() : Node("robot_control_node")
this,
robotControlManager_);
// 初始化 ActionManager（在创建发布器和客户端之后）
// 初始化 ActionManager（内部会创建所需的 MotorService）
action_manager_ = std::make_unique<ActionManager>(
this,
robotControlManager_,
motorCmdPub_,
motorParamClient_);
robotControlManager_);
action_manager_->initialize();
const bool ret = activateController("trajectory_controller");

58
src/robot_control/src/services/motor_service.cpp Normal file
View File

@@ -0,0 +1,58 @@
#include "services/motor_service.hpp"
#include <cmath>
namespace robot_control
{
MotorService::MotorService(rclcpp::Node* node)
: node_(node)
{
if (!node_) {
throw std::runtime_error("MotorService requires a valid rclcpp::Node pointer");
}
motor_cmd_pub_ = node_->create_publisher<MotorCmd>("/motor_cmd", 10);
motor_param_client_ = node_->create_client<MotorParam>("/motor_param");
}
void MotorService::configure_wheel_speed(double move_distance, double ratio, bool is_jog_mode)
{
// 与原 MoveWheelAction 中逻辑保持一致：
// 仅在向前运动且非点动模式时，调整电机速度到 ratio * 51
if ((move_distance > 0.1) && !is_jog_mode) {
auto request = std::make_shared<MotorParam::Request>();
request->motor_id = 1;
request->max_speed = static_cast<uint16_t>(std::round((ratio) * 51));
request->add_acc = 8;
request->dec_acc = 8;
if (motor_param_client_) {
motor_param_client_->async_send_request(request);
}
}
}
void MotorService::publish_wheel_command(const std::vector<double>& wheel_commands)
{
if (!motor_cmd_pub_) {
return;
}
MotorCmd wheel_commands_msg;
wheel_commands_msg.target = "rs485";
wheel_commands_msg.type = "bm";
wheel_commands_msg.position = "";
wheel_commands_msg.motor_id = {1, 2};
if (wheel_commands.size() >= 2) {
wheel_commands_msg.motor_angle = {
static_cast<float>(wheel_commands[0]),
static_cast<float>(wheel_commands[1])};
}
motor_cmd_pub_->publish(wheel_commands_msg);
}
} // namespace robot_control