Skip to content

Commit

Permalink
Adds inference of transitions to mode inference
Browse files Browse the repository at this point in the history 
* Adds caching of latest reported states/modes of all systems
* Adds infer_transitions method that compares currently inferred
states/modes to cached ones and returns transitions, if detected

#42

Signed-off-by: Arne Nordmann <arne.nordmann@de.bosch.com>
  • Loading branch information
@norro
norro committed Sep 30, 2020
1 parent ff8f91d commit 5681534
Show file tree
Hide file tree
Showing 2 changed files with 93 additions and 18 deletions.
28 changes: 19 additions & 9 deletions system_modes/include/system_modes/mode_inference.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -54,11 +54,21 @@ class ModeInference
virtual void update_target(const std::string &, StateAndMode);

virtual StateAndMode get(const std::string & part) const;
virtual StateAndMode get_or_infer(const std::string & part) const;

virtual StateAndMode infer(const std::string & part) const;
virtual StateAndMode infer_system(const std::string & part) const;
virtual StateAndMode infer_node(const std::string & part) const;
virtual StateAndMode get_or_infer(const std::string & part);

virtual StateAndMode infer(const std::string & part);
virtual StateAndMode infer_node(const std::string & part);
virtual StateAndMode infer_system(const std::string & part);

/**
* Infers latest transitions of systems
*
* Returns map of last inferred transitions of systems into new states or
* new modes. State transitions of nodes don't have to be inferred, as
* nodes publish their state transitions. For nodes, we only need to infer
* mode transitions.
*/
virtual Deviation infer_transitions();

virtual StateAndMode get_target(const std::string & part) const;
virtual ModeConstPtr get_mode(const std::string & part, const std::string & mode) const;
Expand All @@ -72,9 +82,8 @@ class ModeInference
virtual void add_param_to_mode(ModeBasePtr, const rclcpp::Parameter &);

private:
StatesMap nodes_, nodes_target_;
StatesMap systems_, systems_target_;
Deviation systems_transitions_;
StatesMap nodes_, nodes_target_, nodes_cache_;
StatesMap systems_, systems_target_, systems_cache_;

std::map<std::string, ModeMap> modes_;
ParametersMap parameters_;
Expand All @@ -85,7 +94,8 @@ class ModeInference
param_mutex_;
mutable std::shared_timed_mutex
nodes_target_mutex_, systems_target_mutex_;
mutable std::shared_timed_mutex systems_transitions_mutex_;
mutable std::shared_timed_mutex
nodes_cache_mutex_, systems_cache_mutex_;
};

} // namespace system_modes
83 changes: 74 additions & 9 deletions system_modes/src/system_modes/mode_inference.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -42,13 +42,11 @@ namespace system_modes
{

ModeInference::ModeInference(const string & model_path)
: nodes_(), nodes_target_(),
systems_(), systems_target_(),
systems_transitions_(),
: nodes_(), nodes_target_(), nodes_cache_(),
systems_(), systems_target_(), systems_cache_(),
modes_(),
nodes_mutex_(), systems_mutex_(), modes_mutex_(), parts_mutex_(),
nodes_target_mutex_(), systems_target_mutex_(),
systems_transitions_mutex_()
nodes_target_mutex_(), systems_target_mutex_()
{
this->read_modes_from_model(model_path);
}
Expand Down Expand Up @@ -167,7 +165,7 @@ ModeInference::get(const string & part) const
}

StateAndMode
ModeInference::infer(const string & part) const
ModeInference::infer(const string & part)
{
std::shared_lock<shared_mutex> slock(this->systems_mutex_);
std::shared_lock<shared_mutex> nlock(this->nodes_mutex_);
Expand All @@ -189,7 +187,7 @@ ModeInference::infer(const string & part) const
}

