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

优化代码和流程

Browse Source
This commit is contained in:
NuoDaJia02
2026-01-15 15:37:05 +08:00
parent c929176625
commit c1a7205e10
5 changed files with 93 additions and 108 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
src/brain/config/brain_node_params.yaml
View File

@@ -84,7 +84,7 @@ cerebellum_node:
top_cam_left_arm_orientation: [0.7071, 0.0, 0.0, 0.7071] #左臂相机视觉识别时的四元数
side_cam_right_arm_y_offset: 0.0 # 补偿右臂y轴方向的抓取偏差（前后）
side_cam_right_arm_z_offset: 0.02 # 补偿右臂z轴方向的抓取偏差（左右）
side_cam_left_arm_y_offset: 0.0 # 补偿左臂y轴方向的抓取偏差（前后）
side_cam_left_arm_y_offset: -0.02 # 补偿左臂y轴方向的抓取偏差（前后）
side_cam_left_arm_z_offset: 0.01 # 补偿左臂z轴方向的抓取偏差（左右）
take_object_arm_x: -0.05 # 抓取物体时x坐标
left_camera_frame: "LeftLink6" # 左臂相机坐标系

4
src/brain/config/bt_vision_grasp_dual_arm.params.yaml
View File

@@ -25,7 +25,7 @@
frame_time_stamp: 0
data_array: [0, 0, 0, 0, 0, 0, 95, -22, -90, 0, -68, 90]
data_array: [0, 0, 0, 0, 0, 0, 80, -5, -112, -12, -62, 175]
'
- name: s1_left_arm_initial
@@ -39,7 +39,7 @@
frame_time_stamp: 0
data_array: [85, 22, 90, 0, 68, 90, 0, 0, 0, 0, 0, 0]
data_array: [100, 5, 112, 12, 62, 5, 0, 0, 0, 0, 0, 0]
'
- name: s1_right_arm_pre_cam_take_photo

4
src/brain/config/bt_vision_grasp_dual_arm.xml
View File

@@ -56,8 +56,8 @@
<Parallel name="parallel_init_action">
<GripperCmd1_H name="s1_right_hand_gripper_open" />
<GripperCmd0_H name="s1_left_hand_gripper_open" />
<Arm_H name="s1_right_arm_pre_cam_take_photo" />
<Arm_H name="s1_left_arm_pre_cam_take_photo" />
<!-- <Arm_H name="s1_right_arm_pre_cam_take_photo" />
<Arm_H name="s1_left_arm_pre_cam_take_photo" /> -->
</Parallel>
<RetryUntilSuccessful name="retry_top_cam_vision" num_attempts="5">
<VisionObjectRecognition_H name="s2_top_camera_vision_recg" />

23
src/brain/include/brain/cerebellum_node.hpp
View File

