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

support multi-sch

Browse Source
This commit is contained in:
david
2025-10-22 14:28:30 +08:00
parent 6e59457dd4
commit 26a1c24f83
9 changed files with 244 additions and 47 deletions
Download Patch File Download Diff File Expand all files Collapse all files

0
src/brain/config/bt_carry_boxes.params.yaml → src/brain/config/bt_carry_boxes_sch1.params.yaml
View File

0
src/brain/config/bt_carry_boxes.xml → src/brain/config/bt_carry_boxes_sch1.xml
View File

110
src/brain/config/bt_carry_boxes_sch2.params.yaml Normal file
View File

@@ -0,0 +1,110 @@
- name: root
params: '{}
'
- name: retry_all_action
params: '{}
'
- name: s1_waist_bend_down
params: 'move_pitch_degree: 13.0
move_yaw_degree: -30.0
'
- name: s2_arm_stretch_out
params: 'body_id: 0
data_type: 0
data_length: 14
command_id: 0
frame_time_stamp: 0
data_array: [-0.222853, -0.514124, 0.261742, -0.18186004, -0.63142093, 0.74353241, 0.12407289, 0.086304, 0.471622, 0.033076, -0.76833478, -0.08777985, -0.21307887, 0.59712123]
'
- name: s3_hand_pickup
params: 'mode: 1
effort: 0.0
'
- name: s4_arm_lift
params: 'body_id: 0
data_type: 0
data_length: 14
command_id: 0
frame_time_stamp: 0
data_array: [-0.198632, -0.513333, 0.072526, -0.13862565, -0.71159701, 0.67387035, 0.14251799, 0.253501, 0.476683, 0.265433, -0.51013004, -0.02328561, -0.48576245, 0.70940817]
'
- name: s5_waist_bend_up
params: 'move_pitch_degree: 3.0
move_yaw_degree: 5.0
'
- name: s6_waist_turn_around
params: 'move_pitch_degree: 120.0
move_yaw_degree: 30.0
'
- name: s7_leg_move_back
params: 'move_up_distance: -0.5
'
- name: s8_wheel_move_back
params: 'move_distance: -2.0
move_angle: 0.0
'
- name: s9_waist_bend_down
params: 'move_pitch_degree: -5.0
move_yaw_degree: 0.0
'
- name: s10_arm_stretch_out
params: 'body_id: 0
data_type: 0
data_length: 14
command_id: 0
frame_time_stamp: 0
data_array: [-0.222853, -0.514124, 0.261742, -0.18186004, -0.63142093, 0.74353241, 0.12407289, 0.325695, 0.447487, 0.108462, -0.65995416, 0.33866696, -0.16590247, 0.64980117]
'
- name: s11_hand_release
params: 'mode: 0
effort: 0.0
'
- name: s15_arm_retract
params: 'body_id: 0
data_type: 0
data_length: 14
command_id: 0
frame_time_stamp: 0
data_array: [-0.123562, -0.555266, 0.062529, -0.02276690, -0.75662638, 0.63753626, 0.14333775, 0.082317, 0.557912, 0.123469, -0.63065177, -0.03018818, -0.18611339, 0.75281394]
'

27
src/brain/config/bt_carry_boxes_sch2.xml Normal file
View File

@@ -0,0 +1,27 @@
<root BTCPP_format="4">
<BehaviorTree ID="MainTree">
<Sequence name="root">
<!-- Retry waist + arm + hand until HandControl_H succeeds -->
<RetryUntilSuccessful name="retry_all_action" num_attempts="5">
<Sequence>
<!-- <MoveHome_H name="s0_move_home" /> -->
<MoveWaist_H name="s1_waist_bend_down" />
<Arm_H name="s2_arm_stretch_out" />
<HandControl_H name="s3_hand_pickup" />
<Arm_H name="s4_arm_lift" />
<MoveWaist_H name="s5_waist_bend_up" />
<MoveWaist_H name="s6_waist_turn_around" />
<MoveLeg_H name="s7_leg_move_back" />
<MoveWheel_H name="s8_wheel_move_back" />
<MoveWaist_H name="s9_waist_bend_down" />
<Arm_H name="s10_arm_stretch_out" />
<HandControl_H name="s11_hand_release" />
<!-- <Arm_H name="s12_arm_move_to_snapshot_pose" />
<CameraTakePhoto_H name="s13_camera_take_photo" />
<MoveWaist_H name="s14_waist_bend_up" /> -->
<Arm_H name="s15_arm_retract" />
</Sequence>
</RetryUntilSuccessful>
</Sequence>
</BehaviorTree>
</root>

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

@@ -1,11 +1,17 @@
- name: brain
version: 1.0.0
skill_file: "/config/robot_skills.yaml"
skill_file: /config/robot_skills.yaml
- name: cerebrum_node
version: 1.0.0
bt_config_file: "/config/bt_carry_boxes.xml"
bt_params_file: "/config/bt_carry_boxes.params.yaml"
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}
]
- name: cerebellum_node
version: 1.0.0
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}
]

