From 4944b82296c2b89b86518d39cb3f7d5942c78eab Mon Sep 17 00:00:00 2001 From: Arne Nordmann Date: Wed, 15 Jul 2020 16:15:13 +0200 Subject: [PATCH 01/17] Comparison operator for StateAndMode Signed-off-by: Arne Nordmann --- system_modes/include/system_modes/mode_impl.hpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/system_modes/include/system_modes/mode_impl.hpp b/system_modes/include/system_modes/mode_impl.hpp index da35c4c..687d6a4 100644 --- a/system_modes/include/system_modes/mode_impl.hpp +++ b/system_modes/include/system_modes/mode_impl.hpp @@ -39,6 +39,7 @@ struct StateAndMode state = newstate; mode = newmode; } + bool operator!=(const StateAndMode & cmp) const { return cmp.state == state && // same state @@ -46,6 +47,11 @@ struct StateAndMode (cmp.mode.compare(DEFAULT_MODE) == 0 && mode.empty()) || // we consider empty and (mode.compare(DEFAULT_MODE) == 0 && cmp.mode.empty())); // DEFAULT_MODE the same } + + bool operator==(const StateAndMode & cmp) const + { + return !(*this != cmp); + } }; class ModeImpl From 3cd08494bdb3958a02e65cd9cf73bd8aa3c2845c Mon Sep 17 00:00:00 2001 From: Arne Nordmann Date: Mon, 20 Jul 2020 16:33:33 +0200 Subject: [PATCH 02/17] Moved helpers to mode_impl Signed-off-by: Arne Nordmann --- system_modes/include/system_modes/mode.hpp | 8 -- .../include/system_modes/mode_impl.hpp | 19 ++++ system_modes/src/system_modes/mode.cpp | 89 ------------------ system_modes/src/system_modes/mode_impl.cpp | 94 +++++++++++++++++++ 4 files changed, 113 insertions(+), 97 deletions(-) diff --git a/system_modes/include/system_modes/mode.hpp b/system_modes/include/system_modes/mode.hpp index e67a18e..e6df72a 100644 --- a/system_modes/include/system_modes/mode.hpp +++ b/system_modes/include/system_modes/mode.hpp @@ -108,12 +108,4 @@ inline std::ostream & operator<<(std::ostream & os, const Mode & mode) return os; } -unsigned int state_id_(const std::string & state_label); -const std::string state_label_(unsigned int state_id); - -unsigned int transition_id_(const std::string & transition_label); -const std::string transition_label_(unsigned int transition_id); - -unsigned int goal_state_(unsigned int transition_id); - } // namespace system_modes diff --git a/system_modes/include/system_modes/mode_impl.hpp b/system_modes/include/system_modes/mode_impl.hpp index 687d6a4..35db2aa 100644 --- a/system_modes/include/system_modes/mode_impl.hpp +++ b/system_modes/include/system_modes/mode_impl.hpp @@ -29,6 +29,15 @@ namespace system_modes static const char DEFAULT_MODE[] = "__DEFAULT__"; + +unsigned int state_id_(const std::string & state_label); +const std::string state_label_(unsigned int state_id); + +unsigned int transition_id_(const std::string & transition_label); +const std::string transition_label_(unsigned int transition_id); + +unsigned int goal_state_(unsigned int transition_id); + struct StateAndMode { unsigned int state; @@ -52,6 +61,16 @@ struct StateAndMode { return !(*this != cmp); } + + void from_string(const std::string & sam) { + auto dot = sam.find("."); + if (dot != std::string::npos) { + state = state_id_(sam.substr(0, dot)); + mode = sam.substr(dot + 1); + } else { + state = state_id_(sam); + } + } }; class ModeImpl diff --git a/system_modes/src/system_modes/mode.cpp b/system_modes/src/system_modes/mode.cpp index c230e80..35f138f 100644 --- a/system_modes/src/system_modes/mode.cpp +++ b/system_modes/src/system_modes/mode.cpp @@ -24,7 +24,6 @@ using std::string; using std::vector; -using std::to_string; using std::out_of_range; using std::runtime_error; using rclcpp::Parameter; @@ -177,92 +176,4 @@ Mode::set_part_mode( } } -// TODO(anordman): Can we get this from the rcl default state machine? -static const std::map STATES_ = { - {State::PRIMARY_STATE_UNKNOWN, "unknown"}, - {State::PRIMARY_STATE_UNCONFIGURED, "unconfigured"}, - {State::PRIMARY_STATE_INACTIVE, "inactive"}, - {State::PRIMARY_STATE_ACTIVE, "active"}, - {State::PRIMARY_STATE_FINALIZED, "finalized"}, - {State::TRANSITION_STATE_CONFIGURING, "configuring"}, - {State::TRANSITION_STATE_CLEANINGUP, "cleaningup"}, - {State::TRANSITION_STATE_SHUTTINGDOWN, "shuttingdown"}, - {State::TRANSITION_STATE_ACTIVATING, "activating"}, - {State::TRANSITION_STATE_DEACTIVATING, "deactivating"}, - {State::TRANSITION_STATE_ERRORPROCESSING, "errorprocessing"} -}; - -static const std::map TRANSITIONS_ = { - {Transition::TRANSITION_CREATE, "create"}, - {Transition::TRANSITION_CONFIGURE, "configure"}, - {Transition::TRANSITION_CLEANUP, "cleanup"}, - {Transition::TRANSITION_ACTIVATE, "activate"}, - {Transition::TRANSITION_DEACTIVATE, "deactivate"}, - {Transition::TRANSITION_INACTIVE_SHUTDOWN, "inactive_shutdown"}, - {Transition::TRANSITION_ACTIVE_SHUTDOWN, "active_shutdown"}, - {Transition::TRANSITION_DESTROY, "destroy"} -}; - -static const std::map GOAL_STATES_ = { - {Transition::TRANSITION_CREATE, State::PRIMARY_STATE_UNCONFIGURED}, - {Transition::TRANSITION_CONFIGURE, State::PRIMARY_STATE_INACTIVE}, - {Transition::TRANSITION_CLEANUP, State::PRIMARY_STATE_UNCONFIGURED}, - {Transition::TRANSITION_ACTIVATE, State::PRIMARY_STATE_ACTIVE}, - {Transition::TRANSITION_DEACTIVATE, State::PRIMARY_STATE_INACTIVE}, - {Transition::TRANSITION_INACTIVE_SHUTDOWN, State::PRIMARY_STATE_FINALIZED}, - {Transition::TRANSITION_ACTIVE_SHUTDOWN, State::PRIMARY_STATE_FINALIZED} -}; - -const std::string -state_label_(unsigned int state_id) -{ - try { - return STATES_.at(state_id); - } catch (...) { - return "unknown"; - } -} - -unsigned int -state_id_(const std::string & state_label) -{ - for (auto id : STATES_) { - if (id.second.compare(state_label) == 0) { - return id.first; - } - } - return 0; -} - -const std::string -transition_label_(unsigned int transition_id) -{ - try { - return TRANSITIONS_.at(transition_id); - } catch (...) { - throw out_of_range(string("Unknown transition id ") + to_string(transition_id)); - } -} - -unsigned int -transition_id_(const std::string & transition_label) -{ - for (auto id : TRANSITIONS_) { - if (id.second.compare(transition_label) == 0) { - return id.first; - } - } - throw out_of_range("Unknown transition " + transition_label); -} - -unsigned int -goal_state_(unsigned int transition_id) -{ - try { - return GOAL_STATES_.at(transition_id); - } catch (...) { - throw out_of_range(string("Unknown transition id ") + to_string(transition_id)); - } -} - } // namespace system_modes diff --git a/system_modes/src/system_modes/mode_impl.cpp b/system_modes/src/system_modes/mode_impl.cpp index 5f84dc5..ea71766 100644 --- a/system_modes/src/system_modes/mode_impl.cpp +++ b/system_modes/src/system_modes/mode_impl.cpp @@ -19,13 +19,19 @@ #include #include +#include +#include + using std::pair; using std::mutex; using std::string; using std::vector; +using std::to_string; using std::lock_guard; using std::out_of_range; using rclcpp::Parameter; +using lifecycle_msgs::msg::State; +using lifecycle_msgs::msg::Transition; namespace system_modes { @@ -178,4 +184,92 @@ ModeImpl::get_part_mode(const std::string & part) const } } +// TODO(anordman): Can we get this from the rcl default state machine? +static const std::map STATES_ = { + {State::PRIMARY_STATE_UNKNOWN, "unknown"}, + {State::PRIMARY_STATE_UNCONFIGURED, "unconfigured"}, + {State::PRIMARY_STATE_INACTIVE, "inactive"}, + {State::PRIMARY_STATE_ACTIVE, "active"}, + {State::PRIMARY_STATE_FINALIZED, "finalized"}, + {State::TRANSITION_STATE_CONFIGURING, "configuring"}, + {State::TRANSITION_STATE_CLEANINGUP, "cleaningup"}, + {State::TRANSITION_STATE_SHUTTINGDOWN, "shuttingdown"}, + {State::TRANSITION_STATE_ACTIVATING, "activating"}, + {State::TRANSITION_STATE_DEACTIVATING, "deactivating"}, + {State::TRANSITION_STATE_ERRORPROCESSING, "errorprocessing"} +}; + +static const std::map TRANSITIONS_ = { + {Transition::TRANSITION_CREATE, "create"}, + {Transition::TRANSITION_CONFIGURE, "configure"}, + {Transition::TRANSITION_CLEANUP, "cleanup"}, + {Transition::TRANSITION_ACTIVATE, "activate"}, + {Transition::TRANSITION_DEACTIVATE, "deactivate"}, + {Transition::TRANSITION_INACTIVE_SHUTDOWN, "inactive_shutdown"}, + {Transition::TRANSITION_ACTIVE_SHUTDOWN, "active_shutdown"}, + {Transition::TRANSITION_DESTROY, "destroy"} +}; + +static const std::map GOAL_STATES_ = { + {Transition::TRANSITION_CREATE, State::PRIMARY_STATE_UNCONFIGURED}, + {Transition::TRANSITION_CONFIGURE, State::PRIMARY_STATE_INACTIVE}, + {Transition::TRANSITION_CLEANUP, State::PRIMARY_STATE_UNCONFIGURED}, + {Transition::TRANSITION_ACTIVATE, State::PRIMARY_STATE_ACTIVE}, + {Transition::TRANSITION_DEACTIVATE, State::PRIMARY_STATE_INACTIVE}, + {Transition::TRANSITION_INACTIVE_SHUTDOWN, State::PRIMARY_STATE_FINALIZED}, + {Transition::TRANSITION_ACTIVE_SHUTDOWN, State::PRIMARY_STATE_FINALIZED} +}; + +const std::string +state_label_(unsigned int state_id) +{ + try { + return STATES_.at(state_id); + } catch (...) { + return "unknown"; + } +} + +unsigned int +state_id_(const std::string & state_label) +{ + for (auto id : STATES_) { + if (id.second.compare(state_label) == 0) { + return id.first; + } + } + return 0; +} + +const std::string +transition_label_(unsigned int transition_id) +{ + try { + return TRANSITIONS_.at(transition_id); + } catch (...) { + throw out_of_range(string("Unknown transition id ") + to_string(transition_id)); + } +} + +unsigned int +transition_id_(const std::string & transition_label) +{ + for (auto id : TRANSITIONS_) { + if (id.second.compare(transition_label) == 0) { + return id.first; + } + } + throw out_of_range("Unknown transition " + transition_label); +} + +unsigned int +goal_state_(unsigned int transition_id) +{ + try { + return GOAL_STATES_.at(transition_id); + } catch (...) { + throw out_of_range(string("Unknown transition id ") + to_string(transition_id)); + } +} + } // namespace system_modes From 11a85d2d74ec8c8f1bf333544addcd2ea443d045 Mon Sep 17 00:00:00 2001 From: Arne Nordmann Date: Tue, 21 Jul 2020 14:27:44 +0200 Subject: [PATCH 03/17] Tests for StateAndMode struct Signed-off-by: Arne Nordmann --- system_modes/CMakeLists.txt | 9 +- .../include/system_modes/mode_impl.hpp | 9 ++ .../include/system_modes/mode_manager.hpp | 2 +- .../src/system_modes/mode_inference.cpp | 1 + .../src/system_modes/mode_manager.cpp | 6 +- .../test/test_state_and_mode_struct.cpp | 100 ++++++++++++++++++ 6 files changed, 122 insertions(+), 5 deletions(-) create mode 100644 system_modes/test/test_state_and_mode_struct.cpp diff --git a/system_modes/CMakeLists.txt b/system_modes/CMakeLists.txt index 21c733f..24e748a 100644 --- a/system_modes/CMakeLists.txt +++ b/system_modes/CMakeLists.txt @@ -118,6 +118,14 @@ if(BUILD_TESTING) configure_file(test/modefiles.h.in ${CMAKE_CURRENT_BINARY_DIR}/system_modes/modefiles.h) include_directories(BEFORE ${CMAKE_CURRENT_BINARY_DIR}) + ament_add_gtest(test_state_and_mode_struct test/test_state_and_mode_struct.cpp) + if(TARGET test_state_and_mode_struct) + target_include_directories(test_state_and_mode_struct PUBLIC + ${rclcpp_INCLUDE_DIRS} + ) + target_link_libraries(test_state_and_mode_struct mode) + endif() + ament_add_gtest(test_default_mode test/test_default_mode.cpp) if(TARGET test_default_mode) target_include_directories(test_default_mode PUBLIC @@ -134,7 +142,6 @@ if(BUILD_TESTING) target_link_libraries(test_mode mode) endif() - message(STATUS "CMAKE_CURRENT_BINARY_DIR: ${CMAKE_CURRENT_BINARY_DIR}/system_modes/") ament_add_gtest(test_mode_inference test/test_mode_inference.cpp) if(TARGET test_mode_inference) target_include_directories(test_mode_inference PUBLIC diff --git a/system_modes/include/system_modes/mode_impl.hpp b/system_modes/include/system_modes/mode_impl.hpp index 35db2aa..2278b29 100644 --- a/system_modes/include/system_modes/mode_impl.hpp +++ b/system_modes/include/system_modes/mode_impl.hpp @@ -69,8 +69,17 @@ struct StateAndMode mode = sam.substr(dot + 1); } else { state = state_id_(sam); + mode = ""; } } + + std::string as_string() const + { + if (mode.empty()) { + return state_label_(state); + } + return state_label_(state) + "." + mode; + } }; class ModeImpl diff --git a/system_modes/include/system_modes/mode_manager.hpp b/system_modes/include/system_modes/mode_manager.hpp index f027f43..8e4e8b8 100644 --- a/system_modes/include/system_modes/mode_manager.hpp +++ b/system_modes/include/system_modes/mode_manager.hpp @@ -122,7 +122,7 @@ class ModeManager : public rclcpp::Node mode_request_pub_; // Remember states and modes of the systems - std::map> current_modes_; + std::map current_modes_; }; } // namespace system_modes diff --git a/system_modes/src/system_modes/mode_inference.cpp b/system_modes/src/system_modes/mode_inference.cpp index 9add933..f86c051 100644 --- a/system_modes/src/system_modes/mode_inference.cpp +++ b/system_modes/src/system_modes/mode_inference.cpp @@ -494,6 +494,7 @@ ModeInference::read_modes_from_model(const string & model_path) if (param.get_name().compare("type") != 0) { string mode_name; + // Parse mode definitions std::size_t foundm = param.get_name().find("modes."); if (foundm != string::npos) { std::size_t foundmr = param.get_name().find(".", 6); diff --git a/system_modes/src/system_modes/mode_manager.cpp b/system_modes/src/system_modes/mode_manager.cpp index 3d2c17c..0b1468b 100644 --- a/system_modes/src/system_modes/mode_manager.cpp +++ b/system_modes/src/system_modes/mode_manager.cpp @@ -343,7 +343,7 @@ ModeManager::change_state( // Publish info about this request auto info = std::make_shared(); if (this->current_modes_.find(node_name) != this->current_modes_.end()) { - info->start_state.label = this->current_modes_.at(node_name).first; + info->start_state.label = this->current_modes_.at(node_name).state; } info->transition.label = transition_label_(transition_id); info->transition.id = transition_id; @@ -407,7 +407,7 @@ ModeManager::change_mode( this->current_modes_.emplace( node_name, - std::make_pair(State::PRIMARY_STATE_ACTIVE, mode_name.c_str())); + StateAndMode(State::PRIMARY_STATE_ACTIVE, mode_name.c_str())); auto nodes = this->mode_inference_->get_nodes(); if (std::find(nodes.begin(), nodes.end(), node_name) != nodes.end()) { @@ -461,7 +461,7 @@ ModeManager::change_part_state(const string & node, unsigned int transition) info->transition.label = transition_label_(transition); info->transition.id = transition; if (this->current_modes_.find(node) != this->current_modes_.end()) { - info->start_state.label = this->current_modes_.at(node).first; + info->start_state.label = this->current_modes_.at(node).state; info->start_state.id = state_id_(info->start_state.label); } info->goal_state.id = goal_state_(info->transition.id); diff --git a/system_modes/test/test_state_and_mode_struct.cpp b/system_modes/test/test_state_and_mode_struct.cpp new file mode 100644 index 0000000..0f2d73a --- /dev/null +++ b/system_modes/test/test_state_and_mode_struct.cpp @@ -0,0 +1,100 @@ +// Copyright (c) 2018 - for information on the respective copyright owner +// see the NOTICE file and/or the repository https://github.com/microros/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. + +#include +#include + +#include +#include + +#include "system_modes/mode.hpp" + +using std::string; +using namespace std::string_literals; + +using system_modes::StateAndMode; +using lifecycle_msgs::msg::State; + +class TestStateAndMode : public ::testing::Test +{ +protected: + void SetUp() + { + inactive.state = State::PRIMARY_STATE_INACTIVE; + + active.state = State::PRIMARY_STATE_ACTIVE; + + active_default.state = State::PRIMARY_STATE_ACTIVE; + active_default.mode = "__DEFAULT__"; + + active_foo.state = State::PRIMARY_STATE_ACTIVE; + active_foo.mode = "FOO"; + } + + void TearDown() + { + } + + StateAndMode inactive, active, active_default, active_foo; +}; + +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); + EXPECT_NE(active_default, active_foo); + EXPECT_NE(active_default, inactive); + } +} + +TEST_F(TestStateAndMode, string_getter) { + { + EXPECT_EQ("active"s, active.as_string()); + EXPECT_EQ("active.__DEFAULT__"s, active_default.as_string()); + EXPECT_EQ("active.FOO"s, active_foo.as_string()); + EXPECT_EQ("inactive"s, inactive.as_string()); + } +} + +TEST_F(TestStateAndMode, string_setter) { + { + StateAndMode copy; + + copy.from_string("active"); + EXPECT_EQ(active, copy); + + copy.from_string("active.__DEFAULT__"); + EXPECT_EQ(active_default, copy); + + copy.from_string("active.FOO"); + EXPECT_EQ(active_foo, copy); + + copy.from_string("inactive"); + EXPECT_EQ(inactive, copy); + + copy.from_string(active.as_string()); + EXPECT_EQ(active, copy); + + copy.from_string(active_foo.as_string()); + EXPECT_EQ(active_foo, copy); + + copy.from_string(inactive.as_string()); + EXPECT_EQ(inactive, copy); + } +} From 9fe4406a329f02ca7e1f37b8952a1446c475ce01 Mon Sep 17 00:00:00 2001 From: Arne Nordmann Date: Tue, 21 Jul 2020 16:23:43 +0200 Subject: [PATCH 04/17] StateAndMode improvements Signed-off-by: Arne Nordmann --- .../include/system_modes/mode_impl.hpp | 24 ++++++++++++------- .../src/system_modes/mode_manager.cpp | 3 ++- .../test/test_state_and_mode_struct.cpp | 7 +++++- 3 files changed, 24 insertions(+), 10 deletions(-) diff --git a/system_modes/include/system_modes/mode_impl.hpp b/system_modes/include/system_modes/mode_impl.hpp index 2278b29..44d7c83 100644 --- a/system_modes/include/system_modes/mode_impl.hpp +++ b/system_modes/include/system_modes/mode_impl.hpp @@ -17,6 +17,8 @@ #include #include +#include + #include #include #include @@ -24,6 +26,8 @@ #include #include +using lifecycle_msgs::msg::State; + namespace system_modes { @@ -49,17 +53,22 @@ struct StateAndMode mode = newmode; } - bool operator!=(const StateAndMode & cmp) const + bool operator==(const StateAndMode & cmp) const { - return cmp.state == state && // same state - (cmp.mode.compare(mode) == 0 || // same mode + if (cmp.state != state) { + return false; + } else if (cmp.state != State::PRIMARY_STATE_ACTIVE) { + 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 } - bool operator==(const StateAndMode & cmp) const + bool operator!=(const StateAndMode & cmp) const { - return !(*this != cmp); + return !(*this == cmp); } void from_string(const std::string & sam) { @@ -73,9 +82,8 @@ struct StateAndMode } } - std::string as_string() const - { - if (mode.empty()) { + std::string as_string() const { + if (state != State::PRIMARY_STATE_ACTIVE || mode.empty()) { return state_label_(state); } return state_label_(state) + "." + mode; diff --git a/system_modes/src/system_modes/mode_manager.cpp b/system_modes/src/system_modes/mode_manager.cpp index 0b1468b..01e84f9 100644 --- a/system_modes/src/system_modes/mode_manager.cpp +++ b/system_modes/src/system_modes/mode_manager.cpp @@ -50,6 +50,8 @@ using system_modes::msg::ModeEvent; using system_modes::srv::ChangeMode; using system_modes::srv::GetAvailableModes; +using namespace std::chrono_literals; + namespace system_modes { @@ -327,7 +329,6 @@ ModeManager::on_get_available_modes( } } - bool ModeManager::change_state( const std::string & node_name, diff --git a/system_modes/test/test_state_and_mode_struct.cpp b/system_modes/test/test_state_and_mode_struct.cpp index 0f2d73a..3c789c5 100644 --- a/system_modes/test/test_state_and_mode_struct.cpp +++ b/system_modes/test/test_state_and_mode_struct.cpp @@ -60,15 +60,20 @@ TEST_F(TestStateAndMode, comparison) { EXPECT_NE(active, active_foo); EXPECT_NE(active_default, active_foo); EXPECT_NE(active_default, inactive); + + active_foo.state = State::PRIMARY_STATE_INACTIVE; + EXPECT_EQ(inactive, active_foo); } } 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.FOO"s, active_foo.as_string()); - EXPECT_EQ("inactive"s, inactive.as_string()); + active_foo.state = State::PRIMARY_STATE_INACTIVE; + EXPECT_EQ("inactive"s, active_foo.as_string()); } } From 03684264f95758a56ccce9d68148321dd4db9d3a Mon Sep 17 00:00:00 2001 From: Arne Nordmann Date: Thu, 23 Jul 2020 14:55:52 +0200 Subject: [PATCH 05/17] Version bump to 0.2.3 * Version bump, linting, changelog Signed-off-by: Arne Nordmann --- system_modes/changelog.rst | 9 ++++++ .../include/system_modes/mode_impl.hpp | 10 ++++--- system_modes/package.xml | 2 +- system_modes/src/system_modes/mode_impl.cpp | 30 ++++++++++--------- system_modes_examples/changelog.rst | 22 +++++++++++--- system_modes_examples/package.xml | 2 +- 6 files changed, 51 insertions(+), 24 deletions(-) diff --git a/system_modes/changelog.rst b/system_modes/changelog.rst index 22b514e..158280a 100644 --- a/system_modes/changelog.rst +++ b/system_modes/changelog.rst @@ -2,6 +2,15 @@ Changelog for package system_modes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.2.3 (2020-02-13) +----------- +* improved StateAndMode struct +* testing + +0.2.2 (2020-07-13) +----------- +* introduced StateAndMode struct to bundle lifecycle state and system mode + 0.2.0 (2020-02-13) ----------- * integration with ROS 2 launch diff --git a/system_modes/include/system_modes/mode_impl.hpp b/system_modes/include/system_modes/mode_impl.hpp index 44d7c83..59a0ae4 100644 --- a/system_modes/include/system_modes/mode_impl.hpp +++ b/system_modes/include/system_modes/mode_impl.hpp @@ -61,9 +61,9 @@ struct StateAndMode return true; } - return (cmp.mode.compare(mode) == 0 || // same mode + 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 + (mode.compare(DEFAULT_MODE) == 0 && cmp.mode.empty()); // DEFAULT_MODE the same } bool operator!=(const StateAndMode & cmp) const @@ -71,7 +71,8 @@ struct StateAndMode return !(*this == cmp); } - void from_string(const std::string & sam) { + void from_string(const std::string & sam) + { auto dot = sam.find("."); if (dot != std::string::npos) { state = state_id_(sam.substr(0, dot)); @@ -82,7 +83,8 @@ struct StateAndMode } } - std::string as_string() const { + std::string as_string() const + { if (state != State::PRIMARY_STATE_ACTIVE || mode.empty()) { return state_label_(state); } diff --git a/system_modes/package.xml b/system_modes/package.xml index 863c94f..7546b65 100644 --- a/system_modes/package.xml +++ b/system_modes/package.xml @@ -2,7 +2,7 @@ system_modes - 0.2.1 + 0.2.3 Model-based distributed configuration handling. Arne Nordmann Apache License 2.0 diff --git a/system_modes/src/system_modes/mode_impl.cpp b/system_modes/src/system_modes/mode_impl.cpp index ea71766..9a2d022 100644 --- a/system_modes/src/system_modes/mode_impl.cpp +++ b/system_modes/src/system_modes/mode_impl.cpp @@ -14,6 +14,7 @@ // limitations under the License. #include "system_modes/mode_impl.hpp" +#include #include #include #include @@ -22,6 +23,7 @@ #include #include +using std::map; using std::pair; using std::mutex; using std::string; @@ -117,9 +119,9 @@ ModeImpl::add_parameters(const vector & parameters) void ModeImpl::set_parameter(const Parameter & parameter) { - std::string param_name = parameter.get_name(); + string param_name = parameter.get_name(); std::size_t foundr = parameter.get_name().rfind("ros__parameters"); - if (foundr != std::string::npos) { + if (foundr != string::npos) { param_name = parameter.get_name().substr(foundr + strlen("ros__parameters") + 1); } @@ -142,7 +144,7 @@ ModeImpl::set_parameters(const vector & parameters) void ModeImpl::add_part_mode( - const std::string & part, + const string & part, const StateAndMode stateAndMode) { this->part_modes_[part] = StateAndMode(stateAndMode.state, stateAndMode.mode); @@ -150,7 +152,7 @@ ModeImpl::add_part_mode( void ModeImpl::set_part_mode( - const std::string & part, + const string & part, const StateAndMode stateAndMode) { if (this->part_modes_.find(part) == this->part_modes_.end()) { @@ -162,7 +164,7 @@ ModeImpl::set_part_mode( this->add_part_mode(part, stateAndMode); } -const std::vector +const vector ModeImpl::get_parts() const { vector results; @@ -173,19 +175,19 @@ ModeImpl::get_parts() const } const StateAndMode -ModeImpl::get_part_mode(const std::string & part) const +ModeImpl::get_part_mode(const string & part) const { if (this->part_modes_.count(part)) { return this->part_modes_.at(part); } else { - throw std::out_of_range( + throw out_of_range( "Can't receive modes for part '" + part + "', part not specified."); } } // TODO(anordman): Can we get this from the rcl default state machine? -static const std::map STATES_ = { +static const map STATES_ = { {State::PRIMARY_STATE_UNKNOWN, "unknown"}, {State::PRIMARY_STATE_UNCONFIGURED, "unconfigured"}, {State::PRIMARY_STATE_INACTIVE, "inactive"}, @@ -199,7 +201,7 @@ static const std::map STATES_ = { {State::TRANSITION_STATE_ERRORPROCESSING, "errorprocessing"} }; -static const std::map TRANSITIONS_ = { +static const map TRANSITIONS_ = { {Transition::TRANSITION_CREATE, "create"}, {Transition::TRANSITION_CONFIGURE, "configure"}, {Transition::TRANSITION_CLEANUP, "cleanup"}, @@ -210,7 +212,7 @@ static const std::map TRANSITIONS_ = { {Transition::TRANSITION_DESTROY, "destroy"} }; -static const std::map GOAL_STATES_ = { +static const map GOAL_STATES_ = { {Transition::TRANSITION_CREATE, State::PRIMARY_STATE_UNCONFIGURED}, {Transition::TRANSITION_CONFIGURE, State::PRIMARY_STATE_INACTIVE}, {Transition::TRANSITION_CLEANUP, State::PRIMARY_STATE_UNCONFIGURED}, @@ -220,7 +222,7 @@ static const std::map GOAL_STATES_ = { {Transition::TRANSITION_ACTIVE_SHUTDOWN, State::PRIMARY_STATE_FINALIZED} }; -const std::string +const string state_label_(unsigned int state_id) { try { @@ -231,7 +233,7 @@ state_label_(unsigned int state_id) } unsigned int -state_id_(const std::string & state_label) +state_id_(const string & state_label) { for (auto id : STATES_) { if (id.second.compare(state_label) == 0) { @@ -241,7 +243,7 @@ state_id_(const std::string & state_label) return 0; } -const std::string +const string transition_label_(unsigned int transition_id) { try { @@ -252,7 +254,7 @@ transition_label_(unsigned int transition_id) } unsigned int -transition_id_(const std::string & transition_label) +transition_id_(const string & transition_label) { for (auto id : TRANSITIONS_) { if (id.second.compare(transition_label) == 0) { diff --git a/system_modes_examples/changelog.rst b/system_modes_examples/changelog.rst index 578325b..158280a 100644 --- a/system_modes_examples/changelog.rst +++ b/system_modes_examples/changelog.rst @@ -1,10 +1,24 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package system_modes_examples -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package system_modes +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.2.3 (2020-02-13) +----------- +* improved StateAndMode struct +* testing + +0.2.2 (2020-07-13) +----------- +* introduced StateAndMode struct to bundle lifecycle state and system mode 0.2.0 (2020-02-13) ----------- * integration with ROS 2 launch +* updated docs + +0.1.6 (2019-10-31) +------------------- +* fixed QoS configuration for parameter event subscribers 0.1.5 (2019-10-21) ------------------- @@ -16,4 +30,4 @@ Changelog for package system_modes_examples 0.1.1 (2019-03-08) ------------------- -* first public release for ROS 2 system modes examples +* first public release for ROS 2 system modes diff --git a/system_modes_examples/package.xml b/system_modes_examples/package.xml index f7e903e..eaa3b42 100644 --- a/system_modes_examples/package.xml +++ b/system_modes_examples/package.xml @@ -2,7 +2,7 @@ system_modes_examples - 0.2.1 + 0.2.3 Simple example system for system_modes package. Arne Nordmann Apache License 2.0 From fc11701bc8c41fc30403511eece7f7c4ed71214c Mon Sep 17 00:00:00 2001 From: Arne Nordmann Date: Thu, 23 Jul 2020 15:22:38 +0200 Subject: [PATCH 06/17] Removes redundant node name from mode services * Node name is already encoded in the respective topic name of the service https://github.com/micro-ROS/system_modes/issues/24 Signed-off-by: Arne Nordmann --- system_modes/srv/ChangeMode.srv | 1 - system_modes/srv/GetAvailableModes.srv | 1 - system_modes/srv/GetMode.srv | 1 - 3 files changed, 3 deletions(-) diff --git a/system_modes/srv/ChangeMode.srv b/system_modes/srv/ChangeMode.srv index 5a6cd8a..3cc4b52 100644 --- a/system_modes/srv/ChangeMode.srv +++ b/system_modes/srv/ChangeMode.srv @@ -1,4 +1,3 @@ -string node_name string mode_name --- bool success diff --git a/system_modes/srv/GetAvailableModes.srv b/system_modes/srv/GetAvailableModes.srv index 5d793c0..fdf2ae6 100644 --- a/system_modes/srv/GetAvailableModes.srv +++ b/system_modes/srv/GetAvailableModes.srv @@ -1,3 +1,2 @@ -string node_name --- string[] available_modes diff --git a/system_modes/srv/GetMode.srv b/system_modes/srv/GetMode.srv index 61c8420..7b46269 100644 --- a/system_modes/srv/GetMode.srv +++ b/system_modes/srv/GetMode.srv @@ -1,3 +1,2 @@ -string node_name --- string current_mode From 6d105acdf5b77e048991964a55b3f1ced9b03016 Mon Sep 17 00:00:00 2001 From: Arne Nordmann Date: Thu, 23 Jul 2020 16:27:26 +0200 Subject: [PATCH 07/17] Adapts callbacks to simplified mode services * Adapts mode callbacks for nodes and systems (get mode, change mode, get available modes) * part name (system or node) no longer cames from request object, but is coded in the callback https://github.com/micro-ROS/system_modes/issues/24 Signed-off-by: Arne Nordmann --- .../include/system_modes/mode_manager.hpp | 22 ++++--- system_modes/src/mode_monitor_node.cpp | 7 +-- .../src/system_modes/mode_manager.cpp | 61 +++++++++++++------ 3 files changed, 59 insertions(+), 31 deletions(-) diff --git a/system_modes/include/system_modes/mode_manager.hpp b/system_modes/include/system_modes/mode_manager.hpp index 8e4e8b8..59695b2 100644 --- a/system_modes/include/system_modes/mode_manager.hpp +++ b/system_modes/include/system_modes/mode_manager.hpp @@ -65,26 +65,30 @@ class ModeManager : public rclcpp::Node // Mode service callbacks virtual void on_change_mode( - const std::shared_ptr, + const std::string &, const std::shared_ptr, std::shared_ptr); virtual void on_get_mode( - const std::shared_ptr, - const std::shared_ptr, + const std::string &, std::shared_ptr); virtual void on_get_available_modes( - const std::shared_ptr, - const std::shared_ptr, + const std::string &, std::shared_ptr); virtual bool change_state( - const std::string & node, + const std::string &, unsigned int, bool transitive = true); - virtual bool change_mode(const std::string & node, const std::string & mode); + virtual bool change_mode( + const std::string &, + const std::string &); - virtual void change_part_state(const std::string & node, unsigned int); - virtual void change_part_mode(const std::string & node, const std::string & mode); + virtual void change_part_state( + const std::string &, + unsigned int); + virtual void change_part_mode( + const std::string &, + const std::string &); private: std::shared_ptr mode_inference_; diff --git a/system_modes/src/mode_monitor_node.cpp b/system_modes/src/mode_monitor_node.cpp index 14b7c23..58ced1d 100644 --- a/system_modes/src/mode_monitor_node.cpp +++ b/system_modes/src/mode_monitor_node.cpp @@ -103,9 +103,6 @@ bool parseOptions(int argc, char * argv[]) return true; } - // if (modelfile.empty()) { - // throw invalid_argument("Need path to model file."); - // } return false; } @@ -121,7 +118,7 @@ void mode_change_callback( const string & node_name) { monitor->inference()->update_state(node_name, State::PRIMARY_STATE_ACTIVE); - monitor->inference()->update_mode(node_name, msg->goal_mode.label.c_str()); + monitor->inference()->update_mode(node_name, msg->goal_mode.label); } void transition_request_callback( @@ -145,7 +142,7 @@ void mode_request_callback( { monitor->inference()->update_target( node_name, - StateAndMode(State::PRIMARY_STATE_ACTIVE, msg->goal_mode.label.c_str())); + StateAndMode(State::PRIMARY_STATE_ACTIVE, msg->goal_mode.label)); } void diff --git a/system_modes/src/system_modes/mode_manager.cpp b/system_modes/src/system_modes/mode_manager.cpp index 01e84f9..06d91c2 100644 --- a/system_modes/src/system_modes/mode_manager.cpp +++ b/system_modes/src/system_modes/mode_manager.cpp @@ -47,6 +47,7 @@ using lifecycle_msgs::srv::ChangeState; using lifecycle_msgs::srv::GetAvailableStates; using system_modes::msg::ModeEvent; +using system_modes::srv::GetMode; using system_modes::srv::ChangeMode; using system_modes::srv::GetAvailableModes; @@ -137,18 +138,32 @@ ModeManager::add_system(const std::string & system) // Mode services service_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, std::bind(&ModeManager::on_change_mode, this, _1, _2, _3)); + service_name, + mode_change_callback); service_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, - std::bind(&ModeManager::on_get_mode, this, _1, _2, _3)); + mode_get_callback); service_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, std::bind(&ModeManager::on_get_available_modes, this, _1, _2, _3)); + service_name, + mode_avail_callback); // Lifecycle change clients service_name = system + "/change_state"; @@ -174,18 +189,32 @@ ModeManager::add_node(const std::string & node) // Mode services service_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, std::bind(&ModeManager::on_change_mode, this, _1, _2, _3)); + service_name, + mode_change_callback); service_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, - std::bind(&ModeManager::on_get_mode, this, _1, _2, _3)); + mode_get_callback); service_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, std::bind(&ModeManager::on_get_available_modes, this, _1, _2, _3)); + service_name, + mode_avail_callback); // Lifecycle change clients service_name = node + "/change_state"; @@ -278,31 +307,30 @@ ModeManager::on_get_available_states( void ModeManager::on_change_mode( - const std::shared_ptr, + const std::string & node_name, const std::shared_ptr request, std::shared_ptr response) { RCLCPP_INFO( this->get_logger(), "-> callback change_mode of %s to %s", - request->node_name.c_str(), + node_name.c_str(), request->mode_name.c_str()); // We can't wait for the state/mode transitions - response->success = this->change_mode(request->node_name, request->mode_name); + response->success = this->change_mode(node_name, request->mode_name); } void ModeManager::on_get_mode( - const std::shared_ptr, - const std::shared_ptr request, + const std::string & node_name, std::shared_ptr response) { // TODO(anordman): to be on the safe side, don't use the node name from // the request, but bind it to the callback instead - RCLCPP_INFO(this->get_logger(), "-> callback get_mode of %s", request->node_name.c_str()); + RCLCPP_INFO(this->get_logger(), "-> callback get_mode of %s", node_name.c_str()); try { - auto stateAndMode = this->mode_inference_->infer(request->node_name); + auto stateAndMode = this->mode_inference_->infer(node_name); response->current_mode = stateAndMode.mode; } catch (std::exception & ex) { response->current_mode = "unknown"; @@ -312,8 +340,7 @@ ModeManager::on_get_mode( void ModeManager::on_get_available_modes( - const std::shared_ptr, - const std::shared_ptr request, + const std::string & node_name, std::shared_ptr response) { // TODO(anordman): to be on the safe side, don't use the node name from @@ -321,9 +348,9 @@ ModeManager::on_get_available_modes( RCLCPP_INFO( this->get_logger(), "-> callback get_available_modes of %s", - request->node_name.c_str()); + node_name.c_str()); try { - response->available_modes = mode_inference_->get_available_modes(request->node_name); + response->available_modes = mode_inference_->get_available_modes(node_name); } catch (std::exception & ex) { RCLCPP_INFO(this->get_logger(), " unknown"); } From b6d35bce3ce9a2094c191779db201c7e6be0b820 Mon Sep 17 00:00:00 2001 From: Arne Nordmann Date: Thu, 23 Jul 2020 16:33:09 +0200 Subject: [PATCH 08/17] Version bump to 0.3.0 * Version bump to 0.3.0 due to API change (mode services) * Adapts system modes exaple documentation to new service format https://github.com/micro-ROS/system_modes/issues/24 Signed-off-by: Arne Nordmann --- system_modes/changelog.rst | 7 ++++++- system_modes/package.xml | 2 +- system_modes_examples/README.md | 6 +++--- system_modes_examples/changelog.rst | 7 ++++++- system_modes_examples/package.xml | 2 +- 5 files changed, 17 insertions(+), 7 deletions(-) diff --git a/system_modes/changelog.rst b/system_modes/changelog.rst index 158280a..82e5f35 100644 --- a/system_modes/changelog.rst +++ b/system_modes/changelog.rst @@ -2,7 +2,12 @@ Changelog for package system_modes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -0.2.3 (2020-02-13) +0.3.0 (2020-07-23) +----------- +* changed mode service specifications (less redundancy) +* https://github.com/micro-ROS/system_modes/issues/24 + +0.2.3 (2020-07-23) ----------- * improved StateAndMode struct * testing diff --git a/system_modes/package.xml b/system_modes/package.xml index 7546b65..430e505 100644 --- a/system_modes/package.xml +++ b/system_modes/package.xml @@ -2,7 +2,7 @@ system_modes - 0.2.3 + 0.3.0 Model-based distributed configuration handling. Arne Nordmann Apache License 2.0 diff --git a/system_modes_examples/README.md b/system_modes_examples/README.md index 9c07c59..a3e2b0b 100644 --- a/system_modes_examples/README.md +++ b/system_modes_examples/README.md @@ -50,15 +50,15 @@ In an additional fifth terminal, you may mimic a planning/executive component to The mode monitor should display the following system state: ![mode_monitor](./doc/screenshot-monitor-active.png "Screenshot of the mode monitor") 1. Set your system into *PERFORMANCE* mode with the following ROS 2 command: - $ `ros2 service call /actuation/change_mode system_modes/ChangeMode "{node_name: 'actuation', mode_name: 'PERFORMANCE'}"` + $ `ros2 service call /actuation/change_mode system_modes/ChangeMode "{mode_name: 'PERFORMANCE'}"` To change the *actuation* system into its *PERFORMANCE* mode, the mode manager will change the *drive\_base* to *FAST* mode and activate the *manipulator* node in its *STRONG* mode. The mode monitor should display the following system state: ![mode_monitor](./doc/screenshot-monitor-performance.png "Screenshot of the mode monitor") Note, that the system state and mode as well as the node modes are indicated to be *inferred*, as explained in the [mode inference](../system_modes/README.md#mode-inference) section of the [system_modes](../system_modes/) package. 1. You can further play around with the mode inference. For example, change the mode of the two nodes explicitly so that the target mode and actual mode of the *actuation* system diverge. Execute the following two ROS 2 commands: - $ `ros2 service call /drive_base/change_mode system_modes/ChangeMode "{node_name: 'drive_base', mode_name: 'SLOW'}"` + $ `ros2 service call /drive_base/change_mode system_modes/ChangeMode "{mode_name: 'SLOW'}"` and - $ `ros2 service call /manipulator/change_mode system_modes/ChangeMode "{node_name: 'manipulator', mode_name: 'WEAK'}"` + $ `ros2 service call /manipulator/change_mode system_modes/ChangeMode "{mode_name: 'WEAK'}"` The mode monitor should display the following system state: ![mode_monitor](./doc/screenshot-monitor-moderate.png "Screenshot of the mode monitor") Note, that the mode monitor is able to infer that the system's *actual* mode is now *MODERATE*. This is based on the fact that both its nodes are active, the *drive\_base* is in its *SLOW* mode, and the manipulator is in its *WEAK* mode. However, the last requested mode for the *actuation* system is *PERFORMANCE*, so the monitor infers that the system is still transitioning into its target mode, indicating that the actual system state is *activating* (see [lifecycle](../system_modes/README.md#lifecycle)). diff --git a/system_modes_examples/changelog.rst b/system_modes_examples/changelog.rst index 158280a..82e5f35 100644 --- a/system_modes_examples/changelog.rst +++ b/system_modes_examples/changelog.rst @@ -2,7 +2,12 @@ Changelog for package system_modes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -0.2.3 (2020-02-13) +0.3.0 (2020-07-23) +----------- +* changed mode service specifications (less redundancy) +* https://github.com/micro-ROS/system_modes/issues/24 + +0.2.3 (2020-07-23) ----------- * improved StateAndMode struct * testing diff --git a/system_modes_examples/package.xml b/system_modes_examples/package.xml index eaa3b42..511894b 100644 --- a/system_modes_examples/package.xml +++ b/system_modes_examples/package.xml @@ -2,7 +2,7 @@ system_modes_examples - 0.2.3 + 0.3.0 Simple example system for system_modes package. Arne Nordmann Apache License 2.0 From 00d8cb144f2ea13998f9bacaaaefbbd9ed2bfd30 Mon Sep 17 00:00:00 2001 From: Arne Nordmann Date: Fri, 24 Jul 2020 16:48:52 +0200 Subject: [PATCH 09/17] [WiP] Adds github action to build and test (#33) * Adds github action to build and test * Adds and fixes tests for examples package * Build only on linux for now Builds on windows and macos are failing for different reasons: windows fails due to failing osrf_testing_tools_cpp, macos fails due to missing boost. Signed-off-by: Arne Nordmann --- .github/workflows/lint.yaml | 45 +++++++++++++++++ .github/workflows/test.yaml | 21 ++++++++ system_modes_examples/CMakeLists.txt | 13 +++++ .../launch/drive_base.launch.py | 4 +- .../launch/example_system.launch.py | 5 +- .../launch/manipulator.launch.py | 4 +- system_modes_examples/package.xml | 9 ++++ system_modes_examples/src/drive_base.cpp | 49 +++++++++---------- system_modes_examples/src/manipulator.cpp | 46 +++++++++-------- 9 files changed, 140 insertions(+), 56 deletions(-) create mode 100644 .github/workflows/lint.yaml create mode 100644 .github/workflows/test.yaml diff --git a/.github/workflows/lint.yaml b/.github/workflows/lint.yaml new file mode 100644 index 0000000..c3f8a2a --- /dev/null +++ b/.github/workflows/lint.yaml @@ -0,0 +1,45 @@ +name: Lint system modes +on: + pull_request: + +jobs: + ament_xmllint: + name: ament_xmllint + runs-on: ubuntu-18.04 + steps: + - uses: actions/checkout@master + - uses: ros-tooling/setup-ros@master + - uses: ros-tooling/action-ros-lint@master + with: + linter: xmllint + package-name: system_modes system_modes_examples + + ament_lint_cpp: + name: ament_${{ matrix.linter }} + runs-on: ubuntu-18.04 + strategy: + fail-fast: false + matrix: + linter: [cppcheck, cpplint, uncrustify] + steps: + - uses: actions/checkout@master + - uses: ros-tooling/setup-ros@master + - uses: ros-tooling/action-ros-lint@master + with: + linter: ${{ matrix.linter }} + package-name: system_modes system_modes_examples + + ament_lint_py: + name: ament_${{ matrix.linter }} + runs-on: ubuntu-18.04 + strategy: + fail-fast: false + matrix: + linter: [flake8, pep257] + steps: + - uses: actions/checkout@master + - uses: ros-tooling/setup-ros@master + - uses: ros-tooling/action-ros-lint@master + with: + linter: ${{ matrix.linter }} + package-name: system_modes system_modes_examples diff --git a/.github/workflows/test.yaml b/.github/workflows/test.yaml new file mode 100644 index 0000000..ca522ca --- /dev/null +++ b/.github/workflows/test.yaml @@ -0,0 +1,21 @@ +name: Test diagnostics +on: + pull_request: + push: + branches: + - master + +jobs: + build_and_test: + runs-on: ${{ matrix.os }} + strategy: + fail-fast: false + matrix: + os: [ubuntu-18.04] + steps: + - uses: ros-tooling/setup-ros@master + - uses: ros-tooling/action-ros-ci@master + with: + package-name: system_modes system_modes_examples + colcon-mixin-name: coverage-gcc + colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml diff --git a/system_modes_examples/CMakeLists.txt b/system_modes_examples/CMakeLists.txt index b9526cc..7e71162 100644 --- a/system_modes_examples/CMakeLists.txt +++ b/system_modes_examples/CMakeLists.txt @@ -74,4 +74,17 @@ install(FILES example_modes.yaml ) install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}/) +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + find_package(ament_cmake_gtest REQUIRED) + find_package(ament_cmake_gmock REQUIRED) + # the following line skips the linter which checks for copyrights + # remove the line when a copyright and license is present in all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # remove the line when this package is a git repo + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + ament_package() diff --git a/system_modes_examples/launch/drive_base.launch.py b/system_modes_examples/launch/drive_base.launch.py index fa929c2..76d6c5b 100644 --- a/system_modes_examples/launch/drive_base.launch.py +++ b/system_modes_examples/launch/drive_base.launch.py @@ -25,7 +25,7 @@ def generate_launch_description(): launch.actions.DeclareLaunchArgument('modelfile', description='Path to modelfile') modelfile = (ament_index_python.packages.get_package_share_directory('system_modes_examples') + - '/example_modes.yaml') + '/example_modes.yaml') node = launch_ros.actions.Node( package='system_modes_examples', @@ -36,4 +36,4 @@ def generate_launch_description(): description = launch.LaunchDescription() description.add_action(node) - return description \ No newline at end of file + return description diff --git a/system_modes_examples/launch/example_system.launch.py b/system_modes_examples/launch/example_system.launch.py index e109350..c438903 100644 --- a/system_modes_examples/launch/example_system.launch.py +++ b/system_modes_examples/launch/example_system.launch.py @@ -19,12 +19,11 @@ import launch.actions import launch.launch_description_sources import launch.substitutions -import launch_ros def generate_launch_description(): modelfile = (ament_index_python.packages.get_package_share_directory('system_modes_examples') + - '/example_modes.yaml') + '/example_modes.yaml') mode_manager = launch.actions.IncludeLaunchDescription( launch.launch_description_sources.PythonLaunchDescriptionSource( @@ -49,4 +48,4 @@ def generate_launch_description(): description.add_action(drive_base) description.add_action(manipulator) - return description \ No newline at end of file + return description diff --git a/system_modes_examples/launch/manipulator.launch.py b/system_modes_examples/launch/manipulator.launch.py index 1e38af4..f70a2a3 100644 --- a/system_modes_examples/launch/manipulator.launch.py +++ b/system_modes_examples/launch/manipulator.launch.py @@ -25,7 +25,7 @@ def generate_launch_description(): launch.actions.DeclareLaunchArgument('modelfile', description='Path to modelfile') modelfile = (ament_index_python.packages.get_package_share_directory('system_modes_examples') + - '/example_modes.yaml') + '/example_modes.yaml') node = launch_ros.actions.Node( package='system_modes_examples', @@ -36,4 +36,4 @@ def generate_launch_description(): description = launch.LaunchDescription() description.add_action(node) - return description \ No newline at end of file + return description diff --git a/system_modes_examples/package.xml b/system_modes_examples/package.xml index eaa3b42..9b9ae7e 100644 --- a/system_modes_examples/package.xml +++ b/system_modes_examples/package.xml @@ -14,6 +14,15 @@ system_modes libboost-program-options-dev + ament_cmake_gtest + ament_cmake_gmock + ament_cmake_pep257 + ament_cmake_flake8 + ament_cmake_cpplint + ament_cmake_cppcheck + ament_cmake_uncrustify + ament_lint_auto + ament_cmake diff --git a/system_modes_examples/src/drive_base.cpp b/system_modes_examples/src/drive_base.cpp index 90554d3..7c2c22f 100644 --- a/system_modes_examples/src/drive_base.cpp +++ b/system_modes_examples/src/drive_base.cpp @@ -18,11 +18,8 @@ #include #include -#include -#include #include -#include -#include +#include using lifecycle_msgs::msg::State; using lifecycle_msgs::msg::Transition; @@ -39,34 +36,36 @@ namespace examples class DriveBase : public LifecycleNode { public: - explicit DriveBase() + DriveBase() : LifecycleNode("drive_base") { RCLCPP_INFO(get_logger(), "Constructed lifecycle node '%s'", this->get_name()); // Parameter declaration - this->declare_parameter("max_speed", + this->declare_parameter( + "max_speed", rclcpp::ParameterValue(rclcpp::PARAMETER_NOT_SET), rcl_interfaces::msg::ParameterDescriptor()); - this->declare_parameter("controller", + this->declare_parameter( + "controller", rclcpp::ParameterValue(rclcpp::PARAMETER_NOT_SET), rcl_interfaces::msg::ParameterDescriptor()); auto param_change_callback = - [this](std::vector parameters) -> rcl_interfaces::msg::SetParametersResult - { - auto result = rcl_interfaces::msg::SetParametersResult(); - result.successful = true; - for (auto parameter : parameters) { - RCLCPP_INFO(this->get_logger(), - "parameter '%s' is now: %s", - parameter.get_name().c_str(), - parameter.value_to_string().c_str() - ); - } - return result; - }; - + [this](std::vector parameters) -> rcl_interfaces::msg::SetParametersResult + { + auto result = rcl_interfaces::msg::SetParametersResult(); + result.successful = true; + for (auto parameter : parameters) { + RCLCPP_INFO( + this->get_logger(), + "parameter '%s' is now: %s", + parameter.get_name().c_str(), + parameter.value_to_string().c_str()); + } + return result; + }; + param_change_callback_handle_ = this->add_on_set_parameters_callback(param_change_callback); } @@ -79,7 +78,7 @@ class DriveBase : public LifecycleNode RCLCPP_INFO(get_logger(), "on_configure()", this->get_name()); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; - }; + } rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_activate(const rclcpp_lifecycle::State &) @@ -87,7 +86,7 @@ class DriveBase : public LifecycleNode RCLCPP_INFO(get_logger(), "on_activate()", this->get_name()); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; - }; + } rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State &) @@ -95,7 +94,7 @@ class DriveBase : public LifecycleNode RCLCPP_INFO(get_logger(), "on_deactivate()", this->get_name()); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; - }; + } rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_cleanup(const rclcpp_lifecycle::State &) @@ -103,7 +102,7 @@ class DriveBase : public LifecycleNode RCLCPP_INFO(get_logger(), "on_cleanup()", this->get_name()); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; - }; + } private: rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr param_change_callback_handle_; diff --git a/system_modes_examples/src/manipulator.cpp b/system_modes_examples/src/manipulator.cpp index d4b99b3..4145da7 100644 --- a/system_modes_examples/src/manipulator.cpp +++ b/system_modes_examples/src/manipulator.cpp @@ -18,11 +18,8 @@ #include #include -#include -#include #include -#include -#include +#include using lifecycle_msgs::msg::State; using lifecycle_msgs::msg::Transition; @@ -39,31 +36,32 @@ namespace examples class Manipulator : public LifecycleNode { public: - explicit Manipulator() + Manipulator() : LifecycleNode("manipulator") { RCLCPP_INFO(get_logger(), "Constructed lifecycle node '%s'", this->get_name()); // Parameter declaration - this->declare_parameter("max_torque", + this->declare_parameter( + "max_torque", rclcpp::ParameterValue(rclcpp::PARAMETER_NOT_SET), rcl_interfaces::msg::ParameterDescriptor()); auto param_change_callback = - [this](std::vector parameters) -> rcl_interfaces::msg::SetParametersResult - { - auto result = rcl_interfaces::msg::SetParametersResult(); - result.successful = true; - for (auto parameter : parameters) { - RCLCPP_INFO(this->get_logger(), - "parameter '%s' is now: %s", - parameter.get_name().c_str(), - parameter.value_to_string().c_str() - ); - } - return result; - }; - + [this](std::vector parameters) -> rcl_interfaces::msg::SetParametersResult + { + auto result = rcl_interfaces::msg::SetParametersResult(); + result.successful = true; + for (auto parameter : parameters) { + RCLCPP_INFO( + this->get_logger(), + "parameter '%s' is now: %s", + parameter.get_name().c_str(), + parameter.value_to_string().c_str()); + } + return result; + }; + param_change_callback_handle_ = this->add_on_set_parameters_callback(param_change_callback); } @@ -76,7 +74,7 @@ class Manipulator : public LifecycleNode RCLCPP_INFO(get_logger(), "on_configure()", this->get_name()); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; - }; + } rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_activate(const rclcpp_lifecycle::State &) @@ -84,7 +82,7 @@ class Manipulator : public LifecycleNode RCLCPP_INFO(get_logger(), "on_activate()", this->get_name()); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; - }; + } rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State &) @@ -92,7 +90,7 @@ class Manipulator : public LifecycleNode RCLCPP_INFO(get_logger(), "on_deactivate()", this->get_name()); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; - }; + } rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_cleanup(const rclcpp_lifecycle::State &) @@ -100,7 +98,7 @@ class Manipulator : public LifecycleNode RCLCPP_INFO(get_logger(), "on_cleanup()", this->get_name()); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; - }; + } private: rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr param_change_callback_handle_; From 57dde71d7e5520de3979ae5a1f76ca848f7437ce Mon Sep 17 00:00:00 2001 From: Arne Nordmann Date: Mon, 3 Aug 2020 15:07:14 +0200 Subject: [PATCH 10/17] Un-boost-ify (#39) * Kicks all boost dependencies * All arguments are passed via launch instead of command line now * Uses specific versions of ros-tooling Signed-off-by: Arne Nordmann --- .github/workflows/lint.yaml | 18 ++--- .github/workflows/test.yaml | 7 +- system_modes/CMakeLists.txt | 3 - .../include/system_modes/mode_manager.hpp | 2 +- .../include/system_modes/mode_monitor.hpp | 2 +- system_modes/launch/mode_manager.launch.py | 21 +++--- system_modes/launch/mode_monitor.launch.py | 39 +++++++---- system_modes/package.xml | 1 - system_modes/src/mode_manager_node.cpp | 55 +-------------- system_modes/src/mode_monitor_node.cpp | 70 +------------------ .../src/system_modes/mode_manager.cpp | 13 ++-- .../src/system_modes/mode_monitor.cpp | 42 ++++++----- system_modes_examples/CMakeLists.txt | 3 - .../launch/drive_base.launch.py | 25 +++---- .../launch/manipulator.launch.py | 25 +++---- system_modes_examples/package.xml | 3 +- 16 files changed, 99 insertions(+), 230 deletions(-) diff --git a/.github/workflows/lint.yaml b/.github/workflows/lint.yaml index c3f8a2a..6a1a2e2 100644 --- a/.github/workflows/lint.yaml +++ b/.github/workflows/lint.yaml @@ -7,9 +7,9 @@ jobs: name: ament_xmllint runs-on: ubuntu-18.04 steps: - - uses: actions/checkout@master - - uses: ros-tooling/setup-ros@master - - uses: ros-tooling/action-ros-lint@master + - uses: actions/checkout@v2 + - uses: ros-tooling/setup-ros@0.0.25 + - uses: ros-tooling/action-ros2-lint@0.0.6 with: linter: xmllint package-name: system_modes system_modes_examples @@ -22,9 +22,9 @@ jobs: matrix: linter: [cppcheck, cpplint, uncrustify] steps: - - uses: actions/checkout@master - - uses: ros-tooling/setup-ros@master - - uses: ros-tooling/action-ros-lint@master + - uses: actions/checkout@v2 + - uses: ros-tooling/setup-ros@0.0.25 + - uses: ros-tooling/action-ros2-lint@0.0.6 with: linter: ${{ matrix.linter }} package-name: system_modes system_modes_examples @@ -37,9 +37,9 @@ jobs: matrix: linter: [flake8, pep257] steps: - - uses: actions/checkout@master - - uses: ros-tooling/setup-ros@master - - uses: ros-tooling/action-ros-lint@master + - uses: actions/checkout@v2 + - uses: ros-tooling/setup-ros@0.0.25 + - uses: ros-tooling/action-ros2-lint@0.0.6 with: linter: ${{ matrix.linter }} package-name: system_modes system_modes_examples diff --git a/.github/workflows/test.yaml b/.github/workflows/test.yaml index ca522ca..b7fa198 100644 --- a/.github/workflows/test.yaml +++ b/.github/workflows/test.yaml @@ -1,4 +1,4 @@ -name: Test diagnostics +name: Test system modes on: pull_request: push: @@ -13,9 +13,10 @@ jobs: matrix: os: [ubuntu-18.04] steps: - - uses: ros-tooling/setup-ros@master - - uses: ros-tooling/action-ros-ci@master + - uses: ros-tooling/setup-ros@0.0.25 + - uses: ros-tooling/action-ros-ci@0.0.15 with: package-name: system_modes system_modes_examples + target-ros2-distro: foxy colcon-mixin-name: coverage-gcc colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml diff --git a/system_modes/CMakeLists.txt b/system_modes/CMakeLists.txt index 24e748a..df26d4c 100644 --- a/system_modes/CMakeLists.txt +++ b/system_modes/CMakeLists.txt @@ -25,7 +25,6 @@ find_package(rclcpp_lifecycle REQUIRED) find_package(rosidl_typesupport_cpp REQUIRED) find_package(rosidl_default_generators REQUIRED) find_package(lifecycle_msgs REQUIRED) -find_package(Boost 1.58 COMPONENTS program_options REQUIRED) # generate service rosidl_generate_interfaces(${PROJECT_NAME} @@ -52,8 +51,6 @@ ament_target_dependencies(mode "lifecycle_msgs" "rosidl_typesupport_cpp" "std_msgs" - "Boost" - "Boost_PROGRAM_OPTIONS" "builtin_interfaces" ) #rosidl_target_interfaces(mode diff --git a/system_modes/include/system_modes/mode_manager.hpp b/system_modes/include/system_modes/mode_manager.hpp index 8e4e8b8..30d0dd9 100644 --- a/system_modes/include/system_modes/mode_manager.hpp +++ b/system_modes/include/system_modes/mode_manager.hpp @@ -40,7 +40,7 @@ namespace system_modes class ModeManager : public rclcpp::Node { public: - explicit ModeManager(const std::string & model_path); + ModeManager(); ModeManager(const ModeManager &) = delete; std::shared_ptr inference(); diff --git a/system_modes/include/system_modes/mode_monitor.hpp b/system_modes/include/system_modes/mode_monitor.hpp index 2ea27ce..4c06c59 100644 --- a/system_modes/include/system_modes/mode_monitor.hpp +++ b/system_modes/include/system_modes/mode_monitor.hpp @@ -37,7 +37,7 @@ namespace system_modes class ModeMonitor : public rclcpp::Node { public: - ModeMonitor(const std::string & model_path, unsigned int rate, bool verbose, bool clear); + ModeMonitor(); ModeMonitor(const ModeMonitor &) = delete; std::shared_ptr inference(); diff --git a/system_modes/launch/mode_manager.launch.py b/system_modes/launch/mode_manager.launch.py index 9b2a510..d9600a8 100644 --- a/system_modes/launch/mode_manager.launch.py +++ b/system_modes/launch/mode_manager.launch.py @@ -21,15 +21,12 @@ def generate_launch_description(): - launch.actions.DeclareLaunchArgument('modelfile', description='Path to modelfile') - - node = launch_ros.actions.Node( - package='system_modes', - node_executable='mode_manager', - parameters=[{'modelfile': launch.substitutions.LaunchConfiguration('modelfile')}], - output='screen') - - description = launch.LaunchDescription() - description.add_action(node) - - return description + return launch.LaunchDescription([ + launch.actions.DeclareLaunchArgument( + 'modelfile', + description='Path to modelfile'), + launch_ros.actions.Node( + package='system_modes', + executable='mode_manager', + parameters=[{'modelfile': launch.substitutions.LaunchConfiguration('modelfile')}], + output='screen')]) diff --git a/system_modes/launch/mode_monitor.launch.py b/system_modes/launch/mode_monitor.launch.py index 2801980..a5ee3e4 100644 --- a/system_modes/launch/mode_monitor.launch.py +++ b/system_modes/launch/mode_monitor.launch.py @@ -15,21 +15,34 @@ import launch import launch.actions -import launch.substitutions import launch_ros.actions def generate_launch_description(): - launch.actions.DeclareLaunchArgument('modelfile', description='Path to modelfile') - - node = launch_ros.actions.Node( - package='system_modes', - node_executable='mode_monitor', - parameters=[{'modelfile': launch.substitutions.LaunchConfiguration('modelfile')}], - output='screen') - - description = launch.LaunchDescription() - description.add_action(node) - - return description + return launch.LaunchDescription([ + launch.actions.DeclareLaunchArgument( + 'modelfile', + description='Path to modelfile'), + launch.actions.DeclareLaunchArgument( + 'debug', + default_value='false', + description='Debug'), + launch.actions.DeclareLaunchArgument( + 'verbose', + default_value='false', + description='Print mode parametrization'), + launch.actions.DeclareLaunchArgument( + 'rate', + default_value='1000', + description='Monitor refresh rate in ms'), + launch_ros.actions.Node( + package='system_modes', + executable='mode_monitor', + parameters=[{ + 'modelfile': launch.substitutions.LaunchConfiguration('modelfile'), + 'debug': launch.substitutions.LaunchConfiguration('debug'), + 'verbose': launch.substitutions.LaunchConfiguration('verbose'), + 'rate': launch.substitutions.LaunchConfiguration('rate') + }], + output='screen')]) diff --git a/system_modes/package.xml b/system_modes/package.xml index 7546b65..b5a310f 100644 --- a/system_modes/package.xml +++ b/system_modes/package.xml @@ -14,7 +14,6 @@ std_msgs builtin_interfaces rosidl_default_generators - libboost-program-options-dev ament_cmake_gtest ament_cmake_gmock diff --git a/system_modes/src/mode_manager_node.cpp b/system_modes/src/mode_manager_node.cpp index 92cf8d2..47ba776 100644 --- a/system_modes/src/mode_manager_node.cpp +++ b/system_modes/src/mode_manager_node.cpp @@ -18,8 +18,6 @@ #include #include -#include - #include #include #include @@ -46,47 +44,12 @@ using system_modes::msg::ModeEvent; using lifecycle_msgs::msg::State; using lifecycle_msgs::msg::TransitionEvent; -using boost::program_options::value; -using boost::program_options::variables_map; -using boost::program_options::command_line_parser; -using boost::program_options::options_description; -using boost::program_options::positional_options_description; - using rcl_interfaces::msg::ParameterType; using rcl_interfaces::msg::ParameterEvent; string modelfile, loglevel; -options_description options("Allowed options"); - shared_ptr manager; -bool parseOptions(int argc, char * argv[]) -{ - options.add_options()("help", "Help message and options")( - "modelfile", value(&modelfile), - "Path to yaml model file")( - "__log_level", value(&loglevel), - "ROS2 log level")( - "ros-args", value>()->multitoken(), "ROS args")( - "params-file", value>()->multitoken(), "ROS params file"); - - positional_options_description positional_options; - positional_options.add("modelfile", 1); - - variables_map map; - store( - command_line_parser(argc, argv) - .options(options) - .positional(positional_options) - .run(), map); - notify(map); - - if (map.count("help")) { - return true; - } - return false; -} - void transition_callback( const TransitionEvent::SharedPtr msg, const string & node_name) @@ -147,26 +110,10 @@ int main(int argc, char * argv[]) { using namespace std::placeholders; - // Handle commandline arguments. - try { - if (parseOptions(argc, argv)) { - cout << "Usage: mode_manager MODELFILE" << std::endl; - cout << options; - cout << "Or specify the MODELFILE by ROS parameter 'modelfile'." << std::endl << std::endl; - return EXIT_SUCCESS; - } - } catch (const std::exception & e) { - cerr << "Error parsing command line: " << e.what() << std::endl; - cout << "Usage: mode_manager [MODELFILE]" << std::endl; - cout << options; - cout << "Or specify the MODELFILE by ROS parameter 'modelfile'." << std::endl << std::endl; - return EXIT_FAILURE; - } - setvbuf(stdout, NULL, _IONBF, BUFSIZ); rclcpp::init(argc, argv); - manager = std::make_shared(modelfile); + manager = std::make_shared(); vector>> state_sub_; diff --git a/system_modes/src/mode_monitor_node.cpp b/system_modes/src/mode_monitor_node.cpp index 14b7c23..27dd139 100644 --- a/system_modes/src/mode_monitor_node.cpp +++ b/system_modes/src/mode_monitor_node.cpp @@ -18,8 +18,6 @@ #include #include -#include - #include #include #include @@ -49,13 +47,6 @@ using system_modes::msg::ModeEvent; using lifecycle_msgs::msg::State; using lifecycle_msgs::msg::TransitionEvent; -using boost::program_options::value; -using boost::program_options::bool_switch; -using boost::program_options::variables_map; -using boost::program_options::command_line_parser; -using boost::program_options::options_description; -using boost::program_options::positional_options_description; - using rcl_interfaces::msg::ParameterType; using rcl_interfaces::msg::ParameterEvent; @@ -63,52 +54,9 @@ string modelfile, loglevel; bool debug = false; unsigned int rate = 1000; bool verbose = false; -options_description options("Allowed options"); shared_ptr monitor; -bool parseOptions(int argc, char * argv[]) -{ - options.add_options()("help", "Help message and options")( - "modelfile", - value(&modelfile), "Path to yaml model file")( - "__log_level", - value(&loglevel), "ROS 2 log level")( - "debug,d", bool_switch(&debug)->default_value(false), - "Debug mode (don't clear screen)")( - "rate,r", value(&rate)->default_value(1000), - "Update rate in milliseconds")( - "verbose,v", bool_switch(&verbose)->default_value(false), - "Verbose (displays mode parameters)")( - "ros-args", value>()->multitoken(), - "ROS args")( - "params-file", value>()->multitoken(), - "ROS params file"); - - positional_options_description positional_options; - positional_options.add("modelfile", 1); - positional_options.add("debug", 0); - positional_options.add("rate", 1); - positional_options.add("verbose", 0); - - variables_map map; - store( - command_line_parser(argc, argv) - .options(options) - .positional(positional_options) - .run(), map); - notify(map); - - if (map.count("help")) { - return true; - } - - // if (modelfile.empty()) { - // throw invalid_argument("Need path to model file."); - // } - return false; -} - void transition_callback( const TransitionEvent::SharedPtr msg, const string & node_name) @@ -169,26 +117,10 @@ int main(int argc, char * argv[]) { using namespace std::placeholders; - // Handle commandline arguments. - try { - if (parseOptions(argc, argv)) { - cout << "Usage: mode_monitor MODELFILE" << endl; - cout << options; - cout << "Or specify the MODELFILE by ROS parameter 'modelfile'." << std::endl << std::endl; - return EXIT_SUCCESS; - } - } catch (const exception & e) { - cerr << "Error parsing command line: " << e.what() << endl; - cout << "Usage: mode_monitor MODELFILE" << endl; - cout << options; - cout << "Or specify the MODELFILE by ROS parameter 'modelfile'." << std::endl << std::endl; - return EXIT_FAILURE; - } - setvbuf(stdout, NULL, _IONBF, BUFSIZ); rclcpp::init(argc, argv); - monitor = make_shared(modelfile, rate, verbose, !debug); + monitor = make_shared(); vector>> state_sub_; diff --git a/system_modes/src/system_modes/mode_manager.cpp b/system_modes/src/system_modes/mode_manager.cpp index 01e84f9..fa409fb 100644 --- a/system_modes/src/system_modes/mode_manager.cpp +++ b/system_modes/src/system_modes/mode_manager.cpp @@ -55,7 +55,7 @@ using namespace std::chrono_literals; namespace system_modes { -ModeManager::ModeManager(const string & model_path) +ModeManager::ModeManager() : Node("__mode_manager"), mode_inference_(nullptr), state_change_srv_(), get_state_srv_(), states_srv_(), @@ -64,16 +64,11 @@ ModeManager::ModeManager(const string & model_path) state_request_pub_(), mode_request_pub_() { declare_parameter("modelfile", rclcpp::ParameterValue(std::string(""))); + std::string model_path = get_parameter("modelfile").as_string(); if (model_path.empty()) { - rclcpp::Parameter parameter = get_parameter("modelfile"); - std::string alt_model_path = parameter.get_value(); - if (alt_model_path.empty()) { - throw std::invalid_argument("Need path to model file."); - } - mode_inference_ = std::make_shared(alt_model_path); - } else { - mode_inference_ = std::make_shared(model_path); + throw std::invalid_argument("Need path to model file."); } + mode_inference_ = std::make_shared(model_path); for (auto system : this->mode_inference_->get_systems()) { this->add_system(system); diff --git a/system_modes/src/system_modes/mode_monitor.cpp b/system_modes/src/system_modes/mode_monitor.cpp index ee4e41b..521b926 100644 --- a/system_modes/src/system_modes/mode_monitor.cpp +++ b/system_modes/src/system_modes/mode_monitor.cpp @@ -51,6 +51,10 @@ using namespace std::literals::string_literals; namespace system_modes { +static const bool MONITOR_DEFAULT_VERBOSITY = false; +static const bool MONITOR_DEFAULT_DEBUG = false; +static const unsigned int MONITOR_DEFAULT_RATE_MS = 1000; + static const unsigned int MONITOR_WIDTH_PART = 25; static const unsigned int MONITOR_WIDTH_STATE = 30; static const unsigned int MONITOR_WIDTH_MODE = 30; @@ -70,27 +74,29 @@ static const string MONITOR_TEXT_WARN = "\033[21;"s + to_string(MONITOR_TEXT_WAR static const string MONITOR_SEPARATOR = MONITOR_TEXT_PLAIN + " | "; static const string MONITOR_SEPARATOR_BOLD = MONITOR_TEXT_BOLD + " | "; -ModeMonitor::ModeMonitor( - const string & model_path, - unsigned int rate = 1000, - bool verbose = false, - bool clear = true) +ModeMonitor::ModeMonitor() : Node("__mode_monitor"), - mode_inference_(), - model_path_(model_path), - rate_(rate), - clear_screen_(clear), - verbose_(verbose) + mode_inference_() { - RCLCPP_DEBUG(get_logger(), "Constructed mode monitor"); - - declare_parameter("modelfile", rclcpp::ParameterValue(std::string(""))); + declare_parameter( + "modelfile", + rclcpp::ParameterValue(std::string(""))); + declare_parameter( + "rate", + rclcpp::ParameterValue(static_cast(MONITOR_DEFAULT_RATE_MS))); + declare_parameter( + "debug", + rclcpp::ParameterValue(static_cast(MONITOR_DEFAULT_DEBUG))); + declare_parameter( + "verbose", + rclcpp::ParameterValue(static_cast(MONITOR_DEFAULT_VERBOSITY))); + + rate_ = get_parameter("rate").as_int(); + clear_screen_ = !get_parameter("debug").as_bool(); + verbose_ = get_parameter("verbose").as_bool(); + model_path_ = get_parameter("modelfile").as_string(); if (model_path_.empty()) { - rclcpp::Parameter parameter = get_parameter("modelfile"); - model_path_ = parameter.get_value(); - if (model_path_.empty()) { - throw std::invalid_argument("Need path to model file."); - } + throw std::invalid_argument("Need path to model file."); } mode_inference_ = std::make_shared(model_path_); diff --git a/system_modes_examples/CMakeLists.txt b/system_modes_examples/CMakeLists.txt index 7e71162..c6168e6 100644 --- a/system_modes_examples/CMakeLists.txt +++ b/system_modes_examples/CMakeLists.txt @@ -24,7 +24,6 @@ find_package(std_msgs REQUIRED) find_package(rosidl_typesupport_cpp REQUIRED) find_package(rosidl_default_generators REQUIRED) find_package(lifecycle_msgs REQUIRED) -find_package(Boost 1.58 COMPONENTS program_options REQUIRED) find_package(system_modes REQUIRED) # drive_base @@ -39,7 +38,6 @@ ament_target_dependencies(drive_base "lifecycle_msgs" "rosidl_typesupport_cpp" "std_msgs" - "Boost" "system_modes" ) install(TARGETS drive_base @@ -58,7 +56,6 @@ ament_target_dependencies(manipulator "lifecycle_msgs" "rosidl_typesupport_cpp" "std_msgs" - "Boost" "system_modes" ) install(TARGETS manipulator diff --git a/system_modes_examples/launch/drive_base.launch.py b/system_modes_examples/launch/drive_base.launch.py index 76d6c5b..4036980 100644 --- a/system_modes_examples/launch/drive_base.launch.py +++ b/system_modes_examples/launch/drive_base.launch.py @@ -17,23 +17,16 @@ import launch import launch.actions -import launch.launch_description_sources -import launch.substitutions import launch_ros def generate_launch_description(): - launch.actions.DeclareLaunchArgument('modelfile', description='Path to modelfile') - modelfile = (ament_index_python.packages.get_package_share_directory('system_modes_examples') + - '/example_modes.yaml') - - node = launch_ros.actions.Node( - package='system_modes_examples', - node_executable='drive_base', - parameters=[{'modelfile': modelfile}], - output='screen') - - description = launch.LaunchDescription() - description.add_action(node) - - return description + default_modelfile = ( + ament_index_python.packages.get_package_share_directory('system_modes_examples') + + '/example_modes.yaml') + return launch.LaunchDescription([ + launch_ros.actions.Node( + package='system_modes_examples', + executable='drive_base', + parameters=[{'modelfile': default_modelfile}], + output='screen')]) diff --git a/system_modes_examples/launch/manipulator.launch.py b/system_modes_examples/launch/manipulator.launch.py index f70a2a3..ea44a41 100644 --- a/system_modes_examples/launch/manipulator.launch.py +++ b/system_modes_examples/launch/manipulator.launch.py @@ -17,23 +17,16 @@ import launch import launch.actions -import launch.launch_description_sources -import launch.substitutions import launch_ros def generate_launch_description(): - launch.actions.DeclareLaunchArgument('modelfile', description='Path to modelfile') - modelfile = (ament_index_python.packages.get_package_share_directory('system_modes_examples') + - '/example_modes.yaml') - - node = launch_ros.actions.Node( - package='system_modes_examples', - node_executable='manipulator', - parameters=[{'modelfile': modelfile}], - output='screen') - - description = launch.LaunchDescription() - description.add_action(node) - - return description + default_modelfile = ( + ament_index_python.packages.get_package_share_directory('system_modes_examples') + + '/example_modes.yaml') + return launch.LaunchDescription([ + launch_ros.actions.Node( + package='system_modes_examples', + executable='manipulator', + parameters=[{'modelfile': default_modelfile}], + output='screen')]) diff --git a/system_modes_examples/package.xml b/system_modes_examples/package.xml index 9b9ae7e..b2a1a1c 100644 --- a/system_modes_examples/package.xml +++ b/system_modes_examples/package.xml @@ -12,7 +12,6 @@ rclcpp rclcpp_lifecycle system_modes - libboost-program-options-dev ament_cmake_gtest ament_cmake_gmock @@ -22,7 +21,7 @@ ament_cmake_cppcheck ament_cmake_uncrustify ament_lint_auto - + ament_cmake From 8ea52a239fbc0dbc3236800f93db706938d94652 Mon Sep 17 00:00:00 2001 From: Arne Nordmann Date: Wed, 23 Sep 2020 12:01:22 +0200 Subject: [PATCH 11/17] Stricter constness in mode inference Signed-off-by: Arne Nordmann --- .gitignore | 1 + .../include/system_modes/mode_inference.hpp | 21 +++--- .../src/system_modes/mode_inference.cpp | 64 ++++++++++--------- 3 files changed, 46 insertions(+), 40 deletions(-) create mode 100644 .gitignore diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..722d5e7 --- /dev/null +++ b/.gitignore @@ -0,0 +1 @@ +.vscode diff --git a/system_modes/include/system_modes/mode_inference.hpp b/system_modes/include/system_modes/mode_inference.hpp index 1e12f24..9f8e92d 100644 --- a/system_modes/include/system_modes/mode_inference.hpp +++ b/system_modes/include/system_modes/mode_inference.hpp @@ -53,27 +53,29 @@ class ModeInference virtual void update_param(const std::string &, rclcpp::Parameter &); virtual void update_target(const std::string &, StateAndMode); - virtual StateAndMode get(const std::string & part); - virtual StateAndMode get_or_infer(const std::string & part); + 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); - virtual StateAndMode infer_system(const std::string & part); - virtual StateAndMode infer_node(const std::string & part); + 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_target(const std::string & part); - virtual ModeConstPtr get_mode(const std::string & part, const std::string & mode); - virtual std::vector get_available_modes(const std::string & part); + virtual StateAndMode get_target(const std::string & part) const; + virtual ModeConstPtr get_mode(const std::string & part, const std::string & mode) 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 &); + 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 &); private: StatesMap nodes_, nodes_target_; StatesMap systems_, systems_target_; + Deviation systems_transitions_; + std::map modes_; ParametersMap parameters_; @@ -83,6 +85,7 @@ class ModeInference param_mutex_; mutable std::shared_timed_mutex nodes_target_mutex_, systems_target_mutex_; + mutable std::shared_timed_mutex systems_transitions_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 f86c051..f4f130e 100644 --- a/system_modes/src/system_modes/mode_inference.cpp +++ b/system_modes/src/system_modes/mode_inference.cpp @@ -44,9 +44,11 @@ namespace system_modes ModeInference::ModeInference(const string & model_path) : nodes_(), nodes_target_(), systems_(), systems_target_(), + systems_transitions_(), modes_(), nodes_mutex_(), systems_mutex_(), modes_mutex_(), parts_mutex_(), - nodes_target_mutex_(), systems_target_mutex_() + nodes_target_mutex_(), systems_target_mutex_(), + systems_transitions_mutex_() { this->read_modes_from_model(model_path); } @@ -128,7 +130,7 @@ ModeInference::update_target(const string & part, StateAndMode mode) } StateAndMode -ModeInference::get_target(const string & part) +ModeInference::get_target(const string & part) const { std::shared_lock ntlock(this->nodes_target_mutex_); std::shared_lock stlock(this->systems_target_mutex_); @@ -137,10 +139,10 @@ ModeInference::get_target(const string & part) auto its = this->systems_target_.find(part); if (it != this->nodes_target_.end()) { // we know this node - return this->nodes_target_[part]; + return this->nodes_target_.at(part); } else if (its != this->systems_target_.end()) { // we know the system, probably from a mode manager - return this->systems_target_[part]; + return this->systems_target_.at(part); } // might be a system without explicit mode manager, trying to infer the mode @@ -148,7 +150,7 @@ ModeInference::get_target(const string & part) } StateAndMode -ModeInference::get(const string & part) +ModeInference::get(const string & part) const { std::shared_lock nlock(this->nodes_mutex_); @@ -157,15 +159,15 @@ ModeInference::get(const string & part) throw std::out_of_range("Unknown system or node '" + part + "'."); } - if (this->nodes_[part].state == 0 && this->nodes_[part].mode.empty()) { + if (this->nodes_.at(part).state == 0 && this->nodes_.at(part).mode.empty()) { throw std::runtime_error("No solid information about state and mode of '" + part + "'."); } - return this->nodes_[part]; + return this->nodes_.at(part); } StateAndMode -ModeInference::infer(const string & part) +ModeInference::infer(const string & part) const { std::shared_lock slock(this->systems_mutex_); std::shared_lock nlock(this->nodes_mutex_); @@ -187,13 +189,13 @@ ModeInference::infer(const string & part) } StateAndMode -ModeInference::infer_system(const string & part) +ModeInference::infer_system(const string & part) const { unsigned int state = 0; // unknown string mode = ""; std::shared_lock mlock(this->modes_mutex_); - auto default_mode = this->modes_[part][DEFAULT_MODE]; + auto default_mode = this->modes_.at(part).at(DEFAULT_MODE); if (!default_mode) { throw std::out_of_range( "Can't infer for system '" + part + @@ -252,8 +254,8 @@ ModeInference::infer_system(const string & part) return StateAndMode(State::PRIMARY_STATE_INACTIVE, ""); } else if (targetState == State::PRIMARY_STATE_ACTIVE) { ModeConstPtr mode; - if (this->modes_[part].find(targetMode) != this->modes_[part].end()) { - auto mode = this->modes_[part][targetMode]; + if (this->modes_.at(part).find(targetMode) != this->modes_.at(part).end()) { + auto mode = this->modes_.at(part).at(targetMode); // target: active auto inTargetMode = true; @@ -289,7 +291,7 @@ ModeInference::infer_system(const string & part) } // Check all remaining modes. Are we in any mode at all? - for (auto mode : this->modes_[part]) { + for (auto mode : this->modes_.at(part)) { bool foundMode = true; for (auto partpart : default_mode->get_parts()) { auto targetStateAndMode = mode.second->get_part_mode(partpart); @@ -322,12 +324,12 @@ ModeInference::infer_system(const string & part) } StateAndMode -ModeInference::infer_node(const string & part) +ModeInference::infer_node(const string & part) const { std::shared_lock mlock(this->modes_mutex_); std::shared_lock prlock(this->param_mutex_); - auto default_mode = this->modes_[part][DEFAULT_MODE]; + auto default_mode = this->modes_.at(part).at(DEFAULT_MODE); if (!default_mode) { throw std::out_of_range( "Can't infer for node '" + part + @@ -344,13 +346,13 @@ ModeInference::infer_node(const string & part) bool inTargetMode = true; // we know the target mode, so check this one first - if (this->modes_[part].find(targetMode) != this->modes_[part].end()) { - auto mode = this->modes_[part][targetMode]; + if (this->modes_.at(part).find(targetMode) != this->modes_.at(part).end()) { + auto mode = this->modes_.at(part).at(targetMode); for (auto param : mode->get_parameter_names()) { if (!matching_parameters( mode->get_parameter(param), - parameters_[part][param])) + parameters_.at(part).at(param))) { inTargetMode = false; continue; @@ -367,11 +369,11 @@ ModeInference::infer_node(const string & part) // no target mode, so next we check the default mode bool inDefaultMode = true; - auto defaultMode = this->modes_[part][DEFAULT_MODE]; + auto defaultMode = this->modes_.at(part).at(DEFAULT_MODE); for (auto param : defaultMode->get_parameter_names()) { if (!matching_parameters( defaultMode->get_parameter(param), - parameters_[part][param])) + parameters_.at(part).at(param))) { inDefaultMode = false; continue; @@ -382,13 +384,13 @@ ModeInference::infer_node(const string & part) } // no target mode, not default mode, so we try our luck, infering any mode from parameters - for (auto mode : this->modes_[part]) { - auto m = this->modes_[part][mode.first]; + for (auto mode : this->modes_.at(part)) { + auto m = this->modes_.at(part).at(mode.first); bool foundMode = true; for (auto param : defaultMode->get_parameter_names()) { if (!matching_parameters( m->get_parameter(param), - parameters_[part][param])) + parameters_.at(part).at(param))) { foundMode = false; continue; @@ -404,7 +406,7 @@ ModeInference::infer_node(const string & part) } StateAndMode -ModeInference::get_or_infer(const string & part) +ModeInference::get_or_infer(const string & part) const { StateAndMode stateAndMode; try { @@ -437,16 +439,16 @@ ModeInference::get_or_infer(const string & part) } ModeConstPtr -ModeInference::get_mode(const string & part, const string & mode) +ModeInference::get_mode(const string & part, const string & mode) const { std::shared_lock mlock(this->modes_mutex_); auto it = this->modes_.find(part); if (it != this->modes_.end()) { // We know this part - auto its = this->modes_[part].find(mode); - if (its != this->modes_[part].end()) { - return this->modes_[part][mode]; + auto its = this->modes_.at(part).find(mode); + if (its != this->modes_.at(part).end()) { + return this->modes_.at(part).at(mode); } return nullptr; } @@ -454,10 +456,10 @@ ModeInference::get_mode(const string & part, const string & mode) } std::vector -ModeInference::get_available_modes(const std::string & part) +ModeInference::get_available_modes(const std::string & part) const { std::vector modes; - for (auto mode : this->modes_[part]) { + for (auto mode : this->modes_.at(part)) { modes.push_back(mode.first); } return modes; @@ -614,7 +616,7 @@ ModeInference::get_all_parts_of(const string & system) const } bool -ModeInference::matching_parameters(const Parameter & target, const Parameter & actual) +ModeInference::matching_parameters(const Parameter & target, const Parameter & actual) const { // TODO(anordman): consider DONTCARE and value ranges From ff8f91d146747cd18b1dd1d9b067a045d077416a Mon Sep 17 00:00:00 2001 From: Arne Nordmann Date: Wed, 23 Sep 2020 13:02:05 +0200 Subject: [PATCH 12/17] 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 568153451222b594ab03a5856277f02b8f629aad Mon Sep 17 00:00:00 2001 From: Arne Nordmann Date: Wed, 23 Sep 2020 15:04:16 +0200 Subject: [PATCH 13/17] 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 412fe6144258d1a1b2f405019f54efdf207a0152 Mon Sep 17 00:00:00 2001 From: Arne Nordmann Date: Wed, 23 Sep 2020 15:05:37 +0200 Subject: [PATCH 14/17] 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 c883230a89d522679503c7320899962df2079fae Mon Sep 17 00:00:00 2001 From: Arne Nordmann Date: Tue, 29 Sep 2020 14:54:41 +0200 Subject: [PATCH 15/17] 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 From f2305e3df46f5c38b183875849057708b88c8d2e Mon Sep 17 00:00:00 2001 From: Arne Nordmann Date: Wed, 30 Sep 2020 11:38:40 +0200 Subject: [PATCH 16/17] Version push to 0.4.0 Signed-off-by: Arne Nordmann --- system_modes/changelog.rst | 6 ++++++ system_modes/package.xml | 10 ++++++++-- system_modes_examples/changelog.rst | 11 ++++++++--- system_modes_examples/package.xml | 7 +++++-- 4 files changed, 27 insertions(+), 7 deletions(-) diff --git a/system_modes/changelog.rst b/system_modes/changelog.rst index 194bbcd..6c30b07 100644 --- a/system_modes/changelog.rst +++ b/system_modes/changelog.rst @@ -2,6 +2,12 @@ Changelog for package system_modes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.4.0 (2020-09-30) +----------- +* mode event now including start and goal mode +* publish inferred state and mode transitions +* https://github.com/micro-ROS/system_modes/issues/42 + 0.3.0 (2020-07-23) ----------- * removed boost dependencies (was: program options) diff --git a/system_modes/package.xml b/system_modes/package.xml index a81f49f..9a1809a 100644 --- a/system_modes/package.xml +++ b/system_modes/package.xml @@ -2,8 +2,14 @@ system_modes - 0.3.0 - Model-based distributed configuration handling. + 0.4.0 + + The system modes concept assumes that a robotics system is built + from components with a lifecycle. It adds a notion of (sub-)systems, + hiararchically grouping these nodes, as well as a notion of modes + that determine the configuration of these nodes and (sub-)systems in + terms of their parameter values. + Arne Nordmann Apache License 2.0 diff --git a/system_modes_examples/changelog.rst b/system_modes_examples/changelog.rst index 194bbcd..93d225c 100644 --- a/system_modes_examples/changelog.rst +++ b/system_modes_examples/changelog.rst @@ -1,6 +1,11 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package system_modes -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package system_modes_examples +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.4.0 (2020-09-30) +----------- +* publish inferred state and mode transitions +* https://github.com/micro-ROS/system_modes/issues/42 0.3.0 (2020-07-23) ----------- diff --git a/system_modes_examples/package.xml b/system_modes_examples/package.xml index a600fad..98eb97c 100644 --- a/system_modes_examples/package.xml +++ b/system_modes_examples/package.xml @@ -2,8 +2,11 @@ system_modes_examples - 0.3.0 - Simple example system for system_modes package. + 0.4.0 + + Simple example system and according launch files for the system_modes + package. + Arne Nordmann Apache License 2.0 From 0ada932b88c862834cbd4392543463699bac7fac Mon Sep 17 00:00:00 2001 From: Arne Nordmann Date: Wed, 30 Sep 2020 14:33:08 +0200 Subject: [PATCH 17/17] Add dependencies to launch * Adding exec_depend to launch_ros to system_modes package due to included launch files * Adding exec_depend to ros2launch to system_modes_examples package due to step-by-step CLI example Signed-off-by: Arne Nordmann --- system_modes/package.xml | 2 ++ system_modes_examples/package.xml | 2 ++ 2 files changed, 4 insertions(+) diff --git a/system_modes/package.xml b/system_modes/package.xml index 9a1809a..c223088 100644 --- a/system_modes/package.xml +++ b/system_modes/package.xml @@ -21,6 +21,8 @@ builtin_interfaces rosidl_default_generators + launch_ros + ament_cmake_gtest ament_cmake_gmock ament_cmake_pep257 diff --git a/system_modes_examples/package.xml b/system_modes_examples/package.xml index 98eb97c..6ef798d 100644 --- a/system_modes_examples/package.xml +++ b/system_modes_examples/package.xml @@ -16,6 +16,8 @@ rclcpp_lifecycle system_modes + ros2launch + ament_cmake_gtest ament_cmake_gmock ament_cmake_pep257