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

support two gripper

Browse Source
This commit is contained in:
NuoDaJia02
2026-01-07 13:46:55 +08:00
parent 4e0d53d41e
commit ac6bf49e75
12 changed files with 636 additions and 66 deletions
Download Patch File Download Diff File Expand all files Collapse all files

302
src/brain/config/bt_vision_grasp_dual_arm.params.yaml Normal file
View File

@@ -0,0 +1,302 @@
- name: root
params: '{}
'
- name: retry_vision_hand
params: '{}
'
- name: s1_move_waist_turn_right_90
params: &waist_turn_right_90 'move_pitch_degree: 0.0
move_yaw_degree: -90.0
'
- name: s1_gripper_open
params: &gripper_open 'loc: 0
speed: 150
torque: 20
mode: 2
'
- name: s1_right_hand_gripper_open
params: *gripper_open
- name: s1_left_hand_gripper_open
params: *gripper_open
- name: s1_dual_arm_initial
params: &pre_cam_take_photo 'body_id: 2
data_type: 2
data_length: 12
command_id: 0
frame_time_stamp: 0
data_array: [85, 24, 88, -175, -68, -90, 95, -24, -88, -5, -68, 90]
'
- name: s1_wheel_move_to_origin_pickup_position
params: 'move_distance: 0.5
move_angle: 0.0
'
- name: s2_dual_arm_pre_cam_take_photo
params: 'body_id: 2
data_type: 2
data_length: 12
command_id: 0
frame_time_stamp: 0
data_array: [120, 20, 80, 0, 80, 120, 60, -20, -80, 0, -80, 60]
'
- name: s2_top_camera_vision_recg
params: 'camera_position: top
'
- name: s2_dual_arm_cam_take_photo
params: 'body_id: 2
data_type: 2
data_length: 12
command_id: 0
frame_time_stamp: 0
data_array: [120.0, 20.0, 80.0, 0.0, 80.0, 120.0, 60.0, -20.0, -80.0, 0.0, -80.0, 60.0]
'
- name: s3_right_camera_vision_recg
params: 'camera_position: right
'
- name: s3_left_camera_vision_recg
params: 'camera_position: left
'
- name: s3_right_arm_vision_pre_grasp
params: &pre_grasp_right 'body_id: 1
data_type: 101
data_length: 0
command_id: 0
frame_time_stamp: 0
data_array: []
'
- name: s3_left_arm_vision_pre_grasp
params: &pre_grasp_left 'body_id: 0
data_type: 101
data_length: 0
command_id: 0
frame_time_stamp: 0
data_array: []
'
- name: s4_right_arm_vision_grasp
params: 'body_id: 1
data_type: 102
data_length: 0
command_id: 0
frame_time_stamp: 0
data_array: []
'
- name: s4_left_arm_vision_grasp
params: 'body_id: 0
data_type: 102
data_length: 0
command_id: 0
frame_time_stamp: 0
data_array: []
'
- name: s4_right_hand_gripper_close
params: &gripper_close 'loc: 200
speed: 150
torque: 100
mode: 1
'
- name: s4_left_hand_gripper_close
params: *gripper_close
- name: s4_right_arm_take_box
params: 'body_id: 1
data_type: 110
data_length: 0
command_id: 0
frame_time_stamp: 0
data_array: []
'
- name: s4_left_arm_take_box
params: 'body_id: 0
data_type: 110
data_length: 0
command_id: 0
frame_time_stamp: 0
data_array: []
'
- name: s5_right_arm_grasp_box
params: 'body_id: 2
data_type: 2
data_length: 12
command_id: 0
frame_time_stamp: 0
data_array: [85, 24, 88, -175, -68, -90, 96.30, -88.95, -45, -22.88, -16.30, 112.10]
'
- name: s5_left_arm_grasp_box
params: 'body_id: 2
data_type: 2
data_length: 12
command_id: 0
frame_time_stamp: 0
data_array: [96.30, -88.95, -45, -22.88, -16.30, 112.10, 85, 24, 88, -175, -68, -90]
'
- name: s7_right_arm_grasp_box
params: &grasp_box_s7_s9_right 'body_id: 2
data_type: 2
data_length: 12
command_id: 0
frame_time_stamp: 0
data_array: [85, 24, 88, -175, -68, -90, 96.30, -88.95, 13.98, -22.88, -16.30, 112.10]
'
- name: s7_left_arm_grasp_box
params: &grasp_box_s7_s9_left 'body_id: 2
data_type: 2
data_length: 12
command_id: 0
frame_time_stamp: 0
data_array: [96.30, -88.95, 13.98, -22.88, -16.30, 112.10, 85, 24, 88, -175, -68, -90]
'
- name: s7_right_arm_put_down_box
params: 'body_id: 2
data_type: 2
data_length: 12
command_id: 0
frame_time_stamp: 0
data_array: [85, 24, 88, -175, -68, -90, 85.78, -88.95, 13.97, 22.88, -16.29, 67.99]
'
- name: s7_left_arm_put_down_box
params: 'body_id: 2
data_type: 2
data_length: 12
command_id: 0
frame_time_stamp: 0
data_array: [85.78, -88.95, 13.97, 22.88, -16.29, 67.99, 85, 24, 88, -175, -68, -90]
'
- name: s8_right_hand_gripper_open
params: *gripper_open
- name: s8_left_hand_gripper_open
params: *gripper_open
- name: s9_right_arm_grasp_box
params: *grasp_box_s7_s9_right
- name: s9_left_arm_grasp_box
params: *grasp_box_s7_s9_left
- name: s11_right_arm_pre_cam_take_photo
params: *pre_cam_take_photo
- name: s11_left_arm_pre_cam_take_photo
params: *pre_cam_take_photo
- name: s12_right_hand_gripper_open
params: *gripper_open
- name: s12_left_hand_gripper_open
params: *gripper_open
- name: s13_dual_arm_pre_cam_take_photo
params: *pre_cam_take_photo
- name: s14_dual_arm_pre_cam_take_photo
params: *pre_cam_take_photo
- name: s15_right_hand_gripper_open
params: *gripper_open
- name: s15_left_hand_gripper_open
params: *gripper_open
- name: s15_right_arm_pre_grasp
params: *pre_grasp_right
- name: s15_left_arm_pre_grasp
params: *pre_grasp_left
- name: s15_right_arm_pre_cam_take_photo
params: *pre_cam_take_photo
- name: s15_left_arm_pre_cam_take_photo
params: *pre_cam_take_photo
- name: s16_right_arm_pre_cam_take_photo
params: *pre_cam_take_photo

