diff --git a/system_modes/include/system_modes/mode_handling.hpp b/system_modes/include/system_modes/mode_handling.hpp index cae24a6..67f6996 100644 --- a/system_modes/include/system_modes/mode_handling.hpp +++ b/system_modes/include/system_modes/mode_handling.hpp @@ -27,10 +27,38 @@ #include #include "system_modes/mode.hpp" +#include "system_modes/mode_impl.hpp" namespace system_modes { +struct ModeRule +{ + std::string system_; + StateAndMode system_target_; + + std::string part_; + StateAndMode part_actual_; + + StateAndMode new_system_target_; + +/*explicit ModeRule( + const std::string & system, + StateAndMode system_target, + const std::string & part, + StateAndMode part_actual, + StateAndMode new_system_target) + { + system_ = system; + system_target_ = system_target; + part_ = part; + part_actual_ = part_actual; + new_system_target_ = new_system_target; + }*/ +}; + +using RulesMap = std::map; + class ModeHandling { public: @@ -40,12 +68,21 @@ class ModeHandling virtual ~ModeHandling() = default; protected: - virtual void add_rule(const std::string & part, const rclcpp::Parameter & param); - mutable std::shared_timed_mutex rules_mutex_; private: - std::map rules_; + std::map rules_; + + virtual void read_rules_from_model(const std::string & model_path); + virtual void parse_rule( + const std::string & part, + const std::string & rule_name, + const rclcpp::Parameter & rule); + virtual void add_rule( + const std::string & part, + const std::string & rule_name, + const std::string & rule_part, + const rclcpp::Parameter & rule); }; } // namespace system_modes diff --git a/system_modes/include/system_modes/mode_inference.hpp b/system_modes/include/system_modes/mode_inference.hpp index 49c55ad..35538fe 100644 --- a/system_modes/include/system_modes/mode_inference.hpp +++ b/system_modes/include/system_modes/mode_inference.hpp @@ -27,6 +27,7 @@ #include #include "system_modes/mode.hpp" +#include "system_modes/mode_handling.hpp" namespace system_modes { @@ -74,6 +75,8 @@ class ModeInference virtual void add_param_to_mode(ModeBasePtr, const rclcpp::Parameter &); private: + ModeHandling * mode_handling_; + StatesMap nodes_, nodes_target_; StatesMap systems_, systems_target_; std::map modes_; diff --git a/system_modes/src/system_modes/mode_handling.cpp b/system_modes/src/system_modes/mode_handling.cpp index 46167ad..c08e3dd 100644 --- a/system_modes/src/system_modes/mode_handling.cpp +++ b/system_modes/src/system_modes/mode_handling.cpp @@ -31,10 +31,10 @@ using std::endl; using std::pair; using std::mutex; using std::string; -using std::make_pair; using std::lock_guard; using rclcpp::Parameter; using rclcpp::ParameterMap; +using rclcpp::ParameterType; using lifecycle_msgs::msg::State; using shared_mutex = std::shared_timed_mutex; @@ -43,45 +43,114 @@ namespace system_modes { ModeHandling::ModeHandling(const string & model_path) +{ + this->read_rules_from_model(model_path); +} + +void +ModeHandling::read_rules_from_model(const string & model_path) { rcl_params_t * yaml_params = rcl_yaml_node_struct_init(rcl_get_default_allocator()); if (!rcl_parse_yaml_file(model_path.c_str(), yaml_params)) { - throw std::runtime_error("Failed to parse parameters " + model_path); + throw std::runtime_error("Failed to parse rules from " + model_path); } - + //rcl_yaml_node_struct_print(yaml_params); rclcpp::ParameterMap param_map = rclcpp::parameter_map_from(yaml_params); - rcl_yaml_node_struct_fini(yaml_params); - ParameterMap::iterator it; for (it = param_map.begin(); it != param_map.end(); it++) { string part_name(it->first.substr(1)); + for (auto & param : it->second) { + string param_name(param.get_name()); - auto itm = this->rules_.find(part_name); - if (itm == this->rules_.end()) { - this->rules_.emplace(part_name, ModeMap()); + if (param_name.find("rules.") != string::npos) { + this->parse_rule(part_name, param_name.substr(6), param); + } } + } - for (auto & param : it->second) { - std::size_t foundm = param.get_name().find("rules."); - if (foundm != string::npos) { - std::size_t foundmr = param.get_name().find(".", 6); - if (foundmr != string::npos) { - this->add_rule(part_name, param); - } else { - continue; - } - } else { - continue; - } +/* + for (unsigned int n = 0; n < yaml_params->num_nodes; ++n) { + string part_name = yaml_params->node_names[n]; + + for (unsigned int i = 0; i < yaml_params->params[n].num_params; ++i) { + string param_name = yaml_params->params[n].parameter_names[i]; + rcl_variant_t * param_value = + rcl_yaml_node_struct_get(part_name.c_str(), param_name.c_str(), yaml_params); + + if (param_name.find("rules.") != string::npos) { + this->parse_rule(part_name, param_name.substr(6), param_value); + } } + }*/ + + rcl_yaml_node_struct_fini(yaml_params); + +} + +void +ModeHandling::parse_rule( + const string & part, + const string & rule_name, + const rclcpp::Parameter & rule) +{ + // Rule specification + std::size_t split = rule_name.find("."); + if (split == string::npos) { + throw std::runtime_error("ModeHandling::parse_rule() can't parse rule."); + } + string rule_spec = rule_name.substr(split + 1); + string rule_name_ = rule_name.substr(0, split); + + if (rule_spec.compare("if_target") != 0 && + rule_spec.compare("if_part") != 0 && + rule_spec.compare("new_target") != 0) { + throw std::runtime_error("ModeHandling::parse_rule() can't parse rule spec."); + } + + if (rule.get_type() != ParameterType::PARAMETER_STRING && + rule.get_type() != ParameterType::PARAMETER_STRING_ARRAY) { + throw std::runtime_error("ModeHandling::parse_rule() rule is neither string nor string array."); } + + this->add_rule(part, rule_name_, rule_spec, rule); } void -ModeHandling::add_rule(const std::string & part, const Parameter & param) +ModeHandling::add_rule(const string & part, + const std::string & rule_name, + const std::string & rule_spec, + const rclcpp::Parameter & rule_param) { std::unique_lock mlock(this->rules_mutex_); - throw std::runtime_error("ModeHandling::add_rule() not yet implemented"); + + // Insert rule if not existing already + this->rules_.insert(std::make_pair(part, RulesMap())); + auto it = this->rules_[part].insert(std::make_pair(rule_name, ModeRule())); + auto rule = it.first->second; + + rule.system_ = part; + if (rule_spec.compare("if_target") == 0) { + if (rule_param.get_type() != ParameterType::PARAMETER_STRING) { + throw std::runtime_error("ModeHandling::parse_rule() if_target expects string."); + } + rule.system_target_ = StateAndMode(); + rule.system_target_.from_string(rule_param.as_string()); + } else if (rule_spec.compare("if_part") == 0) { + if (rule_param.get_type() != ParameterType::PARAMETER_STRING_ARRAY) { + throw std::runtime_error("ModeHandling::parse_rule() if_part expects string array."); + } + auto spec = rule_param.as_string_array(); + rule.part_ = spec[0]; + rule.system_target_ = StateAndMode(); + rule.system_target_.from_string(spec[1]); + } else if (rule_spec.compare("new_target") == 0) { + if (rule_param.get_type() != ParameterType::PARAMETER_STRING) { + throw std::runtime_error("ModeHandling::parse_rule() new_target expects string."); + } + rule.new_system_target_ = StateAndMode(); + rule.new_system_target_.from_string(rule_param.as_string()); + } + this->rules_[part][rule_name] = rule; } } // namespace system_modes diff --git a/system_modes/src/system_modes/mode_inference.cpp b/system_modes/src/system_modes/mode_inference.cpp index ed8565b..d841898 100644 --- a/system_modes/src/system_modes/mode_inference.cpp +++ b/system_modes/src/system_modes/mode_inference.cpp @@ -42,7 +42,8 @@ namespace system_modes { ModeInference::ModeInference(const string & model_path) -: nodes_(), nodes_target_(), +: mode_handling_(new ModeHandling(model_path)), + nodes_(), nodes_target_(), systems_(), systems_target_(), modes_(), nodes_mutex_(), systems_mutex_(), modes_mutex_(), parts_mutex_(), @@ -494,6 +495,7 @@ ModeInference::read_modes_from_model(const string & model_path) if (param.get_name().compare("type") != 0) { string mode_name; + // Parse mode definitions std::size_t foundm = param.get_name().find("modes."); if (foundm != string::npos) { std::size_t foundmr = param.get_name().find(".", 6); diff --git a/system_modes_examples/example_modes.yaml b/system_modes_examples/example_modes.yaml index 7466501..f4fa297 100644 --- a/system_modes_examples/example_modes.yaml +++ b/system_modes_examples/example_modes.yaml @@ -20,24 +20,21 @@ actuation: rules: degrade_from_MODERATE: if_target: active.MODERATE - if_part: manipulator - if_part_actual: inactive - new_system_target: active.__DEFAULT__ + if_part: [manipulator, inactive] + new_target: active.__DEFAULT__ degrade_from_PERFORMANCE: if_target: active.PERFORMANCE - if_part: manipulator - if_part_actual: inactive - new_system_target: active.__DEFAULT__ + if_part: [manipulator, inactive] + new_target: active.__DEFAULT__ inactive_from_MODERATE: if_target: active.PERFORMANCE - if_part: drive_base - if_part_actual: inactive - new_system_target: inactive + if_part: [drive_base, inactive] + new_target: inactive inactive_from_PERFORMANCE: if_target: active.PERFORMANCE - if_part: drive_base - if_part_actual: inactive - new_system_target: inactive + if_part: [drive_base, inactive] + new_target: inactive + # if system in __DEFAULT__, don't degrade, don't deactivate, but recover manipulator: ros__parameters: