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

refactor(brain): replace arm_action_define constants in cerebellum arm flow

Browse Source
This commit is contained in:
david
2026-02-25 14:20:18 +08:00
parent a02bc399b7
commit ce03b2c6c5
2 changed files with 28 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -1,8 +1,9 @@
#include "brain/cerebellum_hooks.hpp"
#include "brain/cerebellum_node.hpp"
#include "brain/payload_converters.hpp"
#include "arm_action_define.h"
// #include "arm_action_define.h"
#include "interfaces/action/dual_arm.hpp"
#include "interfaces/msg/arm_motion_params.hpp"
#include <rclcpp/rclcpp.hpp>
#include <sstream>
#include <iomanip>
@@ -49,43 +50,43 @@ void CerebellumHooks::ConfigureArmHooks(CerebellumNode * node)
RCLCPP_INFO(node->get_logger(), "[%s]Received Parse Arm goal: body_id=%d, data_type=%d, data_length=%d, command_id=%ld, frame_time_stamp=%ld",
skill_name.c_str(), goal.body_id, goal.data_type, goal.data_length, goal.command_id, goal.frame_time_stamp);
goal.body_id = (CerebellumNode::tls_arm_body_id == LEFT_ARM ||
CerebellumNode::tls_arm_body_id == RIGHT_ARM) ? CerebellumNode::tls_arm_body_id : goal.body_id;
goal.body_id = (CerebellumNode::tls_arm_body_id == interfaces::msg::ArmMotionParams::ARM_LEFT ||
CerebellumNode::tls_arm_body_id == interfaces::msg::ArmMotionParams::ARM_RIGHT) ? CerebellumNode::tls_arm_body_id : goal.body_id;
goal.command_id = ++node->command_id_;
if (goal.body_id == LEFT_ARM) {
if (goal.body_id == interfaces::msg::ArmMotionParams::ARM_LEFT) {
node->command_id_left_arm_ = goal.command_id;
} else if (goal.body_id == RIGHT_ARM) {
} else if (goal.body_id == interfaces::msg::ArmMotionParams::ARM_RIGHT) {
node->command_id_right_arm_ = goal.command_id;
}
goal.frame_time_stamp = node->get_clock()->now().nanoseconds();
if (goal.data_type == ARM_COMMAND_TYPE_POSE_DIRECT_MOVE) {
if (goal.data_length == POSE_DIMENSION*2 && goal.data_array.size() == POSE_DIMENSION*2) {
if (goal.body_id == LEFT_ARM) {
if (goal.data_type == 3) {
if (goal.data_length == 7*2 && goal.data_array.size() == 7*2) {
if (goal.body_id == interfaces::msg::ArmMotionParams::ARM_LEFT) {
goal.data_array = {goal.data_array[0], goal.data_array[1], goal.data_array[2],
goal.data_array[3], goal.data_array[4], goal.data_array[5], goal.data_array[6]};
} else if (goal.body_id == RIGHT_ARM) {
} else if (goal.body_id == interfaces::msg::ArmMotionParams::ARM_RIGHT) {
goal.data_array = {goal.data_array[7], goal.data_array[8], goal.data_array[9],
goal.data_array[10], goal.data_array[11], goal.data_array[12], goal.data_array[13]};
}
goal.data_length = POSE_DIMENSION;
goal.data_length = 7;
}
} else if (goal.data_type == ARM_COMMAND_TYPE_ANGLE_DIRECT_MOVE) {
if (goal.data_length == USED_ARM_DOF*2 && goal.data_array.size() == USED_ARM_DOF*2) {
if (goal.body_id == LEFT_ARM) {
} else if (goal.data_type == 2) {
if (goal.data_length == 6*2 && goal.data_array.size() == 6*2) {
if (goal.body_id == interfaces::msg::ArmMotionParams::ARM_LEFT) {
goal.data_array = {goal.data_array[0], goal.data_array[1], goal.data_array[2],
goal.data_array[3], goal.data_array[4], goal.data_array[5]};
} else if (goal.body_id == RIGHT_ARM) {
} else if (goal.body_id == interfaces::msg::ArmMotionParams::ARM_RIGHT) {
goal.data_array = {goal.data_array[6], goal.data_array[7], goal.data_array[8], goal.data_array[9],
goal.data_array[10], goal.data_array[11]};
}
goal.data_length = USED_ARM_DOF;
goal.data_length = 6;
}
} 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(6, 360.0); //invalid angles
const std::string side = (goal.body_id == RIGHT_ARM) ? "right_arm_" : "left_arm_";
const std::string side = (goal.body_id == interfaces::msg::ArmMotionParams::ARM_RIGHT) ? "right_arm_" : "left_arm_";
const char * action = nullptr;
if (goal.data_type == node->arm_cmd_type_take_photo_) { //take photo
action = "take_photo";
@@ -116,17 +117,17 @@ void CerebellumHooks::ConfigureArmHooks(CerebellumNode * node)
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;
goal.data_type = 3;
goal.data_length = 7;
goal.data_array = {pose.position.x, pose.position.y, pose.position.z,
pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w};
#elif defined(USE_ARM_ANGLE_DIRECT_MOVE)
if (angle.size() == USED_ARM_DOF) {
goal.data_type = ARM_COMMAND_TYPE_ANGLE_DIRECT_MOVE;
goal.data_length = USED_ARM_DOF;
if (angle.size() == 6) {
goal.data_type = 2;
goal.data_length = 6;
goal.data_array = {angle[0], angle[1], angle[2], angle[3], angle[4], angle[5]};
} else {
RCLCPP_ERROR(node->get_logger(), "ARM make goal error: invalid angle size: %ld/%d", angle.size(), USED_ARM_DOF);
RCLCPP_ERROR(node->get_logger(), "ARM make goal error: invalid angle size: %ld/%d", angle.size(), 6);
}
#endif
}
@@ -135,7 +136,7 @@ void CerebellumHooks::ConfigureArmHooks(CerebellumNode * node)
return goal;
}
if (goal.data_array.size() >= USED_ARM_DOF) {
if (goal.data_array.size() >= 6) {
RCLCPP_INFO(node->get_logger(), "[%s] Control params: body_id=%d, data_type=%d, data_length=%d, command_id=%ld, frame_time_stamp=%ld, data_array=[%.6f, %.6f, %.6f, %.6f, %.6f, %.6f]",
skill_name.c_str(), goal.body_id, goal.data_type, goal.data_length, goal.command_id, goal.frame_time_stamp,
goal.data_array[0], goal.data_array[1], goal.data_array[2], goal.data_array[3], goal.data_array[4], goal.data_array[5]);
@@ -158,7 +159,7 @@ void CerebellumHooks::ConfigureArmHooks(CerebellumNode * node)
hooks.on_result = [node](
const std::string & skill_name,
const ActionClientRegistry::GoalHandle<interfaces::action::Arm>::WrappedResult & res) {
const bool success = (res.code == rclcpp_action::ResultCode::SUCCEEDED && res.result && res.result->result == OK);
const bool success = (res.code == rclcpp_action::ResultCode::SUCCEEDED && res.result && res.result->result == 0);
const std::string message = res.result ? std::string("action end") : std::string("No return information");
geometry_msgs::msg::Pose pose = res.result->pose;

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

@@ -52,7 +52,7 @@
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>
#include "arm_action_define.h"
// #include "arm_action_define.h"
namespace brain
{
@@ -884,7 +884,7 @@ bool CerebellumNode::ExecuteBilateralArmAction(
time_steady = std::chrono::steady_clock::now();
RCLCPP_WARN(this->get_logger(), "left_arm_time_steady2");
};
std::thread left_arm_thread(left_arm_worker, LEFT_ARM); // LEFT_ARM=0
std::thread left_arm_thread(left_arm_worker, interfaces::msg::ArmMotionParams::ARM_LEFT); // ARM_LEFT=0
auto right_arm_worker = [this, &ok_out = right_arm_ok, &done_flag = right_arm_finished, &d_out = right_arm_d,
skill, interface_name, topic, timeout, &goal_handle, &time_steady = right_arm_time_steady, instance_name](int body_id) {
@@ -898,7 +898,7 @@ bool CerebellumNode::ExecuteBilateralArmAction(
time_steady = std::chrono::steady_clock::now();
RCLCPP_WARN(this->get_logger(), "right_arm_time_steady2");
};
std::thread right_arm_thread(right_arm_worker, RIGHT_ARM); // RIGHT_ARM=1
std::thread right_arm_thread(right_arm_worker, interfaces::msg::ArmMotionParams::ARM_RIGHT); // ARM_RIGHT=1
const auto start_steady = std::chrono::steady_clock::now();