StateAndMode
ModeInference::infer_system(const string & part) const
ModeInference::infer_system(const string & part)
{
unsigned int state = 0; // unknown
string mode = "";
Expand All @@ -211,6 +209,7 @@ ModeInference::infer_system(const string & part) const

// error-processing?
if (stateAndMode.state == State::TRANSITION_STATE_ERRORPROCESSING) {
this->systems_[part] = StateAndMode(State::TRANSITION_STATE_ERRORPROCESSING, "");
return StateAndMode(State::TRANSITION_STATE_ERRORPROCESSING, "");
}

Expand All @@ -224,6 +223,7 @@ ModeInference::infer_system(const string & part) const
"', inference failed.");
}
}
this->systems_[part] = StateAndMode(state, "");
return StateAndMode(state, "");
}

Expand All @@ -241,13 +241,15 @@ ModeInference::infer_system(const string & part) const
// be in error-processing (by dont-care) and the current entity is requested
// 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, "");
}

// If at least one part is not 'inactive', yet => we are still deactivating
if (stateAndMode.state != State::PRIMARY_STATE_INACTIVE &&
stateAndMode.state != State::PRIMARY_STATE_UNCONFIGURED)
{
this->systems_[part] = StateAndMode(State::TRANSITION_STATE_DEACTIVATING, "");
return StateAndMode(State::TRANSITION_STATE_DEACTIVATING, "");
}
}
Expand All @@ -266,6 +268,7 @@ ModeInference::infer_system(const string & part) const

// 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, "");
}

Expand All @@ -286,6 +289,7 @@ ModeInference::infer_system(const string & part) const
}
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);
}
}
Expand All @@ -299,6 +303,7 @@ ModeInference::infer_system(const string & part) const

// 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, "");
}

Expand All @@ -313,18 +318,20 @@ ModeInference::infer_system(const string & part) const
}
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, "");
}

throw std::runtime_error("Inference failed.");
}

StateAndMode
ModeInference::infer_node(const string & part) const
ModeInference::infer_node(const string & part)
{
std::shared_lock<shared_mutex> mlock(this->modes_mutex_);
std::shared_lock<shared_mutex> prlock(this->param_mutex_);
Expand Down Expand Up @@ -406,7 +413,7 @@ ModeInference::infer_node(const string & part) const
}

StateAndMode
ModeInference::get_or_infer(const string & part) const
ModeInference::get_or_infer(const string & part)
{
StateAndMode stateAndMode;
try {
Expand Down Expand Up @@ -654,4 +661,62 @@ ModeInference::matching_parameters(const Parameter & target, const Parameter & a
return false;
}

Deviation
ModeInference::infer_transitions()
{
Deviation transitions;

{
std::unique_lock<shared_mutex> nlock(this->nodes_mutex_);
std::unique_lock<shared_mutex> nclock(this->nodes_cache_mutex_);
StatesMap::iterator it;
for (it = nodes_.begin(); it != nodes_.end(); it++) {
if (nodes_cache_.count(it->first) < 1) {
nodes_cache_[it->first] = nodes_.at(it->first);
}
try {
auto sm_current = infer_node(it->first);
if (sm_current.state == State::PRIMARY_STATE_ACTIVE &&
sm_current.mode.compare(nodes_cache_.at(it->first).mode) != 0)
{
// Detected a mode transition
transitions[it->first] = make_pair(nodes_cache_.at(it->first), sm_current);

// Cache newly inferred state and mode for next inference of transitions
nodes_cache_[it->first] = sm_current;
}
} catch (...) {
// inference may not work due to too little information
continue;
}
}
}

{
std::unique_lock<shared_mutex> slock(this->systems_mutex_);
std::unique_lock<shared_mutex> sclock(this->systems_cache_mutex_);
StatesMap::iterator it;
for (it = systems_.begin(); it != systems_.end(); it++) {
if (systems_cache_.count(it->first) < 1) {
systems_cache_[it->first] = systems_.at(it->first);
}
try {
auto sm_current = infer_system(it->first);
if (sm_current != systems_cache_[it->first]) {
// Detected a transition
transitions[it->first] = make_pair(systems_cache_.at(it->first), sm_current);

// Cache newly inferred state and mode for next inference of transitions
systems_cache_[it->first] = sm_current;
}
} catch (...) {
// inference may not work due to too little information
continue;
}
}
}

return transitions;
}

} // namespace system_modes

0 comments on commit 5681534

Please sign in to comment.