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

update bt

Browse Source
This commit is contained in:
NuoDaJia02
2025-12-30 18:13:35 +08:00
parent 869e77d3c8
commit c9fc59119e
6 changed files with 111 additions and 32 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -1,17 +1,38 @@
<root BTCPP_format="4" main_tree_to_execute="MainTree">
<BehaviorTree ID="MainTree">
<Sequence name="root">
<Fallback name="fallback_main_action">
<SubTree ID="vision_grasp_subtree"/>
<SubTree ID="recovery_1_subtree"/>
</Fallback>
<SubTree ID="putdown_subtree"/>
<RetryUntilSuccessful name="retry_vision_grasp" num_attempts="3">
<Fallback name="fallback_main_action">
<Sequence name="complete_pick_and_place">
<!-- Vision with recovery -->
<Fallback name="vision_with_recovery">
<SubTree ID="vision_subtree"/>
<SubTree ID="vision_recovery_subtree"/>
</Fallback>
<!-- Grasp with recovery -->
<Fallback name="grasp_with_recovery">
<SubTree ID="grasp_subtree"/>
<SubTree ID="grasp_recovery_subtree"/>
</Fallback>
<!-- Putdown with recovery -->
<Fallback name="putdown_with_recovery">
<SubTree ID="putdown_subtree"/>
<SubTree ID="putdown_recovery_subtree"/>
</Fallback>
</Sequence>
<!-- Overall recovery if the entire sequence fails -->
<SubTree ID="recovery_subtree"/>
</Fallback>
</RetryUntilSuccessful>
</Sequence>
</BehaviorTree>
<BehaviorTree ID="vision_grasp_subtree">
<Sequence name="vision_grasp_root">
<RetryUntilSuccessful name="retry_vision_grasp" num_attempts="3">
<BehaviorTree ID="vision_subtree">
<Sequence name="vision_root">
<RetryUntilSuccessful name="retry_vision" num_attempts="1">
<Sequence>
<Arm_H name="s1_right_arm_pre_cam_take_photo" />
<Parallel name="parallel_action_1">
@@ -27,7 +48,15 @@
<RetryUntilSuccessful name="retry_camera_vision_recg_2" num_attempts="10">
<VisionObjectRecognition_H name="s3_right_camera_vision_recg" />
</RetryUntilSuccessful>
</Sequence>
</RetryUntilSuccessful>
</Sequence>
</BehaviorTree>
<BehaviorTree ID="grasp_subtree">
<Sequence name="grasp_root">
<RetryUntilSuccessful name="retry_grasp" num_attempts="1">
<Sequence>
<Arm_H name="s3_right_arm_vision_pre_grasp" />
<Arm_H name="s4_right_arm_vision_grasp" />
<GripperCmd_H name="s4_gripper_close" />
@@ -40,7 +69,7 @@
<BehaviorTree ID="putdown_subtree">
<Sequence name="putdown_root">
<RetryUntilSuccessful name="retry_putdown" num_attempts="3">
<RetryUntilSuccessful name="retry_putdown" num_attempts="1">
<Sequence>
<!-- <Parallel name="parallel_action_2">
<MoveWheel_H name="s6_wheel_move_to_putdown_position" />
@@ -60,9 +89,42 @@
</Sequence>
</BehaviorTree>
<BehaviorTree ID="recovery_1_subtree">
<Sequence name="recovery_1_root">
<RetryUntilSuccessful name="retry_putdown" num_attempts="3">
<BehaviorTree ID="recovery_subtree">
<Sequence name="recovery_root">
<RetryUntilSuccessful name="retry_putdown" num_attempts="1">
<Sequence>
<GripperCmd_H name="s12_gripper_open" />
<Arm_H name="s13_right_arm_pre_cam_take_photo" />
</Sequence>
</RetryUntilSuccessful>
</Sequence>
</BehaviorTree>
<BehaviorTree ID="vision_recovery_subtree">
<Sequence name="vision_recovery_root">
<RetryUntilSuccessful name="retry_putdown" num_attempts="1">
<Sequence>
<!-- <GripperCmd_H name="s12_gripper_open" /> -->
<Arm_H name="s13_right_arm_pre_cam_take_photo" />
</Sequence>
</RetryUntilSuccessful>
</Sequence>
</BehaviorTree>
<BehaviorTree ID="grasp_recovery_subtree">
<Sequence name="grasp_recovery_root">
<RetryUntilSuccessful name="retry_putdown" num_attempts="1">
<Sequence>
<GripperCmd_H name="s12_gripper_open" />
<Arm_H name="s13_right_arm_pre_cam_take_photo" />
</Sequence>
</RetryUntilSuccessful>
</Sequence>
</BehaviorTree>
<BehaviorTree ID="putdown_recovery_subtree">
<Sequence name="putdown_recovery_root">
<RetryUntilSuccessful name="retry_putdown" num_attempts="1">
<Sequence>
<GripperCmd_H name="s12_gripper_open" />
<Arm_H name="s13_right_arm_pre_cam_take_photo" />

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

