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
2026-01-06 16:05:29 +08:00
parent 19d59c50db
commit 4e0d53d41e
4 changed files with 45 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -52,19 +52,37 @@
<GripperCmd_H name="s1_gripper_open" />
<Arm_H name="s2_right_arm_pre_cam_take_photo" />
</Parallel>
<RetryUntilSuccessful name="retry_camera_vision_recg_1" num_attempts="10">
<!-- <RetryUntilSuccessful name="retry_camera_vision_recg_1" num_attempts="10">
<VisionObjectRecognition_H name="s2_top_camera_vision_recg" />
</RetryUntilSuccessful>
</RetryUntilSuccessful> -->
<SubTree ID="vision_top_cam_subtree"/>
<Arm_H name="s2_right_arm_cam_take_photo" />
<RetryUntilSuccessful name="retry_camera_vision_recg_2" num_attempts="10">
<!-- <RetryUntilSuccessful name="retry_camera_vision_recg_2" num_attempts="10">
<VisionObjectRecognition_H name="s3_right_camera_vision_recg" />
</RetryUntilSuccessful>
</RetryUntilSuccessful> -->
<SubTree ID="vision_hand_cam_subtree"/>
</Sequence>
</RetryUntilSuccessful>
</Sequence>
</BehaviorTree>
<BehaviorTree ID="vision_top_cam_subtree">
<Sequence name="vision_top_cam_root">
<RetryUntilSuccessful name="retry_top_cam_vision_recg_1" num_attempts="10">
<VisionObjectRecognition_H name="s2_top_camera_vision_recg" />
</RetryUntilSuccessful>
</Sequence>
</BehaviorTree>
<BehaviorTree ID="vision_hand_cam_subtree">
<Sequence name="vision_hand_cam_root">
<RetryUntilSuccessful name="retry_hand_cam_vision_recg_1" num_attempts="10">
<VisionObjectRecognition_H name="s3_right_camera_vision_recg" />
</RetryUntilSuccessful>
</Sequence>
</BehaviorTree>
<BehaviorTree ID="grasp_subtree">
<Sequence name="grasp_root">
<RetryUntilSuccessful name="retry_grasp" num_attempts="1">

0
src/brain/launch/brain_launch.py → src/brain/launch/brain.launch.py
View File

0
src/brain/launch/brain_with_params_launch.py → src/brain/launch/brain_with_params.launch.py
View File

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

@@ -628,7 +628,7 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
pose.position.x += 0.08f;
// pose.position.y -= 0.40f;
pose.position.y = 0.34f;
pose.position.y = 0.30f;
pose.position.z += 0.03f;
pose.orientation.x = -0.7071;
pose.orientation.y = 0.0000;
@@ -701,7 +701,7 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
brain::GraspResult result = brain::GraspPoseCalculator::calculate(target_pos,
target_quat, "side", angle, palm, 0.10, 0.20, node->arm_target_frame_);
#ifdef DEBUG_GRASP_POSE_CALCULATION
#ifdef DEBUG_GRASP_POSE_CALCULATION
RCLCPP_INFO(node->get_logger(), "[%s] angle %f, palm %s, pre_grasp_pose[%.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f]",
skill_name.c_str(), angle, palm.c_str(),
result.pre_grasp_pose.position.x(), result.pre_grasp_pose.position.y(), result.pre_grasp_pose.position.z(),
@@ -711,7 +711,7 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
skill_name.c_str(), angle, palm.c_str(),
result.grasp_pose.position.x(), result.grasp_pose.position.y(), result.grasp_pose.position.z(),
result.grasp_pose.orientation.x(), result.grasp_pose.orientation.y(), result.grasp_pose.orientation.z(), result.grasp_pose.orientation.w());
#endif
#endif
//pre grasp
std::vector<double> pre_grasp_joint_angles = current_joint_angles;
if (brain::GraspPoseCalculator::rm_arm_inverse_kinematics(result.pre_grasp_pose.position,
@@ -740,20 +740,29 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
continue;
}
#ifdef DEBUG_GRASP_POSE_CALCULATION
RCLCPP_INFO(node->get_logger(), "pre_grasp_joint_angles: ");
for (const auto & angles: pre_grasp_joint_angles) {
RCLCPP_INFO(node->get_logger(), "%.4f", angles);
#ifdef DEBUG_GRASP_POSE_CALCULATION
if (pre_grasp_joint_angles.size() >= 6) {
RCLCPP_INFO(node->get_logger(), "pre_grasp joint angles: [%.4f, %.4f, %.4f, %.4f, %.4f, %.4f]",
pre_grasp_joint_angles[0], pre_grasp_joint_angles[1], pre_grasp_joint_angles[2],
pre_grasp_joint_angles[3], pre_grasp_joint_angles[4], pre_grasp_joint_angles[5]);
} else {
RCLCPP_ERROR(node->get_logger(), "pre_grasp joint angles size invalid: %ld", pre_grasp_joint_angles.size());
}
RCLCPP_INFO(node->get_logger(), "grasp_joint_angles: ");
for (const auto & angles: grasp_joint_angles) {
RCLCPP_INFO(node->get_logger(), "%.4f", angles);
if (grasp_joint_angles.size() >= 6) {
RCLCPP_INFO(node->get_logger(), "grasp joint angles: [%.4f, %.4f, %.4f, %.4f, %.4f, %.4f]",
grasp_joint_angles[0], grasp_joint_angles[1], grasp_joint_angles[2],
grasp_joint_angles[3], grasp_joint_angles[4], grasp_joint_angles[5]);
} else {
RCLCPP_ERROR(node->get_logger(), "grasp joint angles size invalid: %ld", grasp_joint_angles.size());
}
RCLCPP_INFO(node->get_logger(), "take_object_joint_angles: ");
for (const auto & angles: take_object_joint_angles) {
RCLCPP_INFO(node->get_logger(), "%.4f", angles);
if (take_object_joint_angles.size() >= 6) {
RCLCPP_INFO(node->get_logger(), "take_object joint angles: [%.4f, %.4f, %.4f, %.4f, %.4f, %.4f]",
take_object_joint_angles[0], take_object_joint_angles[1], take_object_joint_angles[2],
take_object_joint_angles[3], take_object_joint_angles[4], take_object_joint_angles[5]);
} else {
RCLCPP_ERROR(node->get_logger(), "take_object joint angles size invalid: %ld", take_object_joint_angles.size());
}
#endif
#endif
geometry_msgs::msg::Pose pre_grasp_msg;
pre_grasp_msg.position.x = result.pre_grasp_pose.position.x();
pre_grasp_msg.position.y = result.pre_grasp_pose.position.y();