From b5fb2e45149a43ef00d04cf4a94ab9624e9106fb Mon Sep 17 00:00:00 2001 From: Arne Nordmann Date: Wed, 23 Sep 2020 13:02:05 +0200 Subject: [PATCH 1/4] ModeEvent now includes start mode (of transition) This is now closer to the according TransitionEvent of lifecycle_msgs, which includes the start_state and goal_state of a transition. Signed-off-by: Arne Nordmann --- system_modes/msg/ModeEvent.msg | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/system_modes/msg/ModeEvent.msg b/system_modes/msg/ModeEvent.msg index 8c19457..47b610d 100644 --- a/system_modes/msg/ModeEvent.msg +++ b/system_modes/msg/ModeEvent.msg @@ -1,2 +1,8 @@ +# The time point at which this event occurred. uint64 timestamp + +# The starting mode from which this event transitioned. +Mode start_mode + +# The end mode of this transition event. Mode goal_mode From 8d1ffd657f2e48c58e9c48dcc9334c8a4cfb6fc3 Mon Sep 17 00:00:00 2001 From: Arne Nordmann Date: Wed, 23 Sep 2020 15:04:16 +0200 Subject: [PATCH 2/4] Adds inference of transitions to mode inference * 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 https://github.com/micro-ROS/system_modes/issues/42 Signed-off-by: Arne Nordmann --- .../include/system_modes/mode_inference.hpp | 28 +++++-- .../src/system_modes/mode_inference.cpp | 83 +++++++++++++++++-- 2 files changed, 93 insertions(+), 18 deletions(-) diff --git a/system_modes/include/system_modes/mode_inference.hpp b/system_modes/include/system_modes/mode_inference.hpp index 9f8e92d..49ce3ec 100644 --- a/system_modes/include/system_modes/mode_inference.hpp +++ b/system_modes/include/system_modes/mode_inference.hpp @@ -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; @@ -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 modes_; ParametersMap parameters_; @@ -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 diff --git a/system_modes/src/system_modes/mode_inference.cpp b/system_modes/src/system_modes/mode_inference.cpp index f4f130e..ec2ef91 100644 --- a/system_modes/src/system_modes/mode_inference.cpp +++ b/system_modes/src/system_modes/mode_inference.cpp @@ -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); } @@ -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 slock(this->systems_mutex_); std::shared_lock nlock(this->nodes_mutex_); @@ -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 = ""; @@ -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, ""); } @@ -224,6 +223,7 @@ ModeInference::infer_system(const string & part) const "', inference failed."); } } + this->systems_[part] = StateAndMode(state, ""); return StateAndMode(state, ""); } @@ -241,6 +241,7 @@ 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, ""); } @@ -248,6 +249,7 @@ ModeInference::infer_system(const string & part) const 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, ""); } } @@ -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, ""); } @@ -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); } } @@ -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, ""); } @@ -313,10 +318,12 @@ 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, ""); } @@ -324,7 +331,7 @@ ModeInference::infer_system(const string & part) const } StateAndMode -ModeInference::infer_node(const string & part) const +ModeInference::infer_node(const string & part) { std::shared_lock mlock(this->modes_mutex_); std::shared_lock prlock(this->param_mutex_); @@ -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 { @@ -654,4 +661,62 @@ ModeInference::matching_parameters(const Parameter & target, const Parameter & a return false; } +Deviation +ModeInference::infer_transitions() +{ + Deviation transitions; + + { + std::unique_lock nlock(this->nodes_mutex_); + std::unique_lock 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 slock(this->systems_mutex_); + std::unique_lock 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 From 9800ac17b50c65f6f67351bf7268ea9f106572a2 Mon Sep 17 00:00:00 2001 From: Arne Nordmann Date: Wed, 23 Sep 2020 15:05:37 +0200 Subject: [PATCH 3/4] Tests for transition inference Signed-off-by: Arne Nordmann --- system_modes/test/test_mode_inference.cpp | 53 ++++++++++++++++++++++- 1 file changed, 52 insertions(+), 1 deletion(-) diff --git a/system_modes/test/test_mode_inference.cpp b/system_modes/test/test_mode_inference.cpp index 4321617..f682519 100644 --- a/system_modes/test/test_mode_inference.cpp +++ b/system_modes/test/test_mode_inference.cpp @@ -31,6 +31,8 @@ using rclcpp::Parameter; using system_modes::ModeInference; using system_modes::StateAndMode; +using system_modes::Deviation; + using lifecycle_msgs::msg::State; /* @@ -141,7 +143,8 @@ TEST(TestModeInference, inference) { inference.update_target("system", active_default); inference.update("part0", inactive); inference.update("part1", active_default); - printf("-----------------\n"); + + // System inference StateAndMode sm = inference.get_or_infer("system"); EXPECT_EQ(State::PRIMARY_STATE_ACTIVE, sm.state); EXPECT_EQ("__DEFAULT__", sm.mode); @@ -164,3 +167,51 @@ TEST(TestModeInference, inference) { // System inference EXPECT_EQ(State::TRANSITION_STATE_ACTIVATING, inference.infer("system").state); } + +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 inactive(State::PRIMARY_STATE_INACTIVE, ""); + inference.update_target("system", active_default); + inference.update("part0", inactive); + inference.update("part1", active_default); + + // Expect to have one initial transition + EXPECT_EQ(1u, inference.infer_transitions().size()); + EXPECT_EQ(0u, inference.infer_transitions().size()); + + // Expect to have one transition of part0 + Parameter foo("foo", 0.2); + Parameter bar("bar", "DBG"); + inference.update_param("part1", foo); + inference.update_param("part1", bar); + 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(State::PRIMARY_STATE_ACTIVE, transitions["part1"].second.state); + EXPECT_EQ("AAA", transitions["part1"].second.mode); + + // Expect to have cleared transitions + EXPECT_EQ(0u, inference.infer_transitions().size()); + + // Expect two have two transitions: part1 and system + 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); + transitions = inference.infer_transitions(); + EXPECT_EQ(2u, transitions.size()); + EXPECT_EQ(State::PRIMARY_STATE_INACTIVE, transitions["part0"].first.state); + 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(State::TRANSITION_STATE_ACTIVATING, transitions["system"].second.state); + + // Expect to have cleared transitions + EXPECT_EQ(0u, inference.infer_transitions().size()); +} From 2ba0ee8edab41727d652978e31db68fe71185871 Mon Sep 17 00:00:00 2001 From: Arne Nordmann Date: Tue, 29 Sep 2020 14:54:41 +0200 Subject: [PATCH 4/4] Mode manager publishs inferred transitions Mode manager is now publishing transitions that are inferred by the mode inference, i.e. state and mode transitions of systems, mode transitions of nodes. State transitions of nodes don't have to be published as lifecycle nodes do this on their own. https://github.com/micro-ROS/system_modes/issues/42 Signed-off-by: Arne Nordmann --- .../include/system_modes/mode_manager.hpp | 13 +- .../src/system_modes/mode_manager.cpp | 128 ++++++++++++------ 2 files changed, 99 insertions(+), 42 deletions(-) diff --git a/system_modes/include/system_modes/mode_manager.hpp b/system_modes/include/system_modes/mode_manager.hpp index ef03578..a01014b 100644 --- a/system_modes/include/system_modes/mode_manager.hpp +++ b/system_modes/include/system_modes/mode_manager.hpp @@ -90,6 +90,8 @@ class ModeManager : public rclcpp::Node const std::string &, const std::string &); + virtual void publish_transitions(); + private: std::shared_ptr mode_inference_; @@ -117,16 +119,23 @@ class ModeManager : public rclcpp::Node std::map param_change_clients_; - // Lifecycle transition request + // Lifecycle transition publisher + std::map::SharedPtr> + transition_pub_; std::map::SharedPtr> state_request_pub_; - // Mode transition request publisher + // Mode transition publisher + std::map::SharedPtr> + mode_transition_pub_; std::map::SharedPtr> mode_request_pub_; // Remember states and modes of the systems std::map current_modes_; + + // Timer to check for and publish recent transitions + rclcpp::TimerBase::SharedPtr transition_timer_; }; } // namespace system_modes diff --git a/system_modes/src/system_modes/mode_manager.cpp b/system_modes/src/system_modes/mode_manager.cpp index bb3577b..97172df 100644 --- a/system_modes/src/system_modes/mode_manager.cpp +++ b/system_modes/src/system_modes/mode_manager.cpp @@ -62,7 +62,8 @@ ModeManager::ModeManager() state_change_srv_(), get_state_srv_(), states_srv_(), mode_change_srv_(), get_mode_srv_(), modes_srv_(), state_change_clients_(), mode_change_clients_(), - state_request_pub_(), mode_request_pub_() + transition_pub_(), state_request_pub_(), + mode_transition_pub_(), mode_request_pub_() { declare_parameter("modelfile", rclcpp::ParameterValue(std::string(""))); std::string model_path = get_parameter("modelfile").as_string(); @@ -90,6 +91,12 @@ ModeManager::ModeManager() RCLCPP_INFO(get_logger(), " - %s/get_mode", node.c_str()); RCLCPP_INFO(get_logger(), " - %s/get_available_modes", node.c_str()); } + + transition_timer_ = this->create_wall_timer( + 1s, + [this]() { + this->publish_transitions(); + }); } std::shared_ptr @@ -101,122 +108,126 @@ ModeManager::inference() void ModeManager::add_system(const std::string & system) { - string service_name; + string topic_name; // Lifecycle services - service_name = system + "/change_state"; + topic_name = system + "/change_state"; std::function, const std::shared_ptr, std::shared_ptr)> state_change_callback = std::bind(&ModeManager::on_change_state, this, system, _2, _3); this->state_change_srv_[system] = this->create_service( - service_name, + topic_name, state_change_callback); - service_name = system + "/get_state"; + topic_name = system + "/get_state"; std::function, const std::shared_ptr, std::shared_ptr)> state_get_callback = std::bind(&ModeManager::on_get_state, this, system, _3); this->get_state_srv_[system] = this->create_service( - service_name, + topic_name, state_get_callback); - service_name = system + "/get_available_states"; + topic_name = system + "/get_available_states"; std::function, const std::shared_ptr, std::shared_ptr)> state_avail_callback = std::bind(&ModeManager::on_get_available_states, this, system, _3); this->states_srv_[system] = this->create_service( - service_name, + topic_name, state_avail_callback); // Mode services - service_name = system + "/change_mode"; + topic_name = system + "/change_mode"; std::function, const std::shared_ptr, std::shared_ptr)> mode_change_callback = std::bind(&ModeManager::on_change_mode, this, system, _2, _3); this->mode_change_srv_[system] = this->create_service( - service_name, + topic_name, mode_change_callback); - service_name = system + "/get_mode"; + topic_name = system + "/get_mode"; std::function, const std::shared_ptr, std::shared_ptr)> mode_get_callback = std::bind(&ModeManager::on_get_mode, this, system, _3); this->get_mode_srv_[system] = this->create_service( - service_name, + topic_name, mode_get_callback); - service_name = system + "/get_available_modes"; + topic_name = system + "/get_available_modes"; std::function, const std::shared_ptr, std::shared_ptr)> mode_avail_callback = std::bind(&ModeManager::on_get_available_modes, this, system, _3); this->modes_srv_[system] = this->create_service( - service_name, + topic_name, mode_avail_callback); // Lifecycle change clients - service_name = system + "/change_state"; - this->state_change_clients_[system] = this->create_client(service_name); + topic_name = system + "/change_state"; + this->state_change_clients_[system] = this->create_client(topic_name); // Mode change clients - service_name = system + "/change_mode"; - this->mode_change_clients_[system] = this->create_client(service_name); - - // State request publisher - service_name = system + "/transition_request_info"; - this->state_request_pub_[system] = this->create_publisher(service_name, 1); - - // Mode request publisher - service_name = system + "/mode_request_info"; - this->mode_request_pub_[system] = this->create_publisher(service_name, 1); + topic_name = system + "/change_mode"; + this->mode_change_clients_[system] = this->create_client(topic_name); + + // Lifecycle transition publisher + topic_name = system + "/transition_event"; + this->transition_pub_[system] = this->create_publisher(topic_name, 1); + topic_name = system + "/transition_request_info"; + this->state_request_pub_[system] = this->create_publisher(topic_name, 1); + + // Mode transition publisher + topic_name = system + "/mode_event"; + this->mode_transition_pub_[system] = this->create_publisher(topic_name, 1); + topic_name = system + "/mode_request_info"; + this->mode_request_pub_[system] = this->create_publisher(topic_name, 1); } void ModeManager::add_node(const std::string & node) { - string service_name; + string topic_name; // Mode services - service_name = node + "/change_mode"; + topic_name = node + "/change_mode"; std::function, const std::shared_ptr, std::shared_ptr)> mode_change_callback = std::bind(&ModeManager::on_change_mode, this, node, _2, _3); this->mode_change_srv_[node] = this->create_service( - service_name, + topic_name, mode_change_callback); - service_name = node + "/get_mode"; + topic_name = node + "/get_mode"; std::function, const std::shared_ptr, std::shared_ptr)> mode_get_callback = std::bind(&ModeManager::on_get_mode, this, node, _3); this->get_mode_srv_[node] = this->create_service( - service_name, + topic_name, mode_get_callback); - service_name = node + "/get_available_modes"; + topic_name = node + "/get_available_modes"; std::function, const std::shared_ptr, std::shared_ptr)> mode_avail_callback = std::bind(&ModeManager::on_get_available_modes, this, node, _3); this->modes_srv_[node] = this->create_service( - service_name, + topic_name, mode_avail_callback); // Lifecycle change clients - service_name = node + "/change_state"; - this->state_change_clients_[node] = this->create_client(service_name); + topic_name = node + "/change_state"; + this->state_change_clients_[node] = this->create_client(topic_name); // Parameter change clients - service_name = node + "/set_parameters_atomically"; + topic_name = node + "/set_parameters_atomically"; this->param_change_clients_[node] = std::make_shared( this->get_node_base_interface(), this->get_node_topics_interface(), @@ -225,12 +236,14 @@ ModeManager::add_node(const std::string & node) node); // State request publisher - service_name = node + "/transition_request_info"; - this->state_request_pub_[node] = this->create_publisher(service_name, 1); + topic_name = node + "/transition_request_info"; + this->state_request_pub_[node] = this->create_publisher(topic_name, 1); // Mode request publisher - service_name = node + "/mode_request_info"; - this->mode_request_pub_[node] = this->create_publisher(service_name, 1); + topic_name = node + "/mode_event"; + this->mode_transition_pub_[node] = this->create_publisher(topic_name, 1); + topic_name = node + "/mode_request_info"; + this->mode_request_pub_[node] = this->create_publisher(topic_name, 1); } void @@ -547,4 +560,39 @@ ModeManager::change_part_mode(const string & node, const string & mode) } } +void +ModeManager::publish_transitions() +{ + auto transitions = mode_inference_->infer_transitions(); + auto systems = mode_inference_->get_systems(); + + for (auto dev = transitions.begin(); dev != transitions.end(); dev++) { + auto part = dev->first; + auto from = dev->second.first; + auto to = dev->second.second; + RCLCPP_DEBUG( + this->get_logger(), + "publish transition of %s from %s to %s.\n", + part.c_str(), from.as_string().c_str(), to.as_string().c_str()); + + if (std::find(systems.begin(), systems.end(), part) != systems.end() && + from.state != to.state) + { + auto info = std::make_shared(); + info->start_state.id = from.state; + info->start_state.label = state_label_(from.state); + info->goal_state.id = to.state; + info->goal_state.label = state_label_(to.state); + this->transition_pub_[part]->publish(TransitionEvent()); + this->transition_pub_[part]->publish(*info); + } + if (from.mode.compare(to.mode) != 0) { + auto info = std::make_shared(); + info->start_mode.label = from.mode; + info->goal_mode.label = to.mode; + this->mode_transition_pub_[part]->publish(*info); + } + } +} + } // namespace system_modes