185
src/brain/config/bt_vision_grasp_dual_arm.xml Normal file
View File

@@ -0,0 +1,185 @@
<root BTCPP_format="4" main_tree_to_execute="MainTree">
<BehaviorTree ID="MainTree">
<Sequence name="root">
<RetryUntilSuccessful name="retry_vision_grasp" num_attempts="3">
<Fallback name="fallback_main_action">
<Sequence name="complete_pick_and_place">
<!-- Phase 1: Shared Vision Setup (Top Camera) -->
<Fallback name="shared_vision_with_recovery">
<SubTree ID="vision_setup_subtree"/>
<Sequence>
<SubTree ID="vision_recovery_subtree"/>
<AlwaysFailure/>
</Sequence>
</Fallback>
<!-- Phase 2: Independent Dual Arm Operation -->
<!-- Asynchronous execution of both arms -->
<Parallel name="dual_arm_asynchronous_grasp" success_count="2" failure_count="1">
<!-- Right Arm Branch -->
<Sequence name="right_arm_branch">
<Fallback name="right_arm_process_with_recovery">
<Sequence name="right_arm_process">
<SubTree ID="right_arm_hand_vision_subtree"/>
<SubTree ID="right_arm_grasp_subtree"/>
<SubTree ID="right_arm_putdown_subtree"/>
</Sequence>
<Sequence>
<SubTree ID="right_arm_grasp_recovery_subtree"/>
<AlwaysFailure/>
</Sequence>
</Fallback>
</Sequence>
<!-- Left Arm Branch -->
<Sequence name="left_arm_branch">
<Fallback name="left_arm_process_with_recovery">
<Sequence name="left_arm_process">
<SubTree ID="left_arm_hand_vision_subtree"/>
<SubTree ID="left_arm_grasp_subtree"/>
<SubTree ID="left_arm_putdown_subtree"/>
</Sequence>
<Sequence>
<SubTree ID="left_arm_grasp_recovery_subtree"/>
<AlwaysFailure/>
</Sequence>
</Fallback>
</Sequence>
</Parallel>
</Sequence>
<!-- Overall fallback recovery -->
<SubTree ID="recovery_subtree"/>
</Fallback>
</RetryUntilSuccessful>
</Sequence>
</BehaviorTree>
<!-- Shared setup and head-camera vision -->
<BehaviorTree ID="vision_setup_subtree">
<Sequence name="vision_setup_root">
<Arm_H name="s1_dual_arm_initial" />
<Parallel name="parallel_init_action">
<GripperCmd0_H name="s1_right_hand_gripper_open" />
<GripperCmd1_H name="s1_left_hand_gripper_open" />
<Arm_H name="s2_dual_arm_pre_cam_take_photo" />
</Parallel>
<RetryUntilSuccessful name="retry_top_cam_vision" num_attempts="10">
<VisionObjectRecognition_H name="s2_top_camera_vision_recg" />
</RetryUntilSuccessful>
<!-- Move both arms to positions where their hand cameras can see potential objects -->
<Arm_H name="s2_dual_arm_cam_take_photo" />
</Sequence>
</BehaviorTree>
<!-- Right Arm Specific Subtrees -->
<BehaviorTree ID="right_arm_hand_vision_subtree">
<Sequence name="right_hand_vision_root">
<RetryUntilSuccessful name="retry_right_hand_cam_vision" num_attempts="10">
<VisionObjectRecognition_H name="s3_right_camera_vision_recg" />
</RetryUntilSuccessful>
</Sequence>
</BehaviorTree>
<BehaviorTree ID="right_arm_grasp_subtree">
<Sequence name="right_arm_grasp_root">
<RetryUntilSuccessful name="retry_right_arm_grasp_action" num_attempts="1">
<Sequence>
<Arm_H name="s3_right_arm_vision_pre_grasp" />
<Arm_H name="s4_right_arm_vision_grasp" />
<GripperCmd0_H name="s4_right_hand_gripper_close" />
<Arm_H name="s4_right_arm_take_box" />
<Arm_H name="s5_right_arm_grasp_box" />
</Sequence>
</RetryUntilSuccessful>
</Sequence>
</BehaviorTree>
<BehaviorTree ID="right_arm_putdown_subtree">
<Sequence name="right_arm_putdown_root">
<RetryUntilSuccessful name="retry_right_arm_putdown" num_attempts="1">
<Sequence>
<Arm_H name="s7_right_arm_grasp_box" />
<Arm_H name="s7_right_arm_put_down_box" />
<GripperCmd0_H name="s8_right_hand_gripper_open" />
<Arm_H name="s9_right_arm_grasp_box" />
<Arm_H name="s11_right_arm_pre_cam_take_photo" />
</Sequence>
</RetryUntilSuccessful>
</Sequence>
</BehaviorTree>
<!-- Left Arm Specific Subtrees -->
<BehaviorTree ID="left_arm_hand_vision_subtree">
<Sequence name="left_hand_vision_root">
<RetryUntilSuccessful name="retry_left_hand_cam_vision" num_attempts="10">
<VisionObjectRecognition_H name="s3_left_camera_vision_recg" />
</RetryUntilSuccessful>
</Sequence>
</BehaviorTree>
<BehaviorTree ID="left_arm_grasp_subtree">
<Sequence name="left_arm_grasp_root">
<RetryUntilSuccessful name="retry_left_arm_grasp_action" num_attempts="1">
<Sequence>
<Arm_H name="s3_left_arm_vision_pre_grasp" />
<Arm_H name="s4_left_arm_vision_grasp" />
<GripperCmd1_H name="s4_left_hand_gripper_close" />
<Arm_H name="s4_left_arm_take_box" />
<Arm_H name="s5_left_arm_grasp_box" />
</Sequence>
</RetryUntilSuccessful>
</Sequence>
</BehaviorTree>
<BehaviorTree ID="left_arm_putdown_subtree">
<Sequence name="left_arm_putdown_root">
<RetryUntilSuccessful name="retry_left_arm_putdown" num_attempts="1">
<Sequence>
<Arm_H name="s7_left_arm_grasp_box" />
<Arm_H name="s7_left_arm_put_down_box" />
<GripperCmd1_H name="s8_left_hand_gripper_open" />
<Arm_H name="s9_left_arm_grasp_box" />
<Arm_H name="s11_left_arm_pre_cam_take_photo" />
</Sequence>
</RetryUntilSuccessful>
</Sequence>
</BehaviorTree>
<!-- Recovery Subtrees -->
<BehaviorTree ID="recovery_subtree">
<Sequence name="recovery_root">
<Parallel name="overall_recovery_parallel">
<GripperCmd0_H name="s12_right_hand_gripper_open" />
<GripperCmd1_H name="s12_left_hand_gripper_open" />
</Parallel>
<Arm_H name="s13_dual_arm_pre_cam_take_photo" />
<ClearDoneInstances_H name="clear_done_instances_overall" />
</Sequence>
</BehaviorTree>
<BehaviorTree ID="vision_recovery_subtree">
<Sequence name="vision_recovery_root">
<Arm_H name="s14_dual_arm_pre_cam_take_photo" />
<ClearDoneInstances_H name="clear_done_instances_vision" />
</Sequence>
</BehaviorTree>
<BehaviorTree ID="right_arm_grasp_recovery_subtree">
<Sequence name="right_grasp_recovery_root">
<GripperCmd0_H name="s15_right_hand_gripper_open" />
<Arm_H name="s15_right_arm_pre_grasp" />
<Arm_H name="s15_right_arm_pre_cam_take_photo" />
<ClearDoneInstances_H name="clear_done_instances_right_grasp" />
</Sequence>
</BehaviorTree>
<BehaviorTree ID="left_arm_grasp_recovery_subtree">
<Sequence name="left_grasp_recovery_root">
<GripperCmd1_H name="s15_left_hand_gripper_open" />
<Arm_H name="s15_left_arm_pre_grasp" />
<Arm_H name="s15_left_arm_pre_cam_take_photo" />
<ClearDoneInstances_H name="clear_done_instances_left_grasp" />
</Sequence>
</BehaviorTree>
</root>

