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

test vision grasp

Browse Source
This commit is contained in:
david
2025-11-20 11:12:01 +08:00
parent eed410e776
commit 9ecdb1be60
7 changed files with 134 additions and 83 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -7,10 +7,10 @@
'
- name: VisionObjectRecognition_H
params: '{}
params: 'camera_position: right
'
- name: Arm_H
- name: right_arm_vision_grasp
params: 'body_id: 0
data_type: 0

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

@@ -5,7 +5,7 @@
<RetryUntilSuccessful name="retry_vision_hand" num_attempts="5">
<Sequence>
<VisionObjectRecognition_H name="VisionObjectRecognition_H" />
<Arm_H name="Arm_H" />
<Arm_H name="right_arm_vision_grasp" />
</Sequence>
</RetryUntilSuccessful>
<!-- After Arm_H eventually succeeds, run the hand -->

6
src/brain/config/robot_config.yaml
View File

@@ -6,12 +6,14 @@
version: 1.0.0
config_params_path: [
{config: /config/bt_carry_boxes_sch1.xml, param: /config/bt_carry_boxes_sch1.params.yaml},
{config: /config/bt_carry_boxes_sch2.xml, param: /config/bt_carry_boxes_sch2.params.yaml}
{config: /config/bt_carry_boxes_sch2.xml, param: /config/bt_carry_boxes_sch2.params.yaml},
{config: /config/bt_vision_grasp.xml, param: /config/bt_vision_grasp.params.yaml}
]
- name: cerebellum_node
version: 1.0.0
config_params_path: [
{config: /config/bt_carry_boxes_sch1.xml, param: /config/bt_carry_boxes_sch1.params.yaml},
{config: /config/bt_carry_boxes_sch2.xml, param: /config/bt_carry_boxes_sch2.params.yaml}
{config: /config/bt_carry_boxes_sch2.xml, param: /config/bt_carry_boxes_sch2.params.yaml},
{config: /config/bt_vision_grasp.xml, param: /config/bt_vision_grasp.params.yaml}
]

40
src/brain/include/brain/action_registry.hpp
View File

