diff --git a/.github/workflows/lint.yaml b/.github/workflows/lint.yaml new file mode 100644 index 0000000..6a1a2e2 --- /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@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 + + 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@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 + + ament_lint_py: + name: ament_${{ matrix.linter }} + runs-on: ubuntu-18.04 + strategy: + fail-fast: false + matrix: + linter: [flake8, pep257] + steps: + - 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 new file mode 100644 index 0000000..b7fa198 --- /dev/null +++ b/.github/workflows/test.yaml @@ -0,0 +1,22 @@ +name: Test system modes +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@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/.gitignore b/.gitignore new file mode 100644 index 0000000..722d5e7 --- /dev/null +++ b/.gitignore @@ -0,0 +1 @@ +.vscode diff --git a/system_modes/CMakeLists.txt b/system_modes/CMakeLists.txt index e930be7..18dc3d9 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} @@ -53,8 +52,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/changelog.rst b/system_modes/changelog.rst index 22b514e..6c30b07 100644 --- a/system_modes/changelog.rst +++ b/system_modes/changelog.rst @@ -2,6 +2,27 @@ 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) +* 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 + +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/include/system_modes/mode_inference.hpp b/system_modes/include/system_modes/mode_inference.hpp index 35538fe..eaf454a 100644 --- a/system_modes/include/system_modes/mode_inference.hpp +++ b/system_modes/include/system_modes/mode_inference.hpp @@ -54,31 +54,40 @@ 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(const std::string & part) const; virtual StateAndMode get_or_infer(const std::string & part); 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_system(const std::string & part); - 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); - + /** + * 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 Deviation get_deviation(); + 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: ModeHandling * mode_handling_; - StatesMap nodes_, nodes_target_; - StatesMap systems_, systems_target_; + StatesMap nodes_, nodes_target_, nodes_cache_; + StatesMap systems_, systems_target_, systems_cache_; std::map modes_; ParametersMap parameters_; @@ -88,6 +97,8 @@ class ModeInference param_mutex_; mutable std::shared_timed_mutex nodes_target_mutex_, systems_target_mutex_; + mutable std::shared_timed_mutex + nodes_cache_mutex_, systems_cache_mutex_; }; } // namespace system_modes diff --git a/system_modes/include/system_modes/mode_manager.hpp b/system_modes/include/system_modes/mode_manager.hpp index d593810..2c09019 100644 --- a/system_modes/include/system_modes/mode_manager.hpp +++ b/system_modes/include/system_modes/mode_manager.hpp @@ -41,11 +41,11 @@ 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(); - virtual void handle_system_deviation(const std::string& reason); + virtual void handle_system_deviation(const std::string & reason); virtual ~ModeManager() = default; @@ -67,26 +67,32 @@ 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 &); + + virtual void publish_transitions(); private: std::shared_ptr mode_inference_; @@ -118,16 +124,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/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 1e1089b..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', - 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 81f3923..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', - 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/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 diff --git a/system_modes/package.xml b/system_modes/package.xml index 430e505..c223088 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 @@ -14,7 +20,8 @@ std_msgs builtin_interfaces rosidl_default_generators - libboost-program-options-dev + + launch_ros 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 15b8f09..9eee40c 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) @@ -150,26 +113,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..e97a6df 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) @@ -121,7 +69,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 +93,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 @@ -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_handling.cpp b/system_modes/src/system_modes/mode_handling.cpp index cb6628a..f680db7 100644 --- a/system_modes/src/system_modes/mode_handling.cpp +++ b/system_modes/src/system_modes/mode_handling.cpp @@ -88,13 +88,15 @@ ModeHandling::add_rule( string rule_name_ = rule_name.substr(0, split); if (rule_spec.compare("if_target") != 0 && - rule_spec.compare("if_part") != 0 && - rule_spec.compare("new_target") != 0) { + rule_spec.compare("if_part") != 0 && + rule_spec.compare("new_target") != 0) + { throw std::runtime_error("ModeHandling::add_rule() can't parse rule spec."); } if (rule_param.get_type() != ParameterType::PARAMETER_STRING && - rule_param.get_type() != ParameterType::PARAMETER_STRING_ARRAY) { + rule_param.get_type() != ParameterType::PARAMETER_STRING_ARRAY) + { throw std::runtime_error("ModeHandling::add_rule() rule is neither string nor string array."); } 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/src/system_modes/mode_inference.cpp b/system_modes/src/system_modes/mode_inference.cpp index 11ffbf2..f242481 100644 --- a/system_modes/src/system_modes/mode_inference.cpp +++ b/system_modes/src/system_modes/mode_inference.cpp @@ -129,7 +129,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_); @@ -138,10 +138,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 @@ -149,7 +149,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_); @@ -158,11 +158,11 @@ 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 @@ -194,7 +194,7 @@ ModeInference::infer_system(const string & part) 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 + @@ -210,6 +210,7 @@ ModeInference::infer_system(const string & part) // error-processing? if (stateAndMode.state == State::TRANSITION_STATE_ERRORPROCESSING) { + this->systems_[part] = StateAndMode(State::TRANSITION_STATE_ERRORPROCESSING, ""); return StateAndMode(State::TRANSITION_STATE_ERRORPROCESSING, ""); } @@ -223,6 +224,7 @@ ModeInference::infer_system(const string & part) "', inference failed."); } } + this->systems_[part] = StateAndMode(state, ""); return StateAndMode(state, ""); } @@ -240,6 +242,7 @@ ModeInference::infer_system(const string & part) // 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, ""); } @@ -247,14 +250,15 @@ ModeInference::infer_system(const string & part) 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, ""); } } 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; @@ -265,6 +269,7 @@ ModeInference::infer_system(const string & part) // TODO(anordman): consider DONT-CARE if (stateAndMode.state == State::TRANSITION_STATE_ERRORPROCESSING) { + this->systems_[part] = StateAndMode(State::TRANSITION_STATE_ERRORPROCESSING, ""); return StateAndMode(State::TRANSITION_STATE_ERRORPROCESSING, ""); } @@ -285,12 +290,13 @@ ModeInference::infer_system(const string & part) } if (inTargetMode) { // Target state and target mode reached, all good! + this->systems_[part] = StateAndMode(State::PRIMARY_STATE_ACTIVE, targetMode); return StateAndMode(State::PRIMARY_STATE_ACTIVE, targetMode); } } // 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); @@ -298,6 +304,7 @@ ModeInference::infer_system(const string & part) // TODO(anordman): consider DONT-CARE if (stateAndMode.state == State::TRANSITION_STATE_ERRORPROCESSING) { + this->systems_[part] = StateAndMode(State::TRANSITION_STATE_ERRORPROCESSING, ""); return StateAndMode(State::TRANSITION_STATE_ERRORPROCESSING, ""); } @@ -312,10 +319,12 @@ ModeInference::infer_system(const string & part) } if (foundMode) { // We are in a non-target mode, this means we are still activating + this->systems_[part] = StateAndMode(State::TRANSITION_STATE_ACTIVATING, mode.first); return StateAndMode(State::TRANSITION_STATE_ACTIVATING, mode.first); } } + this->systems_[part] = StateAndMode(State::TRANSITION_STATE_ACTIVATING, ""); return StateAndMode(State::TRANSITION_STATE_ACTIVATING, ""); } @@ -328,7 +337,7 @@ ModeInference::infer_node(const string & part) std::shared_lock mlock(this->modes_mutex_); std::shared_lock prlock(this->param_mutex_); - auto default_mode = this->modes_[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 + @@ -345,13 +354,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; @@ -368,11 +377,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; @@ -383,13 +392,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; @@ -438,16 +447,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; } @@ -455,10 +464,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; @@ -615,7 +624,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 @@ -653,12 +662,70 @@ 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; +} + Deviation ModeInference::get_deviation() { Deviation deviation; - for (auto const& part : get_all_parts()) { + for (auto const & part : get_all_parts()) { try { auto actual = get_or_infer(part); auto target = get_target(part); diff --git a/system_modes/src/system_modes/mode_manager.cpp b/system_modes/src/system_modes/mode_manager.cpp index 7cefafb..212c952 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; @@ -55,25 +56,23 @@ using namespace std::chrono_literals; namespace system_modes { -ModeManager::ModeManager(const string & model_path) +ModeManager::ModeManager() : Node("__mode_manager"), mode_inference_(nullptr), - model_path_(model_path), + mode_handling_(nullptr), 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(""))); - 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."); - } + std::string model_path = get_parameter("modelfile").as_string(); + if (model_path.empty()) { + throw std::invalid_argument("Need path to model file."); } - mode_inference_ = std::make_shared(model_path_); - mode_handling_ = std::make_shared(model_path_); + mode_inference_ = std::make_shared(model_path); + mode_handling_ = std::make_shared(model_path); for (auto system : this->mode_inference_->get_systems()) { this->add_system(system); @@ -95,11 +94,10 @@ ModeManager::ModeManager(const string & model_path) RCLCPP_INFO(get_logger(), " - %s/get_available_modes", node.c_str()); } - // rules timer - periodic_timer_ = this->create_wall_timer( - 200ms, + transition_timer_ = this->create_wall_timer( + 1s, [this]() { - this->handle_system_deviation("timer"); + this->publish_transitions(); }); } @@ -112,94 +110,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, std::bind(&ModeManager::on_change_mode, this, _1, _2, _3)); + 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, - std::bind(&ModeManager::on_get_mode, this, _1, _2, _3)); + 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, std::bind(&ModeManager::on_get_available_modes, this, _1, _2, _3)); + 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, std::bind(&ModeManager::on_change_mode, this, _1, _2, _3)); + 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, - std::bind(&ModeManager::on_get_mode, this, _1, _2, _3)); + 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, std::bind(&ModeManager::on_get_available_modes, this, _1, _2, _3)); + 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(), @@ -208,12 +238,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 @@ -285,31 +317,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"; @@ -319,8 +350,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 @@ -328,9 +358,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"); } @@ -539,7 +569,7 @@ ModeManager::handle_system_deviation(const std::string &) if (deviation.empty()) {return;} // handle deviation - for (auto const& dev : deviation) { + for (auto const & dev : deviation) { RCLCPP_WARN( this->get_logger(), "Deviation detected in system or part '%s': should be %s, but is %s.", @@ -548,7 +578,7 @@ ModeManager::handle_system_deviation(const std::string &) dev.second.second.as_string().c_str()); auto rules = this->mode_handling_->get_rules_for(dev.first, dev.second.first); - for (auto const &rule : rules) { + for (auto const & rule : rules) { try { auto part_actual = mode_inference_->get_or_infer(rule.part); if (rule.part_actual == part_actual) { @@ -557,12 +587,13 @@ ModeManager::handle_system_deviation(const std::string &) auto system_actual = mode_inference_->get_or_infer(rule.system); if (system_actual.state != rule.new_system_target.state) { // TODO(anordman): this is hacky and needs lifecycle servicing - if ((system_actual.state == State::PRIMARY_STATE_ACTIVE - || State::TRANSITION_STATE_ACTIVATING) && - rule.new_system_target.state == State::PRIMARY_STATE_INACTIVE) { + if ((system_actual.state == State::PRIMARY_STATE_ACTIVE || + State::TRANSITION_STATE_ACTIVATING) && + rule.new_system_target.state == State::PRIMARY_STATE_INACTIVE) + { change_state(rule.system, transition_id_("deactivate"), true); } else if (system_actual.state == State::PRIMARY_STATE_INACTIVE && - rule.new_system_target.state == State::PRIMARY_STATE_ACTIVE) { + rule.new_system_target.state == State::PRIMARY_STATE_ACTIVE) { change_state(rule.system, transition_id_("activate"), true); } if (!rule.new_system_target.mode.empty()) { @@ -579,4 +610,39 @@ ModeManager::handle_system_deviation(const std::string &) } } +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 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/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 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()); +} diff --git a/system_modes_examples/CMakeLists.txt b/system_modes_examples/CMakeLists.txt index b9526cc..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 @@ -74,4 +71,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/README.md b/system_modes_examples/README.md index 2e315c6..2726aaa 100644 --- a/system_modes_examples/README.md +++ b/system_modes_examples/README.md @@ -1,9 +1,9 @@ +General information about this repository, including legal information, build instructions and known issues/limitations, can be found in the [README](../README.md) of the repository root. + # The system_modes_examples package This [ROS 2](https://index.ros.org/doc/ros2/) package provides a simple example for the use of the [system_modes](../system_modes/) package. It contains two ROS 2 LifecycleNodes, a *drive\_base* node and a *manipulator* node, as well as simple a model file (yaml). -General information about this repository, including legal information, build instructions and known issues/limitations, can be found in the [README](../README.md) of the repository root. - ## Example Model File The SMH file [example_modes.yaml](./example_modes.yaml) specifies an *actuation* system consisting of the *drive\_base* node and the *manipulator* node, system modes for the *actuation* system, as well as system modes for the two nodes: @@ -12,8 +12,6 @@ The SMH file [example_modes.yaml](./example_modes.yaml) specifies an *actuation* * The *drive\_base* node has a default mode, a *FAST* mode, and a *SLOW* mode, configuring different values for its *max_speed* and its controller (*PID* or *MPC*). * The *actuation* system comprises of these two nodes. It has a default mode, a *PERFORMANCE* mode, and a *MODERATE* mode, changing the modes of its two nodes accordingly. -The SMH file additionally specifies four [Error Handling Rules](../system_modes/README.md#error-handling-and-rules) that specify how the system reacts to situation where the actual system state is diverging from the intended state. The four exemplary rules specify how the *actuation* system goes into a degraded mode or deactivates depending on which of its parts becomes inactive. - ## Running the Example ### Setup @@ -52,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'}"` +2. 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 "{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)). @@ -70,13 +68,13 @@ In an additional fifth terminal, you may mimic a planning/executive component to In order to see the [Error Handling and Rules](../system_modes/README.md#error-handling-and-rules) in action, try the following: 1. Bring the system back into its *PERFORMANCE* mode with the following command: - $ `ros2 service call /actuation/change_mode system_modes/ChangeMode "{node_name: 'actuation', mode_name: 'PERFORMANCE'}"` -1. Deactivate the manipulator node with the following command: + $ `ros2 service call /actuation/change_mode system_modes/ChangeMode "{mode_name: 'PERFORMANCE'}"` +2. Deactivate the manipulator node with the following command: $ `ros2 service call /manipulator/change_state lifecycle_msgs/ChangeState "{transition: {id: 4, label: deactivate}}"` The system will detect a deviation between the *intended* and the *actual* system state for which it recognizes a rule, i.e. the *degrade_from_PERFORMANCE* rule from the SMH file [example_modes.yaml](./example_modes.yaml). Following this rule, the mode manager sends the *actuation* system into its default mode. This is visible in the logging output of the terminal running the mode manager: ![mode_manager](./doc/screenshot-manager-deviation.png "Screenshot of the mode manager") 1. To try another rule, bring the system back into its *PERFORMANCE* mode with the following command: - $ `ros2 service call /actuation/change_mode system_modes/ChangeMode "{node_name: 'actuation', mode_name: 'PERFORMANCE'}"` -1. This time, deactivate the drive_base node with the following command: - $ `ros2 service call /drive_base/change_state lifecycle_msgs/ChangeState "{transition: {id: 4l, label: deactivate}}"` + $ `ros2 service call /actuation/change_mode system_modes/ChangeMode "{mode_name: 'PERFORMANCE'}"` +2. This time, deactivate the drive_base node with the following command: + $ `ros2 service call /drive_base/change_state lifecycle_msgs/ChangeState "{transition: {id: 4, label: deactivate}}"` The system will again detect a deviation between the *intended* and the *actual* system state for which it recognizes the *inactive_from_PERFORMANCE* rule. The mode manager will therefore deactivate the *actuation* system. diff --git a/system_modes_examples/changelog.rst b/system_modes_examples/changelog.rst index 578325b..93d225c 100644 --- a/system_modes_examples/changelog.rst +++ b/system_modes_examples/changelog.rst @@ -2,9 +2,34 @@ 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) +----------- +* removed boost dependencies (was: program options) +* 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 + +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 +41,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/launch/drive_base.launch.py b/system_modes_examples/launch/drive_base.launch.py index 497a993..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', - executable='drive_base', - parameters=[{'modelfile': modelfile}], - output='screen') - - description = launch.LaunchDescription() - description.add_action(node) - - return description \ No newline at end of file + 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/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 54074d3..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', - executable='manipulator', - parameters=[{'modelfile': modelfile}], - output='screen') - - description = launch.LaunchDescription() - description.add_action(node) - - return description \ No newline at end of file + 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 511894b..6ef798d 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 @@ -12,7 +15,17 @@ rclcpp rclcpp_lifecycle system_modes - libboost-program-options-dev + + ros2launch + + 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_;