diff --git a/src/brain/config/bt_carry_boxes.params.yaml b/src/brain/config/bt_carry_boxes_sch1.params.yaml similarity index 100% rename from src/brain/config/bt_carry_boxes.params.yaml rename to src/brain/config/bt_carry_boxes_sch1.params.yaml diff --git a/src/brain/config/bt_carry_boxes.xml b/src/brain/config/bt_carry_boxes_sch1.xml similarity index 100% rename from src/brain/config/bt_carry_boxes.xml rename to src/brain/config/bt_carry_boxes_sch1.xml diff --git a/src/brain/config/bt_carry_boxes_sch2.params.yaml b/src/brain/config/bt_carry_boxes_sch2.params.yaml new file mode 100644 index 0000000..a8f57f1 --- /dev/null +++ b/src/brain/config/bt_carry_boxes_sch2.params.yaml @@ -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] + + ' diff --git a/src/brain/config/bt_carry_boxes_sch2.xml b/src/brain/config/bt_carry_boxes_sch2.xml new file mode 100644 index 0000000..61b6e61 --- /dev/null +++ b/src/brain/config/bt_carry_boxes_sch2.xml @@ -0,0 +1,27 @@ + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/brain/config/robot_config.yaml b/src/brain/config/robot_config.yaml index 05ba070..f08f9cc 100644 --- a/src/brain/config/robot_config.yaml +++ b/src/brain/config/robot_config.yaml @@ -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 \ No newline at end of file + 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} + ] \ No newline at end of file diff --git a/src/brain/include/brain/cerebrum_node.hpp b/src/brain/include/brain/cerebrum_node.hpp index 5657ce6..5bd65a1 100644 --- a/src/brain/include/brain/cerebrum_node.hpp +++ b/src/brain/include/brain/cerebrum_node.hpp @@ -198,9 +198,11 @@ private: std::unique_ptr robot_config_params_; std::string active_sequence_; std::string share_directory_; + + std::vector 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_; diff --git a/src/brain/include/brain/robot_config.hpp b/src/brain/include/brain/robot_config.hpp index 6a81eda..d5c0401 100644 --- a/src/brain/include/brain/robot_config.hpp +++ b/src/brain/include/brain/robot_config.hpp @@ -21,6 +21,12 @@ struct Entry { std::unordered_map 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 BtConfigFile(const std::string& name) const { - return GetValue(name, "bt_config_file"); - } - - std::optional BtParamsFile(const std::string& name) const { - return GetValue(name, "bt_params_file"); + std::optional> 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 res; + for (const auto & it : n) { + BtConfigParam p; + if (it["config"] && it["config"].IsScalar()) p.config = it["config"].as(); + if (it["param"] && it["param"].IsScalar()) p.param = it["param"].as(); + res.push_back(std::move(p)); + } + return res; + } + } catch (...) {} + return std::nullopt; } private: diff --git a/src/brain/src/cerebrum_node.cpp b/src/brain/src/cerebrum_node.cpp index 571b1f2..6da3bbc 100644 --- a/src/brain/src/cerebrum_node.cpp +++ b/src/brain/src/cerebrum_node.cpp @@ -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(bt_params_file_path_); + robot_config_params_ = std::make_unique(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; } diff --git a/src/scripts/gen_robot_config.py b/src/scripts/gen_robot_config.py index 5b42bcb..83ea866 100644 --- a/src/scripts/gen_robot_config.py +++ b/src/scripts/gen_robot_config.py @@ -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 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> {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 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();\n") + out.write(" if (it[\"param\"] && it[\"param\"].IsScalar()) p.param = it[\"param\"].as();\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 {func}(const std::string& name) const " + "{\n" )