@@ -379,16 +379,16 @@ public:
}
auto result_future = e->client->async_get_result(goal_handle);
// auto deadline = std::chrono::steady_clock::now() + timeout;
// if (result_future.wait_until(deadline) != std::future_status::ready) {
// RCLCPP_ERROR(logger, "Timed out waiting for result on '%s', sending cancel", name.c_str());
// try {
// e->client->async_cancel_goal(goal_handle);
// } catch (const std::exception & ex) {
// RCLCPP_WARN(logger, "Failed cancelling goal for '%s': %s", name.c_str(), ex.what());
// }
// return std::nullopt;
// }
auto deadline = std::chrono::steady_clock::now() + timeout;
if (result_future.wait_until(deadline) != std::future_status::ready) {
RCLCPP_ERROR(logger, "Timed out waiting for result on '%s', sending cancel", name.c_str());
try {
e->client->async_cancel_goal(goal_handle);
} catch (const std::exception & ex) {
RCLCPP_WARN(logger, "Failed cancelling goal for '%s': %s", name.c_str(), ex.what());
}
return std::nullopt;
}
typename rclcpp_action::ClientGoalHandle<ActionT>::WrappedResult wrapped_result;
try {
wrapped_result = result_future.get();
@@ -460,16 +460,16 @@ public:
}
auto result_future = e->client->async_get_result(goal_handle);
// auto deadline = std::chrono::steady_clock::now() + timeout;
// if (result_future.wait_until(deadline) != std::future_status::ready) {
// RCLCPP_ERROR(logger, "Timed out waiting for result on '%s', sending cancel", name.c_str());
// try {
// e->client->async_cancel_goal(goal_handle);
// } catch (const std::exception & ex) {
// RCLCPP_WARN(logger, "Failed cancelling goal for '%s': %s", name.c_str(), ex.what());
// }
// return std::nullopt;
// }
auto deadline = std::chrono::steady_clock::now() + timeout;
if (result_future.wait_until(deadline) != std::future_status::ready) {
RCLCPP_ERROR(logger, "Timed out waiting for result on '%s', sending cancel", name.c_str());
try {
e->client->async_cancel_goal(goal_handle);
} catch (const std::exception & ex) {
RCLCPP_WARN(logger, "Failed cancelling goal for '%s': %s", name.c_str(), ex.what());
}
return std::nullopt;
}
typename rclcpp_action::ClientGoalHandle<ActionT>::WrappedResult wrapped_result;
try {
wrapped_result = result_future.get();

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

@@ -88,8 +88,9 @@ private:
int64_t command_id_{0};
std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
std::string arm_target_frame_{"base_link_leftarm"};
double arm_transform_timeout_sec_{0.2};
std::string arm_target_frame_{"base_link_rightarm"};
double arm_transform_timeout_sec_{1};
double grab_width_{0.0};
// Current accepted sequence epoch (from client) used to filter/reject stale goals
std::atomic<uint32_t> current_epoch_{0};

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

@@ -125,7 +125,7 @@ void CerebellumNode::ConfigureArmHooks()
return goal;
}
RCLCPP_INFO(this->get_logger(), "[%s]Received Arm goal: body_id=%d, data_type=%d, data_length=%d, command_id=%ld, frame_time_stamp=%ld, data_array=",
RCLCPP_INFO(this->get_logger(), "[%s]Received Parse Arm goal: body_id=%d, data_type=%d, data_length=%d, command_id=%ld, frame_time_stamp=%ld",
skill_name.c_str(), goal.body_id, goal.data_type, goal.data_length, goal.command_id, goal.frame_time_stamp);
goal.body_id = (tls_arm_body_id == LEFT_ARM ||
@@ -389,6 +389,9 @@ void CerebellumNode::ConfigureMoveWheelHooks()
return goal;
}
RCLCPP_INFO(this->get_logger(), "[%s] move_distance=%f, move_angle=%f",
skill_name.c_str(), goal.move_distance, goal.move_angle);
return goal;
};
hooks.on_feedback = [this](
@@ -514,15 +517,18 @@ void CerebellumNode::ConfigureVisionObjectRecognitionHooks()
hooks.make_request = [this](const std::string & skill_name) {
auto req = std::make_shared<VisionSrv::Request>();
// Prefer payload_yaml from calls
if (TryParseCallPayload<VisionSrv::Request>(skill_name, *req)) {
if (!TryParseCallPayload<VisionSrv::Request>(skill_name, *req)) {
RCLCPP_ERROR(this->get_logger(), "[%s] Payload Parse failed", skill_name.c_str());
return req;
}
// fallback to parameter
const std::string param = "vision_object_recognition.camera_position";
if (!this->has_parameter(param)) {
this->declare_parameter<std::string>(param, std::string("left"));
}
req->camera_position = this->get_parameter(param).as_string();
// const std::string param = "vision_object_recognition.camera_position";
// if (!this->has_parameter(param)) {
// this->declare_parameter<std::string>(param, std::string("right"));
// }
// req->camera_position = this->get_parameter(param).as_string();
RCLCPP_INFO(this->get_logger(), "[%s] camera_position: %s", skill_name.c_str(), req->camera_position.c_str());
return req;
};
hooks.on_response = [this](
@@ -538,36 +544,79 @@ void CerebellumNode::ConfigureVisionObjectRecognitionHooks()
}
detail = resp->info.empty() ? std::string("success pose") : resp->info;
const auto stamp_sec = rclcpp::Time(resp->header.stamp).seconds();
RCLCPP_INFO(
this->get_logger(),
"[%s] success=%d classes=%zu frame_id=%s stamp=%.3f info=%s",
skill_name.c_str(), resp->success, resp->objects.size(), resp->header.frame_id.c_str(),
stamp_sec, resp->info.c_str());
RCLCPP_INFO(this->get_logger(), "[%s] success=%d classes=%zu frame_id=%s stamp=%.3f info=%s",
skill_name.c_str(), resp->success, resp->objects.size(),
resp->header.frame_id.c_str(), stamp_sec, resp->info.c_str());
for (size_t i = 0; i < resp->objects.size(); ++i) {
const auto & obj = resp->objects[i];
if (target_frame_ == obj.class_name) {
target_pose_[target_frame_].header = resp->header;
target_pose_[target_frame_].pose = obj.pose_list[0];
target_pose_[target_frame_].header.frame_id = "RightLink6";
target_pose_[target_frame_].pose = obj.pose;
target_pose_[target_frame_].pose.position.z -= 0.21;
grab_width_ = obj.grab_width;
RCLCPP_INFO(this->get_logger(), "target_frame_: %s, updated target_pose", target_frame_.c_str());
}
RCLCPP_INFO(
this->get_logger(), " [%zu] class='%s' id=%d poses=%zu", i,
obj.class_name.c_str(), obj.class_id, obj.pose_list.size());
for (size_t j = 0; j < obj.pose_list.size() && j < 2; ++j) {
const auto & p = obj.pose_list[j];
RCLCPP_INFO(
this->get_logger(),
" pose[%zu]: pos(%.3f, %.3f, %.3f) quat(%.3f, %.3f, %.3f, %.3f)", j,
p.position.x, p.position.y, p.position.z,
p.orientation.x, p.orientation.y, p.orientation.z, p.orientation.w);
}
if (obj.pose_list.size() > 2) {
RCLCPP_INFO(
this->get_logger(), " ... (%zu more poses omitted)", obj.pose_list.size() - 2);
RCLCPP_INFO(this->get_logger(), " [%zu] class='%s' id=%d", i,
obj.class_name.c_str(), obj.class_id);
const auto & p = obj.pose;
RCLCPP_INFO(this->get_logger(),
" pos(%.4f, %.4f, %.4f) quat(%.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 = TransformPoseToArmFrame(target_pose_[target_frame_], pose_in_arm);
if (!transformed) {
RCLCPP_WARN(this->get_logger(), "[%s] 坐标系转换失败，继续使用 %s 坐标系数据",
skill_name.c_str(), pose_in_arm.header.frame_id.c_str());
return false;
} else {
RCLCPP_INFO(this->get_logger(), "[%s] 目标姿态已转换到 %s 坐标系",
skill_name.c_str(), arm_target_frame_.c_str());
RCLCPP_INFO(this->get_logger(),
" pos(%.4f, %.4f, %.4f) quat(%.4f, %.4f, %.4f, %.4f)",
pose_in_arm.pose.position.x,
pose_in_arm.pose.position.y,
pose_in_arm.pose.position.z,
pose_in_arm.pose.orientation.x,
pose_in_arm.pose.orientation.y,
pose_in_arm.pose.orientation.z,
pose_in_arm.pose.orientation.w);
}
}
// const auto pose_it = target_pose_.find(target_frame_);
// if (pose_it != target_pose_.end()) {
// geometry_msgs::msg::PoseStamped pose_in_arm;
// const bool transformed = TransformPoseToArmFrame(pose_it->second, pose_in_arm);
// const auto & pose_ref = transformed ? pose_in_arm : pose_it->second;
// if (!transformed) {
// RCLCPP_WARN(this->get_logger(), "[%s] 坐标系转换失败，继续使用 %s 坐标系数据",
// skill_name.c_str(), pose_ref.header.frame_id.c_str());
// } else {
// RCLCPP_INFO(this->get_logger(), "[%s] 目标姿态已转换到 %s 坐标系",
// skill_name.c_str(), arm_target_frame_.c_str());
// RCLCPP_INFO(this->get_logger(),
// " pos(%.4f, %.4f, %.4f) quat(%.4f, %.4f, %.4f, %.4f)",
// pose_ref.pose.position.x,
// pose_ref.pose.position.y,
// pose_ref.pose.position.z,
// pose_ref.pose.orientation.x,
// pose_ref.pose.orientation.y,
// pose_ref.pose.orientation.z,
// pose_ref.pose.orientation.w);
// }
// } else {
// RCLCPP_WARN(this->get_logger(), "[%s] 未找到目标 %s 的姿态缓存，使用默认坐标",
// skill_name.c_str(), target_frame_.c_str());
// }
return true;
};
skill_manager_->configure_service_hooks<VisionSrv>("VisionObjectRecognition", std::move(hooks));
@@ -620,7 +669,7 @@ void CerebellumNode::ConfigureMapLoadHooks()
hooks.call_timeout = [](const std::string &) { return std::chrono::seconds(5); };
hooks.make_request = [this](const std::string & skill_name) {
auto req = std::make_shared<MapLoadSrv::Request>();
if (TryParseCallPayload<MapLoadSrv::Request>(skill_name, *req)) { return req; }
if (!TryParseCallPayload<MapLoadSrv::Request>(skill_name, *req)) { return req; }
// Fallback to parameter
if (!this->has_parameter("map_load_url")) {
this->declare_parameter<std::string>("map_load_url", std::string(""));
@@ -653,7 +702,7 @@ void CerebellumNode::ConfigureMapSaveHooks()
hooks.call_timeout = [](const std::string &) { return std::chrono::seconds(5); };
hooks.make_request = [this](const std::string & skill_name) {
auto req = std::make_shared<MapSaveSrv::Request>();
if (TryParseCallPayload<MapSaveSrv::Request>(skill_name, *req)) { return req; }
if (!TryParseCallPayload<MapSaveSrv::Request>(skill_name, *req)) { return req; }
// Fallback to parameter group
if (!this->has_parameter("map_save_url")) {
this->declare_parameter<std::string>("map_save_url", std::string("/tmp/humanoid_map"));
@@ -1275,15 +1324,14 @@ bool CerebellumNode::ExecuteBilateralArmAction(
RCLCPP_INFO(this->get_logger(), "[Arm] send two goals: body_id=1 and body_id=0, index=%d, total_skills=%d", index, total_skills);
RCLCPP_INFO(this->get_logger(), "[Arm] instance='%s' timeout=%ld ms", instance_name.c_str(), static_cast<long>(timeout_ms_override));
std::atomic<bool> finished1{false}, finished2{false};
bool ok1 = false, ok2 = false;
std::string d1, d2;
std::atomic<bool> right_arm_finished{false}, left_arm_finished{false};
bool right_arm_ok = false, left_arm_ok = false;
std::string right_arm_d, left_arm_d;
auto left_arm_time_steady = std::chrono::steady_clock::now();
auto right_arm_time_steady = std::chrono::steady_clock::now();
auto worker = [this, &ok_out = ok1, &done_flag = finished1, &d_out = d1,
skill, topic, timeout, &goal_handle, &time_steady = left_arm_time_steady](int body_id) {
auto left_arm_worker = [this, &ok_out = left_arm_ok, &done_flag = left_arm_finished, &d_out = left_arm_d,
skill, topic, timeout, &goal_handle, &time_steady = left_arm_time_steady](int body_id) {
tls_arm_body_id = body_id;
const auto * prev_calls = CerebellumNode::tls_current_calls_;
CerebellumNode::tls_current_calls_ = &goal_handle->get_goal()->calls;
@@ -1294,10 +1342,10 @@ bool CerebellumNode::ExecuteBilateralArmAction(
time_steady = std::chrono::steady_clock::now();
RCLCPP_WARN(this->get_logger(), "left_arm_time_steady2");
};
std::thread left_arm_thread(left_arm_worker, LEFT_ARM); // LEFT_ARM=0
std::thread t1(worker, RIGHT_ARM); // RIGHT_ARM=1
auto worker2 = [this, &ok_out = ok2, &done_flag = finished2, &d_out = d2,
skill, topic, timeout, &goal_handle, &time_steady = right_arm_time_steady](int body_id) {
auto right_arm_worker = [this, &ok_out = right_arm_ok, &done_flag = right_arm_finished, &d_out = right_arm_d,
skill, topic, timeout, &goal_handle, &time_steady = right_arm_time_steady](int body_id) {
tls_arm_body_id = body_id;
const auto * prev_calls = CerebellumNode::tls_current_calls_;
CerebellumNode::tls_current_calls_ = &goal_handle->get_goal()->calls;
@@ -1308,11 +1356,11 @@ bool CerebellumNode::ExecuteBilateralArmAction(
time_steady = std::chrono::steady_clock::now();
RCLCPP_WARN(this->get_logger(), "right_arm_time_steady2");
};
std::thread t2(worker2, LEFT_ARM); // LEFT_ARM=0
std::thread right_arm_thread(right_arm_worker, RIGHT_ARM); // RIGHT_ARM=1
const auto start_steady = std::chrono::steady_clock::now();
while (!(finished1.load(std::memory_order_acquire) && finished2.load(std::memory_order_acquire))) {
while (!(right_arm_finished.load(std::memory_order_acquire) && left_arm_finished.load(std::memory_order_acquire))) {
double elapsed_ms = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::steady_clock::now() - start_steady).count();
PublishFeedbackStage(goal_handle, "RUN", skill, elapsed_ms, "");
std::this_thread::sleep_for(std::chrono::milliseconds(100));
@@ -1322,30 +1370,30 @@ bool CerebellumNode::ExecuteBilateralArmAction(
RCLCPP_WARN(this->get_logger(), "arm_exe_bias: %f", arm_exe_bias);
if (timeout_ms_override > 0 && std::abs(arm_exe_bias) > timeout_ms_override) {
ok1 = false;
ok2 = false;
right_arm_ok = false;
left_arm_ok = false;
RCLCPP_ERROR(this->get_logger(), "Fail in Arm arm_exe_bias: %f, timeout_ms_override=%ld", arm_exe_bias, static_cast<long>(timeout_ms_override));
}
if (t1.joinable()) {
if (right_arm_thread.joinable()) {
try {
t1.join();
right_arm_thread.join();
} catch (const std::exception& e) {
RCLCPP_ERROR(this->get_logger(), "Exception in Arm thread 1 join: %s", e.what());
}
}
if (t2.joinable()) {
if (left_arm_thread.joinable()) {
try {
t2.join();
left_arm_thread.join();
} catch (const std::exception& e) {
RCLCPP_ERROR(this->get_logger(), "Exception in Arm thread 2 join: %s", e.what());
}
}
if (!ok1 || !ok2) {
if (!right_arm_ok || !left_arm_ok) {
std::ostringstream oss;
if (!ok1) { oss << "Arm(body_id=1) failed"; if (!d1.empty()) oss << ": " << d1; }
if (!ok2) { if (!oss.str().empty()) oss << "; "; oss << "Arm(body_id=0) failed"; if (!d2.empty()) oss << ": " << d2; }
if (!right_arm_ok) { oss << "Arm(body_id=1) failed"; if (!right_arm_d.empty()) oss << ": " << right_arm_d; }
if (!left_arm_ok) { if (!oss.str().empty()) oss << "; "; oss << "Arm(body_id=0) failed"; if (!left_arm_d.empty()) oss << ": " << left_arm_d; }
detail = oss.str();
return false;
}
@@ -1383,7 +1431,7 @@ bool CerebellumNode::ExecuteServiceSkill(
for (const auto & iface : spec.interfaces) {
const auto pos = iface.rfind('.');
if (pos == std::string::npos || pos + 1 >= iface.size()) {continue;}
if (to_lower_str(iface.substr(pos + 1)) == "service") {
if (to_lower_str(iface.substr(pos + 1)) == "srv") {
service_base = iface.substr(0, pos);
break;
}

10
src/modules/vision/vision_object_recg/src/vision_object_recg_node.cpp
View File

@@ -78,12 +78,12 @@ private:
pose1.orientation.z = -0.19539139;
pose1.orientation.w = 0.73153153;
obj1.pose_list.push_back(pose1);
// obj1.pose_list.push_back(pose1);
// 第二个同类实例(示例)
geometry_msgs::msg::Pose pose1b = pose1;
pose1b.position.y += 0.05;
obj1.pose_list.push_back(pose1b);
// obj1.pose_list.push_back(pose1b);
interfaces::msg::PoseClassAndID obj2;
obj2.class_name = "cup";
@@ -93,15 +93,15 @@ private:
pose2.position.y = -0.2;
pose2.position.z = 0.78;
pose2.orientation.w = 1.0;
obj2.pose_list.push_back(pose2);
// obj2.pose_list.push_back(pose2);
res->objects.clear();
res->objects.emplace_back(obj1);
res->objects.emplace_back(obj2);
std::ostringstream oss;
oss << "Recognized " << res->objects.size() << " object classes (total instances: "
<< (obj1.pose_list.size() + obj2.pose_list.size()) << ")";
// oss << "Recognized " << res->objects.size() << " object classes (total instances: "
// << (obj1.pose_list.size() + obj2.pose_list.size()) << ")";
res->info = oss.str();
} else {
res->success = false;