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

Update dual arm goal mapping and skills

Browse Source
This commit is contained in:
NuoDaJia02
2026-02-04 17:40:39 +08:00
parent 742e5ec67e
commit 97363f1e32
6 changed files with 167 additions and 105 deletions
Download Patch File Download Diff File Expand all files Collapse all files

19
src/brain/config/robot_skills.yaml
View File

@@ -1,14 +1,9 @@
- name: VisionObjectRecognitionSrv
- name: VisionObjectRecognition
version: 1.0.0
description: "视觉识别指定物体"
interfaces:
- name: VisionObjectRecognition.srv
default_topic: "/vision_object_recognition"
- name: VisionObjectRecognitionAct
version: 1.0.0
description: "视觉识别指定物体"
interfaces:
- name: VisionObjectRecognition.action
default_topic: "/vision_object_recognition"
@@ -24,12 +19,12 @@
# interfaces:
# - MapSave.srv
# - name: Arm
# version: 1.0.0
# description: "手臂控制"
# interfaces:
# - name: Arm.action
# default_topic: "ArmAction"
- name: Arm
version: 1.0.0
description: "手臂控制"
interfaces:
- name: Arm.action
default_topic: "Arm"
- name: DualArm
version: 1.0.0

1
src/brain/include/brain/cerebellum_hooks.hpp
View File

@@ -12,6 +12,7 @@ public:
static void ConfigureActionHooks(CerebellumNode * node);
static void ConfigureArmSpaceControlHooks(CerebellumNode * node);
static void ConfigureArmHooks(CerebellumNode * node);
static void ConfigureDualArmHooks(CerebellumNode * node);
static void ConfigureHandControlHooks(CerebellumNode * node);
static void ConfigureCameraTakePhotoHooks(CerebellumNode * node);
static void ConfigureLegControlHooks(CerebellumNode * node);

46
src/brain/include/brain/payload_converters.hpp
View File

@@ -74,6 +74,29 @@ inline interfaces::action::VisionObjectRecognition::Goal from_yaml<interfaces::a
return g;
}
// Arm::Goal: body_id, data_type, data_length, command_id, frame_time_stamp, data_array
template<>
inline interfaces::action::Arm::Goal from_yaml<interfaces::action::Arm::Goal>(const YAML::Node & n)
{
interfaces::action::Arm::Goal g;
if (n && n.IsMap()) {
if (n["body_id"]) g.body_id = n["body_id"].as<int8_t>();
if (n["data_type"]) g.data_type = n["data_type"].as<int8_t>();
if (n["data_length"]) g.data_length = n["data_length"].as<int16_t>();
if (n["command_id"]) g.command_id = n["command_id"].as<int64_t>();
if (n["frame_time_stamp"]) g.frame_time_stamp = n["frame_time_stamp"].as<int64_t>();
if (n["data_array"]) {
auto vec = n["data_array"].as<std::vector<double>>();
g.data_array.assign(vec.begin(), vec.end());
}
if (!g.data_array.empty()) {
using DL = decltype(g.data_length);
g.data_length = static_cast<DL>(g.data_array.size());
}
}
return g;
}
// DualArm::Goal: arm_motion_params, velocity_scaling
template<>
inline interfaces::action::DualArm::Goal from_yaml<interfaces::action::DualArm::Goal>(const YAML::Node & n)
@@ -236,29 +259,6 @@ inline interfaces::action::SlamMode::Goal from_yaml<interfaces::action::SlamMode
return g;
}
// Arm::Goal: body_id, data_type, data_length, command_id, frame_time_stamp, data_array
template<>
inline interfaces::action::Arm::Goal from_yaml<interfaces::action::Arm::Goal>(const YAML::Node & n)
{
interfaces::action::Arm::Goal g;
if (n && n.IsMap()) {
if (n["body_id"]) g.body_id = n["body_id"].as<int8_t>();
if (n["data_type"]) g.data_type = n["data_type"].as<int8_t>();
if (n["data_length"]) g.data_length = n["data_length"].as<int16_t>();
if (n["command_id"]) g.command_id = n["command_id"].as<int64_t>();
if (n["frame_time_stamp"]) g.frame_time_stamp = n["frame_time_stamp"].as<int64_t>();
if (n["data_array"]) {
auto vec = n["data_array"].as<std::vector<double>>();
g.data_array.assign(vec.begin(), vec.end());
}
if (!g.data_array.empty()) {
using DL = decltype(g.data_length);
g.data_length = static_cast<DL>(g.data_array.size());
}
}
return g;
}
// MoveToPosition::Goal: target_x, target_y, target_z, target_angle_pitch, target_angle_roll, target_angle_yaw
template<>
inline interfaces::action::MoveToPosition::Goal from_yaml<interfaces::action::MoveToPosition::Goal>(const YAML::Node & n)