13
src/brain/config/robot_skills.yaml
View File

@@ -63,9 +63,16 @@
- name: MoveHome.action
default_topic: "MoveHome"
- name: GripperCmd
- name: GripperCmd0
version: 1.0.0
description: "夹爪控制"
description: "夹爪0控制"
interfaces:
- name: GripperCmd.action
default_topic: "gripper_cmd0"
default_topic: "gripper_cmd0"
- name: GripperCmd1
version: 1.0.0
description: "夹爪1控制"
interfaces:
- name: GripperCmd.action
default_topic: "gripper_cmd1"

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

@@ -249,6 +249,9 @@ public:
std::function<void(const typename GoalHandle<ActionT>::WrappedResult &)> on_result,
std::function<void(const std::shared_ptr<GoalHandle<ActionT>> &)> on_goal_response = nullptr)
{
if (entries_.find(name) != entries_.end()) {
RCLCPP_WARN(node_->get_logger(), "[ActionClientRegistry] Overwriting existing client for '%s'", name.c_str());
}
entries_[name] =
std::make_unique<Entry<ActionT>>(
node_, name, std::move(make_goal), std::move(

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

@@ -281,6 +281,7 @@ private:
* @brief Internal helper: run the action worker thread and emit RUN feedback until completion.
* Extracted to reduce cyclomatic complexity of ExecuteActionSkill.
* @param skill Skill name.
* @param interface_name Interface base name for dispatching.
* @param topic Action topic.
* @param timeout_ns Timeout duration (nanoseconds).
* @param index Skill index.
@@ -291,6 +292,7 @@ private:
*/
bool RunActionSkillWithProgress(
const std::string & skill,
const std::string & interface_name,
const std::string & topic,
std::chrono::nanoseconds timeout_ns,
const std::string & instance_name,
@@ -304,6 +306,7 @@ private:
* @brief Internal helper: run bilateral arm actions concurrently.
* Extracted to reduce cyclomatic complexity of ExecuteActionSkill.
* @param skill Skill name.
* @param interface_name Interface base name for dispatching.
* @param topic Action topic.
* @param timeout Action timeout.
* @param index Skill index.
@@ -314,6 +317,7 @@ private:
*/
bool ExecuteBilateralArmAction(
const std::string & skill,
const std::string & interface_name,
const std::string & topic,
std::chrono::nanoseconds timeout,
const std::string & instance_name,

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

@@ -287,10 +287,17 @@ private:
const std::string & topic,
const std::string & internal_skill)
{
auto it = action_override_registrars_.find(base);
// PRIORITY 1: Match exact skill name (e.g. "GripperCmd0") for highly specific overrides
auto it = action_override_registrars_.find(internal_skill);
// PRIORITY 2: Match interface base name (e.g. "GripperCmd") for generic overrides
if (it == action_override_registrars_.end()) {
it = action_override_registrars_.find(base);
}
if (it != action_override_registrars_.end()) {
it->second(topic, internal_skill);
RCLCPP_INFO(node_->get_logger(), "Overridden action client registration: %s", base.c_str());
RCLCPP_INFO(node_->get_logger(), "Registered action client '%s' via Hook on '%s'",
internal_skill.c_str(), it->first.c_str());
return;
}
using GoalHandle = typename ActionClientRegistry::GoalHandle<ActionT>;

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

@@ -503,57 +503,63 @@ void CerebellumHooks::ConfigureMoveHomeHooks(CerebellumNode * node)
void CerebellumHooks::ConfigureGripperCmdControlHooks(CerebellumNode * node)
{
// GripperCmd
SkillManager::ActionHookOptions<interfaces::action::GripperCmd> hooks;
hooks.make_goal = [node](const std::string & skill_name) {
interfaces::action::GripperCmd::Goal goal{};
// Plan A: per-call YAML overrides
if (!node->TryParseCallPayload<interfaces::action::GripperCmd::Goal>(skill_name, goal)) {
RCLCPP_ERROR(node->get_logger(), "Failed to parse call payload for skill %s", skill_name.c_str());
auto setup_gripper_hook = [node](const std::string & target_skill) {
// GripperCmd
SkillManager::ActionHookOptions<interfaces::action::GripperCmd> hooks;
hooks.make_goal = [node](const std::string & skill_name) {
interfaces::action::GripperCmd::Goal goal{};
// Plan A: per-call YAML overrides
if (!node->TryParseCallPayload<interfaces::action::GripperCmd::Goal>(skill_name, goal)) {
RCLCPP_ERROR(node->get_logger(), "Failed to parse call payload for skill %s", skill_name.c_str());
return goal;
}
// goal.loc = 30; //grab_width_; //TODO transfer
RCLCPP_INFO(node->get_logger(), "[%s] action make_goal params, loc: %d, speed: %d, torque: %d, mode: %d",
skill_name.c_str(), goal.loc, goal.speed, goal.torque, goal.mode);
return goal;
}
};
hooks.on_feedback = [node](
const std::string & skill_name,
ActionClientRegistry::GoalHandle<interfaces::action::GripperCmd>::SharedPtr,
const std::shared_ptr<const interfaces::action::GripperCmd::Feedback> & feedback) {
if (!feedback) {return;}
// goal.loc = 30; //grab_width_; //TODO transfer
RCLCPP_INFO(node->get_logger(), "[%s] action feedback, loc: %d, speed: %d, torque: %d",
skill_name.c_str(), feedback->loc, feedback->speed, feedback->torque);
RCLCPP_INFO(node->get_logger(), "[%s] action make_goal params, loc: %d, speed: %d, torque: %d, mode: %d",
skill_name.c_str(), goal.loc, goal.speed, goal.torque, goal.mode);
};
hooks.on_result = [node](
const std::string & skill_name,
const ActionClientRegistry::GoalHandle<interfaces::action::GripperCmd>::WrappedResult & res) {
return goal;
};
hooks.on_feedback = [node](
const std::string & skill_name,
ActionClientRegistry::GoalHandle<interfaces::action::GripperCmd>::SharedPtr,
const std::shared_ptr<const interfaces::action::GripperCmd::Feedback> & feedback) {
if (!feedback) {return;}
if (res.code == rclcpp_action::ResultCode::CANCELED) {
RCLCPP_WARN(node->get_logger(), "[%s] was cancelled", skill_name.c_str());
} else if (res.code == rclcpp_action::ResultCode::SUCCEEDED) {
RCLCPP_INFO(node->get_logger(), "[%s] action on_result, state: %s",
skill_name.c_str(), res.result->state.c_str());
} else {
RCLCPP_ERROR(node->get_logger(), "[%s] was failed(code=%d)", skill_name.c_str(), static_cast<int>(res.code));
}
RCLCPP_INFO(node->get_logger(), "[%s] action feedback, loc: %d, speed: %d, torque: %d",
skill_name.c_str(), feedback->loc, feedback->speed, feedback->torque);
};
hooks.on_goal_response = [node](
const std::string & skill_name,
const std::shared_ptr<ActionClientRegistry::GoalHandle<interfaces::action::GripperCmd>> & handle) {
if (static_cast<bool>(handle)) {
RCLCPP_INFO(node->get_logger(), "[%s] goal has been accepted by server", skill_name.c_str());
} else {
RCLCPP_WARN(node->get_logger(), "[%s] goal was rejected by server", skill_name.c_str());
}
};
node->skill_manager_->configure_action_hooks<interfaces::action::GripperCmd>(target_skill, std::move(hooks));
};
};
hooks.on_result = [node](
const std::string & skill_name,
const ActionClientRegistry::GoalHandle<interfaces::action::GripperCmd>::WrappedResult & res) {
if (res.code == rclcpp_action::ResultCode::CANCELED) {
RCLCPP_WARN(node->get_logger(), "[%s] was cancelled", skill_name.c_str());
} else if (res.code == rclcpp_action::ResultCode::SUCCEEDED) {
RCLCPP_INFO(node->get_logger(), "[%s] action on_result, state: %s",
skill_name.c_str(), res.result->state.c_str());
} else {
RCLCPP_ERROR(node->get_logger(), "[%s] was failed(code=%d)", skill_name.c_str(), static_cast<int>(res.code));
}
};
hooks.on_goal_response = [node](
const std::string & skill_name,
const std::shared_ptr<ActionClientRegistry::GoalHandle<interfaces::action::GripperCmd>> & handle) {
if (static_cast<bool>(handle)) {
RCLCPP_INFO(node->get_logger(), "[%s] goal has been accepted by server", skill_name.c_str());
} else {
RCLCPP_WARN(node->get_logger(), "[%s] goal was rejected by server", skill_name.c_str());
}
};
node->skill_manager_->configure_action_hooks<interfaces::action::GripperCmd>("GripperCmd", std::move(hooks));
setup_gripper_hook("GripperCmd0");
setup_gripper_hook("GripperCmd1");
setup_gripper_hook("GripperCmd");
}
#define DEBUG_GRASP_POSE_CALCULATION

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

@@ -611,8 +611,19 @@ bool CerebellumNode::ExecuteActionSkill(
RCLCPP_ERROR(this->get_logger(), "[%s] %s, TOPIC: %s", skill.c_str(), detail.c_str(), topic.c_str());
return false;
}
// Resolve interface base name (e.g., "GripperCmd" from "GripperCmd0")
std::string interface_base;
if (!spec.interfaces.empty()) {
std::string first = spec.interfaces.front();
auto pos = first.rfind('.');
interface_base = (pos == std::string::npos) ? first : first.substr(0, pos);
} else {
interface_base = skill;
}
RCLCPP_INFO(this->get_logger(), "SKILL: [%s], DETAIL:[%s], TOPIC:[%s]", skill.c_str(), detail.c_str(), topic.c_str());
RCLCPP_INFO(this->get_logger(), "SKILL: [%s] (Base: %s), TOPIC:[%s]",
skill.c_str(), interface_base.c_str(), topic.c_str());
const float base_progress =
(total_skills > 0) ? (static_cast<float>(index) / static_cast<float>(total_skills)) : 0.f;
@@ -638,7 +649,7 @@ bool CerebellumNode::ExecuteActionSkill(
effective_timeout = std::chrono::milliseconds(timeout_ms_override);
}
if (skill == "Arm") {
if (interface_base == "Arm") {
interfaces::action::Arm::Goal goal{};
if (!TryParseCallPayload<interfaces::action::Arm::Goal>(skill, goal)) {
RCLCPP_ERROR(this->get_logger(), "Failed to parse call payload for skill %s", skill.c_str());
@@ -646,7 +657,7 @@ bool CerebellumNode::ExecuteActionSkill(
}
if (goal.body_id >= 2) { //left and right
return ExecuteBilateralArmAction(skill, topic, effective_timeout,
return ExecuteBilateralArmAction(skill, interface_base, topic, effective_timeout,
instance_name, timeout_ms_override, index, total_skills, goal_handle, detail);
} else if (goal.body_id == 0 || goal.body_id == 1) { //left or right
tls_arm_body_id = goal.body_id;
@@ -655,7 +666,7 @@ bool CerebellumNode::ExecuteActionSkill(
return false;
}
}
return RunActionSkillWithProgress(skill, topic, effective_timeout,
return RunActionSkillWithProgress(skill, interface_base, topic, effective_timeout,
instance_name, timeout_ms_override, index, total_skills, goal_handle, detail);
}
@@ -676,6 +687,7 @@ bool CerebellumNode::ExecuteActionSkill(
*/
bool CerebellumNode::RunActionSkillWithProgress(
const std::string & skill,
const std::string & interface_name,
const std::string & topic,
std::chrono::nanoseconds timeout_ns,
const std::string & instance_name,
@@ -693,9 +705,10 @@ bool CerebellumNode::RunActionSkillWithProgress(
std::thread worker([&]() {
const auto * prev_calls = CerebellumNode::tls_current_calls_;
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);
RCLCPP_INFO(this->get_logger(), "[%s] instance='%s' timeout=%.3fs dispatch='%s' topic='%s'",
skill.c_str(), instance_name.c_str(), timeout_ns.count()/1e9, interface_name.c_str(), topic.c_str());
ok = brain::dispatch_skill_action<0, brain::SkillActionTypes>(
skill, topic, action_clients_.get(), logger, timeout_ns, instance_name, local_detail);
interface_name, topic, action_clients_.get(), logger, timeout_ns, instance_name, local_detail);
CerebellumNode::tls_current_calls_ = prev_calls;
finished.store(true, std::memory_order_release);
});
@@ -738,6 +751,7 @@ bool CerebellumNode::RunActionSkillWithProgress(
*/
bool CerebellumNode::ExecuteBilateralArmAction(
const std::string & skill,
const std::string & interface_name,
const std::string & topic,
std::chrono::nanoseconds timeout,
const std::string & instance_name,
@@ -757,12 +771,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, instance_name](int body_id) {
skill, interface_name, 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, instance_name, d_out);
interface_name, 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();
@@ -771,12 +785,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, instance_name](int body_id) {
skill, interface_name, 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, instance_name, d_out);
interface_name, 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();

12
src/brain/src/skill_manager.cpp
View File

@@ -311,6 +311,8 @@ void SkillManager::register_interfaces_(const SkillSpec & s)
return prefix + "/" + base;
};
const std::string skill_snake = to_snake_case(s.name);
for (const auto & iface : s.interfaces) {
auto parsed = parse_interface_entry(iface);
if (!parsed) {
@@ -323,8 +325,8 @@ void SkillManager::register_interfaces_(const SkillSpec & s)
const std::string base_snake = to_snake_case(parsed->base);
auto resolve_topic_name = [&](const std::string & kind_suffix) {
// Compute the prior default (prefix + base_snake), but allow per-trait default topic when provided.
const std::string prefix_default = join_topic(prefix_value, base_snake);
// Compute the prior default (prefix + skill_snake), but allow per-trait default topic when provided.
const std::string prefix_default = join_topic(prefix_value, skill_snake);
const auto & action_defaults = get_action_default_topics();
const auto & service_defaults = get_service_default_topics();
std::string trait_default;
@@ -342,8 +344,10 @@ void SkillManager::register_interfaces_(const SkillSpec & s)
yaml_override = it_yaml->second;
}
const bool has_trait_default = !trait_default.empty();
const std::string computed_default = !yaml_override.empty() ? yaml_override : (has_trait_default ? trait_default : prefix_default);
const std::string param_name = base_snake + "." + kind_suffix + "_name";
// If skill name doesn't match interface base, it's a specific instance; prefer prefix_default (skill_snake) over generic trait_default.
const std::string computed_default = !yaml_override.empty() ? yaml_override :
((skill_snake == base_snake && has_trait_default) ? trait_default : prefix_default);
const std::string param_name = skill_snake + "." + kind_suffix + "_name";
std::string topic_override;
if (node_->get_parameter(param_name, topic_override)) {
// If user explicitly sets an empty parameter, fall back to computed_default.

29
src/scripts/gen_bt_params.py
View File

@@ -220,6 +220,7 @@ def main():
ap = argparse.ArgumentParser(description='Generate YAML params for BT nodes.')
ap.add_argument('--xml', default='src/brain/config/bt_carry_boxes.xml', help='Path to BehaviorTree XML')
ap.add_argument('--interfaces', default='../hivecore_robot_interfaces', help='Path to hivecore_robot_interfaces root')
ap.add_argument('--skills', default='src/brain/config/robot_skills.yaml', help='Path to robot_skills.yaml')
ap.add_argument('--out', default='src/brain/config/bt_carry_boxes.params.default.yaml', help='Output YAML path')
args = ap.parse_args()
@@ -227,18 +228,46 @@ def main():
repo_root = os.path.abspath(os.path.join(os.path.dirname(__file__), '..', '..'))
xml_path = args.xml if os.path.isabs(args.xml) else os.path.join(repo_root, args.xml)
interfaces_root = args.interfaces if os.path.isabs(args.interfaces) else os.path.join(repo_root, args.interfaces)
skills_path = args.skills if os.path.isabs(args.skills) else os.path.join(repo_root, args.skills)
out_path = args.out if os.path.isabs(args.out) else os.path.join(repo_root, args.out)
if not os.path.exists(xml_path):
print(f'ERROR: XML not found: {xml_path}', file=sys.stderr)
sys.exit(2)
# Load mapping from skills.yaml if available
skill_to_interface: Dict[str, str] = {}
if os.path.exists(skills_path):
try:
with open(skills_path, 'r', encoding='utf-8') as f:
skills_data = yaml.safe_load(f)
if isinstance(skills_data, list):
for skill in skills_data:
name = skill.get('name')
if not name: continue
ifaces = skill.get('interfaces', [])
for iface in ifaces:
token = None
if isinstance(iface, str):
token = iface
elif isinstance(iface, dict):
token = iface.get('name')
if token and '.' in token:
base = token.split('.', 1)[0]
skill_to_interface[name] = base
break # Use first action/srv found
except Exception as e:
print(f"WARN: Failed to parse skills YAML {skills_path}: {e}", file=sys.stderr)
nodes = collect_bt_nodes(xml_path)
results = []
for instance_name, node_tag in nodes:
# Map tag to interface base by stripping trailing '_H' if present
base = node_tag[:-2] if node_tag.endswith('_H') else node_tag
# If tag (like SkillName_H) refers to a skill in robot_skills.yaml, resolve to its interface
base = skill_to_interface.get(base, base)
action_file = locate_action_file(interfaces_root, base, "action")
if action_file == None:
action_file = locate_action_file(interfaces_root, base, "srv")

4
src/scripts/gen_payload_converters.py
View File

@@ -348,6 +348,8 @@ def main():
kind = kind.lower()
header_snake = to_snake(base)
if kind.startswith('action'):
if base in added_actions:
continue
includes.add(f'"interfaces/action/{header_snake}.hpp"')
# locate .action file in common subdirs
action_dir = resolve_interface_subdir(interfaces_root, 'action')
@@ -379,6 +381,8 @@ def main():
converters.append(gen_converter_action(base, fields, header_snake))
added_actions.add(base)
elif kind.startswith('srv') or kind.startswith('service'):
if base in added_srvs:
continue
includes.add(f'"interfaces/srv/{header_snake}.hpp"')
srv_dir = resolve_interface_subdir(interfaces_root, 'srv')
tried_paths = []

9
src/scripts/gen_skill_config.py
View File

@@ -95,6 +95,7 @@ def parse_iface_item(item):
return None
for entry in skills:
skill_name = entry.get('name', '')
if 'interfaces' not in entry:
continue
for iface in entry['interfaces']:
@@ -105,13 +106,17 @@ for entry in skills:
if kind == 'action':
if base not in skill_action_types:
skill_action_types.append(base)
# Prefer default_topic from a skill whose name matches the interface base,
# otherwise don't overwrite if we already have one from a prior matching or non-matching entry.
if dtopic:
action_default_map[base] = dtopic
if base not in action_default_map or skill_name == base:
action_default_map[base] = dtopic
elif kind == 'srv':
if base not in skill_service_types:
skill_service_types.append(base)
if dtopic:
service_default_map[base] = dtopic
if base not in service_default_map or skill_name == base:
service_default_map[base] = dtopic
# Helper to map base name to include path / C++ type name.
# Assumption: interfaces live in package `interfaces` and types are under `interfaces::action::Name` or `interfaces::srv::Name`.