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

add more services

Browse Source
This commit is contained in:
NuoDaJia
2026-03-03 14:41:32 +08:00
parent 1a5309f9c1
commit 99b3d4a7a2
13 changed files with 1020 additions and 81 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
src/robot_control/CMakeLists.txt
View File

@@ -55,6 +55,9 @@ set(SOURCES
src/core/controller_factory.cpp
src/core/remote_controller.cpp
src/services/motor_service.cpp
src/services/arm_hardware_service.cpp
src/services/kinematics_service.cpp
src/services/arm_status_service.cpp
src/core/motion_parameters.cpp
src/actions/action_manager.cpp
src/actions/move_home_action.cpp

40
src/robot_control/include/controllers/arm_control.hpp
View File

@@ -107,6 +107,46 @@ namespace robot_control
double velocity_scaling,
double acceleration_scaling);
/**
* @brief 仅进行逆解/可达性求解，不写入执行轨迹
* @param arm_id 手臂ID（0=左臂，1=右臂）
* @param target_pose 目标位姿
* @param out_joint_angles 输出关节角（弧度）
* @param error_msg 输出错误信息（可选）
* @return true 求解成功，false 失败
*/
bool SolveInverseKinematics(
uint8_t arm_id,
const geometry_msgs::msg::Pose& target_pose,
std::vector<double>* out_joint_angles,
std::string* error_msg = nullptr) const;
/**
* @brief 获取当前末端位姿（MoveGroup 当前状态）
* @param arm_id 手臂ID（0=左臂，1=右臂）
* @param out_pose 输出位姿
* @param error_msg 输出错误信息（可选）
* @return true 获取成功，false 失败
*/
bool GetCurrentEndEffectorPose(
uint8_t arm_id,
geometry_msgs::msg::Pose* out_pose,
std::string* error_msg = nullptr) const;
/**
* @brief 根据给定关节角计算当前臂末端位姿（FK）
* @param arm_id 手臂ID（0=左臂，1=右臂）
* @param joint_values 该臂关节角（弧度）
* @param out_pose 输出位姿
* @param error_msg 输出错误信息（可选）
* @return true 成功，false 失败
*/
bool ComputeEndEffectorPoseFromJointValues(
uint8_t arm_id,
const std::vector<double>& joint_values,
geometry_msgs::msg::Pose* out_pose,
std::string* error_msg = nullptr) const;
/**
* @brief 检查MoveIt是否已初始化
* @return true 所有move group已初始化，false 未初始化

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

@@ -21,6 +21,7 @@
#include "sensor_msgs/msg/joint_state.hpp"
#include "sensor_msgs/msg/imu.hpp"
#include "trajectory_msgs/msg/joint_trajectory.hpp"
#include "geometry_msgs/msg/pose.hpp"
#include "core/motion_parameters.hpp"
#include "controllers/arm_control.hpp"
#include "controllers/leg_control.hpp"
@@ -155,6 +156,32 @@ namespace robot_control
double velocity_scaling,
double acceleration_scaling);
/**
* @brief 单臂逆解（通过 MoveIt）
* @param arm_id 手臂ID（0=左臂，1=右臂）
* @param target_pose 目标笛卡尔位姿
* @param out_joint_angles 输出关节角（弧度）
* @param error_msg 输出错误信息（可选）
* @return true 成功，false 失败
*/
bool SolveArmInverseKinematics(
uint8_t arm_id,
const geometry_msgs::msg::Pose& target_pose,
std::vector<double>* out_joint_angles,
std::string* error_msg = nullptr) const;
/**
* @brief 获取单臂当前末端笛卡尔位姿（相对 MoveGroup 基座）
* @param arm_id 手臂ID（0=左臂，1=右臂）
* @param out_pose 输出位姿
* @param error_msg 输出错误信息（可选）
* @return true 成功，false 失败
*/
bool GetArmCurrentPose(
uint8_t arm_id,
geometry_msgs::msg::Pose* out_pose,
std::string* error_msg = nullptr) const;
/**
* @brief 导出最近一次双臂规划轨迹（12关节，包含 time/pos/vel/acc）
*/
@@ -275,6 +302,20 @@ namespace robot_control
* @return 索引列表
*/
std::vector<size_t> GetJointIndicesFromNames(const std::vector<std::string>& joint_names) const;
/**
* @brief 按关节名构建子控制器所需的 positions/velocities/efforts
* @param joint_names 目标关节名列表
* @param out_positions 输出位置
* @param out_velocities 输出速度
* @param out_efforts 输出力矩
* @return true 构建成功且有至少一个关节数据，false 失败或无有效数据
*/
bool BuildControllerJointStatesByNames(
const std::vector<std::string>& joint_names,
std::vector<double>* out_positions,
std::vector<double>* out_velocities,
std::vector<double>* out_efforts) const;
// Quaternion conversion utilities (private helpers)
void QuaternionToRPYRad(double qw, double qx, double qy, double qz, double &roll, double &pitch, double &yaw);

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