20
src/brain/include/brain/skill_config.hpp
View File

@@ -44,6 +44,7 @@ struct SkillServiceTrait;
using SkillActionTypes = std::tuple<
interfaces::action::VisionObjectRecognition,
interfaces::action::Arm,
interfaces::action::DualArm,
interfaces::action::CameraTakePhoto,
interfaces::action::MoveWaist,
@@ -51,7 +52,6 @@ using SkillActionTypes = std::tuple<
interfaces::action::MoveWheel,
interfaces::action::MoveHome,
interfaces::action::GripperCmd,
interfaces::action::Arm,
interfaces::action::ArmSpaceControl,
interfaces::action::ExecuteBtAction,
interfaces::action::HandControl,
@@ -86,6 +86,15 @@ struct SkillActionTrait<interfaces::action::VisionObjectRecognition>
static std::string message(const interfaces::action::VisionObjectRecognition::Result & r) {return r.info;}
};
template<>
struct SkillActionTrait<interfaces::action::Arm>
{
static constexpr const char * skill_name = "Arm";
static constexpr const char * default_topic = "Arm";
static bool success(const interfaces::action::Arm::Result & r, const std::string & instance_name) {(void)instance_name; return (r.result == 0);}
static std::string message(const interfaces::action::Arm::Result & r) {(void)r; return "completed";}
};
template<>
struct SkillActionTrait<interfaces::action::DualArm>
{
@@ -164,15 +173,6 @@ struct SkillActionTrait<interfaces::action::GripperCmd>
static std::string message(const interfaces::action::GripperCmd::Result & r) {return r.state;}
};
template<>
struct SkillActionTrait<interfaces::action::Arm>
{
static constexpr const char * skill_name = "Arm";
static constexpr const char * default_topic = "";
static bool success(const interfaces::action::Arm::Result & r, const std::string & instance_name) {(void)instance_name; return (r.result == 0);}
static std::string message(const interfaces::action::Arm::Result & r) {(void)r; return "completed";}
};
template<>
struct SkillActionTrait<interfaces::action::ArmSpaceControl>
{

183
src/brain/src/cerebellum_hooks.cpp
View File

@@ -2,6 +2,7 @@
#include "brain/cerebellum_node.hpp"
#include "brain/payload_converters.hpp"
#include "arm_action_define.h"
#include "interfaces/action/dual_arm.hpp"
#include <rclcpp/rclcpp.hpp>
#include <sstream>
#include <iomanip>
@@ -17,6 +18,7 @@ void CerebellumHooks::ConfigureActionHooks(CerebellumNode * node)
return;
}
ConfigureArmHooks(node);
ConfigureDualArmHooks(node);
ConfigureHandControlHooks(node);
ConfigureCameraTakePhotoHooks(node);
ConfigureMoveWaistHooks(node);
@@ -82,75 +84,37 @@ void CerebellumHooks::ConfigureArmHooks(CerebellumNode * node)
} else if (goal.data_type >= node->arm_cmd_type_custom_min_ && goal.data_type <= node->arm_cmd_type_custom_max_) {
geometry_msgs::msg::Pose pose;
std::vector<double> angle{360.0, 360.0, 360.0, 360.0, 360.0, 360.0}; //invalid angles
const std::string side = (goal.body_id == RIGHT_ARM) ? "right_arm_" : "left_arm_";
const char * action = nullptr;
if (goal.data_type == node->arm_cmd_type_take_photo_) { //take photo
if (goal.body_id == RIGHT_ARM) {
if (!node->arm_poses_["right_arm_take_photo"].empty()) {
pose = node->arm_poses_["right_arm_take_photo"].back();
}
if (!node->arm_angles_["right_arm_take_photo"].empty()) {
angle = node->arm_angles_["right_arm_take_photo"].back();
}
} else {
if (!node->arm_poses_["left_arm_take_photo"].empty()) {
pose = node->arm_poses_["left_arm_take_photo"].back();
}
if (!node->arm_angles_["left_arm_take_photo"].empty()) {
angle = node->arm_angles_["left_arm_take_photo"].back();
}
}
action = "take_photo";
} else if (goal.data_type == node->arm_cmd_type_pre_grasp_) { // pre-grasp
if (goal.body_id == RIGHT_ARM) {
if (!node->arm_poses_["right_arm_pre_grasp"].empty()) {
pose = node->arm_poses_["right_arm_pre_grasp"].back();
}
if (!node->arm_angles_["right_arm_pre_grasp"].empty()) {
angle = node->arm_angles_["right_arm_pre_grasp"].back();
}
} else {
if (!node->arm_poses_["left_arm_pre_grasp"].empty()) {
pose = node->arm_poses_["left_arm_pre_grasp"].back();
}
if (!node->arm_angles_["left_arm_pre_grasp"].empty()) {
angle = node->arm_angles_["left_arm_pre_grasp"].back();
}
}
action = "pre_grasp";
} else if (goal.data_type == node->arm_cmd_type_grasp_) { // grasp
if (goal.body_id == RIGHT_ARM) {
if (!node->arm_poses_["right_arm_grasp"].empty()) {
pose = node->arm_poses_["right_arm_grasp"].back();
}
if (!node->arm_angles_["right_arm_grasp"].empty()) {
angle = node->arm_angles_["right_arm_grasp"].back();
}
} else {
if (!node->arm_poses_["left_arm_grasp"].empty()) {
pose = node->arm_poses_["left_arm_grasp"].back();
}
if (!node->arm_angles_["left_arm_grasp"].empty()) {
angle = node->arm_angles_["left_arm_grasp"].back();
}
}
action = "grasp";
} else if (goal.data_type == node->arm_cmd_type_take_object_) { //take object
if (goal.body_id == RIGHT_ARM) {
if (!node->arm_poses_["right_arm_take_object"].empty()) {
pose = node->arm_poses_["right_arm_take_object"].back();
}
if (!node->arm_angles_["right_arm_take_object"].empty()) {
angle = node->arm_angles_["right_arm_take_object"].back();
}
} else {
if (!node->arm_poses_["left_arm_take_object"].empty()) {
pose = node->arm_poses_["left_arm_take_object"].back();
}
if (!node->arm_angles_["left_arm_take_object"].empty()) {
angle = node->arm_angles_["left_arm_take_object"].back();
}
}
action = "take_object";
} else {
RCLCPP_ERROR(node->get_logger(), "ARM make goal error: invalid data_type");
return goal;
}
const auto load_cached_pose_angle = [node, &pose, &angle](
const std::string & side_prefix, const std::string & action_suffix) {
const std::string key = side_prefix + action_suffix;
const auto pose_it = node->arm_poses_.find(key);
if (pose_it != node->arm_poses_.end() && !pose_it->second.empty()) {
pose = pose_it->second.back();
}
const auto angle_it = node->arm_angles_.find(key);
if (angle_it != node->arm_angles_.end() && !angle_it->second.empty()) {
angle = angle_it->second.back();
}
};
load_cached_pose_angle(side, action);
#ifdef USE_ARM_POSE_DIRECT_MOVE
goal.data_type = ARM_COMMAND_TYPE_POSE_DIRECT_MOVE;
goal.data_length = POSE_DIMENSION;
@@ -232,6 +196,105 @@ void CerebellumHooks::ConfigureArmHooks(CerebellumNode * node)
node->skill_manager_->configure_action_hooks<interfaces::action::Arm>("Arm", std::move(hooks));
}
void CerebellumHooks::ConfigureDualArmHooks(CerebellumNode * node)
{
// DualArm
SkillManager::ActionHookOptions<interfaces::action::DualArm> hooks;
hooks.make_goal = [node](const std::string & skill_name) {
interfaces::action::DualArm::Goal goal{};
if (!node->TryParseCallPayload<interfaces::action::DualArm::Goal>(skill_name, goal)) {
RCLCPP_ERROR(node->get_logger(), "Failed to parse call payload for skill %s", skill_name.c_str());
return goal;
}
RCLCPP_INFO(node->get_logger(), "[%s] DualArm goal: arm_motion_params=%zu, velocity_scaling=%d",
skill_name.c_str(), goal.arm_motion_params.size(), goal.velocity_scaling);
const auto load_cached_pose_angle = [node](
const std::string & side_prefix, const std::string & action_suffix,
geometry_msgs::msg::Pose & pose_out, std::vector<double> & angle_out) {
const std::string key = side_prefix + action_suffix;
const auto pose_it = node->arm_poses_.find(key);
if (pose_it != node->arm_poses_.end() && !pose_it->second.empty()) {
pose_out = pose_it->second.back();
}
const auto angle_it = node->arm_angles_.find(key);
if (angle_it != node->arm_angles_.end() && !angle_it->second.empty()) {
angle_out = angle_it->second.back();
}
};
for (size_t i = 0; i < goal.arm_motion_params.size(); ++i) {
auto & p = goal.arm_motion_params[i];
const char * action = nullptr;
if (p.motion_type == node->arm_cmd_type_take_photo_) {
action = "take_photo";
} else if (p.motion_type == node->arm_cmd_type_pre_grasp_) {
action = "pre_grasp";
} else if (p.motion_type == node->arm_cmd_type_grasp_) {
action = "grasp";
} else if (p.motion_type == node->arm_cmd_type_take_object_) {
action = "take_object";
}
if (action) {
geometry_msgs::msg::Pose pose;
std::vector<double> angle;
const std::string side = (p.arm_id == interfaces::msg::ArmMotionParams::ARM_RIGHT)
? "right_arm_"
: "left_arm_";
load_cached_pose_angle(side, action, pose, angle);
p.target_pose = pose;
if (!angle.empty()) {
p.target_joints = angle;
}
}
RCLCPP_INFO(node->get_logger(),
"[%s] param[%zu]: arm_id=%u motion_type=%u blend_radius=%d pose=(%.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f) joints=(%.4f, %.4f, %.4f, %.4f, %.4f, %.4f)",
skill_name.c_str(), i, p.arm_id, p.motion_type, p.blend_radius,
p.target_pose.position.x, p.target_pose.position.y, p.target_pose.position.z,
p.target_pose.orientation.x, p.target_pose.orientation.y, p.target_pose.orientation.z, p.target_pose.orientation.w,
p.target_joints[0], p.target_joints[1], p.target_joints[2], p.target_joints[3], p.target_joints[4], p.target_joints[5]);
}
return goal;
};
hooks.on_feedback = [node](
const std::string & skill_name,
ActionClientRegistry::GoalHandle<interfaces::action::DualArm>::SharedPtr,
const std::shared_ptr<const interfaces::action::DualArm::Feedback> & feedback) {
if (!feedback) {return;}
RCLCPP_INFO(node->get_logger(), "[%s] progress=%.2f status=%u", skill_name.c_str(),
feedback->progress, feedback->status);
};
hooks.on_result = [node](
const std::string & skill_name,
const ActionClientRegistry::GoalHandle<interfaces::action::DualArm>::WrappedResult & res) {
const bool success = res.code == rclcpp_action::ResultCode::SUCCEEDED && res.result && res.result->success;
const std::string message = res.result ? res.result->message : std::string("No return information");
if (success) {
RCLCPP_INFO(node->get_logger(), "[%s] Success: %s (progress=%.2f)",
skill_name.c_str(), message.c_str(), res.result->final_progress);
} else if (res.code == rclcpp_action::ResultCode::CANCELED) {
RCLCPP_WARN(node->get_logger(), "[%s] was cancelled", skill_name.c_str());
} else {
RCLCPP_ERROR(node->get_logger(), "[%s] failed (code=%d): %s",
skill_name.c_str(), static_cast<int>(res.code), message.c_str());
}
};
hooks.on_goal_response = [node](
const std::string & skill_name,
const std::shared_ptr<ActionClientRegistry::GoalHandle<interfaces::action::DualArm>> & handle) {
if (static_cast<bool>(handle)) {
RCLCPP_INFO(node->get_logger(), "[%s] goal has been accepted by server", skill_name.c_str());
} else {
RCLCPP_WARN(node->get_logger(), "[%s] goal was rejected by server", skill_name.c_str());
}
};
node->skill_manager_->configure_action_hooks<interfaces::action::DualArm>("DualArm", std::move(hooks));
}
void CerebellumHooks::ConfigureHandControlHooks(CerebellumNode * node)
{
// HandControl

3
src/brain/src/cerebellum_node.cpp
View File

@@ -739,6 +739,7 @@ bool CerebellumNode::ExecuteActionSkill(
effective_timeout = std::chrono::milliseconds(timeout_ms_override);
}
#if (0)
if (interface_base == "Arm") {
interfaces::action::Arm::Goal goal{};
if (!TryParseCallPayload<interfaces::action::Arm::Goal>(skill, goal)) {
@@ -756,6 +757,8 @@ bool CerebellumNode::ExecuteActionSkill(
return false;
}
}
#endif
return RunActionSkillWithProgress(skill, interface_base, topic, effective_timeout,
instance_name, timeout_ms_override, index, total_skills, goal_handle, detail);
}