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

test vision detect

Browse Source
This commit is contained in:
david
2025-12-04 10:10:07 +08:00
parent 9fe000911f
commit 948785a1a8
3 changed files with 39 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -2,7 +2,7 @@
<BehaviorTree ID="MainTree">
<Sequence name="root">
<!-- Retry vision + hand until GripperControl_H succeeds -->
<RetryUntilSuccessful name="retry_vision_hand" num_attempts="3">
<RetryUntilSuccessful name="retry_vision_hand" num_attempts="20">
<Sequence>
<!-- <GripperCmd_H name="gripper_open" /> -->
<VisionObjectRecognition_H name="VisionObjectRecognition_H" />

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

@@ -94,7 +94,8 @@ private:
std::mutex stats_mutex_;
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr stats_pub_;
std::unordered_map<std::string, geometry_msgs::msg::PoseStamped> target_pose_;
std::string target_frame_{"bottle"};
std::vector<std::string> target_frames_{"apple", "bottle"};
std::string target_frame_{"apple"};
int64_t command_id_{0};
int64_t command_id_left_arm_ {0};
int64_t command_id_right_arm_{0};

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

@@ -521,27 +521,37 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
using VisionSrv = interfaces::srv::VisionObjectRecognition;
auto UpdateTargetPose = [node](const interfaces::msg::PoseClassAndID & obj, const std_msgs::msg::Header & header) -> bool {
if (node->target_frame_ == obj.class_name) {
node->target_pose_[node->target_frame_].header = header;
node->target_pose_[node->target_frame_].header.frame_id = "RightLink6";
node->target_pose_[node->target_frame_].pose = obj.pose;
// node->target_pose_[node->target_frame_].pose.position.x -= obj.grab_width / 2;
// node->target_pose_[node->target_frame_].pose.position.z -= 0.175;
node->grab_width_ = obj.grab_width;
RCLCPP_INFO(node->get_logger(), "target_frame_: %s, grab_width_: %f, updated target_pose", node->target_frame_.c_str(), node->grab_width_);
return true;
} else {
RCLCPP_WARN(node->get_logger(), "object class_name[%s] not match target_frame_[%s]",
obj.class_name.c_str(), node->target_frame_.c_str());
return false;
for (auto &tf: node->target_frames_) {
if (tf == obj.class_name) {
node->target_pose_[tf].header = header;
node->target_pose_[tf].header.frame_id = "RightLink6";
node->target_pose_[tf].pose = obj.pose;
// node->target_pose_[tf].pose.position.x -= obj.grab_width / 2;
// node->target_pose_[tf].pose.position.z -= 0.175;
node->grab_width_ = obj.grab_width;
// RCLCPP_INFO(node->get_logger(), "target_frame_: %s, grab_width_: %f, updated target_pose",
// tf.c_str(), node->grab_width_);
const auto & p = obj.pose;
RCLCPP_INFO(node->get_logger(), "target_frame: %s, grab_width: %f, target_pose: %.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f",
tf.c_str(), node->grab_width_,
p.position.x, p.position.y, p.position.z, p.orientation.x, p.orientation.y, p.orientation.z, p.orientation.w);
return true;
}
// else {
// RCLCPP_WARN(node->get_logger(), "object class_name[%s] not match target_frame_[%s]",
// obj.class_name.c_str(), tf.c_str());
// return false;
// }
}
RCLCPP_WARN(node->get_logger(), "object class_name not match target frames");
return false;
};
auto CalculateGraspPoses = [node](const std::string & skill_name) -> bool {
const auto & p = node->target_pose_[node->target_frame_].pose;
RCLCPP_INFO(node->get_logger(), "target_pose: %.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f",
p.position.x, p.position.y, p.position.z, p.orientation.x, p.orientation.y, p.orientation.z, p.orientation.w);
// const auto & p = node->target_pose_[node->target_frame_].pose;
// RCLCPP_INFO(node->get_logger(), "target_pose: %.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f",
// p.position.x, p.position.y, p.position.z, p.orientation.x, p.orientation.y, p.orientation.z, p.orientation.w);
geometry_msgs::msg::PoseStamped pose_in_arm;
const bool transformed = node->TransformPoseToArmFrame(node->target_pose_[node->target_frame_], pose_in_arm);
@@ -566,11 +576,15 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
return false;
}
best_angles.clear();
// best_angles.clear();
best_angles.push_back(-45);
best_angles.push_back(0);
best_angles.push_back(-60);
best_angles.push_back(-75);
best_angles.push_back(-90);
{
std::vector<std::string> palm_facings = {"up"}; //{"up", "down"};
std::vector<std::string> palm_facings = {"down"}; //{"up", "down"};
std::vector<double> current_joint_angles;
{
std::lock_guard<std::mutex> lk(node->joint_state_mutex_);
@@ -700,13 +714,15 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
for (size_t i = 0; i < resp->objects.size(); ++i) {
const auto & obj = resp->objects[i];
RCLCPP_INFO(node->get_logger(), " [%zu] class='%s' id=%d", i, obj.class_name.c_str(), obj.class_id);
// RCLCPP_INFO(node->get_logger(), " [%zu] class='%s' id=%d", i, obj.class_name.c_str(), obj.class_id);
if (!UpdateTargetPose(obj, resp->header)) {
continue;
}
#ifdef ENABLE_CAL_GRASP_POSE
if (!CalculateGraspPoses(skill_name)) {
continue;
}
#endif
}
if (node->pre_grasp_poses_.empty() || node->grasp_poses_.empty()) {