@@ -14,15 +14,25 @@
#pragma once
#include <memory>
#include <cstdint>
#include <vector>
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "std_msgs/msg/float64_multi_array.hpp"
#include "std_msgs/msg/string.hpp"
#include "core/robot_control_manager.hpp"
#include "core/remote_controller.hpp"
#include "services/arm_hardware_service.hpp"
#include "services/kinematics_service.hpp"
#include "services/arm_status_service.hpp"
#include "control_msgs/msg/dynamic_interface_group_values.hpp"
#include "control_msgs/msg/dynamic_joint_state.hpp"
#include <controller_manager_msgs/srv/switch_controller.hpp>
#include "interfaces/srv/inverse_kinematics.hpp"
#include "interfaces/srv/set_arm_enable.hpp"
#include "interfaces/srv/clear_arm_error.hpp"
#include "interfaces/msg/robot_arm_status.hpp"
namespace robot_control
{
@@ -57,20 +67,31 @@ namespace robot_control
rclcpp::Publisher<control_msgs::msg::DynamicInterfaceGroupValues>::SharedPtr left_gpio_pub_; ///< 左臂GPIO发布器
rclcpp::Publisher<control_msgs::msg::DynamicInterfaceGroupValues>::SharedPtr right_gpio_pub_; ///< 右臂GPIO发布器
rclcpp::Publisher<interfaces::msg::RobotArmStatus>::SharedPtr robot_arm_status_pub_; ///< 机械臂状态发布器
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr joyCommandSub_; ///< 手柄命令订阅器
rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr jointStatesSub_; ///< 关节状态订阅器
rclcpp::Subscription<MotorPos>::SharedPtr wheelStatesSub_; ///< 轮子状态订阅器
rclcpp::Subscription<ImuMsg>::SharedPtr imuMsgSub_; ///< IMU消息订阅器
rclcpp::Subscription<control_msgs::msg::DynamicJointState>::SharedPtr dynamicJointStatesSub_; ///< 动态关节状态订阅器
rclcpp::Client<controller_manager_msgs::srv::SwitchController>::SharedPtr switch_client_; ///< 控制器切换客户端
rclcpp::Service<interfaces::srv::InverseKinematics>::SharedPtr inverse_kinematics_srv_; ///< 单臂逆解服务
rclcpp::Service<interfaces::srv::SetArmEnable>::SharedPtr set_arm_enable_srv_; ///< 上下使能服务
rclcpp::Service<interfaces::srv::ClearArmError>::SharedPtr clear_arm_error_srv_; ///< 清错服务
rclcpp::TimerBase::SharedPtr robot_arm_status_timer_; ///< 状态发布定时器
// ==================== 核心组件 ====================
RobotControlManager robotControlManager_; ///< 机器人控制管理器
std::unique_ptr<ActionManager> action_manager_; ///< 动作管理器
std::unique_ptr<RemoteController> remoteController_; ///< 遥控器控制器
std::unique_ptr<ArmHardwareService> arm_hardware_service_; ///< 机械臂GPIO服务实现
std::unique_ptr<KinematicsService> kinematics_service_; ///< 运动学服务实现
std::unique_ptr<ArmStatusService> arm_status_service_; ///< 机械臂状态服务实现
// ==================== 控制状态 ====================
bool is_robot_enabled_; ///< 机器人是否启用
std::vector<bool> joint_enabled_cache_; ///< 关节使能缓存（双臂顺序）
std::vector<int32_t> joint_error_code_cache_; ///< 关节错误码缓存（双臂顺序）
// ==================== 回调函数 ====================
@@ -97,8 +118,6 @@ namespace robot_control
* @param msg IMU消息
*/
void ImuMsgCallback(const ImuMsg::SharedPtr msg);
/**
* @brief 激活指定名称的控制器
* @param controller_name 控制器名称
@@ -130,7 +149,6 @@ namespace robot_control
* @param enable 使能值（1=使能，0=禁用）
*/
void motor_enable(int id, double enable);
/**
* @brief 根据 dual_arm_joint_names_ 拆分左右臂关节名称
*/

64
src/robot_control/include/services/arm_hardware_service.hpp Normal file
View File

@@ -0,0 +1,64 @@
#pragma once
#include <cstdint>
#include <memory>
#include <vector>
#include "rclcpp/rclcpp.hpp"
#include "control_msgs/msg/dynamic_interface_group_values.hpp"
#include "interfaces/srv/set_arm_enable.hpp"
#include "interfaces/srv/clear_arm_error.hpp"
#include "core/robot_control_manager.hpp"
namespace robot_control
{
class ArmHardwareService
{
public:
ArmHardwareService(
rclcpp::Node* node,
RobotControlManager* manager,
const rclcpp::Publisher<control_msgs::msg::DynamicInterfaceGroupValues>::SharedPtr& left_gpio_pub,
const rclcpp::Publisher<control_msgs::msg::DynamicInterfaceGroupValues>::SharedPtr& right_gpio_pub,
std::vector<bool>* joint_enabled_cache,
std::vector<int32_t>* joint_error_code_cache);
void HandleSetArmEnable(
const std::shared_ptr<interfaces::srv::SetArmEnable::Request> request,
std::shared_ptr<interfaces::srv::SetArmEnable::Response> response);
void HandleClearArmError(
const std::shared_ptr<interfaces::srv::ClearArmError::Request> request,
std::shared_ptr<interfaces::srv::ClearArmError::Response> response);
private:
bool validate_arm_and_joint_(int8_t arm_id, int8_t joint_num) const;
bool set_motor_enable_by_scope_(int8_t arm_id, int8_t joint_num, bool enable);
bool clear_motor_error_by_scope_(int8_t arm_id, int8_t joint_num);
bool split_arm_joint_names_(
const MotionParameters& motion_params,
std::vector<std::string>& left,
std::vector<std::string>& right) const;
void publish_gpio_command_for_arm_(
const rclcpp::Publisher<control_msgs::msg::DynamicInterfaceGroupValues>::SharedPtr& pub,
const std::vector<std::string>& joints,
const std::string& interface_name,
int local_joint_num,
double value) const;
void update_cached_joint_state_(
int8_t arm_id,
int8_t joint_num,
bool enabled,
int32_t error_code);
void clear_error_cache_(int8_t arm_id, int8_t joint_num);
rclcpp::Node* node_{nullptr};
RobotControlManager* manager_{nullptr};
rclcpp::Publisher<control_msgs::msg::DynamicInterfaceGroupValues>::SharedPtr left_gpio_pub_;
rclcpp::Publisher<control_msgs::msg::DynamicInterfaceGroupValues>::SharedPtr right_gpio_pub_;
std::vector<bool>* joint_enabled_cache_{nullptr};
std::vector<int32_t>* joint_error_code_cache_{nullptr};
};
} // namespace robot_control

36
src/robot_control/include/services/arm_status_service.hpp Normal file
View File

@@ -0,0 +1,36 @@
#pragma once
#include <cstdint>
#include <memory>
#include <vector>
#include "rclcpp/rclcpp.hpp"
#include "control_msgs/msg/dynamic_joint_state.hpp"
#include "interfaces/msg/robot_arm_status.hpp"
#include "core/robot_control_manager.hpp"
namespace robot_control
{
class ArmStatusService
{
public:
ArmStatusService(
rclcpp::Node* node,
RobotControlManager* manager,
const rclcpp::Publisher<interfaces::msg::RobotArmStatus>::SharedPtr& status_pub,
std::vector<bool>* joint_enabled_cache,
std::vector<int32_t>* joint_error_code_cache);
void OnDynamicJointStates(const control_msgs::msg::DynamicJointState::SharedPtr msg);
void PublishRobotArmStatus();
private:
rclcpp::Node* node_{nullptr};
RobotControlManager* manager_{nullptr};
rclcpp::Publisher<interfaces::msg::RobotArmStatus>::SharedPtr status_pub_;
std::vector<bool>* joint_enabled_cache_{nullptr};
std::vector<int32_t>* joint_error_code_cache_{nullptr};
};
} // namespace robot_control

26
src/robot_control/include/services/kinematics_service.hpp Normal file
View File

@@ -0,0 +1,26 @@
#pragma once
#include <memory>
#include "rclcpp/rclcpp.hpp"
#include "interfaces/srv/inverse_kinematics.hpp"
#include "core/robot_control_manager.hpp"
namespace robot_control
{
class KinematicsService
{
public:
KinematicsService(rclcpp::Node* node, RobotControlManager* manager);
void HandleInverseKinematics(
const std::shared_ptr<interfaces::srv::InverseKinematics::Request> request,
std::shared_ptr<interfaces::srv::InverseKinematics::Response> response);
private:
rclcpp::Node* node_{nullptr};
RobotControlManager* manager_{nullptr};
};
} // namespace robot_control

168
src/robot_control/src/controllers/arm_control.cpp
View File

@@ -6,8 +6,11 @@
#include <cmath>
#include <algorithm>
#include <limits>
#include <unordered_map>
#include <Eigen/Geometry>
#include "rclcpp/rclcpp.hpp"
#include "moveit/move_group_interface/move_group_interface.h"
#include "moveit/robot_state/robot_state.h"
#include "moveit_msgs/msg/move_it_error_codes.hpp"
#ifndef M_PI
@@ -306,6 +309,171 @@ bool ArmControl::PlanCartesianMotion(
}
}
bool ArmControl::SolveInverseKinematics(
uint8_t arm_id,
const geometry_msgs::msg::Pose& target_pose,
std::vector<double>* out_joint_angles,
std::string* error_msg) const
{
if (!out_joint_angles) {
if (error_msg) *error_msg = "out_joint_angles is null";
return false;
}
auto move_group = GetMoveGroup(arm_id);
if (!move_group) {
if (error_msg) *error_msg = "invalid arm_id or MoveGroup not initialized";
return false;
}
try {
moveit::core::RobotStatePtr state = move_group->getCurrentState(2.0);
if (!state) {
if (error_msg) *error_msg = "Failed to get current robot state";
return false;
}
const moveit::core::JointModelGroup* jmg =
state->getJointModelGroup(move_group->getName());
if (!jmg) {
if (error_msg) *error_msg = "JointModelGroup not found for " + move_group->getName();
return false;
}
const bool ik_ok = state->setFromIK(jmg, target_pose, 0.05);
if (!ik_ok) {
if (error_msg) *error_msg = "setFromIK failed";
return false;
}
std::vector<double> group_positions;
state->copyJointGroupPositions(jmg, group_positions);
const std::vector<std::string>& group_joint_names = jmg->getVariableNames();
if (group_positions.size() != group_joint_names.size() || group_positions.empty()) {
if (error_msg) *error_msg = "IK output size mismatch";
return false;
}
// 按 MoveGroup 对外 joint 顺序重排，确保 service 返回顺序稳定。
std::unordered_map<std::string, double> value_by_name;
value_by_name.reserve(group_joint_names.size());
for (size_t i = 0; i < group_joint_names.size(); ++i) {
value_by_name[group_joint_names[i]] = group_positions[i];
}
const std::vector<std::string>& target_order = move_group->getJointNames();
out_joint_angles->clear();
out_joint_angles->reserve(target_order.size());
for (const auto& name : target_order) {
auto it = value_by_name.find(name);
if (it == value_by_name.end()) {
if (error_msg) *error_msg = "IK result missing joint: " + name;
return false;
}
out_joint_angles->push_back(it->second);
}
return !out_joint_angles->empty();
} catch (const std::exception& e) {
if (error_msg) *error_msg = e.what();
return false;
}
}
bool ArmControl::GetCurrentEndEffectorPose(
uint8_t arm_id,
geometry_msgs::msg::Pose* out_pose,
std::string* error_msg) const
{
if (!out_pose) {
if (error_msg) *error_msg = "out_pose is null";
return false;
}
auto move_group = GetMoveGroup(arm_id);
if (!move_group) {
if (error_msg) *error_msg = "invalid arm_id or MoveGroup not initialized";
return false;
}
try {
*out_pose = move_group->getCurrentPose().pose;
return true;
} catch (const std::exception& e) {
if (error_msg) *error_msg = e.what();
return false;
}
}
bool ArmControl::ComputeEndEffectorPoseFromJointValues(
uint8_t arm_id,
const std::vector<double>& joint_values,
geometry_msgs::msg::Pose* out_pose,
std::string* error_msg) const
{
if (!out_pose) {
if (error_msg) *error_msg = "out_pose is null";
return false;
}
auto move_group = GetMoveGroup(arm_id);
if (!move_group) {
if (error_msg) *error_msg = "invalid arm_id or MoveGroup not initialized";
return false;
}
try {
moveit::core::RobotStatePtr state = move_group->getCurrentState(2.0);
if (!state) {
if (error_msg) *error_msg = "Failed to get current robot state";
return false;
}
const moveit::core::JointModelGroup* jmg =
state->getJointModelGroup(move_group->getName());
if (!jmg) {
if (error_msg) *error_msg = "JointModelGroup not found for " + move_group->getName();
return false;
}
const size_t expected = jmg->getVariableCount();
if (joint_values.size() != expected) {
if (error_msg) {
*error_msg = "joint_values size mismatch, expected " +
std::to_string(expected) + ", got " +
std::to_string(joint_values.size());
}
return false;
}
state->setJointGroupPositions(jmg, joint_values);
state->update();
std::string tip_link = move_group->getEndEffectorLink();
if (tip_link.empty()) {
const auto& links = move_group->getLinkNames();
if (links.empty()) {
if (error_msg) *error_msg = "No link names available in MoveGroup";
return false;
}
tip_link = links.back();
}
const Eigen::Isometry3d& tf = state->getGlobalLinkTransform(tip_link);
out_pose->position.x = tf.translation().x();
out_pose->position.y = tf.translation().y();
out_pose->position.z = tf.translation().z();
const Eigen::Quaterniond q(tf.rotation());
out_pose->orientation.w = q.w();
out_pose->orientation.x = q.x();
out_pose->orientation.y = q.y();
out_pose->orientation.z = q.z();
return true;
} catch (const std::exception& e) {
if (error_msg) *error_msg = e.what();
return false;
}
}
bool ArmControl::timeParameterizeAndStore_(
uint8_t arm_id,
moveit::planning_interface::MoveGroupInterface::Plan& plan,

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

@@ -251,6 +251,67 @@ bool RobotControlManager::PlanDualArmJointMotion(
return arm_controller_->PlanJointMotion(2, target, velocity_scaling, acceleration_scaling);
}
bool RobotControlManager::SolveArmInverseKinematics(
uint8_t arm_id,
const geometry_msgs::msg::Pose& target_pose,
std::vector<double>* out_joint_angles,
std::string* error_msg) const
{
if (!arm_controller_enabled_ || !arm_controller_) {
if (error_msg) *error_msg = "Arm controller disabled";
return false;
}
if (arm_id != 0 && arm_id != 1) {
if (error_msg) *error_msg = "Invalid arm_id (expected 0=left, 1=right)";
return false;
}
if (!arm_controller_->IsMoveItInitialized()) {
if (error_msg) *error_msg = "MoveIt not initialized";
return false;
}
return arm_controller_->SolveInverseKinematics(arm_id, target_pose, out_joint_angles, error_msg);
}
bool RobotControlManager::GetArmCurrentPose(
uint8_t arm_id,
geometry_msgs::msg::Pose* out_pose,
std::string* error_msg) const
{
if (!arm_controller_enabled_ || !arm_controller_) {
if (error_msg) *error_msg = "Arm controller disabled";
return false;
}
if (arm_id != 0 && arm_id != 1) {
if (error_msg) *error_msg = "Invalid arm_id (expected 0=left, 1=right)";
return false;
}
if (!arm_controller_->IsMoveItInitialized()) {
if (error_msg) *error_msg = "MoveIt not initialized";
return false;
}
const size_t arm_dof = motionParams_.arm_dof_;
if (motionParams_.dual_arm_joint_names_.size() < 2 * arm_dof) {
if (error_msg) *error_msg = "dual_arm_joint_names_ size is invalid";
return false;
}
std::vector<std::string> arm_joint_names;
arm_joint_names.reserve(arm_dof);
const size_t start_idx = (arm_id == 0) ? 0 : arm_dof;
for (size_t i = 0; i < arm_dof; ++i) {
arm_joint_names.push_back(motionParams_.dual_arm_joint_names_[start_idx + i]);
}
std::vector<double> joint_values;
if (!GetJointPositionsByNames(arm_joint_names, &joint_values, error_msg)) {
return false;
}
return arm_controller_->ComputeEndEffectorPoseFromJointValues(
arm_id, joint_values, out_pose, error_msg);
}
bool RobotControlManager::ExportDualArmPlannedTrajectory(trajectory_msgs::msg::JointTrajectory& out) const
{
if (!arm_controller_enabled_ || !arm_controller_) return false;
@@ -507,9 +568,17 @@ void RobotControlManager::UpdateJointStates(const sensor_msgs::msg::JointState::
for (size_t i = 0; i < jointStates_.name.size(); i++)
{
if (joint_name_mapping_initialized_ && motionParams_.joint_name_to_index_.find(jointStates_.name[i]) != motionParams_.joint_name_to_index_.end())
{
size_t internal_index = motionParams_.joint_name_to_index_[jointStates_.name[i]];
if (joint_name_mapping_initialized_) {
auto it = motionParams_.joint_name_to_index_.find(jointStates_.name[i]);
if (it == motionParams_.joint_name_to_index_.end()) {
continue;
}
const size_t internal_index = it->second;
if (internal_index >= jointPositions_.size() ||
internal_index >= jointVelocities_.size() ||
internal_index >= jointEfforts_.size()) {
continue;
}
const double pos = jointStates_.position[i];
// 过滤 NaN/Inf，避免传播脏数据
@@ -530,87 +599,62 @@ void RobotControlManager::UpdateJointStates(const sensor_msgs::msg::JointState::
}
}
static int count = 0;
if (!isJointInitValueSet_)
{
if (count > 500)
{
isJointInitValueSet_ = true;
count = 0;
RCLCPP_INFO(logger(), "All joints are initialized");
}
count++;
}
// Update the waist controller (使用名称映射后的索引)
if (waist_controller_ && joint_name_mapping_initialized_ && !motionParams_.waist_joint_names_.empty())
{
std::vector<double> waistPositions;
std::vector<double> waistVelocities;
std::vector<double> waistEfforts;
std::vector<size_t> waist_indices = GetJointIndicesFromNames(motionParams_.waist_joint_names_);
for (size_t idx : waist_indices)
{
if (idx < jointPositions_.size())
{
waistPositions.push_back(jointPositions_[idx]);
waistVelocities.push_back((idx < jointVelocities_.size()) ? jointVelocities_[idx] : 0.0);
waistEfforts.push_back((idx < jointEfforts_.size()) ? jointEfforts_[idx] : 0.0);
// 初始化完成判定：关节映射完成 + 全部轴 position 已有效且不为 -99(非OP哨兵值)
bool all_axes_ready = false;
if (joint_name_mapping_initialized_ &&
motionParams_.total_joints_ > 0 &&
jointPositions_.size() >= motionParams_.total_joints_) {
all_axes_ready = true;
constexpr double kNotOpPosition = -99.0;
constexpr double kEps = 1e-6;
for (size_t i = 0; i < motionParams_.total_joints_; ++i) {
const double pos = jointPositions_[i];
if (!std::isfinite(pos) || std::abs(pos - kNotOpPosition) < kEps) {
all_axes_ready = false;
break;
}
}
if (!waistPositions.empty())
{
waist_controller_->UpdateJointStates(waistPositions, waistVelocities, waistEfforts);
}
if (all_axes_ready && !isJointInitValueSet_) {
RCLCPP_INFO(logger(), "All joints are initialized and entered OP state");
}
isJointInitValueSet_ = all_axes_ready;
// 使用统一函数更新各子控制器关节状态
if (waist_controller_) {
std::vector<double> positions;
std::vector<double> velocities;
std::vector<double> efforts;
if (BuildControllerJointStatesByNames(
motionParams_.waist_joint_names_,
&positions,
&velocities,
&efforts)) {
waist_controller_->UpdateJointStates(positions, velocities, efforts);
}
}
// Update the leg controller (使用名称映射后的索引)
if (leg_controller_ && joint_name_mapping_initialized_ && !motionParams_.leg_joint_names_.empty())
{
std::vector<double> legPositions;
std::vector<double> legVelocities;
std::vector<double> legEfforts;
std::vector<size_t> leg_indices = GetJointIndicesFromNames(motionParams_.leg_joint_names_);
for (size_t idx : leg_indices)
{
if (idx < jointPositions_.size())
{
legPositions.push_back(jointPositions_[idx]);
legVelocities.push_back((idx < jointVelocities_.size()) ? jointVelocities_[idx] : 0.0);
legEfforts.push_back((idx < jointEfforts_.size()) ? jointEfforts_[idx] : 0.0);
}
}
if (!legPositions.empty())
{
leg_controller_->UpdateJointStates(legPositions, legVelocities, legEfforts);
if (leg_controller_) {
std::vector<double> positions;
std::vector<double> velocities;
std::vector<double> efforts;
if (BuildControllerJointStatesByNames(
motionParams_.leg_joint_names_,
&positions,
&velocities,
&efforts)) {
leg_controller_->UpdateJointStates(positions, velocities, efforts);
}
}
// Update the arm controller (使用名称映射后的索引)
if (arm_controller_ && joint_name_mapping_initialized_ && !motionParams_.dual_arm_joint_names_.empty())
{
std::vector<double> armPositions;
std::vector<double> armVelocities;
std::vector<double> armEfforts;
std::vector<size_t> arm_indices = GetJointIndicesFromNames(motionParams_.dual_arm_joint_names_);
for (size_t idx : arm_indices)
{
if (idx < jointPositions_.size())
{
armPositions.push_back(jointPositions_[idx]);
armVelocities.push_back((idx < jointVelocities_.size()) ? jointVelocities_[idx] : 0.0);
armEfforts.push_back((idx < jointEfforts_.size()) ? jointEfforts_[idx] : 0.0);
}
}
if (!armPositions.empty())
{
arm_controller_->UpdateJointStates(armPositions, armVelocities, armEfforts);
if (arm_controller_) {
std::vector<double> positions;
std::vector<double> velocities;
std::vector<double> efforts;
if (BuildControllerJointStatesByNames(
motionParams_.dual_arm_joint_names_,
&positions,
&velocities,
&efforts)) {
arm_controller_->UpdateJointStates(positions, velocities, efforts);
}
}
}
@@ -675,3 +719,37 @@ std::vector<size_t> RobotControlManager::GetJointIndicesFromNames(const std::vec
}
return indices;
}
bool RobotControlManager::BuildControllerJointStatesByNames(
const std::vector<std::string>& joint_names,
std::vector<double>* out_positions,
std::vector<double>* out_velocities,
std::vector<double>* out_efforts) const
{
if (!out_positions || !out_velocities || !out_efforts) {
return false;
}
out_positions->clear();
out_velocities->clear();
out_efforts->clear();
if (!joint_name_mapping_initialized_ || joint_names.empty()) {
return false;
}
const std::vector<size_t> indices = GetJointIndicesFromNames(joint_names);
out_positions->reserve(indices.size());
out_velocities->reserve(indices.size());
out_efforts->reserve(indices.size());
for (const size_t idx : indices) {
if (idx >= jointPositions_.size()) {
continue;
}
out_positions->push_back(jointPositions_[idx]);
out_velocities->push_back((idx < jointVelocities_.size()) ? jointVelocities_[idx] : 0.0);
out_efforts->push_back((idx < jointEfforts_.size()) ? jointEfforts_[idx] : 0.0);
}
return !out_positions->empty();
}

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

@@ -1,4 +1,6 @@
#include <thread>
#include <algorithm>
#include <unistd.h>
#include "rclcpp/rclcpp.hpp"
#include "rclcpp/executors/single_threaded_executor.hpp"
#include "core/robot_control_node.hpp"
@@ -19,11 +21,59 @@ RobotControlNode::RobotControlNode() : Node("robot_control_node")
"/left_arm_gpio_controller/commands", 10);
right_gpio_pub_ = this->create_publisher<control_msgs::msg::DynamicInterfaceGroupValues>(
"/right_arm_gpio_controller/commands", 10);
robot_arm_status_pub_ = this->create_publisher<interfaces::msg::RobotArmStatus>(
"/robot_control/arm_status", 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));
jointStatesSub_ = this->create_subscription<sensor_msgs::msg::JointState>("/joint_states", 10,std::bind(&RobotControlNode::JointStatesCallback, this, std::placeholders::_1));
wheelStatesSub_ = this->create_subscription<MotorPos>("/motor_pos", 10,std::bind(&RobotControlNode::WheelStatesCallback, this, std::placeholders::_1));
joyCommandSub_ = this->create_subscription<std_msgs::msg::String>("/gamepad_msg", 10,std::bind(&RobotControlNode::JoyCommandCallback, this, std::placeholders::_1));
arm_hardware_service_ = std::make_unique<ArmHardwareService>(
this,
&robotControlManager_,
left_gpio_pub_,
right_gpio_pub_,
&joint_enabled_cache_,
&joint_error_code_cache_);
kinematics_service_ = std::make_unique<KinematicsService>(
this,
&robotControlManager_);
arm_status_service_ = std::make_unique<ArmStatusService>(
this,
&robotControlManager_,
robot_arm_status_pub_,
&joint_enabled_cache_,
&joint_error_code_cache_);
dynamicJointStatesSub_ = this->create_subscription<control_msgs::msg::DynamicJointState>(
"/dynamic_joint_states",
10,
std::bind(
&ArmStatusService::OnDynamicJointStates,
arm_status_service_.get(),
std::placeholders::_1));
inverse_kinematics_srv_ = this->create_service<interfaces::srv::InverseKinematics>(
"/robot_control/inverse_kinematics",
std::bind(
&KinematicsService::HandleInverseKinematics,
kinematics_service_.get(),
std::placeholders::_1,
std::placeholders::_2));
set_arm_enable_srv_ = this->create_service<interfaces::srv::SetArmEnable>(
"/robot_control/set_arm_enable",
std::bind(
&ArmHardwareService::HandleSetArmEnable,
arm_hardware_service_.get(),
std::placeholders::_1,
std::placeholders::_2));
clear_arm_error_srv_ = this->create_service<interfaces::srv::ClearArmError>(
"/robot_control/clear_arm_error",
std::bind(
&ArmHardwareService::HandleClearArmError,
arm_hardware_service_.get(),
std::placeholders::_1,
std::placeholders::_2));
// 初始化 RemoteController（用于 gamepad/jog 相关逻辑）
remoteController_ = std::make_unique<RemoteController>(
@@ -59,6 +109,17 @@ RobotControlNode::RobotControlNode() : Node("robot_control_node")
} else {
RCLCPP_WARN(this->get_logger(), "MoveIt initialization failed or arm controller not enabled");
}
const MotionParameters& motion_params = robotControlManager_.GetMotionParameters();
const size_t arm_joint_count = motion_params.arm_dof_ * 2;
joint_enabled_cache_.assign(arm_joint_count, false);
joint_error_code_cache_.assign(arm_joint_count, 0);
int status_period_ms = this->declare_parameter<int>("robot_arm_status_period_ms", 20);
status_period_ms = std::clamp(status_period_ms, 10, 20);
robot_arm_status_timer_ = this->create_wall_timer(
std::chrono::milliseconds(status_period_ms),
std::bind(&ArmStatusService::PublishRobotArmStatus, arm_status_service_.get()));
RCLCPP_INFO(this->get_logger(), "robot_control_node created");
}

256
src/robot_control/src/services/arm_hardware_service.cpp Normal file
View File

@@ -0,0 +1,256 @@
#include "services/arm_hardware_service.hpp"
#include <unistd.h>
namespace robot_control
{
ArmHardwareService::ArmHardwareService(
rclcpp::Node* node,
RobotControlManager* manager,
const rclcpp::Publisher<control_msgs::msg::DynamicInterfaceGroupValues>::SharedPtr& left_gpio_pub,
const rclcpp::Publisher<control_msgs::msg::DynamicInterfaceGroupValues>::SharedPtr& right_gpio_pub,
std::vector<bool>* joint_enabled_cache,
std::vector<int32_t>* joint_error_code_cache)
: node_(node),
manager_(manager),
left_gpio_pub_(left_gpio_pub),
right_gpio_pub_(right_gpio_pub),
joint_enabled_cache_(joint_enabled_cache),
joint_error_code_cache_(joint_error_code_cache)
{
}
void ArmHardwareService::HandleSetArmEnable(
const std::shared_ptr<interfaces::srv::SetArmEnable::Request> request,
std::shared_ptr<interfaces::srv::SetArmEnable::Response> response)
{
if (!request || !response) {
return;
}
if (!validate_arm_and_joint_(request->arm_id, request->joint_num)) {
response->success = false;
response->message = "Invalid arm_id or joint_num";
return;
}
const bool ok = set_motor_enable_by_scope_(
request->arm_id,
request->joint_num,
request->enable);
response->success = ok;
response->message = ok ? "SetArmEnable success" : "SetArmEnable failed";
}
void ArmHardwareService::HandleClearArmError(
const std::shared_ptr<interfaces::srv::ClearArmError::Request> request,
std::shared_ptr<interfaces::srv::ClearArmError::Response> response)
{
if (!request || !response) {
return;
}
if (!validate_arm_and_joint_(request->arm_id, request->joint_num)) {
response->success = false;
response->message = "Invalid arm_id or joint_num";
return;
}
const bool ok = clear_motor_error_by_scope_(request->arm_id, request->joint_num);
response->success = ok;
response->message = ok ? "ClearArmError success" : "ClearArmError failed";
}
bool ArmHardwareService::validate_arm_and_joint_(int8_t arm_id, int8_t joint_num) const
{
if (!manager_) {
return false;
}
const MotionParameters& motion_params = manager_->GetMotionParameters();
const int8_t arm_dof = static_cast<int8_t>(motion_params.arm_dof_);
if (!(arm_id == 0 || arm_id == 1 || arm_id == 2)) {
return false;
}
if (joint_num < 0 || joint_num > arm_dof) {
return false;
}
return true;
}
bool ArmHardwareService::set_motor_enable_by_scope_(int8_t arm_id, int8_t joint_num, bool enable)
{
if (!manager_) {
return false;
}
const MotionParameters& motion_params = manager_->GetMotionParameters();
std::vector<std::string> left;
std::vector<std::string> right;
if (!split_arm_joint_names_(motion_params, left, right)) {
return false;
}
if (arm_id == 0 || arm_id == 2) {
publish_gpio_command_for_arm_(left_gpio_pub_, left, "enable", joint_num, enable ? 1.0 : 0.0);
}
if (arm_id == 1 || arm_id == 2) {
publish_gpio_command_for_arm_(right_gpio_pub_, right, "enable", joint_num, enable ? 1.0 : 0.0);
}
update_cached_joint_state_(arm_id, joint_num, enable, 0);
return true;
}
bool ArmHardwareService::clear_motor_error_by_scope_(int8_t arm_id, int8_t joint_num)
{
if (!manager_) {
return false;
}
const MotionParameters& motion_params = manager_->GetMotionParameters();
std::vector<std::string> left;
std::vector<std::string> right;
if (!split_arm_joint_names_(motion_params, left, right)) {
return false;
}
if (arm_id == 0 || arm_id == 2) {
publish_gpio_command_for_arm_(left_gpio_pub_, left, "fault", joint_num, 1.0);
}
if (arm_id == 1 || arm_id == 2) {
publish_gpio_command_for_arm_(right_gpio_pub_, right, "fault", joint_num, 1.0);
}
usleep(500);
if (arm_id == 0 || arm_id == 2) {
publish_gpio_command_for_arm_(left_gpio_pub_, left, "fault", joint_num, 0.0);
}
if (arm_id == 1 || arm_id == 2) {
publish_gpio_command_for_arm_(right_gpio_pub_, right, "fault", joint_num, 0.0);
}
clear_error_cache_(arm_id, joint_num);
return true;
}
bool ArmHardwareService::split_arm_joint_names_(
const MotionParameters& motion_params,
std::vector<std::string>& left,
std::vector<std::string>& right) const
{
left.clear();
right.clear();
if (motion_params.dual_arm_joint_names_.empty() || motion_params.arm_dof_ == 0) {
return false;
}
const size_t arm_dof = motion_params.arm_dof_;
if (motion_params.dual_arm_joint_names_.size() < 2 * arm_dof) {
return false;
}
left.assign(
motion_params.dual_arm_joint_names_.begin(),
motion_params.dual_arm_joint_names_.begin() + arm_dof);
right.assign(
motion_params.dual_arm_joint_names_.begin() + arm_dof,
motion_params.dual_arm_joint_names_.begin() + 2 * arm_dof);
return true;
}
void ArmHardwareService::publish_gpio_command_for_arm_(
const rclcpp::Publisher<control_msgs::msg::DynamicInterfaceGroupValues>::SharedPtr& pub,
const std::vector<std::string>& joints,
const std::string& interface_name,
int local_joint_num,
double value) const
{
if (!pub) {
return;
}
control_msgs::msg::DynamicInterfaceGroupValues msg;
for (size_t i = 0; i < joints.size(); ++i) {
msg.interface_groups.push_back(joints[i]);
control_msgs::msg::InterfaceValue v;
v.interface_names = {interface_name};
if (local_joint_num == 0) {
v.values = {value};
} else {
v.values = {(static_cast<int>(i + 1) == local_joint_num) ? value : 0.0};
}
msg.interface_values.push_back(v);
}
pub->publish(msg);
}
void ArmHardwareService::update_cached_joint_state_(
int8_t arm_id,
int8_t joint_num,
bool enabled,
int32_t error_code)
{
if (!manager_ || !joint_enabled_cache_ || !joint_error_code_cache_) {
return;
}
const MotionParameters& motion_params = manager_->GetMotionParameters();
const size_t arm_dof = motion_params.arm_dof_;
if (arm_dof == 0) {
return;
}
auto apply_one = [this, arm_dof, enabled, error_code](int8_t arm, int8_t joint) {
const size_t base = (arm == 0) ? 0 : arm_dof;
if (joint == 0) {
for (size_t i = 0; i < arm_dof; ++i) {
(*joint_enabled_cache_)[base + i] = enabled;
(*joint_error_code_cache_)[base + i] = error_code;
}
return;
}
const size_t idx = base + static_cast<size_t>(joint - 1);
if (idx < joint_enabled_cache_->size() && idx < joint_error_code_cache_->size()) {
(*joint_enabled_cache_)[idx] = enabled;
(*joint_error_code_cache_)[idx] = error_code;
}
};
if (arm_id == 2) {
apply_one(0, joint_num);
apply_one(1, joint_num);
return;
}
apply_one(arm_id, joint_num);
}
void ArmHardwareService::clear_error_cache_(int8_t arm_id, int8_t joint_num)
{
if (!manager_ || !joint_error_code_cache_) {
return;
}
const MotionParameters& cache_motion_params = manager_->GetMotionParameters();
const size_t arm_dof = cache_motion_params.arm_dof_;
auto clear_error_cache = [this, arm_dof](int8_t arm, int8_t joint) {
const size_t base = (arm == 0) ? 0 : arm_dof;
if (joint == 0) {
for (size_t i = 0; i < arm_dof; ++i) {
(*joint_error_code_cache_)[base + i] = 0;
}
return;
}
const size_t idx = base + static_cast<size_t>(joint - 1);
if (idx < joint_error_code_cache_->size()) {
(*joint_error_code_cache_)[idx] = 0;
}
};
if (arm_id == 2) {
clear_error_cache(0, joint_num);
clear_error_cache(1, joint_num);
} else {
clear_error_cache(arm_id, joint_num);
}
}
} // namespace robot_control

105
src/robot_control/src/services/arm_status_service.cpp Normal file
View File

@@ -0,0 +1,105 @@
#include "services/arm_status_service.hpp"
#include <algorithm>
namespace robot_control
{
ArmStatusService::ArmStatusService(
rclcpp::Node* node,
RobotControlManager* manager,
const rclcpp::Publisher<interfaces::msg::RobotArmStatus>::SharedPtr& status_pub,
std::vector<bool>* joint_enabled_cache,
std::vector<int32_t>* joint_error_code_cache)
: node_(node),
manager_(manager),
status_pub_(status_pub),
joint_enabled_cache_(joint_enabled_cache),
joint_error_code_cache_(joint_error_code_cache)
{
}
void ArmStatusService::OnDynamicJointStates(const control_msgs::msg::DynamicJointState::SharedPtr msg)
{
if (!msg || !manager_ || !joint_enabled_cache_ || !joint_error_code_cache_) {
return;
}
const MotionParameters& motion_params = manager_->GetMotionParameters();
const size_t arm_dof = motion_params.arm_dof_;
if (arm_dof == 0 || motion_params.dual_arm_joint_names_.size() < 2 * arm_dof) {
return;
}
for (size_t i = 0; i < msg->joint_names.size() && i < msg->interface_values.size(); ++i) {
const std::string& joint_name = msg->joint_names[i];
auto joint_it = std::find(
motion_params.dual_arm_joint_names_.begin(),
motion_params.dual_arm_joint_names_.end(),
joint_name);
if (joint_it == motion_params.dual_arm_joint_names_.end()) {
continue;
}
const size_t joint_idx = static_cast<size_t>(
std::distance(motion_params.dual_arm_joint_names_.begin(), joint_it));
if (joint_idx >= joint_enabled_cache_->size() || joint_idx >= joint_error_code_cache_->size()) {
continue;
}
const auto& iface = msg->interface_values[i];
for (size_t k = 0; k < iface.interface_names.size() && k < iface.values.size(); ++k) {
const std::string& name = iface.interface_names[k];
const double value = iface.values[k];
if (name == "enable") {
(*joint_enabled_cache_)[joint_idx] = (value > 0.5);
} else if (name == "fault" || name == "error" || name == "error_code") {
(*joint_error_code_cache_)[joint_idx] = static_cast<int32_t>(value);
}
}
}
}
void ArmStatusService::PublishRobotArmStatus()
{
if (!node_ || !manager_ || !status_pub_) {
return;
}
const MotionParameters& motion_params = manager_->GetMotionParameters();
const size_t arm_dof = motion_params.arm_dof_;
if (arm_dof == 0 || motion_params.dual_arm_joint_names_.size() < 2 * arm_dof) {
return;
}
interfaces::msg::RobotArmStatus msg;
msg.stamp = node_->now();
msg.joint_names = motion_params.dual_arm_joint_names_;
std::vector<double> positions;
std::string err;
if (manager_->GetJointPositionsByNames(msg.joint_names, &positions, &err)) {
msg.joint_positions = positions;
} else {
msg.joint_positions.assign(msg.joint_names.size(), 0.0);
}
if (joint_enabled_cache_) {
msg.joint_enabled = *joint_enabled_cache_;
}
if (joint_error_code_cache_) {
msg.joint_error_codes = *joint_error_code_cache_;
}
if (msg.joint_enabled.size() != msg.joint_names.size()) {
msg.joint_enabled.assign(msg.joint_names.size(), false);
}
if (msg.joint_error_codes.size() != msg.joint_names.size()) {
msg.joint_error_codes.assign(msg.joint_names.size(), 0);
}
(void)manager_->GetArmCurrentPose(0, &msg.left_arm_pose, nullptr);
(void)manager_->GetArmCurrentPose(1, &msg.right_arm_pose, nullptr);
status_pub_->publish(msg);
}
} // namespace robot_control

43
src/robot_control/src/services/kinematics_service.cpp Normal file
View File

@@ -0,0 +1,43 @@
#include "services/kinematics_service.hpp"
namespace robot_control
{
KinematicsService::KinematicsService(rclcpp::Node* node, RobotControlManager* manager)
: node_(node), manager_(manager)
{
}
void KinematicsService::HandleInverseKinematics(
const std::shared_ptr<interfaces::srv::InverseKinematics::Request> request,
std::shared_ptr<interfaces::srv::InverseKinematics::Response> response)
{
if (!request || !response || !manager_) {
return;
}
std::vector<double> joint_angles;
std::string error_msg;
const bool ok = manager_->SolveArmInverseKinematics(
static_cast<uint8_t>(request->arm_id),
request->pose,
&joint_angles,
&error_msg);
response->result = ok ? 0 : -1;
response->joint_angles.clear();
response->joint_angles.reserve(joint_angles.size());
for (double v : joint_angles) {
response->joint_angles.push_back(static_cast<float>(v));
}
if (!ok && node_) {
RCLCPP_ERROR(
node_->get_logger(),
"InverseKinematics failed (arm_id=%d): %s",
static_cast<int>(request->arm_id),
error_msg.c_str());
}
}
} // namespace robot_control