From 35062589d8d9af6f607868b4059408dabd98ace1 Mon Sep 17 00:00:00 2001 From: "Nordmann Arne (CR/ADT3)" Date: Thu, 10 Jun 2021 14:51:12 +0200 Subject: [PATCH 01/11] Mode handling was part of inference by mistake Signed-off-by: Nordmann Arne (CR/ADT3) --- system_modes/include/system_modes/mode_inference.hpp | 2 -- system_modes/src/system_modes/mode_inference.cpp | 3 +-- 2 files changed, 1 insertion(+), 4 deletions(-) diff --git a/system_modes/include/system_modes/mode_inference.hpp b/system_modes/include/system_modes/mode_inference.hpp index b02eed1..6166e06 100644 --- a/system_modes/include/system_modes/mode_inference.hpp +++ b/system_modes/include/system_modes/mode_inference.hpp @@ -85,8 +85,6 @@ class ModeInference virtual void add_param_to_mode(ModeBasePtr, const rclcpp::Parameter &); private: - ModeHandling * mode_handling_; - StatesMap nodes_, nodes_target_, nodes_cache_; StatesMap systems_, systems_target_, systems_cache_; std::map modes_; diff --git a/system_modes/src/system_modes/mode_inference.cpp b/system_modes/src/system_modes/mode_inference.cpp index bc27da3..e5ef341 100644 --- a/system_modes/src/system_modes/mode_inference.cpp +++ b/system_modes/src/system_modes/mode_inference.cpp @@ -42,8 +42,7 @@ namespace system_modes { ModeInference::ModeInference(const string & model_path) -: mode_handling_(new ModeHandling(model_path)), - nodes_(), nodes_target_(), +: nodes_(), nodes_target_(), systems_(), systems_target_(), modes_(), nodes_mutex_(), systems_mutex_(), modes_mutex_(), parts_mutex_(), From 16e640f9c76bc7bd8f5a408aff24db2a3818595f Mon Sep 17 00:00:00 2001 From: "Nordmann Arne (CR/ADT3)" Date: Fri, 11 Jun 2021 08:31:56 +0200 Subject: [PATCH 02/11] Default modes can have arbitrary names now https://github.com/micro-ROS/system_modes/issues/69 Signed-off-by: Nordmann Arne (CR/ADT3) --- system_modes/include/system_modes/mode.hpp | 3 +-- system_modes/src/system_modes/mode.cpp | 4 ++-- 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/system_modes/include/system_modes/mode.hpp b/system_modes/include/system_modes/mode.hpp index 34f5e11..dd33771 100644 --- a/system_modes/include/system_modes/mode.hpp +++ b/system_modes/include/system_modes/mode.hpp @@ -74,8 +74,7 @@ class ModeBase class DefaultMode : public ModeBase { public: - DefaultMode(); - explicit DefaultMode(const std::string & mode_name) = delete; + explicit DefaultMode(const std::string & mode_name = DEFAULT_MODE); // cppcheck-suppress unknownMacro RCLCPP_DISABLE_COPY(DefaultMode) diff --git a/system_modes/src/system_modes/mode.cpp b/system_modes/src/system_modes/mode.cpp index 35f138f..41e241a 100644 --- a/system_modes/src/system_modes/mode.cpp +++ b/system_modes/src/system_modes/mode.cpp @@ -105,8 +105,8 @@ ModeBase::print() const return os.str(); } -DefaultMode::DefaultMode() -: ModeBase(DEFAULT_MODE) +DefaultMode::DefaultMode(const std::string & mode_name) +: ModeBase(mode_name) { } From 4db96a8b9a63cf6a5ea1483cf8a4c2b7ce26a99c Mon Sep 17 00:00:00 2001 From: "Nordmann Arne (CR/ADT3)" Date: Fri, 11 Jun 2021 08:33:03 +0200 Subject: [PATCH 03/11] More flexibility in defining default mode Default mode can now be defined by * Naming a mode '__DEFAULT__' (backword compatible) * Specifying the parameter '__default' * Neither of these two, then te first mode of a part will be its default https://github.com/micro-ROS/system_modes/issues/69 Signed-off-by: Nordmann Arne (CR/ADT3) --- .../src/system_modes/mode_inference.cpp | 57 ++++++++++++++++--- 1 file changed, 48 insertions(+), 9 deletions(-) diff --git a/system_modes/src/system_modes/mode_inference.cpp b/system_modes/src/system_modes/mode_inference.cpp index e5ef341..d36d08f 100644 --- a/system_modes/src/system_modes/mode_inference.cpp +++ b/system_modes/src/system_modes/mode_inference.cpp @@ -505,13 +505,14 @@ ModeInference::read_modes_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 modes and parameters from: " + model_path); } rclcpp::ParameterMap param_map = rclcpp::parameter_map_from(yaml_params); rcl_yaml_node_struct_fini(yaml_params); - ParameterMap::iterator it; + ParameterMap::iterator it, part_start; + string old_part_name; for (it = param_map.begin(); it != param_map.end(); it++) { string part_name(it->first.substr(1)); @@ -520,6 +521,49 @@ ModeInference::read_modes_from_model(const string & model_path) this->modes_.emplace(part_name, ModeMap()); } + // First, look for a default mode + string default_mode_name; + part_start = it; + for (auto & param : it->second) { + 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); + if (foundmr != string::npos) { + mode_name = param.get_name().substr(foundm + 6, foundmr - foundm - 6); + } else { + continue; + } + } else { + continue; + } + + // Take the first one as backup + if (default_mode_name.empty()) { + default_mode_name = mode_name; + } + + std::size_t found = param.get_name().find("ros__parameters"); + if (found != string::npos && param.get_name().compare("__default") == 0) { + default_mode_name = mode_name; + } + + // valid parameter, add to mode map + if (mode_name.compare(DEFAULT_MODE) == 0) { + default_mode_name = DEFAULT_MODE; + continue; + } + } + } + if (default_mode_name.empty()) { + throw std::runtime_error("Failed to find default mode for " + part_name); + } + it = part_start; + + // Build up modes ModeBasePtr mode; DefaultModePtr default_mode; for (auto & param : it->second) { @@ -541,15 +585,10 @@ ModeInference::read_modes_from_model(const string & model_path) // valid parameter, add to mode map if (!mode || mode->get_name().compare(mode_name) != 0) { - if (mode_name.compare(DEFAULT_MODE) != 0) { - if (!default_mode) { - throw std::runtime_error( - "Could not find default mode for mode '" + - mode_name + "'. Make sure, default mode is defined first."); - } + if (mode_name.compare(default_mode_name) != 0) { mode = std::make_shared(mode_name, default_mode); } else { - default_mode = std::make_shared(); + default_mode = std::make_shared(default_mode_name); mode = default_mode; } } From b0683be282da3dfa58f4b6071cb9e68e8381180f Mon Sep 17 00:00:00 2001 From: "Nordmann Arne (CR/ADT3)" Date: Thu, 17 Jun 2021 14:54:53 +0200 Subject: [PATCH 04/11] Mode implementation no longer relying on deprectaed default mode name Signed-off-by: Nordmann Arne (CR/ADT3) --- system_modes/include/system_modes/mode.hpp | 4 ++++ system_modes/include/system_modes/mode_impl.hpp | 4 +--- system_modes/src/system_modes/mode.cpp | 13 +++++++++++-- 3 files changed, 16 insertions(+), 5 deletions(-) diff --git a/system_modes/include/system_modes/mode.hpp b/system_modes/include/system_modes/mode.hpp index dd33771..ecef6bf 100644 --- a/system_modes/include/system_modes/mode.hpp +++ b/system_modes/include/system_modes/mode.hpp @@ -65,6 +65,10 @@ class ModeBase virtual const std::vector get_parts() const; virtual const StateAndMode get_part_mode(const std::string & part) const; + /** + * Created StateAndMode struct that describes this mode + */ + virtual const StateAndMode sm() const; virtual std::string print() const; protected: diff --git a/system_modes/include/system_modes/mode_impl.hpp b/system_modes/include/system_modes/mode_impl.hpp index 39863f4..543c875 100644 --- a/system_modes/include/system_modes/mode_impl.hpp +++ b/system_modes/include/system_modes/mode_impl.hpp @@ -61,9 +61,7 @@ struct StateAndMode return true; } - return cmp.mode.compare(mode) == 0 || // same mode - (cmp.mode.compare(DEFAULT_MODE) == 0 && mode.empty()) || // we consider empty and - (mode.compare(DEFAULT_MODE) == 0 && cmp.mode.empty()); // DEFAULT_MODE the same + return cmp.mode.compare(mode) == 0; } bool operator!=(const StateAndMode & cmp) const diff --git a/system_modes/src/system_modes/mode.cpp b/system_modes/src/system_modes/mode.cpp index 41e241a..0b9caad 100644 --- a/system_modes/src/system_modes/mode.cpp +++ b/system_modes/src/system_modes/mode.cpp @@ -105,6 +105,15 @@ ModeBase::print() const return os.str(); } +const StateAndMode +ModeBase::sm() const +{ + StateAndMode sm; + sm.state = State::PRIMARY_STATE_ACTIVE; + sm.mode = this->get_name(); + return sm; +} + DefaultMode::DefaultMode(const std::string & mode_name) : ModeBase(mode_name) { @@ -128,7 +137,7 @@ DefaultMode::set_part_mode( const StateAndMode stateAndMode) { if (stateAndMode.mode.empty()) { - this->mode_impl_.add_part_mode(part, StateAndMode(stateAndMode.state, DEFAULT_MODE)); + this->mode_impl_.add_part_mode(part, StateAndMode(stateAndMode.state, "")); } else { this->mode_impl_.add_part_mode(part, stateAndMode); } @@ -170,7 +179,7 @@ Mode::set_part_mode( const StateAndMode stateAndMode) { if (stateAndMode.mode.empty()) { - this->mode_impl_.add_part_mode(part, StateAndMode(stateAndMode.state, DEFAULT_MODE)); + this->mode_impl_.add_part_mode(part, StateAndMode(stateAndMode.state, "")); } else { this->mode_impl_.add_part_mode(part, stateAndMode); } From c81d2644d9bad671cf2839c577b846d5668c1f99 Mon Sep 17 00:00:00 2001 From: "Nordmann Arne (CR/ADT3)" Date: Thu, 17 Jun 2021 14:56:22 +0200 Subject: [PATCH 05/11] Adapted mode inference * Now parsing default modes for all system parts first, including its custom name * Introduced new matching function that considers custom default mode names https://github.com/micro-ROS/system_modes/issues/69 Signed-off-by: Nordmann Arne (CR/ADT3) --- .../include/system_modes/mode_inference.hpp | 13 +- system_modes/src/mode_manager_node.cpp | 5 +- system_modes/src/mode_monitor_node.cpp | 6 +- .../src/system_modes/mode_inference.cpp | 193 ++++++++++++------ .../src/system_modes/mode_manager.cpp | 18 +- 5 files changed, 158 insertions(+), 77 deletions(-) diff --git a/system_modes/include/system_modes/mode_inference.hpp b/system_modes/include/system_modes/mode_inference.hpp index 6166e06..03fb460 100644 --- a/system_modes/include/system_modes/mode_inference.hpp +++ b/system_modes/include/system_modes/mode_inference.hpp @@ -62,6 +62,11 @@ class ModeInference virtual StateAndMode infer_node(const std::string & part); virtual StateAndMode infer_system(const std::string & part); + virtual bool matching_modes( + const std::string & part, + const StateAndMode & a, + const StateAndMode & b) const; + /** * Infers latest transitions of systems * @@ -75,12 +80,15 @@ class ModeInference virtual StateAndMode get_target(const std::string & part) const; virtual ModeConstPtr get_mode(const std::string & part, const std::string & mode) const; + virtual ModeConstPtr get_default_mode(const std::string & part) const; + virtual const std::string get_default_mode_name(const std::string & part) const; virtual std::vector get_available_modes(const std::string & part) const; virtual ~ModeInference() = default; protected: virtual bool matching_parameters(const rclcpp::Parameter &, const rclcpp::Parameter &) const; + virtual void read_modes_from_model(const std::string & model_path); virtual void add_param_to_mode(ModeBasePtr, const rclcpp::Parameter &); @@ -88,12 +96,13 @@ class ModeInference StatesMap nodes_, nodes_target_, nodes_cache_; StatesMap systems_, systems_target_, systems_cache_; std::map modes_; + std::map default_modes_; ParametersMap parameters_; mutable std::shared_timed_mutex nodes_mutex_, systems_mutex_, - modes_mutex_, parts_mutex_, - param_mutex_; + modes_mutex_, default_modes_mutex_, + parts_mutex_, param_mutex_; mutable std::shared_timed_mutex nodes_target_mutex_, systems_target_mutex_; mutable std::shared_timed_mutex diff --git a/system_modes/src/mode_manager_node.cpp b/system_modes/src/mode_manager_node.cpp index 8eca50d..0f168a1 100644 --- a/system_modes/src/mode_manager_node.cpp +++ b/system_modes/src/mode_manager_node.cpp @@ -28,8 +28,6 @@ #include "system_modes/mode_manager.hpp" -using std::cerr; -using std::cout; using std::string; using std::vector; using std::function; @@ -37,7 +35,6 @@ using std::make_pair; using std::shared_ptr; using system_modes::ModeManager; -using system_modes::DEFAULT_MODE; using system_modes::StateAndMode; using system_modes_msgs::msg::ModeEvent; @@ -77,7 +74,7 @@ void transition_request_callback( } else { manager->inference()->update_target( node_name, - StateAndMode(msg->goal_state.id, DEFAULT_MODE)); + StateAndMode(msg->goal_state.id, manager->inference()->get_default_mode_name(node_name))); } } diff --git a/system_modes/src/mode_monitor_node.cpp b/system_modes/src/mode_monitor_node.cpp index 7029de7..e1d6052 100644 --- a/system_modes/src/mode_monitor_node.cpp +++ b/system_modes/src/mode_monitor_node.cpp @@ -28,9 +28,6 @@ #include "system_modes/mode_monitor.hpp" -using std::cerr; -using std::cout; -using std::endl; using std::string; using std::vector; using std::function; @@ -41,7 +38,6 @@ using std::make_shared; using std::invalid_argument; using system_modes::ModeMonitor; -using system_modes::DEFAULT_MODE; using system_modes::StateAndMode; using system_modes_msgs::msg::ModeEvent; using lifecycle_msgs::msg::State; @@ -78,7 +74,7 @@ void transition_request_callback( } else { monitor->inference()->update_target( node_name, - StateAndMode(msg->goal_state.id, DEFAULT_MODE)); + StateAndMode(msg->goal_state.id, monitor->inference()->get_default_mode_name(node_name))); } } diff --git a/system_modes/src/system_modes/mode_inference.cpp b/system_modes/src/system_modes/mode_inference.cpp index d36d08f..ef80c50 100644 --- a/system_modes/src/system_modes/mode_inference.cpp +++ b/system_modes/src/system_modes/mode_inference.cpp @@ -45,8 +45,8 @@ ModeInference::ModeInference(const string & model_path) : nodes_(), nodes_target_(), systems_(), systems_target_(), modes_(), - nodes_mutex_(), systems_mutex_(), modes_mutex_(), parts_mutex_(), - nodes_target_mutex_(), systems_target_mutex_() + nodes_mutex_(), systems_mutex_(), modes_mutex_(), default_modes_mutex_(), + parts_mutex_(), nodes_target_mutex_(), systems_target_mutex_() { this->read_modes_from_model(model_path); } @@ -204,12 +204,7 @@ ModeInference::infer_system(const string & part) string mode = ""; std::shared_lock mlock(this->modes_mutex_); - auto default_mode = this->modes_.at(part).at(DEFAULT_MODE); - if (!default_mode) { - throw std::out_of_range( - "Can't infer for system '" + part + - "', missing default mode."); - } + auto default_mode = get_default_mode(part); // If we don't know the target state/mode, we can hardly infer anything std::shared_lock stlock(this->systems_target_mutex_); @@ -254,7 +249,6 @@ ModeInference::infer_system(const string & part) // to switch to inactive, then the actual state of the current entity will // go to error-processing until the mentioned part recovers. this->systems_[part] = StateAndMode(State::TRANSITION_STATE_ERRORPROCESSING, ""); - return StateAndMode(State::TRANSITION_STATE_ERRORPROCESSING, ""); } @@ -263,7 +257,6 @@ ModeInference::infer_system(const string & part) stateAndMode.state != State::PRIMARY_STATE_UNCONFIGURED) { this->systems_[part] = StateAndMode(State::TRANSITION_STATE_DEACTIVATING, ""); - return StateAndMode(State::TRANSITION_STATE_DEACTIVATING, ""); } } @@ -284,20 +277,10 @@ ModeInference::infer_system(const string & part) // TODO(anordman): consider DONT-CARE if (stateAndMode.state == State::TRANSITION_STATE_ERRORPROCESSING) { this->systems_[part] = StateAndMode(State::TRANSITION_STATE_ERRORPROCESSING, ""); - return StateAndMode(State::TRANSITION_STATE_ERRORPROCESSING, ""); } - // TODO(anordman): overly complictated. intent: we are in our target - // mode, if actual and target state are the same OR they can be - // considered same, i.e. unconfigured and inactive - if ( - (stateAndMode.state != targetStateAndMode.state && - !(targetStateAndMode.state == State::PRIMARY_STATE_INACTIVE && - stateAndMode.state == State::PRIMARY_STATE_UNCONFIGURED)) || - (stateAndMode.state == State::PRIMARY_STATE_ACTIVE && - stateAndMode.mode.compare(targetStateAndMode.mode) != 0)) - { + if (!matching_modes(partpart, stateAndMode, targetStateAndMode)) { // not in target state, or active but not in target mode inTargetMode = false; continue; @@ -306,7 +289,6 @@ ModeInference::infer_system(const string & part) if (inTargetMode) { // Target state and target mode reached, all good! this->systems_[part] = StateAndMode(State::PRIMARY_STATE_ACTIVE, targetMode); - return StateAndMode(State::PRIMARY_STATE_ACTIVE, targetMode); } } @@ -321,14 +303,10 @@ ModeInference::infer_system(const string & part) // TODO(anordman): consider DONT-CARE if (stateAndMode.state == State::TRANSITION_STATE_ERRORPROCESSING) { this->systems_[part] = StateAndMode(State::TRANSITION_STATE_ERRORPROCESSING, ""); - return StateAndMode(State::TRANSITION_STATE_ERRORPROCESSING, ""); } - if (stateAndMode.state != targetStateAndMode.state || - (stateAndMode.state == State::PRIMARY_STATE_ACTIVE && - stateAndMode.mode.compare(targetStateAndMode.mode) != 0)) - { + if (!matching_modes(partpart, stateAndMode, targetStateAndMode)) { // not in target state, or active but not in target mode foundMode = false; continue; @@ -337,13 +315,11 @@ ModeInference::infer_system(const string & part) if (foundMode) { // We are in a non-target mode, this means we are still activating this->systems_[part] = StateAndMode(State::TRANSITION_STATE_ACTIVATING, mode.first); - return StateAndMode(State::TRANSITION_STATE_ACTIVATING, mode.first); } } this->systems_[part] = StateAndMode(State::TRANSITION_STATE_ACTIVATING, ""); - return StateAndMode(State::TRANSITION_STATE_ACTIVATING, ""); } @@ -356,13 +332,6 @@ ModeInference::infer_node(const string & part) std::shared_lock mlock(this->modes_mutex_); std::shared_lock prlock(this->param_mutex_); - auto default_mode = this->modes_.at(part).at(DEFAULT_MODE); - if (!default_mode) { - throw std::out_of_range( - "Can't infer for node '" + part + - "', missing default mode."); - } - // Do we know the target mode? try { auto target = this->get_target(part); @@ -396,18 +365,18 @@ ModeInference::infer_node(const string & part) // no target mode, so next we check the default mode bool inDefaultMode = true; - auto defaultMode = this->modes_.at(part).at(DEFAULT_MODE); + auto defaultMode = get_default_mode(part); for (auto param : defaultMode->get_parameter_names()) { if (!matching_parameters( defaultMode->get_parameter(param), parameters_.at(part).at(param))) { inDefaultMode = false; - continue; + break; } } if (inDefaultMode) { - return StateAndMode(State::PRIMARY_STATE_ACTIVE, DEFAULT_MODE); + return get_default_mode(part)->sm(); } // no target mode, not default mode, so we try our luck, infering any mode from parameters @@ -425,7 +394,7 @@ ModeInference::infer_node(const string & part) } if (foundMode) { // We are in a non-target mode, this means we are still activating - return StateAndMode(State::PRIMARY_STATE_ACTIVE, mode.first); + return mode.second->sm(); } } @@ -485,6 +454,20 @@ ModeInference::get_mode(const string & part, const string & mode) const return nullptr; } +ModeConstPtr +ModeInference::get_default_mode(const string & part) const +{ + std::shared_lock mlock(this->modes_mutex_); + return modes_.at(part).at(get_default_mode_name(part)); +} + +const std::string +ModeInference::get_default_mode_name(const string & part) const +{ + std::shared_lock mlock(this->default_modes_mutex_); + return default_modes_.at(part); +} + std::vector ModeInference::get_available_modes(const std::string & part) const { @@ -501,6 +484,7 @@ ModeInference::read_modes_from_model(const string & model_path) std::unique_lock nlock(this->nodes_mutex_); std::unique_lock slock(this->systems_mutex_); std::unique_lock mlock(this->modes_mutex_); + std::unique_lock dmlock(this->default_modes_mutex_); std::unique_lock plock(this->parts_mutex_); rcl_params_t * yaml_params = rcl_yaml_node_struct_init(rcl_get_default_allocator()); @@ -511,7 +495,7 @@ ModeInference::read_modes_from_model(const string & model_path) rclcpp::ParameterMap param_map = rclcpp::parameter_map_from(yaml_params); rcl_yaml_node_struct_fini(yaml_params); - ParameterMap::iterator it, part_start; + ParameterMap::iterator it, part_start_it; string old_part_name; for (it = param_map.begin(); it != param_map.end(); it++) { string part_name(it->first.substr(1)); @@ -523,9 +507,12 @@ ModeInference::read_modes_from_model(const string & model_path) // First, look for a default mode string default_mode_name; - part_start = it; + part_start_it = it; for (auto & param : it->second) { - if (param.get_name().compare("type") != 0) { + if (param.get_name().compare("defaultmode") == 0) { + default_mode_name = param.value_to_string(); + break; + } else if (param.get_name().compare("type") != 0) { string mode_name; // Parse mode definitions @@ -546,26 +533,31 @@ ModeInference::read_modes_from_model(const string & model_path) default_mode_name = mode_name; } - std::size_t found = param.get_name().find("ros__parameters"); - if (found != string::npos && param.get_name().compare("__default") == 0) { - default_mode_name = mode_name; - } - - // valid parameter, add to mode map + /** + * Check for mode with name '__DEFAULT__' + * + * Necessary for backward-compatibility, will get deprecated soon, see + * https://github.com/micro-ROS/system_modes/issues/69 + */ if (mode_name.compare(DEFAULT_MODE) == 0) { default_mode_name = DEFAULT_MODE; - continue; + std::cout << "WARNING: Specifying the default mode just by its name '__DEFAULT__'" << + " is deprecated. Please specify the 'defaultmode' parameter for '" << + part_name << "' instead." << std::endl; + break; } } } if (default_mode_name.empty()) { throw std::runtime_error("Failed to find default mode for " + part_name); } - it = part_start; - // Build up modes - ModeBasePtr mode; - DefaultModePtr default_mode; + default_modes_[part_name] = default_mode_name; + it = part_start_it; + + // Build up default mode + DefaultModePtr default_mode = std::make_shared(default_mode_name); + it = part_start_it; for (auto & param : it->second) { if (param.get_name().compare("type") != 0) { string mode_name; @@ -582,15 +574,73 @@ ModeInference::read_modes_from_model(const string & model_path) } else { continue; } + if (mode_name.compare(default_mode_name) != 0) { + continue; + } // valid parameter, add to mode map - if (!mode || mode->get_name().compare(mode_name) != 0) { - if (mode_name.compare(default_mode_name) != 0) { - mode = std::make_shared(mode_name, default_mode); + std::size_t found = param.get_name().find("ros__parameters"); + if (found != string::npos) { + this->add_param_to_mode(default_mode, param); + } else { + // part mode + string part_part = param.get_name(); + std::size_t foundr = param.get_name().rfind("."); + if (foundr != string::npos) { + part_part = param.get_name().substr(foundr + 1); + } + + string state(param.value_to_string()); + string smode; + foundr = param.value_to_string().rfind("."); + if (foundr != string::npos) { + state = param.value_to_string().substr(0, foundr); + smode = param.value_to_string().substr(foundr + 1); + } + + default_mode->set_part_mode(part_part, StateAndMode(state_id_(state), smode)); + } + this->modes_[part_name].emplace(default_mode_name, default_mode); + + } else { + if (param.value_to_string().compare("node") != 0) { + this->systems_.emplace( + part_name, + StateAndMode(State::PRIMARY_STATE_UNKNOWN, "")); + } else { + this->nodes_.emplace( + part_name, + StateAndMode(State::PRIMARY_STATE_UNKNOWN, "")); + } + } + } + + // Build up non-default modes + ModeBasePtr mode; + it = part_start_it; + for (auto & param : it->second) { + 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); + if (foundmr != string::npos) { + mode_name = param.get_name().substr(foundm + 6, foundmr - foundm - 6); } else { - default_mode = std::make_shared(default_mode_name); - mode = default_mode; + continue; } + } else { + continue; + } + if (mode_name.compare(default_mode_name) == 0) { + continue; + } + + // valid parameter, add to mode map + if (!mode || mode->get_name().compare(mode_name) != 0) { + mode = std::make_shared(mode_name, default_mode); } std::size_t found = param.get_name().find("ros__parameters"); if (found != string::npos) { @@ -685,7 +735,7 @@ ModeInference::get_all_parts_of(const string & system) const if (system.empty()) { return std::vector(); } - return this->modes_.at(system).at(DEFAULT_MODE)->get_parts(); + return get_default_mode(system)->get_parts(); } bool @@ -727,6 +777,29 @@ ModeInference::matching_parameters(const Parameter & target, const Parameter & a return false; } +bool +ModeInference::matching_modes(const std::string & part, + const StateAndMode & a, + const StateAndMode & b) const +{ + if (a == b) { + return true; + } + + if (a.state == State::PRIMARY_STATE_ACTIVE && + b.state == State::PRIMARY_STATE_ACTIVE && + ((a.mode.empty() && b.mode.compare(get_default_mode_name(part)) == 0) || + (b.mode.empty() && a.mode.compare(get_default_mode_name(part)) == 0))) + { + return true; // We consider empty mode to be the same as default mode + } + + // TODO(anordman): considered unconfigured and inactive the same as well? + // Should be fine from a system_modes point of view? + + return false; +} + Deviation ModeInference::infer_transitions() { diff --git a/system_modes/src/system_modes/mode_manager.cpp b/system_modes/src/system_modes/mode_manager.cpp index cb7b2d3..0df52c9 100644 --- a/system_modes/src/system_modes/mode_manager.cpp +++ b/system_modes/src/system_modes/mode_manager.cpp @@ -405,8 +405,8 @@ ModeManager::change_state( this->change_part_state(part, transition_id); } } else { - RCLCPP_DEBUG(this->get_logger(), " active, so trigger mode transition to __DEFAULT__"); - this->change_mode(node_name, DEFAULT_MODE); + RCLCPP_DEBUG(this->get_logger(), " active, so trigger mode transition to default"); + this->change_mode(node_name, mode_inference_->get_default_mode_name(node_name)); } return true; } @@ -481,7 +481,7 @@ ModeManager::change_mode( // state/transition and mode via mode inference this->change_part_state(part, Transition::TRANSITION_ACTIVATE); if (stateAndMode.mode.empty()) { - this->change_part_mode(part, DEFAULT_MODE); + this->change_part_mode(part, mode_inference_->get_default_mode_name(part)); } else { this->change_part_mode(part, stateAndMode.mode); } @@ -530,13 +530,19 @@ ModeManager::change_part_state(const string & node, unsigned int transition) } void -ModeManager::change_part_mode(const string & node, const string & mode) +ModeManager::change_part_mode(const string & node, const string & m) { RCLCPP_DEBUG( this->get_logger(), - " changing mode of part %s to %s", + " changing mode of part %s to mode '%s'", node.c_str(), - mode.c_str()); + m.c_str()); + + // We consider empty mode = default mode + auto mode = m; + if (mode.empty()) { + mode = mode_inference_->get_default_mode_name(node); + } auto request = std::make_shared(); request->mode_name = mode; From c864b22b1bae348dd7a4eb7760ea7f3a776155e3 Mon Sep 17 00:00:00 2001 From: "Nordmann Arne (CR/ADT3)" Date: Thu, 17 Jun 2021 14:59:07 +0200 Subject: [PATCH 06/11] Adapts example and tests Signed-off-by: Nordmann Arne (CR/ADT3) --- .../src/system_modes/mode_inference.cpp | 3 +- .../launchtest/modes_observer_test_node.cpp | 2 - .../launchtest/two_mixed_nodes_modes.yaml | 12 +-- system_modes/test/test_default_mode.cpp | 5 +- system_modes/test/test_mode_handling.cpp | 6 +- system_modes/test/test_mode_inference.cpp | 78 +++++++++++++++---- system_modes/test/test_modes.yaml | 12 +-- system_modes/test/test_modes_rules.yaml | 10 ++- .../test/test_state_and_mode_struct.cpp | 7 +- system_modes_examples/example_modes.yaml | 12 +-- 10 files changed, 103 insertions(+), 44 deletions(-) diff --git a/system_modes/src/system_modes/mode_inference.cpp b/system_modes/src/system_modes/mode_inference.cpp index ef80c50..797e883 100644 --- a/system_modes/src/system_modes/mode_inference.cpp +++ b/system_modes/src/system_modes/mode_inference.cpp @@ -778,7 +778,8 @@ ModeInference::matching_parameters(const Parameter & target, const Parameter & a } bool -ModeInference::matching_modes(const std::string & part, +ModeInference::matching_modes( + const std::string & part, const StateAndMode & a, const StateAndMode & b) const { diff --git a/system_modes/test/launchtest/modes_observer_test_node.cpp b/system_modes/test/launchtest/modes_observer_test_node.cpp index 3874a6c..e4f927a 100644 --- a/system_modes/test/launchtest/modes_observer_test_node.cpp +++ b/system_modes/test/launchtest/modes_observer_test_node.cpp @@ -29,7 +29,6 @@ #include "system_modes/mode_observer.hpp" -using std::cerr; using std::cout; using std::string; using std::vector; @@ -38,7 +37,6 @@ using std::make_pair; using std::shared_ptr; using system_modes::ModeObserver; -using system_modes::DEFAULT_MODE; using system_modes::StateAndMode; using system_modes_msgs::msg::ModeEvent; diff --git a/system_modes/test/launchtest/two_mixed_nodes_modes.yaml b/system_modes/test/launchtest/two_mixed_nodes_modes.yaml index b0933cf..d44ce19 100644 --- a/system_modes/test/launchtest/two_mixed_nodes_modes.yaml +++ b/system_modes/test/launchtest/two_mixed_nodes_modes.yaml @@ -21,8 +21,9 @@ sys: A: ros__parameters: type: node + defaultmode: DEFAULT modes: - __DEFAULT__: + DEFAULT: ros__parameters: foo: 0.1 AA: @@ -35,11 +36,8 @@ A: B: ros__parameters: type: node + defaultmode: DEFAULT modes: - __DEFAULT__: - ros__parameters: - foo: 0.1 - bar: ONE EE: ros__parameters: foo: 0.2 @@ -48,3 +46,7 @@ B: ros__parameters: foo: 0.9 bar: THREE + DEFAULT: + ros__parameters: + foo: 0.1 + bar: ONE diff --git a/system_modes/test/test_default_mode.cpp b/system_modes/test/test_default_mode.cpp index 44624ae..05525c1 100644 --- a/system_modes/test/test_default_mode.cpp +++ b/system_modes/test/test_default_mode.cpp @@ -40,7 +40,7 @@ class TestDefaultMode : public ::testing::Test void SetUp() { - default_mode = DefaultModePtr(new DefaultMode()); + default_mode = DefaultModePtr(new DefaultMode("custom name")); } void TearDown() @@ -57,6 +57,9 @@ TEST_F(TestDefaultMode, construction_and_destruction) { { DefaultMode mode; EXPECT_EQ("__DEFAULT__", mode.get_name()); + + DefaultMode mode_named("custom default mode name"); + EXPECT_EQ("custom default mode name", mode_named.get_name()); } } diff --git a/system_modes/test/test_mode_handling.cpp b/system_modes/test/test_mode_handling.cpp index f487acb..6eb7ca9 100644 --- a/system_modes/test/test_mode_handling.cpp +++ b/system_modes/test/test_mode_handling.cpp @@ -72,6 +72,8 @@ TEST(TestModeFilesParse, parse_rules) { TEST(TestModeFilesParse, test_rules) { ModeHandling * handling = new ModeHandling(MODE_FILE_RULES); + StateAndMode active_default(State::PRIMARY_STATE_ACTIVE, "DEFAULT"); + auto rules = handling->get_rules_for("system", StateAndMode(State::PRIMARY_STATE_ACTIVE, "AA")); EXPECT_EQ("degrade_from_AA", rules[0].name); @@ -79,7 +81,7 @@ TEST(TestModeFilesParse, test_rules) { EXPECT_EQ(StateAndMode(State::PRIMARY_STATE_ACTIVE, "AA"), rules[0].system_target); EXPECT_EQ("part0", rules[0].part); EXPECT_EQ(StateAndMode(State::PRIMARY_STATE_INACTIVE), rules[0].part_actual); - EXPECT_EQ(StateAndMode(State::PRIMARY_STATE_ACTIVE), rules[0].new_system_target); + EXPECT_EQ(active_default, rules[0].new_system_target); EXPECT_EQ("inactive_from_AA", rules[1].name); EXPECT_EQ("system", rules[1].system); @@ -95,7 +97,7 @@ TEST(TestModeFilesParse, test_rules) { EXPECT_EQ(StateAndMode(State::PRIMARY_STATE_ACTIVE, "BB"), rules[0].system_target); EXPECT_EQ("part0", rules[0].part); EXPECT_EQ(StateAndMode(State::PRIMARY_STATE_INACTIVE), rules[0].part_actual); - EXPECT_EQ(StateAndMode(State::PRIMARY_STATE_ACTIVE), rules[0].new_system_target); + EXPECT_EQ(active_default, rules[0].new_system_target); EXPECT_EQ("inactive_from_BB", rules[1].name); EXPECT_EQ("system", rules[1].system); diff --git a/system_modes/test/test_mode_inference.cpp b/system_modes/test/test_mode_inference.cpp index f682519..d3569bf 100644 --- a/system_modes/test/test_mode_inference.cpp +++ b/system_modes/test/test_mode_inference.cpp @@ -47,6 +47,48 @@ TEST(TestModeFilesParse, parsing) { EXPECT_NO_THROW(inference = new ModeInference(MODE_FILE_CORRECT)); EXPECT_EQ(3u, inference->get_all_parts().size()); + + EXPECT_EQ("DEFAULT", inference->get_default_mode_name("system")); + EXPECT_EQ("__DEFAULT__", inference->get_default_mode_name("part0")); + EXPECT_EQ("NORMAL", inference->get_default_mode_name("part1")); + + EXPECT_EQ( + State::PRIMARY_STATE_INACTIVE, + inference->get_default_mode("system")->get_part_mode("part0").state); + EXPECT_EQ( + State::PRIMARY_STATE_ACTIVE, + inference->get_default_mode("system")->get_part_mode("part1").state); + + EXPECT_EQ(0.1, inference->get_default_mode("part0")->get_parameter("foo").as_double()); + EXPECT_EQ("WARN", inference->get_default_mode("part0")->get_parameter("bar").as_string()); + + EXPECT_EQ(0.1, inference->get_default_mode("part1")->get_parameter("foo").as_double()); + EXPECT_EQ("WARN", inference->get_default_mode("part1")->get_parameter("bar").as_string()); +} + +TEST(TestModeFilesParse, comparison) { + ModeInference * inference = new ModeInference(MODE_FILE_CORRECT); + + StateAndMode active_empty(State::PRIMARY_STATE_ACTIVE, ""); + StateAndMode active_default__(State::PRIMARY_STATE_ACTIVE, "__DEFAULT__"); + StateAndMode active_default(State::PRIMARY_STATE_ACTIVE, "DEFAULT"); + StateAndMode active_normal(State::PRIMARY_STATE_ACTIVE, "NORMAL"); + StateAndMode inactive(State::PRIMARY_STATE_INACTIVE, ""); + + EXPECT_TRUE(inference->matching_modes("system", active_default, active_default)); + EXPECT_FALSE(inference->matching_modes("system", active_empty, active_default__)); + EXPECT_TRUE(inference->matching_modes("system", active_empty, active_default)); + EXPECT_FALSE(inference->matching_modes("system", active_empty, active_normal)); + + EXPECT_TRUE(inference->matching_modes("part0", active_default__, active_default__)); + EXPECT_TRUE(inference->matching_modes("part0", active_empty, active_default__)); + EXPECT_FALSE(inference->matching_modes("part0", active_empty, active_default)); + EXPECT_FALSE(inference->matching_modes("part0", active_empty, active_normal)); + + EXPECT_TRUE(inference->matching_modes("part1", active_normal, active_normal)); + EXPECT_FALSE(inference->matching_modes("part1", active_empty, active_default__)); + EXPECT_FALSE(inference->matching_modes("part1", active_empty, active_default)); + EXPECT_TRUE(inference->matching_modes("part1", active_empty, active_normal)); } TEST(TestModeFilesParse, initialization) { @@ -65,11 +107,11 @@ TEST(TestModeFilesParse, initialization) { EXPECT_EQ(3u, inference->get_available_modes("part1").size()); // default modes - auto mode = inference->get_mode("system", "__DEFAULT__"); + auto mode = inference->get_default_mode("system"); EXPECT_EQ(2u, mode->get_parts().size()); EXPECT_EQ(State::PRIMARY_STATE_INACTIVE, mode->get_part_mode("part0").state); EXPECT_EQ(State::PRIMARY_STATE_ACTIVE, mode->get_part_mode("part1").state); - mode = inference->get_mode("part0", "__DEFAULT__"); + mode = inference->get_default_mode("part0"); EXPECT_EQ(2u, mode->get_parameters().size()); EXPECT_EQ(0.1, mode->get_parameter("foo").as_double()); EXPECT_EQ("WARN", mode->get_parameter("bar").as_string()); @@ -106,15 +148,17 @@ TEST(TestModeInference, update_target) { TEST(TestModeInference, update_state_and_mode) { ModeInference inference(MODE_FILE_CORRECT); - StateAndMode active_default(State::PRIMARY_STATE_ACTIVE, "__DEFAULT__"); + StateAndMode active_default(State::PRIMARY_STATE_ACTIVE, "DEFAULT"); + StateAndMode active_normal(State::PRIMARY_STATE_ACTIVE, "NORMAL"); StateAndMode inactive(State::PRIMARY_STATE_INACTIVE, ""); inference.update_target("system", active_default); EXPECT_THROW(inference.update_state("system", active_default.state), std::out_of_range); EXPECT_THROW(inference.update_mode("system", active_default.mode), std::out_of_range); + inference.update("part0", inactive); - inference.update_state("part1", active_default.state); - inference.update_mode("part1", active_default.mode); + inference.update_state("part1", active_normal.state); + inference.update_mode("part1", active_normal.mode); EXPECT_EQ(active_default.state, inference.get_or_infer("system").state); EXPECT_EQ(active_default.mode, inference.get_or_infer("system").mode); } @@ -138,30 +182,32 @@ TEST(TestModeInference, inference) { ModeInference inference(MODE_FILE_CORRECT); // update node modes, test inferred system mode - StateAndMode active_default(State::PRIMARY_STATE_ACTIVE, "__DEFAULT__"); + StateAndMode active_system_default(State::PRIMARY_STATE_ACTIVE, "DEFAULT"); + StateAndMode active_part0_default(State::PRIMARY_STATE_ACTIVE, "__DEFAULT__"); + StateAndMode active_part1_default(State::PRIMARY_STATE_ACTIVE, "NORMAL"); StateAndMode inactive(State::PRIMARY_STATE_INACTIVE, ""); - inference.update_target("system", active_default); + inference.update_target("system", active_system_default); inference.update("part0", inactive); - inference.update("part1", active_default); + inference.update("part1", active_part1_default); // System inference StateAndMode sm = inference.get_or_infer("system"); EXPECT_EQ(State::PRIMARY_STATE_ACTIVE, sm.state); - EXPECT_EQ("__DEFAULT__", sm.mode); + EXPECT_EQ(active_system_default.mode, sm.mode); // Node inference Parameter foo("foo", 0.2); Parameter bar("bar", "DBG"); inference.update_param("part1", foo); inference.update_param("part1", bar); - EXPECT_EQ(active_default.state, inference.infer("part1").state); + EXPECT_EQ(active_part1_default.state, inference.infer("part1").state); EXPECT_EQ("AAA", inference.infer("part1").mode); Parameter foo2("foo", 0.1); Parameter bar2("bar", "DBG"); inference.update_state("part0", State::PRIMARY_STATE_ACTIVE); inference.update_param("part0", foo2); inference.update_param("part0", bar2); - EXPECT_EQ(active_default.state, inference.infer("part0").state); + EXPECT_EQ(active_part1_default.state, inference.infer("part0").state); EXPECT_EQ("FOO", inference.infer("part0").mode); // System inference @@ -172,11 +218,13 @@ TEST(TestModeInference, infer_transitions) { ModeInference inference(MODE_FILE_CORRECT); // update node modes, test inferred system mode - StateAndMode active_default(State::PRIMARY_STATE_ACTIVE, "__DEFAULT__"); + StateAndMode active_default__(State::PRIMARY_STATE_ACTIVE, "__DEFAULT__"); + StateAndMode active_default(State::PRIMARY_STATE_ACTIVE, "DEFAULT"); + StateAndMode active_normal(State::PRIMARY_STATE_ACTIVE, "NORMAL"); StateAndMode inactive(State::PRIMARY_STATE_INACTIVE, ""); inference.update_target("system", active_default); inference.update("part0", inactive); - inference.update("part1", active_default); + inference.update("part1", active_normal); // Expect to have one initial transition EXPECT_EQ(1u, inference.infer_transitions().size()); @@ -190,7 +238,7 @@ TEST(TestModeInference, infer_transitions) { auto transitions = inference.infer_transitions(); EXPECT_EQ(1u, transitions.size()); EXPECT_EQ(State::PRIMARY_STATE_ACTIVE, transitions["part1"].first.state); - EXPECT_EQ("__DEFAULT__", transitions["part1"].first.mode); + EXPECT_EQ("NORMAL", transitions["part1"].first.mode); EXPECT_EQ(State::PRIMARY_STATE_ACTIVE, transitions["part1"].second.state); EXPECT_EQ("AAA", transitions["part1"].second.mode); @@ -209,7 +257,7 @@ TEST(TestModeInference, infer_transitions) { EXPECT_EQ(State::PRIMARY_STATE_ACTIVE, transitions["part0"].second.state); EXPECT_EQ("FOO", transitions["part0"].second.mode); EXPECT_EQ(State::PRIMARY_STATE_ACTIVE, transitions["system"].first.state); - EXPECT_EQ("__DEFAULT__", transitions["system"].first.mode); + EXPECT_EQ("DEFAULT", transitions["system"].first.mode); EXPECT_EQ(State::TRANSITION_STATE_ACTIVATING, transitions["system"].second.state); // Expect to have cleared transitions diff --git a/system_modes/test/test_modes.yaml b/system_modes/test/test_modes.yaml index 1000db0..651769c 100644 --- a/system_modes/test/test_modes.yaml +++ b/system_modes/test/test_modes.yaml @@ -4,11 +4,12 @@ system: ros__parameters: type: system + defaultmode: DEFAULT parts: part0 part1 modes: - __DEFAULT__: + DEFAULT: part0: inactive part1: active AA: @@ -38,11 +39,8 @@ part0: part1: ros__parameters: type: node + defaultmode: NORMAL modes: - __DEFAULT__: - ros__parameters: - foo: 0.1 - bar: WARN AAA: ros__parameters: foo: 0.2 @@ -51,4 +49,8 @@ part1: ros__parameters: foo: 0.9 bar: ERR + NORMAL: + ros__parameters: + foo: 0.1 + bar: WARN diff --git a/system_modes/test/test_modes_rules.yaml b/system_modes/test/test_modes_rules.yaml index fbccc69..f228ed9 100644 --- a/system_modes/test/test_modes_rules.yaml +++ b/system_modes/test/test_modes_rules.yaml @@ -4,11 +4,12 @@ system: ros__parameters: type: system + defaultmode: DEFAULT parts: part0 part1 modes: - __DEFAULT__: + DEFAULT: part0: inactive part1: active AA: @@ -21,11 +22,11 @@ system: degrade_from_AA: if_target: active.AA if_part: [part0, inactive] - new_target: active.__DEFAULT__ + new_target: active.DEFAULT degrade_from_BB: if_target: active.BB if_part: [part0, inactive] - new_target: active.__DEFAULT__ + new_target: active.DEFAULT inactive_from_AA: if_target: active.AA if_part: [part1, inactive] @@ -55,8 +56,9 @@ part0: part1: ros__parameters: type: node + defaultmode: DEFAULT modes: - __DEFAULT__: + DEFAULT: ros__parameters: foo: 0.1 bar: WARN diff --git a/system_modes/test/test_state_and_mode_struct.cpp b/system_modes/test/test_state_and_mode_struct.cpp index 52098b3..0441934 100644 --- a/system_modes/test/test_state_and_mode_struct.cpp +++ b/system_modes/test/test_state_and_mode_struct.cpp @@ -39,7 +39,7 @@ class TestStateAndMode : public ::testing::Test active.state = State::PRIMARY_STATE_ACTIVE; active_default.state = State::PRIMARY_STATE_ACTIVE; - active_default.mode = "__DEFAULT__"; + active_default.mode = "DEFAULT"; active_foo.state = State::PRIMARY_STATE_ACTIVE; active_foo.mode = "FOO"; @@ -63,7 +63,6 @@ TEST_F(TestStateAndMode, comparison) { { EXPECT_EQ(inactive, inactive); EXPECT_EQ(active, active); - EXPECT_EQ(active, active_default); EXPECT_NE(active, inactive); EXPECT_NE(active, active_foo); @@ -79,7 +78,7 @@ TEST_F(TestStateAndMode, string_getter) { { EXPECT_EQ("inactive"s, inactive.as_string()); EXPECT_EQ("active"s, active.as_string()); - EXPECT_EQ("active.__DEFAULT__"s, active_default.as_string()); + EXPECT_EQ("active.DEFAULT"s, active_default.as_string()); EXPECT_EQ("active.FOO"s, active_foo.as_string()); active_foo.state = State::PRIMARY_STATE_INACTIVE; EXPECT_EQ("inactive"s, active_foo.as_string()); @@ -93,7 +92,7 @@ TEST_F(TestStateAndMode, string_setter) { copy.from_string("active"); EXPECT_EQ(active, copy); - copy.from_string("active.__DEFAULT__"); + copy.from_string("active.DEFAULT"); EXPECT_EQ(active_default, copy); copy.from_string("active.FOO"); diff --git a/system_modes_examples/example_modes.yaml b/system_modes_examples/example_modes.yaml index 692d277..14484f5 100644 --- a/system_modes_examples/example_modes.yaml +++ b/system_modes_examples/example_modes.yaml @@ -7,8 +7,9 @@ actuation: parts: manipulator drive_base + defaultmode: DEFAULT modes: - __DEFAULT__: + DEFAULT: manipulator: inactive drive_base: active MODERATE: @@ -40,21 +41,22 @@ manipulator: ros__parameters: type: node modes: - __DEFAULT__: - ros__parameters: - max_torque: 0.1 WEAK: ros__parameters: max_torque: 0.1 STRONG: ros__parameters: max_torque: 0.2 + __DEFAULT__: + ros__parameters: + max_torque: 0.1 drive_base: ros__parameters: type: node + defaultmode: NORMAL modes: - __DEFAULT__: + NORMAL: ros__parameters: max_speed: 0.1 controller: PID From a56c0e2d6ff45a9e6dd52b3aac97f1281c31a5b3 Mon Sep 17 00:00:00 2001 From: "Nordmann Arne (CR/ADT3)" Date: Thu, 17 Jun 2021 15:22:13 +0200 Subject: [PATCH 07/11] Documented default mode specification Signed-off-by: Nordmann Arne (CR/ADT3) --- system_modes/README.md | 13 ++++--------- 1 file changed, 4 insertions(+), 9 deletions(-) diff --git a/system_modes/README.md b/system_modes/README.md index a4da0fd..a48157e 100644 --- a/system_modes/README.md +++ b/system_modes/README.md @@ -40,7 +40,7 @@ System modes extend the *ACTIVE* state of the ROS 2 lifecycle and allow to speci * **Modes of nodes** consist of parameter values. * **Modes of (sub-)system)** consist of modes of their *parts*. -For example, a node representing an actuator might provide different modes that specify certain maximum speed or maximum torque values. An actuation sub-system, grouping several actuator nodes, might provide modes that activate/deactivate certain contained actuator nodes and/or change their modes based on its own modes. +For example, a node representing an actuator might provide different modes that specify certain maximum speed or maximum torque values. An actuation sub-system, grouping several actuator nodes, might provide modes that activate/deactivate certain contained actuator nodes and/or change their modes based on its own modes. Each part (system or node) has to specify a default mode that is selected, if no specific mode is requested. Both, the [system hierarchy](#hierarchical-system-modeling) as well as the system modes are specified in a system modes and hierarchy model file (SHM file, yaml format) that can be parsed by the [mode inference](#mode-inference) mechanism. The SMH file adheres to the following format *(curly brackets indicate placeholders, square brackets indicate optional parts, ellipses indicate repeatability)*: @@ -48,13 +48,11 @@ Both, the [system hierarchy](#hierarchical-system-modeling) as well as the syste {system}: ros__parameters: type: system + defaultmode: {MODE} parts: - {node} + {node or system} […] modes: - __DEFAULT__: - {node}: {state}[.{MODE}] - […] {MODE}: {node}: {state}[.{MODE}] […] @@ -64,11 +62,8 @@ Both, the [system hierarchy](#hierarchical-system-modeling) as well as the syste {node}: ros__parameters: type: node + defaultmode: {MODE} modes: - __DEFAULT__: - ros__parameters: - {parameter}: {value} - […] {MODE}: ros__parameters: {parameter}: {value} From bc6db34c0d27b68c9fbb24d22c0ac63366a0e58e Mon Sep 17 00:00:00 2001 From: "Nordmann Arne (CR/ADT3)" Date: Fri, 18 Jun 2021 09:01:00 +0200 Subject: [PATCH 08/11] Minor cleanup Signed-off-by: Nordmann Arne (CR/ADT3) --- system_modes/src/system_modes/mode.cpp | 5 +---- system_modes/src/system_modes/mode_inference.cpp | 7 +------ 2 files changed, 2 insertions(+), 10 deletions(-) diff --git a/system_modes/src/system_modes/mode.cpp b/system_modes/src/system_modes/mode.cpp index 0b9caad..4fb755b 100644 --- a/system_modes/src/system_modes/mode.cpp +++ b/system_modes/src/system_modes/mode.cpp @@ -108,10 +108,7 @@ ModeBase::print() const const StateAndMode ModeBase::sm() const { - StateAndMode sm; - sm.state = State::PRIMARY_STATE_ACTIVE; - sm.mode = this->get_name(); - return sm; + return StateAndMode(State::PRIMARY_STATE_ACTIVE, this->get_name()); } DefaultMode::DefaultMode(const std::string & mode_name) diff --git a/system_modes/src/system_modes/mode_inference.cpp b/system_modes/src/system_modes/mode_inference.cpp index 797e883..a8afbd9 100644 --- a/system_modes/src/system_modes/mode_inference.cpp +++ b/system_modes/src/system_modes/mode_inference.cpp @@ -528,11 +528,6 @@ ModeInference::read_modes_from_model(const string & model_path) continue; } - // Take the first one as backup - if (default_mode_name.empty()) { - default_mode_name = mode_name; - } - /** * Check for mode with name '__DEFAULT__' * @@ -541,7 +536,7 @@ ModeInference::read_modes_from_model(const string & model_path) */ if (mode_name.compare(DEFAULT_MODE) == 0) { default_mode_name = DEFAULT_MODE; - std::cout << "WARNING: Specifying the default mode just by its name '__DEFAULT__'" << + std::cerr << "WARNING: Specifying the default mode just by its name '__DEFAULT__'" << " is deprecated. Please specify the 'defaultmode' parameter for '" << part_name << "' instead." << std::endl; break; From 7958be536935a4df60f53322f45ff605c135b528 Mon Sep 17 00:00:00 2001 From: "Nordmann Arne (CR/ADT3)" Date: Fri, 18 Jun 2021 13:52:18 +0200 Subject: [PATCH 09/11] Fixed default modes in example files Signed-off-by: Nordmann Arne (CR/ADT3) --- system_modes_examples/example_modes.yaml | 3 +- .../example_modes_with_namespace.yaml | 12 +++-- .../example_system_start_drive_base.launch.py | 4 +- .../launch/example_system_started.launch.py | 2 +- .../example_system_with_namespace.launch.py | 51 +++++++++++++++++++ 5 files changed, 64 insertions(+), 8 deletions(-) create mode 100644 system_modes_examples/launch/example_system_with_namespace.launch.py diff --git a/system_modes_examples/example_modes.yaml b/system_modes_examples/example_modes.yaml index 14484f5..7daf45a 100644 --- a/system_modes_examples/example_modes.yaml +++ b/system_modes_examples/example_modes.yaml @@ -40,6 +40,7 @@ actuation: manipulator: ros__parameters: type: node + defaultmode: DEFAULT modes: WEAK: ros__parameters: @@ -47,7 +48,7 @@ manipulator: STRONG: ros__parameters: max_torque: 0.2 - __DEFAULT__: + DEFAULT: ros__parameters: max_torque: 0.1 diff --git a/system_modes_examples/example_modes_with_namespace.yaml b/system_modes_examples/example_modes_with_namespace.yaml index a98f21b..f0a0a95 100644 --- a/system_modes_examples/example_modes_with_namespace.yaml +++ b/system_modes_examples/example_modes_with_namespace.yaml @@ -4,12 +4,13 @@ actuation: ros__parameters: type: system + defaultmode: DEFAULT parts: drive_base left/manipulator right/manipulator modes: - __DEFAULT__: + DEFAULT: drive_base: active left/manipulator: inactive right/manipulator: active.WEAK @@ -25,8 +26,9 @@ actuation: left/manipulator: ros__parameters: type: node + defaultmode: DEFAULT modes: - __DEFAULT__: + DEFAULT: ros__parameters: max_torque: 0.1 WEAK: @@ -39,8 +41,9 @@ left/manipulator: drive_base: ros__parameters: type: node + defaultmode: NORMAL modes: - __DEFAULT__: + NORMAL: ros__parameters: max_speed: 0.1 controller: PID @@ -56,8 +59,9 @@ drive_base: right/manipulator: ros__parameters: type: node + defaultmode: DEFAULT modes: - __DEFAULT__: + DEFAULT: ros__parameters: max_torque: 0.11 WEAK: diff --git a/system_modes_examples/launch/example_system_start_drive_base.launch.py b/system_modes_examples/launch/example_system_start_drive_base.launch.py index db65c6b..460a0ec 100644 --- a/system_modes_examples/launch/example_system_start_drive_base.launch.py +++ b/system_modes_examples/launch/example_system_start_drive_base.launch.py @@ -73,7 +73,7 @@ def generate_launch_description(): drive_base_change_mode_to_DEFAULT = launch.actions.EmitEvent( event=launch_system_modes.events.ChangeMode( system_part_matcher=launch.events.matchers.matches_action(drive_base), - mode_name='__DEFAULT__', + mode_name='NORMAL', )) drive_base_change_mode_to_FAST = launch.actions.EmitEvent( @@ -98,7 +98,7 @@ def generate_launch_description(): on_DEFAULT_mode = launch.actions.RegisterEventHandler( launch_system_modes.event_handlers.OnModeChanged( target_system_part=drive_base, - goal_mode='__DEFAULT__', + goal_mode='NORMAL', entities=[drive_base_change_mode_to_FAST])) description = launch.LaunchDescription() diff --git a/system_modes_examples/launch/example_system_started.launch.py b/system_modes_examples/launch/example_system_started.launch.py index 37f584b..66aaa2b 100644 --- a/system_modes_examples/launch/example_system_started.launch.py +++ b/system_modes_examples/launch/example_system_started.launch.py @@ -84,7 +84,7 @@ def generate_launch_description(): on_DEFAULT_mode = launch.actions.RegisterEventHandler( launch_system_modes.event_handlers.OnModeChanged( target_system_part=actuation, - goal_mode='__DEFAULT__', + goal_mode='DEFAULT', entities=[actuation_change_mode_to_PERFORMANCE])) description = launch.LaunchDescription() diff --git a/system_modes_examples/launch/example_system_with_namespace.launch.py b/system_modes_examples/launch/example_system_with_namespace.launch.py new file mode 100644 index 0000000..c438903 --- /dev/null +++ b/system_modes_examples/launch/example_system_with_namespace.launch.py @@ -0,0 +1,51 @@ +# Copyright (c) 2020 - for information on the respective copyright owner +# see the NOTICE file and/or the repository https://github.com/micro-ROS/system_modes. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import ament_index_python.packages + +import launch +import launch.actions +import launch.launch_description_sources +import launch.substitutions + + +def generate_launch_description(): + modelfile = (ament_index_python.packages.get_package_share_directory('system_modes_examples') + + '/example_modes.yaml') + + mode_manager = launch.actions.IncludeLaunchDescription( + launch.launch_description_sources.PythonLaunchDescriptionSource( + ament_index_python.packages.get_package_share_directory( + 'system_modes') + '/launch/mode_manager.launch.py'), + launch_arguments={'modelfile': modelfile}.items()) + + drive_base = launch.actions.IncludeLaunchDescription( + launch.launch_description_sources.PythonLaunchDescriptionSource( + ament_index_python.packages.get_package_share_directory( + 'system_modes_examples') + '/launch/drive_base.launch.py'), + launch_arguments={'modelfile': modelfile}.items()) + + manipulator = launch.actions.IncludeLaunchDescription( + launch.launch_description_sources.PythonLaunchDescriptionSource( + ament_index_python.packages.get_package_share_directory( + 'system_modes_examples') + '/launch/manipulator.launch.py'), + launch_arguments={'modelfile': modelfile}.items()) + + description = launch.LaunchDescription() + description.add_action(mode_manager) + description.add_action(drive_base) + description.add_action(manipulator) + + return description From 483eebf47cd83f84c7b32063b712665000c70639 Mon Sep 17 00:00:00 2001 From: "Nordmann Arne (CR/ADT3)" Date: Fri, 18 Jun 2021 14:44:01 +0200 Subject: [PATCH 10/11] Make defaultmodeptr const, more testing Signed-off-by: Nordmann Arne (CR/ADT3) --- system_modes/include/system_modes/mode.hpp | 2 +- system_modes/test/test_default_mode.cpp | 4 ++-- system_modes/test/test_mode.cpp | 11 +++++++++-- 3 files changed, 12 insertions(+), 5 deletions(-) diff --git a/system_modes/include/system_modes/mode.hpp b/system_modes/include/system_modes/mode.hpp index ecef6bf..787be3d 100644 --- a/system_modes/include/system_modes/mode.hpp +++ b/system_modes/include/system_modes/mode.hpp @@ -38,7 +38,7 @@ class Mode; using ModeBasePtr = std::shared_ptr; using ModeConstPtr = std::shared_ptr; using ModePtr = std::shared_ptr; -using DefaultModePtr = std::shared_ptr; +using DefaultModePtr = std::shared_ptr; using ModeMap = std::map; diff --git a/system_modes/test/test_default_mode.cpp b/system_modes/test/test_default_mode.cpp index 05525c1..7477d22 100644 --- a/system_modes/test/test_default_mode.cpp +++ b/system_modes/test/test_default_mode.cpp @@ -40,14 +40,14 @@ class TestDefaultMode : public ::testing::Test void SetUp() { - default_mode = DefaultModePtr(new DefaultMode("custom name")); + default_mode = std::make_shared("custom name"); } void TearDown() { } - DefaultModePtr default_mode; + std::shared_ptr default_mode; }; /* diff --git a/system_modes/test/test_mode.cpp b/system_modes/test/test_mode.cpp index e331b05..bb69974 100644 --- a/system_modes/test/test_mode.cpp +++ b/system_modes/test/test_mode.cpp @@ -22,12 +22,14 @@ #include #include "system_modes/mode.hpp" +#include "system_modes/mode_impl.hpp" using std::string; using std::vector; using rclcpp::Parameter; using system_modes::Mode; +using system_modes::DEFAULT_MODE; using system_modes::DefaultMode; using system_modes::DefaultModePtr; @@ -41,7 +43,7 @@ class TestMode : public ::testing::Test void SetUp() { - default_mode = DefaultModePtr(new DefaultMode()); + default_mode = std::make_shared(); Parameter param1("foo", "bar"); Parameter param2("fubar", 0.123); @@ -57,7 +59,7 @@ class TestMode : public ::testing::Test { } - DefaultModePtr default_mode; + std::shared_ptr default_mode; }; /* @@ -68,6 +70,11 @@ TEST_F(TestMode, construction_and_destruction) { vector parameter_names({"foo", "fubar"}); DefaultModePtr default_mode_null; + DefaultModePtr default_mode_no_custom_name(new DefaultMode()); + EXPECT_EQ(DEFAULT_MODE, default_mode_no_custom_name->get_name()); + DefaultModePtr default_mode_custom_name(new DefaultMode("custom")); + EXPECT_EQ("custom", default_mode_custom_name->get_name()); + Mode * mode; EXPECT_THROW( From 15640d37fa74fb7c67ace456dbc44edd407228f2 Mon Sep 17 00:00:00 2001 From: "Nordmann Arne (CR/ADT3)" Date: Fri, 18 Jun 2021 14:44:43 +0200 Subject: [PATCH 11/11] Refactored parsing of SMH file * Less code duplication, less code Signed-off-by: Nordmann Arne (CR/ADT3) --- .../include/system_modes/mode_inference.hpp | 22 +- .../src/system_modes/mode_inference.cpp | 400 +++++++++--------- 2 files changed, 228 insertions(+), 194 deletions(-) diff --git a/system_modes/include/system_modes/mode_inference.hpp b/system_modes/include/system_modes/mode_inference.hpp index 03fb460..9249ad6 100644 --- a/system_modes/include/system_modes/mode_inference.hpp +++ b/system_modes/include/system_modes/mode_inference.hpp @@ -87,12 +87,28 @@ class ModeInference virtual ~ModeInference() = default; protected: - virtual bool matching_parameters(const rclcpp::Parameter &, const rclcpp::Parameter &) const; + virtual void parse_smh_file(const std::string & model_path); - virtual void read_modes_from_model(const std::string & model_path); virtual void add_param_to_mode(ModeBasePtr, const rclcpp::Parameter &); + virtual bool matching_parameters(const rclcpp::Parameter &, const rclcpp::Parameter &) const; private: + /** + * Parse SMH and find all parts (systems, nodes) and all their modes + * + * Post condition: We know all + * * Parts (systems, nodes) + * * Default modes of all parts + * * Non-default modes of all parts + * Members nodes_, systems_, default_modes_, and modes_ are populated. + */ + virtual void find_parts_and_modes(const rclcpp::ParameterMap & smh); + virtual void parse_mode( + const rclcpp::ParameterMap & smh, + const std::string & part_name, + const std::string & mode_name, + bool is_default_mode = false); + StatesMap nodes_, nodes_target_, nodes_cache_; StatesMap systems_, systems_target_, systems_cache_; std::map modes_; @@ -102,7 +118,7 @@ class ModeInference mutable std::shared_timed_mutex nodes_mutex_, systems_mutex_, modes_mutex_, default_modes_mutex_, - parts_mutex_, param_mutex_; + param_mutex_; mutable std::shared_timed_mutex nodes_target_mutex_, systems_target_mutex_; mutable std::shared_timed_mutex diff --git a/system_modes/src/system_modes/mode_inference.cpp b/system_modes/src/system_modes/mode_inference.cpp index a8afbd9..ad4f07d 100644 --- a/system_modes/src/system_modes/mode_inference.cpp +++ b/system_modes/src/system_modes/mode_inference.cpp @@ -46,9 +46,9 @@ ModeInference::ModeInference(const string & model_path) systems_(), systems_target_(), modes_(), nodes_mutex_(), systems_mutex_(), modes_mutex_(), default_modes_mutex_(), - parts_mutex_(), nodes_target_mutex_(), systems_target_mutex_() + nodes_target_mutex_(), systems_target_mutex_() { - this->read_modes_from_model(model_path); + this->parse_smh_file(model_path); } void @@ -104,7 +104,7 @@ ModeInference::update_param(const string & node, Parameter & param) // TODO(anordman): Think about, how to handle the leading slash - never? always? string nodename = node; const auto start = nodename.find_first_not_of("/"); - if (start != std::string::npos) { + if (start != string::npos) { nodename = nodename.substr(start); } @@ -461,17 +461,17 @@ ModeInference::get_default_mode(const string & part) const return modes_.at(part).at(get_default_mode_name(part)); } -const std::string +const string ModeInference::get_default_mode_name(const string & part) const { std::shared_lock mlock(this->default_modes_mutex_); return default_modes_.at(part); } -std::vector -ModeInference::get_available_modes(const std::string & part) const +std::vector +ModeInference::get_available_modes(const string & part) const { - std::vector modes; + std::vector modes; for (auto mode : this->modes_.at(part)) { modes.push_back(mode.first); } @@ -479,198 +479,40 @@ ModeInference::get_available_modes(const std::string & part) const } void -ModeInference::read_modes_from_model(const string & model_path) +ModeInference::parse_smh_file(const string & model_path) { - std::unique_lock nlock(this->nodes_mutex_); - std::unique_lock slock(this->systems_mutex_); - std::unique_lock mlock(this->modes_mutex_); - std::unique_lock dmlock(this->default_modes_mutex_); - std::unique_lock plock(this->parts_mutex_); - + // Parse yaml to parameter map 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 modes and parameters from: " + model_path); } - - rclcpp::ParameterMap param_map = rclcpp::parameter_map_from(yaml_params); + ParameterMap smh = rclcpp::parameter_map_from(yaml_params); rcl_yaml_node_struct_fini(yaml_params); - ParameterMap::iterator it, part_start_it; - string old_part_name; - for (it = param_map.begin(); it != param_map.end(); it++) { - string part_name(it->first.substr(1)); - - auto itm = this->modes_.find(part_name); - if (itm == this->modes_.end()) { - this->modes_.emplace(part_name, ModeMap()); - } - - // First, look for a default mode - string default_mode_name; - part_start_it = it; - for (auto & param : it->second) { - if (param.get_name().compare("defaultmode") == 0) { - default_mode_name = param.value_to_string(); - break; - } else 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); - if (foundmr != string::npos) { - mode_name = param.get_name().substr(foundm + 6, foundmr - foundm - 6); - } else { - continue; - } - } else { - continue; - } + // Find all parts (systems, nodes) and their modes + find_parts_and_modes(smh); - /** - * Check for mode with name '__DEFAULT__' - * - * Necessary for backward-compatibility, will get deprecated soon, see - * https://github.com/micro-ROS/system_modes/issues/69 - */ - if (mode_name.compare(DEFAULT_MODE) == 0) { - default_mode_name = DEFAULT_MODE; - std::cerr << "WARNING: Specifying the default mode just by its name '__DEFAULT__'" << - " is deprecated. Please specify the 'defaultmode' parameter for '" << - part_name << "' instead." << std::endl; - break; - } - } - } - if (default_mode_name.empty()) { - throw std::runtime_error("Failed to find default mode for " + part_name); - } - - default_modes_[part_name] = default_mode_name; - it = part_start_it; - - // Build up default mode - DefaultModePtr default_mode = std::make_shared(default_mode_name); - it = part_start_it; - for (auto & param : it->second) { - 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); - if (foundmr != string::npos) { - mode_name = param.get_name().substr(foundm + 6, foundmr - foundm - 6); - } else { - continue; - } - } else { - continue; - } - if (mode_name.compare(default_mode_name) != 0) { - continue; - } - - // valid parameter, add to mode map - std::size_t found = param.get_name().find("ros__parameters"); - if (found != string::npos) { - this->add_param_to_mode(default_mode, param); - } else { - // part mode - string part_part = param.get_name(); - std::size_t foundr = param.get_name().rfind("."); - if (foundr != string::npos) { - part_part = param.get_name().substr(foundr + 1); - } - - string state(param.value_to_string()); - string smode; - foundr = param.value_to_string().rfind("."); - if (foundr != string::npos) { - state = param.value_to_string().substr(0, foundr); - smode = param.value_to_string().substr(foundr + 1); - } - - default_mode->set_part_mode(part_part, StateAndMode(state_id_(state), smode)); - } - this->modes_[part_name].emplace(default_mode_name, default_mode); + // Parse all modes - start with default modes... + std::map::const_iterator mit; + for (mit = default_modes_.begin(); mit != default_modes_.end(); mit++) { + parse_mode(smh, mit->first, mit->second, true); + } - } else { - if (param.value_to_string().compare("node") != 0) { - this->systems_.emplace( - part_name, - StateAndMode(State::PRIMARY_STATE_UNKNOWN, "")); - } else { - this->nodes_.emplace( - part_name, - StateAndMode(State::PRIMARY_STATE_UNKNOWN, "")); - } - } + // then non-default mdoes of systems + for (auto system = std::begin(systems_); system != std::end(systems_); ++system) { + ModeMap::const_iterator mit; + auto modes = modes_.at(system->first); + for (mit = modes.begin(); mit != modes.end(); mit++) { + parse_mode(smh, system->first, mit->first); } + } - // Build up non-default modes - ModeBasePtr mode; - it = part_start_it; - for (auto & param : it->second) { - 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); - if (foundmr != string::npos) { - mode_name = param.get_name().substr(foundm + 6, foundmr - foundm - 6); - } else { - continue; - } - } else { - continue; - } - if (mode_name.compare(default_mode_name) == 0) { - continue; - } - - // valid parameter, add to mode map - if (!mode || mode->get_name().compare(mode_name) != 0) { - mode = std::make_shared(mode_name, default_mode); - } - std::size_t found = param.get_name().find("ros__parameters"); - if (found != string::npos) { - this->add_param_to_mode(mode, param); - } else { - // part mode - string part_part = param.get_name(); - std::size_t foundr = param.get_name().rfind("."); - if (foundr != string::npos) { - part_part = param.get_name().substr(foundr + 1); - } - - string state(param.value_to_string()); - string smode; - foundr = param.value_to_string().rfind("."); - if (foundr != string::npos) { - state = param.value_to_string().substr(0, foundr); - smode = param.value_to_string().substr(foundr + 1); - } - - mode->set_part_mode(part_part, StateAndMode(state_id_(state), smode)); - } - this->modes_[part_name].emplace(mode->get_name(), mode); - - } else { - if (param.value_to_string().compare("node") != 0) { - this->systems_.emplace( - part_name, - StateAndMode(State::PRIMARY_STATE_UNKNOWN, "")); - } else { - this->nodes_.emplace( - part_name, - StateAndMode(State::PRIMARY_STATE_UNKNOWN, "")); - } - } + // then non-default modes of nodes + for (auto node = std::begin(nodes_); node != std::end(nodes_); ++node) { + ModeMap::const_iterator mit; + auto modes = modes_.at(node->first); + for (mit = modes.begin(); mit != modes.end(); mit++) { + parse_mode(smh, node->first, mit->first); } } } @@ -679,9 +521,9 @@ void ModeInference::add_param_to_mode(ModeBasePtr mode, const Parameter & param) { // TODO(anordman): All of this has to be easier - std::string param_name = param.get_name(); + string param_name = param.get_name(); std::size_t foundr = param.get_name().rfind("ros__parameters"); - if (foundr != std::string::npos) { + if (foundr != string::npos) { param_name = param_name.substr(foundr + strlen("ros__parameters") + 1); } auto paramMsg = param.to_parameter_msg(); @@ -774,7 +616,7 @@ ModeInference::matching_parameters(const Parameter & target, const Parameter & a bool ModeInference::matching_modes( - const std::string & part, + const string & part, const StateAndMode & a, const StateAndMode & b) const { @@ -874,4 +716,180 @@ ModeInference::get_deviation() return deviation; } +void +ModeInference::find_parts_and_modes(const ParameterMap & smh) +{ + std::unique_lock nlock(this->nodes_mutex_); + std::unique_lock slock(this->systems_mutex_); + std::unique_lock mlock(this->modes_mutex_); + std::unique_lock dmlock(this->default_modes_mutex_); + + ParameterMap::const_iterator it, part_start_it; + string part_name_tmp; + + for (it = smh.begin(); it != smh.end(); it++) { + string part_name(it->first.substr(1)); + + auto itm = modes_.find(part_name); + if (itm == modes_.end()) { + modes_.emplace(part_name, ModeMap()); + } + + // First, find default mode for each part + string default_mode_name; + part_start_it = it; + for (auto & param : it->second) { + if (param.get_name().compare("defaultmode") == 0) { + default_mode_name = param.value_to_string(); + break; + } else 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); + if (foundmr != string::npos) { + mode_name = param.get_name().substr(foundm + 6, foundmr - foundm - 6); + } else { + continue; + } + } else { + continue; + } + + /** + * Check for mode with name '__DEFAULT__' + * + * Necessary for backward-compatibility, will get deprecated soon, see + * https://github.com/micro-ROS/system_modes/issues/69 + */ + if (mode_name.compare(DEFAULT_MODE) == 0) { + default_mode_name = DEFAULT_MODE; + std::cerr << "WARNING: Specifying the default mode just by its name '__DEFAULT__'" << + " is deprecated. Please specify the 'defaultmode' parameter for '" << + part_name << "' instead." << std::endl; + break; + } + } + } + if (default_mode_name.empty()) { + throw std::runtime_error("Failed to find default mode for " + part_name); + } + default_modes_[part_name] = default_mode_name; + + // Find non-default modes + ModeBasePtr mode; + it = part_start_it; + for (auto & param : it->second) { + 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); + if (foundmr != string::npos) { + mode_name = param.get_name().substr(foundm + 6, foundmr - foundm - 6); + } else { + continue; + } + } else { + continue; + } + if (!mode_name.empty() && + mode_name.compare(default_mode_name) != 0 && + modes_.find(mode_name) == modes_.end()) + { + modes_[part_name].emplace(mode_name, mode); + } + } else { + if (param.value_to_string().compare("node") != 0) { + systems_.emplace( + part_name, + StateAndMode(State::PRIMARY_STATE_UNKNOWN, "")); + } else { + nodes_.emplace( + part_name, + StateAndMode(State::PRIMARY_STATE_UNKNOWN, "")); + } + } + } + } +} + +void +ModeInference::parse_mode( + const ParameterMap & smh, + const string & part_name, + const string & mode_name, + bool is_default_mode) +{ + ModeBasePtr mode; + if (is_default_mode) { + mode = std::make_shared(mode_name); + } else { + mode = std::make_shared( + mode_name, + std::static_pointer_cast(get_default_mode(part_name))); + } + + for (ParameterMap::const_iterator it = smh.begin(); it != smh.end(); it++) { + if (it->first.substr(1).compare(part_name) != 0) { + continue; + } + + // First, look for a default mode + for (auto & param : it->second) { + if (param.get_name().compare("defaultmode") == 0 || + param.get_name().compare("type") == 0) + { + continue; + } + + // Mode of this parameter + std::size_t foundm = param.get_name().find("modes."); + if (foundm != string::npos) { + std::size_t foundmr = param.get_name().find(".", 6); + if (foundmr != string::npos) { + auto name = param.get_name().substr(foundm + 6, foundmr - foundm - 6); + if (name.compare(mode_name) != 0) { + continue; + } + } else { + continue; + } + } else { + continue; + } + + // valid parameter, add to mode + std::size_t found = param.get_name().find("ros__parameters"); + if (found != string::npos) { + this->add_param_to_mode(mode, param); + } else { + // part mode + string part_part = param.get_name(); + std::size_t foundr = param.get_name().rfind("."); + if (foundr != string::npos) { + part_part = param.get_name().substr(foundr + 1); + } + + string state(param.value_to_string()); + string part_mode; + foundr = param.value_to_string().rfind("."); + if (foundr != string::npos) { + state = param.value_to_string().substr(0, foundr); + part_mode = param.value_to_string().substr(foundr + 1); + } + + mode->set_part_mode(part_part, StateAndMode(state_id_(state), part_mode)); + } + } + } + + std::unique_lock mlock(this->modes_mutex_); + modes_[part_name][mode_name] = mode; +} + } // namespace system_modes