@@ -60,6 +60,8 @@ public:
std::string camera_position_;
std::unordered_map<std::string, std::string> gripper_info_;
private:
struct GoalExecutionContext { std::atomic<bool> cancel{false}; };
std::unordered_map<std::uintptr_t, std::shared_ptr<GoalExecutionContext>> goal_ctxs_;

31
src/brain/include/brain/skill_config.hpp
View File

@@ -46,7 +46,7 @@ struct SkillActionTrait<interfaces::action::Arm>
{
static constexpr const char * skill_name = "Arm";
static constexpr const char * default_topic = "ArmAction";
static bool success(const interfaces::action::Arm::Result & r) {return (r.result == 0);}
static bool success(const interfaces::action::Arm::Result & r, const std::string & instance_name) {(void)instance_name; return (r.result == 0);}
static std::string message(const interfaces::action::Arm::Result & r) {(void)r; return "completed";}
};
@@ -55,7 +55,7 @@ struct SkillActionTrait<interfaces::action::CameraTakePhoto>
{
static constexpr const char * skill_name = "CameraTakePhoto";
static constexpr const char * default_topic = "";
static bool success(const interfaces::action::CameraTakePhoto::Result & r) {return r.success;}
static bool success(const interfaces::action::CameraTakePhoto::Result & r, const std::string & instance_name) {(void)instance_name; return r.success;}
static std::string message(const interfaces::action::CameraTakePhoto::Result & r) {return r.message;}
};
@@ -64,7 +64,7 @@ struct SkillActionTrait<interfaces::action::HandControl>
{
static constexpr const char * skill_name = "HandControl";
static constexpr const char * default_topic = "";
static bool success(const interfaces::action::HandControl::Result & r) {return r.success;}
static bool success(const interfaces::action::HandControl::Result & r, const std::string & instance_name) {(void)instance_name; return r.success;}
static std::string message(const interfaces::action::HandControl::Result & r) {return r.message;}
};
@@ -73,7 +73,7 @@ struct SkillActionTrait<interfaces::action::MoveWaist>
{
static constexpr const char * skill_name = "MoveWaist";
static constexpr const char * default_topic = "MoveWaist";
static bool success(const interfaces::action::MoveWaist::Result & r) {return r.success;}
static bool success(const interfaces::action::MoveWaist::Result & r, const std::string & instance_name) {(void)instance_name; return r.success;}
static std::string message(const interfaces::action::MoveWaist::Result & r) {return r.message;}
};
@@ -82,7 +82,7 @@ struct SkillActionTrait<interfaces::action::MoveLeg>
{
static constexpr const char * skill_name = "MoveLeg";
static constexpr const char * default_topic = "MoveLeg";
static bool success(const interfaces::action::MoveLeg::Result & r) {return r.success;}
static bool success(const interfaces::action::MoveLeg::Result & r, const std::string & instance_name) {(void)instance_name; return r.success;}
static std::string message(const interfaces::action::MoveLeg::Result & r) {return r.message;}
};
@@ -91,7 +91,7 @@ struct SkillActionTrait<interfaces::action::MoveWheel>
{
static constexpr const char * skill_name = "MoveWheel";
static constexpr const char * default_topic = "MoveWheel";
static bool success(const interfaces::action::MoveWheel::Result & r) {return r.success;}
static bool success(const interfaces::action::MoveWheel::Result & r, const std::string & instance_name) {(void)instance_name; return r.success;}
static std::string message(const interfaces::action::MoveWheel::Result & r) {return r.message;}
};
@@ -100,7 +100,7 @@ struct SkillActionTrait<interfaces::action::MoveHome>
{
static constexpr const char * skill_name = "MoveHome";
static constexpr const char * default_topic = "MoveHome";
static bool success(const interfaces::action::MoveHome::Result & r) {return r.success;}
static bool success(const interfaces::action::MoveHome::Result & r, const std::string & instance_name) {(void)instance_name; return r.success;}
static std::string message(const interfaces::action::MoveHome::Result & r) {return r.message;}
};
@@ -109,7 +109,22 @@ struct SkillActionTrait<interfaces::action::GripperCmd>
{
static constexpr const char * skill_name = "GripperCmd";
static constexpr const char * default_topic = "gripper_cmd0";
static bool success(const interfaces::action::GripperCmd::Result & r) {return (r.result == 1);}
static bool success(const interfaces::action::GripperCmd::Result & r, const std::string & instance_name) {
(void)instance_name;
if (r.result == 0) return false;
//如果instance_name中带有"open"，则表示是打开手指的命令
if (instance_name.find("open") != std::string::npos) {
//如果r.state中包含"手指已到达指定的位置"，则认为成功
return r.state.find("手指已到达指定的位置") != std::string::npos;
} else if (instance_name.find("close") != std::string::npos) {
//如果r.state中包含"手指在闭合检测到物体"，则认为成功
return (r.state.find("手指在闭合检测到物体") != std::string::npos);
} else {
return false;
}
}
static std::string message(const interfaces::action::GripperCmd::Result & r) {return r.state;}
};

8
src/brain/include/brain/skill_manager.hpp
View File

@@ -324,7 +324,7 @@ private:
if (!result_cb) {
result_cb = [this, internal_skill](const typename GoalHandle::WrappedResult & res) {
if (res.code == rclcpp_action::ResultCode::SUCCEEDED && res.result &&
SkillActionTrait<ActionT>::success(*res.result)) {
SkillActionTrait<ActionT>::success(*res.result, "")) {
RCLCPP_INFO(node_->get_logger(), "action succeeded: %s", internal_skill.c_str());
} else if (res.code == rclcpp_action::ResultCode::CANCELED) {
RCLCPP_WARN(node_->get_logger(), "action cancelled: %s", internal_skill.c_str());
@@ -365,7 +365,7 @@ private:
template<size_t I = 0, typename TupleT, typename RegistryT>
bool dispatch_skill_action(
const std::string & skill, const std::string & topic, RegistryT * reg,
rclcpp::Logger logger, std::chrono::nanoseconds timeout, std::string & detail)
rclcpp::Logger logger, std::chrono::nanoseconds timeout, const std::string & instance_name, std::string & detail)
{
if constexpr (I == std::tuple_size<TupleT>::value) {
return false;
@@ -374,7 +374,7 @@ bool dispatch_skill_action(
if (skill == SkillActionTrait<ActionT>::skill_name) {
auto opt = reg->template send_and_wait<ActionT>(topic, logger, timeout);
if (opt && opt->code == rclcpp_action::ResultCode::SUCCEEDED && opt->result) {
bool ok = SkillActionTrait<ActionT>::success(*opt->result);
bool ok = SkillActionTrait<ActionT>::success(*opt->result, instance_name);
detail = ok ? SkillActionTrait<ActionT>::message(*opt->result) : std::string(
"result flag false");
return ok;
@@ -382,7 +382,7 @@ bool dispatch_skill_action(
detail = "action failed";
return false;
}
return dispatch_skill_action<I + 1, TupleT>(skill, topic, reg, logger, timeout, detail);
return dispatch_skill_action<I + 1, TupleT>(skill, topic, reg, logger, timeout, instance_name, detail);
}
}

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

@@ -677,7 +677,7 @@ bool CerebellumNode::RunActionSkillWithProgress(
CerebellumNode::tls_current_calls_ = &goal_handle->get_goal()->calls;
RCLCPP_INFO(this->get_logger(), "[%s] instance='%s' timeout=%.3fs", skill.c_str(), instance_name.c_str(), timeout_ns.count()/1e9);
ok = brain::dispatch_skill_action<0, brain::SkillActionTypes>(
skill, topic, action_clients_.get(), logger, timeout_ns, local_detail);
skill, topic, action_clients_.get(), logger, timeout_ns, instance_name, local_detail);
CerebellumNode::tls_current_calls_ = prev_calls;
finished.store(true, std::memory_order_release);
});
@@ -739,12 +739,12 @@ bool CerebellumNode::ExecuteBilateralArmAction(
auto right_arm_time_steady = std::chrono::steady_clock::now();
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) {
skill, topic, timeout, &goal_handle, &time_steady = left_arm_time_steady, instance_name](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;
ok_out = brain::dispatch_skill_action<0, brain::SkillActionTypes>(
skill, topic, action_clients_.get(), this->get_logger(), timeout, d_out);
skill, topic, action_clients_.get(), this->get_logger(), timeout, instance_name, d_out);
CerebellumNode::tls_current_calls_ = prev_calls;
done_flag.store(true, std::memory_order_release);
time_steady = std::chrono::steady_clock::now();
@@ -753,12 +753,12 @@ bool CerebellumNode::ExecuteBilateralArmAction(
std::thread left_arm_thread(left_arm_worker, LEFT_ARM); // LEFT_ARM=0
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) {
skill, topic, timeout, &goal_handle, &time_steady = right_arm_time_steady, instance_name](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;
ok_out = brain::dispatch_skill_action<0, brain::SkillActionTypes>(
skill, topic, action_clients_.get(), this->get_logger(), timeout, d_out);
skill, topic, action_clients_.get(), this->get_logger(), timeout, instance_name, d_out);
CerebellumNode::tls_current_calls_ = prev_calls;
done_flag.store(true, std::memory_order_release);
time_steady = std::chrono::steady_clock::now();

6
src/scripts/gen_skill_config.py
View File

@@ -251,11 +251,11 @@ with OUT_HEADER.open('w') as out:
result_fields = parse_action_result_fields(action_file_found) if action_file_found is not None else {}
success_field, message_field, success_kind = pick_success_and_message_fields(result_fields)
if success_kind == 'bool' and success_field is not None:
out.write(f' static bool success(const {action_cpp_type(a)}::Result & r) {{return r.{success_field};}}\n')
out.write(f' static bool success(const {action_cpp_type(a)}::Result & r, const std::string & instance_name) {{(void)instance_name; return r.{success_field};}}\n')
elif success_kind == 'int_zero' and success_field is not None:
out.write(f' static bool success(const {action_cpp_type(a)}::Result & r) {{return (r.{success_field} == 0);}}\n')
out.write(f' static bool success(const {action_cpp_type(a)}::Result & r, const std::string & instance_name) {{(void)instance_name; return (r.{success_field} == 0);}}\n')
else:
out.write(f' static bool success(const {action_cpp_type(a)}::Result & r) {{(void)r; return true;}}\n')
out.write(f' static bool success(const {action_cpp_type(a)}::Result & r, const std::string & instance_name) {{(void)instance_name; (void)r; return true;}}\n')
if message_field is not None:
out.write(f' static std::string message(const {action_cpp_type(a)}::Result & r) {{return r.{message_field};}}\n')
else: