Moves rules parsing to ModeHandling

* Mode handling uses yaml parser to parse mode rules from SMH file

#13

Signed-off-by: Arne Nordmann <arne.nordmann@de.bosch.com>

system_modes/include/system_modes/mode_handling.hpp

@@ -27,10 +27,38 @@
 #include <utility>
 
 #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<std::string, ModeRule>;
+
 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<std::string, ModeMap> rules_;
+  std::map<std::string, RulesMap> 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

system_modes/include/system_modes/mode_inference.hpp

@@ -27,6 +27,7 @@
 #include <utility>
 
 #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<std::string, ModeMap> modes_;

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<shared_mutex> 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

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);

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: