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

update grasp sch and data

Browse Source
This commit is contained in:
david
2025-11-24 13:55:03 +08:00
parent 79ade2d741
commit e8f1021af1
3 changed files with 36 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files

26
src/brain/config/bt_vision_grasp.params.yaml
View File

@@ -5,9 +5,21 @@
- name: retry_vision_hand
params: '{}
'
- name: gripper_open
params: 'devid: 6
loc: 0
speed: 20
torque: 20
mode: 2
'
- name: VisionObjectRecognition_H
params: 'camera_position: ''''
params: 'camera_position: right
'
- name: right_arm_vision_grasp
@@ -24,15 +36,15 @@
data_array: []
'
- name: JzCmd_H
params: 'devid: 0
- name: gripper_close
params: 'devid: 6
loc: 0
loc: 30
speed: 0
speed: 20
torque: 0
torque: 20
mode: 0
mode: 2
'

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

@@ -2,14 +2,16 @@
<BehaviorTree ID="MainTree">
<Sequence name="root">
<!-- Retry vision + hand until GripperControl_H succeeds -->
<RetryUntilSuccessful name="retry_vision_hand" num_attempts="5">
<RetryUntilSuccessful name="retry_vision_hand" num_attempts="3">
<Sequence>
<JzCmd_H name="gripper_open" />
<VisionObjectRecognition_H name="VisionObjectRecognition_H" />
<Arm_H name="right_arm_vision_grasp" />
<!-- <Arm_H name="right_arm_vision_grasp" /> -->
<JzCmd_H name="gripper_close" />
</Sequence>
</RetryUntilSuccessful>
<!-- After Arm_H eventually succeeds, run the hand -->
<JzCmd_H name="JzCmd_H" />
<!-- <JzCmd_H name="JzCmd_H" /> -->
</Sequence>
</BehaviorTree>
</root>

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

@@ -160,11 +160,18 @@ void CerebellumNode::ConfigureArmHooks()
}
goal.data_length = USED_ARM_DOF;
}
} else {
RCLCPP_ERROR(this->get_logger(), "ARM make goal error: data_type[%d] not support", goal.data_type);
return goal;
}
RCLCPP_INFO(this->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]);
if (goal.data_array.size() >= USED_ARM_DOF) {
RCLCPP_INFO(this->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]);
} else {
RCLCPP_ERROR(this->get_logger(), "ARM make goal error: invalid data_array size[%ld]", goal.data_array.size());
}
return goal;
};
@@ -510,6 +517,8 @@ void CerebellumNode::ConfigureJzCmdControlHooks()
return goal;
}
// goal.loc = 30; //grab_width_; //TODO transfer
RCLCPP_INFO(this->get_logger(), "[%s] action make_goal params, devid: %d, loc: %d, speed: %d, torque: %d, mode: %d",
skill_name.c_str(), goal.devid, goal.loc, goal.speed, goal.torque, goal.mode);