6
src/brain/include/brain/cerebrum_node.hpp
View File

@@ -198,9 +198,11 @@ private:
std::unique_ptr<brain::robot_config::RobotConfig> robot_config_params_;
std::string active_sequence_;
std::string share_directory_;
std::vector<brain::robot_config::BtConfigParam> bt_config_params_paths_;
brain::robot_config::BtConfigParam current_bt_config_params_path_;
std::string robot_skill_file_path_;
std::string bt_config_file_path_;
std::string bt_params_file_path_;
std::filesystem::file_time_type bt_xml_last_write_time_{}; // last observed mod time (for hot reload)
rclcpp::TimerBase::SharedPtr task_timer_;
rclcpp::TimerBase::SharedPtr bt_timer_;

29
src/brain/include/brain/robot_config.hpp
View File

@@ -21,6 +21,12 @@ struct Entry {
std::unordered_map<std::string, std::string> fields;
};
// Pair type for config + params entries
struct BtConfigParam {
std::string config;
std::string param;
};
// Loader and query interface for robot configuration.
class RobotConfig {
public:
@@ -86,12 +92,23 @@ class RobotConfig {
return GetValue(name, "skill_file");
}
std::optional<std::string> BtConfigFile(const std::string& name) const {
return GetValue(name, "bt_config_file");
}
std::optional<std::string> BtParamsFile(const std::string& name) const {
return GetValue(name, "bt_params_file");
std::optional<std::vector<BtConfigParam>> ConfigParamsPath(const std::string& name) const {
auto v = GetValue(name, "config_params_path");
if (!v) return std::nullopt;
try {
YAML::Node n = YAML::Load(*v);
if (n.IsSequence()) {
std::vector<BtConfigParam> res;
for (const auto & it : n) {
BtConfigParam p;
if (it["config"] && it["config"].IsScalar()) p.config = it["config"].as<std::string>();
if (it["param"] && it["param"].IsScalar()) p.param = it["param"].as<std::string>();
res.push_back(std::move(p));
}
return res;
}
} catch (...) {}
return std::nullopt;
}
private:

71
src/brain/src/cerebrum_node.cpp
View File

@@ -108,7 +108,7 @@ CerebrumNode::CerebrumNode(const rclcpp::NodeOptions & options)
RegisterActionClient();
bt_timer_ = this->create_wall_timer(10ms, [this]() {ExecuteBehaviorTree();});
task_timer_ = this->create_wall_timer(20000ms, [this]() {CerebrumTask();});
task_timer_ = this->create_wall_timer(10000ms, [this]() {CerebrumTask();});
CreateServices();
@@ -302,14 +302,21 @@ void CerebrumNode::CerebrumTask()
return;
}
if (!bt_config_file_path_.empty()) {
BuildBehaviorTreeFromFile(bt_config_file_path_);
} else {
// Use correctly cased API methods
RunVlmModel();
CancelActiveExecuteBtGoal();
UpdateBehaviorTree();
for (const auto & path_param : bt_config_params_paths_) {
// path_param is a pair {config, param}; compare the config path string against the current path
if (path_param.config == current_bt_config_params_path_.config) {
continue;
}
BuildBehaviorTreeFromFile(path_param.config);
current_bt_config_params_path_ = path_param;
RCLCPP_WARN(this->get_logger(), "CerebrumTask Switching to BT config file path: %s", path_param.config.c_str());
return;
}
// Use correctly cased API methods
// RunVlmModel();
// CancelActiveExecuteBtGoal();
// UpdateBehaviorTree();
}
/**
@@ -588,17 +595,19 @@ void CerebrumNode::DeclareBtActionParamsForSkillInstance(
//READ PARAMS FROM ROBOT CONFIG FILE
std::string instance_params;
if (bt_params_file_path_.empty()) {
if (current_bt_config_params_path_.param.empty()) {
RCLCPP_WARN(this->get_logger(), "BT params file path is empty; cannot load sample params");
} else {
try {
robot_config_params_ = std::make_unique<brain::robot_config::RobotConfig>(bt_params_file_path_);
robot_config_params_ = std::make_unique<brain::robot_config::RobotConfig>(current_bt_config_params_path_.param);
auto params = robot_config_params_->GetValue(instance_name, "params");
if (params == std::nullopt) {
RCLCPP_WARN(this->get_logger(), "BT params file %s does not contain params for %s", bt_params_file_path_.c_str(), instance_name.c_str());
RCLCPP_WARN(this->get_logger(), "BT params file %s does not contain params for %s",
current_bt_config_params_path_.param.c_str(), instance_name.c_str());
} else {
instance_params = *params;
RCLCPP_INFO(this->get_logger(), "Loaded BT params for %s, instance_params: %s", instance_name.c_str(), instance_params.c_str());
RCLCPP_INFO(this->get_logger(), "Loaded BT params for %s, instance_params: %s",
instance_name.c_str(), instance_params.c_str());
}
} catch (const std::exception & e) {
RCLCPP_ERROR(this->get_logger(), "[%s] read params failed: %s", skill_name.c_str(), e.what());
@@ -1021,14 +1030,16 @@ void CerebrumNode::CreateServices()
return;
}
if (!bt_config_file_path_.empty()) {
BuildBehaviorTreeFromFile(bt_config_file_path_);
} else {
RunVlmModel();
CancelActiveExecuteBtGoal();
UpdateBehaviorTree();
for (const auto & path_param : bt_config_params_paths_) {
// path_param is a pair {config, param}; compare the config path string against the current path
if (path_param.config == current_bt_config_params_path_.config) {
continue;
}
BuildBehaviorTreeFromFile(path_param.config);
current_bt_config_params_path_ = path_param;
RCLCPP_WARN(this->get_logger(), "cerebrum/rebuild_now Service Switching to BT config file path: %s", path_param.config.c_str());
return;
}
resp->success = true;
resp->message = "Rebuild triggered";
});
@@ -1051,22 +1062,16 @@ bool CerebrumNode::LoadRobotConfiguration()
robot_skill_file_path_ = share_directory_ + *skill_file;
RCLCPP_WARN(this->get_logger(), "skill file %s", robot_skill_file_path_.c_str());
auto bt_config_file = robot_config_->BtConfigFile("cerebrum_node");
if (bt_config_file == std::nullopt) {
RCLCPP_ERROR(this->get_logger(), "No bt_config_file entry found for 'cerebrum_node' in robot_config.yaml");
return false;
}
bt_config_file_path_ = share_directory_ + *bt_config_file;
RCLCPP_WARN(this->get_logger(), "bt config file %s", bt_config_file_path_.c_str());
auto bt_params_file = robot_config_->BtParamsFile("cerebrum_node");
if (bt_params_file == std::nullopt) {
RCLCPP_ERROR(this->get_logger(), "No bt_params_file entry found for 'cerebrum_node' in robot_config.yaml");
return false;
auto path = robot_config_->ConfigParamsPath("cerebrum_node");
if (path != std::nullopt) {
for (const auto & p : *path) {
if (!p.config.empty() && !p.param.empty()) {
bt_config_params_paths_.push_back({share_directory_ + p.config, share_directory_ + p.param});
RCLCPP_WARN(this->get_logger(), "bt config param %s %s", p.config.c_str(), p.param.c_str());
}
}
}
bt_params_file_path_ = share_directory_ + *bt_params_file;
RCLCPP_WARN(this->get_logger(), "bt params file %s", bt_params_file_path_.c_str());
return true;
}

34
src/scripts/gen_robot_config.py
View File

@@ -69,7 +69,7 @@ def main() -> None:
# Collect all keys across items (ensure 'name' exists first)
key_set = {"name"}
preferred_order = ["version", "skill_file", "bt_config_file", "bt_params_file"]
preferred_order = ["version", "skill_file"]
for item in data:
if isinstance(item, dict):
for k in item.keys():
@@ -103,6 +103,13 @@ def main() -> None:
out.write(" std::string name;\n")
out.write(" std::unordered_map<std::string, std::string> fields;\n")
out.write("};\n\n")
# If YAML contains config_params_path, emit a small struct type used by the accessor
if 'config_params_path' in keys:
out.write("// Pair type for config + params entries\n")
out.write("struct BtConfigParam {\n")
out.write(" std::string config;\n")
out.write(" std::string param;\n")
out.write("};\n\n")
out.write("// Loader and query interface for robot configuration.\n")
out.write("class RobotConfig {\n")
out.write(" public:\n")
@@ -161,11 +168,34 @@ def main() -> None:
out.write(" return it->second;\n")
out.write(" }\n\n")
# Typed getters for all keys except 'name'
# Typed getters for all keys except 'name' -- simple string accessors
for k in keys:
if k == "name":
continue
func = to_camel(k)
# Special-case config_params_path to return parsed sequence of {config,param}
if k == 'config_params_path':
out.write(
f" std::optional<std::vector<BtConfigParam>> {func}(const std::string& name) const " + "{\n"
)
out.write(f" auto v = GetValue(name, \"{k}\");\n")
out.write(" if (!v) return std::nullopt;\n")
out.write(" try {\n")
out.write(" YAML::Node n = YAML::Load(*v);\n")
out.write(" if (n.IsSequence()) {\n")
out.write(" std::vector<BtConfigParam> res;\n")
out.write(" for (const auto & it : n) {\n")
out.write(" BtConfigParam p;\n")
out.write(" if (it[\"config\"] && it[\"config\"].IsScalar()) p.config = it[\"config\"].as<std::string>();\n")
out.write(" if (it[\"param\"] && it[\"param\"].IsScalar()) p.param = it[\"param\"].as<std::string>();\n")
out.write(" res.push_back(std::move(p));\n")
out.write(" }\n")
out.write(" return res;\n")
out.write(" }\n")
out.write(" } catch (...) {}\n")
out.write(" return std::nullopt;\n")
out.write(" }\n\n")
continue
out.write(
f" std::optional<std::string> {func}(const std::string& name) const " + "{\n"
)