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

optimize code

Browse Source
This commit is contained in:
NuoDaJia02
2025-12-27 13:29:39 +08:00
parent 994f2a574f
commit f56490c5b1
3 changed files with 42 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files

23
src/brain/config/bt_vision_grasp.xml
View File

@@ -1,30 +1,37 @@
<root BTCPP_format="4">
<BehaviorTree ID="MainTree">
<Sequence name="root">
<RetryUntilSuccessful name="retry_vision_hand" num_attempts="3">
<RetryUntilSuccessful name="retry_vision_grasp" num_attempts="3">
<Sequence>
<Arm_H name="s1_right_arm_pre_cam_take_photo" />
<Parallel name="parallel_action_1">
<MoveWaist_H name="s1_move_waist_turn_right_90" />
<!-- <MoveWaist_H name="s1_move_waist_turn_right_90" /> -->
<GripperCmd_H name="s1_gripper_open" />
</Parallel>
<!-- <Arm_H name="s1_right_arm_pre_cam_take_photo" /> -->
<VisionObjectRecognition_H name="s2_top_camera_vision_recg" />
<RetryUntilSuccessful name="retry_camera_vision_recg_1" num_attempts="3">
<VisionObjectRecognition_H name="s2_top_camera_vision_recg" />
</RetryUntilSuccessful>
<Arm_H name="s2_right_arm_cam_take_photo" />
<VisionObjectRecognition_H name="s3_right_camera_vision_recg" />
<RetryUntilSuccessful name="retry_camera_vision_recg_2" num_attempts="3">
<VisionObjectRecognition_H name="s3_right_camera_vision_recg" />
</RetryUntilSuccessful>
<Arm_H name="s3_right_arm_vision_pre_grasp" />
<Arm_H name="s4_right_arm_vision_grasp" />
<GripperCmd_H name="s4_gripper_close" />
<Arm_H name="s4_right_arm_take_box" />
<Arm_H name="s5_right_arm_grasp_box" />
<Parallel name="parallel_action_2">
<Arm_H name="s5_right_arm_grasp_box" />
<!-- <MoveWaist_H name="s6_move_waist_turn_left_180" /> -->
</Parallel>
<MoveWaist_H name="s6_move_waist_turn_left_180" />
<Arm_H name="s7_right_arm_put_down_box" />
<GripperCmd_H name="s8_gripper_open" />
<Arm_H name="s9_right_arm_grasp_box" />
<MoveWaist_H name="s10_move_waist_turn_right_90" />
<!-- <MoveWaist_H name="s10_move_waist_turn_right_90" /> -->
<Arm_H name="s11_right_arm_pre_cam_take_photo" />
</Sequence>
</RetryUntilSuccessful>

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

@@ -303,7 +303,7 @@ inline interfaces::srv::MotorInfo::Request from_yaml<interfaces::srv::MotorInfo:
return r;
}
// MotorParam::Request: motor_id, max_speed, add_acc, dec_acc
// MotorParam::Request: motor_id, max_speed, accel
template<>
inline interfaces::srv::MotorParam::Request from_yaml<interfaces::srv::MotorParam::Request>(const YAML::Node & n)
{
@@ -311,8 +311,7 @@ inline interfaces::srv::MotorParam::Request from_yaml<interfaces::srv::MotorPara
if (n && n.IsMap()) {
if (n["motor_id"]) r.motor_id = n["motor_id"].as<uint16_t>();
if (n["max_speed"]) r.max_speed = n["max_speed"].as<uint16_t>();
if (n["add_acc"]) r.add_acc = n["add_acc"].as<uint16_t>();
if (n["dec_acc"]) r.dec_acc = n["dec_acc"].as<uint16_t>();
if (n["accel"]) r.accel = n["accel"].as<uint16_t>();
}
return r;
}

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

@@ -85,21 +85,31 @@ void CerebellumHooks::ConfigureArmHooks(CerebellumNode * node)
} else if (goal.data_type >= 100 && goal.data_type <= 110) {
geometry_msgs::msg::Pose pose;
if (goal.data_type == 100) {
pose = node->right_hand_take_photo_pose_[0];
if (!node->right_hand_take_photo_pose_.empty()) {
pose = node->right_hand_take_photo_pose_[0];
}
}
else if (goal.data_type == 101) { // pre-grasp
pose = node->pre_grasp_poses_[0];
if (!node->pre_grasp_poses_.empty()) {
pose = node->pre_grasp_poses_[0];
}
} else if (goal.data_type == 102) { // grasp
pose = node->grasp_poses_[0];
if (!node->grasp_poses_.empty()) {
pose = node->grasp_poses_[0];
}
pose.position.y += 0.03;
node->right_hand_take_object_pose_ = pose;
node->right_hand_take_object_pose_.position.x -= 0.06;
} else if (goal.data_type == 103) { // left arm pre-grasp
pose = node->pre_grasp_poses_[1];
if (!node->pre_grasp_poses_.empty()) {
pose = node->pre_grasp_poses_[1];
}
} else if (goal.data_type == 104) { // left arm grasp
pose = node->grasp_poses_[1];
if (!node->grasp_poses_.empty()) {
pose = node->grasp_poses_[1];
}
} else if (goal.data_type == 110) {
pose = node->right_hand_take_object_pose_;
} else {
@@ -588,6 +598,8 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
// p.position.x, p.position.y, p.position.z, p.orientation.x, p.orientation.y, p.orientation.z, p.orientation.w);
if (node->camera_position_ == "top") {
node->right_hand_take_photo_pose_.clear();
geometry_msgs::msg::Pose pose = node->target_pose_[node->target_frame_].pose;
RCLCPP_INFO(node->get_logger(), "[%s] target pose %s frame, camera_position: %s",
skill_name.c_str(), node->arm_target_frame_.c_str(), node->camera_position_.c_str());
@@ -597,6 +609,7 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
pose.position.x += 0.08f;
pose.position.y -= 0.40f;
pose.position.z += 0.03f;
pose.orientation.x = -0.7071;
pose.orientation.y = 0.0000;
pose.orientation.z = -0.0000;
@@ -606,12 +619,11 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
pose.position.x, pose.position.y, pose.position.z, pose.orientation.x,
pose.orientation.y, pose.orientation.z, pose.orientation.w);
if (pose.position.x < 0.1 || pose.position.y < 0.1 || pose.position.z < 0.15 ||
if (pose.position.x < 0.1 || pose.position.y < 0.1 || pose.position.z < 0.1 ||
pose.position.x > 0.8 || pose.position.y > 0.8 || pose.position.z > 1.0) {
RCLCPP_ERROR(node->get_logger(), "invalid pose value, cal grasp pose failed");
return false;
}
node->right_hand_take_photo_pose_.push_back(pose);
return true;
@@ -787,7 +799,12 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
#endif
}
if (node->camera_position_ != "top") {
if (node->camera_position_ == "top") {
if (node->right_hand_take_photo_pose_.empty()) {
RCLCPP_WARN(node->get_logger(), "[%s] no valid right hand take photo poses generated", skill_name.c_str());
return false;
}
} else {
if (node->pre_grasp_poses_.empty() || node->grasp_poses_.empty()) {
RCLCPP_WARN(node->get_logger(), "[%s] no valid grasp poses generated", skill_name.c_str());
return false;