@@ -4,6 +4,8 @@
#include <atomic>
#include <limits>
#include <memory>
#include <vector>
#include <unordered_map>
#include <rclcpp/rclcpp.hpp>
#include <rclcpp_action/rclcpp_action.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
@@ -43,25 +45,8 @@ public:
*/
explicit CerebellumNode(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
std::vector<geometry_msgs::msg::Pose> left_arm_take_photo_poses_;
std::vector<geometry_msgs::msg::Pose> left_arm_pre_grasp_poses_;
std::vector<geometry_msgs::msg::Pose> left_arm_grasp_poses_;
std::vector<geometry_msgs::msg::Pose> left_arm_take_object_pose_;
std::vector<geometry_msgs::msg::Pose> right_arm_take_photo_poses_;
std::vector<geometry_msgs::msg::Pose> right_arm_pre_grasp_poses_;
std::vector<geometry_msgs::msg::Pose> right_arm_grasp_poses_;
std::vector<geometry_msgs::msg::Pose> right_arm_take_object_pose_;
std::vector<std::vector<double>> left_arm_take_photo_angles_;
std::vector<std::vector<double>> left_arm_pre_grasp_angles_;
std::vector<std::vector<double>> left_arm_grasp_angles_;
std::vector<std::vector<double>> left_arm_take_object_angles_;
std::vector<std::vector<double>> right_arm_take_photo_angles_;
std::vector<std::vector<double>> right_arm_pre_grasp_angles_;
std::vector<std::vector<double>> right_arm_grasp_angles_;
std::vector<std::vector<double>> right_arm_take_object_angles_;
std::unordered_map<std::string, std::vector<geometry_msgs::msg::Pose>> arm_poses_;
std::unordered_map<std::string, std::vector<std::vector<double>>> arm_angles_;
std::vector<double> left_arm_joint_angles_;
std::vector<double> right_arm_joint_angles_;

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

@@ -84,66 +84,66 @@ void CerebellumHooks::ConfigureArmHooks(CerebellumNode * node)
std::vector<double> angle{360.0, 360.0, 360.0, 360.0, 360.0, 360.0}; //invalid angles
if (goal.data_type == node->arm_cmd_type_take_photo_) { //take photo
if (goal.body_id == RIGHT_ARM) {
if (!node->right_arm_take_photo_poses_.empty()) {
pose = node->right_arm_take_photo_poses_[node->right_arm_take_photo_poses_.size()-1];
if (!node->arm_poses_["right_arm_take_photo"].empty()) {
pose = node->arm_poses_["right_arm_take_photo"].back();
}
if (!node->right_arm_take_photo_angles_.empty()) {
angle = node->right_arm_take_photo_angles_[node->right_arm_take_photo_angles_.size()-1];
if (!node->arm_angles_["right_arm_take_photo"].empty()) {
angle = node->arm_angles_["right_arm_take_photo"].back();
}
} else {
if (!node->left_arm_take_photo_poses_.empty()) {
pose = node->left_arm_take_photo_poses_[node->left_arm_take_photo_poses_.size()-1];
if (!node->arm_poses_["left_arm_take_photo"].empty()) {
pose = node->arm_poses_["left_arm_take_photo"].back();
}
if (!node->left_arm_take_photo_angles_.empty()) {
angle = node->left_arm_take_photo_angles_[node->left_arm_take_photo_angles_.size()-1];
if (!node->arm_angles_["left_arm_take_photo"].empty()) {
angle = node->arm_angles_["left_arm_take_photo"].back();
}
}
} else if (goal.data_type == node->arm_cmd_type_pre_grasp_) { // pre-grasp
if (goal.body_id == RIGHT_ARM) {
if (!node->right_arm_pre_grasp_poses_.empty()) {
pose = node->right_arm_pre_grasp_poses_[node->right_arm_pre_grasp_poses_.size()-1];
if (!node->arm_poses_["right_arm_pre_grasp"].empty()) {
pose = node->arm_poses_["right_arm_pre_grasp"].back();
}
if (!node->right_arm_pre_grasp_angles_.empty()) {
angle = node->right_arm_pre_grasp_angles_[node->right_arm_pre_grasp_angles_.size()-1];
if (!node->arm_angles_["right_arm_pre_grasp"].empty()) {
angle = node->arm_angles_["right_arm_pre_grasp"].back();
}
} else {
if (!node->left_arm_pre_grasp_poses_.empty()) {
pose = node->left_arm_pre_grasp_poses_[node->left_arm_pre_grasp_poses_.size()-1];
if (!node->arm_poses_["left_arm_pre_grasp"].empty()) {
pose = node->arm_poses_["left_arm_pre_grasp"].back();
}
if (!node->left_arm_pre_grasp_angles_.empty()) {
angle = node->left_arm_pre_grasp_angles_[node->left_arm_pre_grasp_angles_.size()-1];
if (!node->arm_angles_["left_arm_pre_grasp"].empty()) {
angle = node->arm_angles_["left_arm_pre_grasp"].back();
}
}
} else if (goal.data_type == node->arm_cmd_type_grasp_) { // grasp
if (goal.body_id == RIGHT_ARM) {
if (!node->right_arm_grasp_poses_.empty()) {
pose = node->right_arm_grasp_poses_[node->right_arm_grasp_poses_.size()-1];
if (!node->arm_poses_["right_arm_grasp"].empty()) {
pose = node->arm_poses_["right_arm_grasp"].back();
}
if (!node->right_arm_grasp_angles_.empty()) {
angle = node->right_arm_grasp_angles_[node->right_arm_grasp_angles_.size()-1];
if (!node->arm_angles_["right_arm_grasp"].empty()) {
angle = node->arm_angles_["right_arm_grasp"].back();
}
} else {
if (!node->left_arm_grasp_poses_.empty()) {
pose = node->left_arm_grasp_poses_[node->left_arm_grasp_poses_.size()-1];
if (!node->arm_poses_["left_arm_grasp"].empty()) {
pose = node->arm_poses_["left_arm_grasp"].back();
}
if (!node->left_arm_grasp_angles_.empty()) {
angle = node->left_arm_grasp_angles_[node->left_arm_grasp_angles_.size()-1];
if (!node->arm_angles_["left_arm_grasp"].empty()) {
angle = node->arm_angles_["left_arm_grasp"].back();
}
}
} else if (goal.data_type == node->arm_cmd_type_take_object_) { //take object
if (goal.body_id == RIGHT_ARM) {
if (!node->right_arm_take_object_pose_.empty()) {
pose = node->right_arm_take_object_pose_[node->right_arm_take_object_pose_.size()-1];
if (!node->arm_poses_["right_arm_take_object"].empty()) {
pose = node->arm_poses_["right_arm_take_object"].back();
}
if (!node->right_arm_take_object_angles_.empty()) {
angle = node->right_arm_take_object_angles_[node->right_arm_take_object_angles_.size()-1];
if (!node->arm_angles_["right_arm_take_object"].empty()) {
angle = node->arm_angles_["right_arm_take_object"].back();
}
} else {
if (!node->left_arm_take_object_pose_.empty()) {
pose = node->left_arm_take_object_pose_[node->left_arm_take_object_pose_.size()-1];
if (!node->arm_poses_["left_arm_take_object"].empty()) {
pose = node->arm_poses_["left_arm_take_object"].back();
}
if (!node->left_arm_take_object_angles_.empty()) {
angle = node->left_arm_take_object_angles_[node->left_arm_take_object_angles_.size()-1];
if (!node->arm_angles_["left_arm_take_object"].empty()) {
angle = node->arm_angles_["left_arm_take_object"].back();
}
}
} else {
@@ -725,11 +725,11 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
#endif
if(target_arm == "right_arm") {
node->right_arm_take_photo_poses_.push_back(pose);
node->right_arm_take_photo_angles_.push_back(joint_angles);
node->arm_poses_["right_arm_take_photo"].push_back(pose);
node->arm_angles_["right_arm_take_photo"].push_back(joint_angles);
} else if (target_arm == "left_arm") {
node->left_arm_take_photo_poses_.push_back(pose);
node->left_arm_take_photo_angles_.push_back(joint_angles);
node->arm_poses_["left_arm_take_photo"].push_back(pose);
node->arm_angles_["left_arm_take_photo"].push_back(joint_angles);
}
return true;
@@ -882,19 +882,19 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
take_object_msg.orientation.w = take_object_pose.orientation.w();
if (node->camera_position_ == "right") {
node->right_arm_pre_grasp_poses_.push_back(pre_grasp_msg);
node->right_arm_grasp_poses_.push_back(grasp_msg);
node->right_arm_take_object_pose_.push_back(take_object_msg);
node->right_arm_pre_grasp_angles_.push_back(pre_grasp_joint_angles);
node->right_arm_grasp_angles_.push_back(grasp_joint_angles);
node->right_arm_take_object_angles_.push_back(take_object_joint_angles);
node->arm_poses_["right_arm_pre_grasp"].push_back(pre_grasp_msg);
node->arm_poses_["right_arm_grasp"].push_back(grasp_msg);
node->arm_poses_["right_arm_take_object"].push_back(take_object_msg);
node->arm_angles_["right_arm_pre_grasp"].push_back(pre_grasp_joint_angles);
node->arm_angles_["right_arm_grasp"].push_back(grasp_joint_angles);
node->arm_angles_["right_arm_take_object"].push_back(take_object_joint_angles);
} else if (node->camera_position_ == "left") {
node->left_arm_pre_grasp_poses_.push_back(pre_grasp_msg);
node->left_arm_grasp_poses_.push_back(grasp_msg);
node->left_arm_take_object_pose_.push_back(take_object_msg);
node->left_arm_pre_grasp_angles_.push_back(pre_grasp_joint_angles);
node->left_arm_grasp_angles_.push_back(grasp_joint_angles);
node->left_arm_take_object_angles_.push_back(take_object_joint_angles);
node->arm_poses_["left_arm_pre_grasp"].push_back(pre_grasp_msg);
node->arm_poses_["left_arm_grasp"].push_back(grasp_msg);
node->arm_poses_["left_arm_take_object"].push_back(take_object_msg);
node->arm_angles_["left_arm_pre_grasp"].push_back(pre_grasp_joint_angles);
node->arm_angles_["left_arm_grasp"].push_back(grasp_joint_angles);
node->arm_angles_["left_arm_take_object"].push_back(take_object_joint_angles);
}
RCLCPP_INFO(node->get_logger(), "Generated Pose Success (grasp_type: %s, Angle: %.1f, Palm: %s)", result.name.c_str(), angle, palm.c_str());
@@ -956,24 +956,24 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
skill_name.c_str(), resp->success, resp->objects.size(), resp->header.frame_id.c_str(), stamp_sec, resp->info.c_str());
if (node->camera_position_ == "left") {
node->left_arm_pre_grasp_poses_.clear();
node->left_arm_grasp_poses_.clear();
node->left_arm_take_object_pose_.clear();
node->left_arm_pre_grasp_angles_.clear();
node->left_arm_grasp_angles_.clear();
node->left_arm_take_object_angles_.clear();
node->arm_poses_["left_arm_pre_grasp"].clear();
node->arm_poses_["left_arm_grasp"].clear();
node->arm_poses_["left_arm_take_object"].clear();
node->arm_angles_["left_arm_pre_grasp"].clear();
node->arm_angles_["left_arm_grasp"].clear();
node->arm_angles_["left_arm_take_object"].clear();
} else if (node->camera_position_ == "right") {
node->right_arm_pre_grasp_poses_.clear();
node->right_arm_grasp_poses_.clear();
node->right_arm_take_object_pose_.clear();
node->right_arm_pre_grasp_angles_.clear();
node->right_arm_grasp_angles_.clear();
node->right_arm_take_object_angles_.clear();
node->arm_poses_["right_arm_pre_grasp"].clear();
node->arm_poses_["right_arm_grasp"].clear();
node->arm_poses_["right_arm_take_object"].clear();
node->arm_angles_["right_arm_pre_grasp"].clear();
node->arm_angles_["right_arm_grasp"].clear();
node->arm_angles_["right_arm_take_object"].clear();
} else if (node->camera_position_ == "top") {
node->left_arm_take_photo_poses_.clear();
node->left_arm_take_photo_angles_.clear();
node->right_arm_take_photo_poses_.clear();
node->right_arm_take_photo_angles_.clear();
node->arm_poses_["left_arm_take_photo"].clear();
node->arm_angles_["left_arm_take_photo"].clear();
node->arm_poses_["right_arm_take_photo"].clear();
node->arm_angles_["right_arm_take_photo"].clear();
}
for (size_t i = 0; i < resp->objects.size(); ++i) {
@@ -990,56 +990,56 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
}
if (node->camera_position_ == "top") {
if (node->right_arm_take_photo_poses_.empty() && node->left_arm_take_photo_poses_.empty()) {
if (node->arm_poses_["right_arm_take_photo"].empty() && node->arm_poses_["left_arm_take_photo"].empty()) {
RCLCPP_WARN(node->get_logger(), "[%s] no valid hand take photo poses generated", skill_name.c_str());
return false;
}
if (node->right_arm_take_photo_angles_.empty() && node->left_arm_take_photo_angles_.empty()) {
if (node->arm_angles_["right_arm_take_photo"].empty() && node->arm_angles_["left_arm_take_photo"].empty()) {
RCLCPP_WARN(node->get_logger(), "[%s] no valid hand take photo angles generated", skill_name.c_str());
return false;
}
} else if (node->camera_position_ == "right") {
if (node->right_arm_pre_grasp_poses_.empty() || node->right_arm_grasp_poses_.empty()) {
if (node->arm_poses_["right_arm_pre_grasp"].empty() || node->arm_poses_["right_arm_grasp"].empty()) {
RCLCPP_WARN(node->get_logger(), "[%s] no valid grasp poses generated", skill_name.c_str());
return false;
}
if (node->right_arm_pre_grasp_poses_.size() != node->right_arm_grasp_poses_.size()) {
RCLCPP_WARN(node->get_logger(), "[%s] right_arm_pre_grasp_poses_ size %zu not match right_arm_grasp_poses_ size %zu",
skill_name.c_str(), node->right_arm_pre_grasp_poses_.size(), node->right_arm_grasp_poses_.size());
if (node->arm_poses_["right_arm_pre_grasp"].size() != node->arm_poses_["right_arm_grasp"].size()) {
RCLCPP_WARN(node->get_logger(), "[%s] right_arm_pre_grasp size %zu not match right_arm_grasp size %zu",
skill_name.c_str(), node->arm_poses_["right_arm_pre_grasp"].size(), node->arm_poses_["right_arm_grasp"].size());
return false;
}
if (node->right_arm_pre_grasp_angles_.empty() || node->right_arm_grasp_angles_.empty() || node->right_arm_take_object_angles_.empty()) {
if (node->arm_angles_["right_arm_pre_grasp"].empty() || node->arm_angles_["right_arm_grasp"].empty() || node->arm_angles_["right_arm_take_object"].empty()) {
RCLCPP_WARN(node->get_logger(), "[%s] no valid grasp poses generated", skill_name.c_str());
return false;
}
if (node->right_arm_pre_grasp_angles_.size() != node->right_arm_grasp_angles_.size() ||
node->right_arm_grasp_angles_.size() != node->right_arm_take_object_angles_.size() ||
node->right_arm_pre_grasp_angles_.size() != node->right_arm_take_object_angles_.size()) {
if (node->arm_angles_["right_arm_pre_grasp"].size() != node->arm_angles_["right_arm_grasp"].size() ||
node->arm_angles_["right_arm_grasp"].size() != node->arm_angles_["right_arm_take_object"].size() ||
node->arm_angles_["right_arm_pre_grasp"].size() != node->arm_angles_["right_arm_take_object"].size()) {
RCLCPP_WARN(node->get_logger(), "[%s] angles size not match [%zu %zu %zu]",
skill_name.c_str(), node->right_arm_pre_grasp_angles_.size(), node->right_arm_grasp_angles_.size(), node->right_arm_take_object_angles_.size());
skill_name.c_str(), node->arm_angles_["right_arm_pre_grasp"].size(), node->arm_angles_["right_arm_grasp"].size(), node->arm_angles_["right_arm_take_object"].size());
return false;
}
} else if (node->camera_position_ == "left") {
if (node->left_arm_pre_grasp_poses_.empty() || node->left_arm_grasp_poses_.empty()) {
if (node->arm_poses_["left_arm_pre_grasp"].empty() || node->arm_poses_["left_arm_grasp"].empty()) {
RCLCPP_WARN(node->get_logger(), "[%s] no valid grasp poses generated", skill_name.c_str());
return false;
}
if (node->left_arm_pre_grasp_poses_.size() != node->left_arm_grasp_poses_.size()) {
RCLCPP_WARN(node->get_logger(), "[%s] left_arm_pre_grasp_poses_ size %zu not match left_arm_grasp_poses_ size %zu",
skill_name.c_str(), node->left_arm_pre_grasp_poses_.size(), node->left_arm_grasp_poses_.size());
if (node->arm_poses_["left_arm_pre_grasp"].size() != node->arm_poses_["left_arm_grasp"].size()) {
RCLCPP_WARN(node->get_logger(), "[%s] left_arm_pre_grasp size %zu not match left_arm_grasp size %zu",
skill_name.c_str(), node->arm_poses_["left_arm_pre_grasp"].size(), node->arm_poses_["left_arm_grasp"].size());
return false;
}
if (node->left_arm_pre_grasp_angles_.empty() || node->left_arm_grasp_angles_.empty() || node->left_arm_take_object_angles_.empty()) {
if (node->arm_angles_["left_arm_pre_grasp"].empty() || node->arm_angles_["left_arm_grasp"].empty() || node->arm_angles_["left_arm_take_object"].empty()) {
RCLCPP_WARN(node->get_logger(), "[%s] no valid grasp poses generated", skill_name.c_str());
return false;
}
if (node->left_arm_pre_grasp_angles_.size() != node->left_arm_grasp_angles_.size() ||
node->left_arm_grasp_angles_.size() != node->left_arm_take_object_angles_.size() ||
node->left_arm_pre_grasp_angles_.size() != node->left_arm_take_object_angles_.size()) {
if (node->arm_angles_["left_arm_pre_grasp"].size() != node->arm_angles_["left_arm_grasp"].size() ||
node->arm_angles_["left_arm_grasp"].size() != node->arm_angles_["left_arm_take_object"].size() ||
node->arm_angles_["left_arm_pre_grasp"].size() != node->arm_angles_["left_arm_take_object"].size()) {
RCLCPP_WARN(node->get_logger(), "[%s] angles size not match [%zu %zu %zu]",
skill_name.c_str(), node->left_arm_pre_grasp_angles_.size(), node->left_arm_grasp_angles_.size(), node->left_arm_take_object_angles_.size());
skill_name.c_str(), node->arm_angles_["left_arm_pre_grasp"].size(), node->arm_angles_["left_arm_grasp"].size(), node->arm_angles_["left_arm_take_object"].size());
return false;
}
}