From c2cb71306334fba8cf2039dbe41483a1230be22f Mon Sep 17 00:00:00 2001 From: Arne Nordmann Date: Fri, 24 Jul 2020 16:54:17 +0200 Subject: [PATCH 01/15] Reintroduce macos build and windows build Signed-off-by: Arne Nordmann --- .github/workflows/test.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/test.yaml b/.github/workflows/test.yaml index ca522ca..36ba7f1 100644 --- a/.github/workflows/test.yaml +++ b/.github/workflows/test.yaml @@ -11,7 +11,7 @@ jobs: strategy: fail-fast: false matrix: - os: [ubuntu-18.04] + os: [ubuntu-18.04, macos-latest, windows-latest] steps: - uses: ros-tooling/setup-ros@master - uses: ros-tooling/action-ros-ci@master From d8935ccf73ec271c92a531ae1640e025a33461a8 Mon Sep 17 00:00:00 2001 From: Arne Nordmann Date: Mon, 3 Aug 2020 12:25:28 +0200 Subject: [PATCH 02/15] Kicked all boost dependencies Signed-off-by: Arne Nordmann --- system_modes/CMakeLists.txt | 3 - system_modes/launch/mode_manager.launch.py | 2 +- system_modes/launch/mode_monitor.launch.py | 2 +- system_modes/package.xml | 1 - system_modes/src/mode_manager_node.cpp | 53 --------------- system_modes/src/mode_monitor_node.cpp | 68 ------------------- system_modes_examples/CMakeLists.txt | 3 - .../launch/drive_base.launch.py | 2 +- .../launch/manipulator.launch.py | 2 +- system_modes_examples/package.xml | 3 +- 10 files changed, 5 insertions(+), 134 deletions(-) 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/launch/mode_manager.launch.py b/system_modes/launch/mode_manager.launch.py index 9b2a510..1e1089b 100644 --- a/system_modes/launch/mode_manager.launch.py +++ b/system_modes/launch/mode_manager.launch.py @@ -25,7 +25,7 @@ def generate_launch_description(): node = launch_ros.actions.Node( package='system_modes', - node_executable='mode_manager', + 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..81f3923 100644 --- a/system_modes/launch/mode_monitor.launch.py +++ b/system_modes/launch/mode_monitor.launch.py @@ -25,7 +25,7 @@ def generate_launch_description(): node = launch_ros.actions.Node( package='system_modes', - node_executable='mode_monitor', + executable='mode_monitor', parameters=[{'modelfile': launch.substitutions.LaunchConfiguration('modelfile')}], 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..84e3d84 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,22 +110,6 @@ 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); diff --git a/system_modes/src/mode_monitor_node.cpp b/system_modes/src/mode_monitor_node.cpp index 14b7c23..862b67c 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,22 +117,6 @@ 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); 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..c0fa052 100644 --- a/system_modes_examples/launch/drive_base.launch.py +++ b/system_modes_examples/launch/drive_base.launch.py @@ -29,7 +29,7 @@ def generate_launch_description(): node = launch_ros.actions.Node( package='system_modes_examples', - node_executable='drive_base', + executable='drive_base', parameters=[{'modelfile': modelfile}], output='screen') diff --git a/system_modes_examples/launch/manipulator.launch.py b/system_modes_examples/launch/manipulator.launch.py index f70a2a3..e16bb8c 100644 --- a/system_modes_examples/launch/manipulator.launch.py +++ b/system_modes_examples/launch/manipulator.launch.py @@ -29,7 +29,7 @@ def generate_launch_description(): node = launch_ros.actions.Node( package='system_modes_examples', - node_executable='manipulator', + executable='manipulator', parameters=[{'modelfile': 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 f8b5092c5f15ca9b6c51e225e6c5c40a94e8945d Mon Sep 17 00:00:00 2001 From: Arne Nordmann Date: Mon, 3 Aug 2020 14:03:44 +0200 Subject: [PATCH 03/15] All ros args now via launch instead of commandline Signed-off-by: Arne Nordmann --- .../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 | 35 +++++++++++----- system_modes/src/mode_manager_node.cpp | 2 +- system_modes/src/mode_monitor_node.cpp | 2 +- .../src/system_modes/mode_manager.cpp | 13 ++---- .../src/system_modes/mode_monitor.cpp | 42 +++++++++++-------- 8 files changed, 65 insertions(+), 54 deletions(-) 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 1e1089b..fdad550 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..10e976b 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( + 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')}], - output='screen') - - description = launch.LaunchDescription() - description.add_action(node) - - return description + 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/src/mode_manager_node.cpp b/system_modes/src/mode_manager_node.cpp index 84e3d84..47ba776 100644 --- a/system_modes/src/mode_manager_node.cpp +++ b/system_modes/src/mode_manager_node.cpp @@ -113,7 +113,7 @@ int main(int argc, char * argv[]) 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 862b67c..27dd139 100644 --- a/system_modes/src/mode_monitor_node.cpp +++ b/system_modes/src/mode_monitor_node.cpp @@ -120,7 +120,7 @@ int main(int argc, char * argv[]) 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_); From a86313bbe271debff3545c8ad78038e815b2412a Mon Sep 17 00:00:00 2001 From: Arne Nordmann Date: Mon, 3 Aug 2020 14:04:59 +0200 Subject: [PATCH 04/15] Minor improvement of launch files Signed-off-by: Arne Nordmann --- .../launch/drive_base.launch.py | 24 +++++++------------ .../launch/manipulator.launch.py | 24 +++++++------------ 2 files changed, 16 insertions(+), 32 deletions(-) diff --git a/system_modes_examples/launch/drive_base.launch.py b/system_modes_examples/launch/drive_base.launch.py index c0fa052..2a4cdef 100644 --- a/system_modes_examples/launch/drive_base.launch.py +++ b/system_modes_examples/launch/drive_base.launch.py @@ -17,23 +17,15 @@ 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 + 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 e16bb8c..5bad474 100644 --- a/system_modes_examples/launch/manipulator.launch.py +++ b/system_modes_examples/launch/manipulator.launch.py @@ -17,23 +17,15 @@ 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 + 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')]) From 6bc39fcaee119cdb28929cd92596fa6de9f13c58 Mon Sep 17 00:00:00 2001 From: Arne Nordmann Date: Mon, 3 Aug 2020 14:11:49 +0200 Subject: [PATCH 05/15] flake Signed-off-by: Arne Nordmann --- system_modes/launch/mode_manager.launch.py | 18 +++---- system_modes/launch/mode_monitor.launch.py | 50 +++++++++---------- .../launch/drive_base.launch.py | 17 ++++--- .../launch/manipulator.launch.py | 17 ++++--- 4 files changed, 52 insertions(+), 50 deletions(-) diff --git a/system_modes/launch/mode_manager.launch.py b/system_modes/launch/mode_manager.launch.py index fdad550..d9600a8 100644 --- a/system_modes/launch/mode_manager.launch.py +++ b/system_modes/launch/mode_manager.launch.py @@ -21,12 +21,12 @@ def generate_launch_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')]) + 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 10e976b..a5ee3e4 100644 --- a/system_modes/launch/mode_monitor.launch.py +++ b/system_modes/launch/mode_monitor.launch.py @@ -21,28 +21,28 @@ def generate_launch_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')]) + 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_examples/launch/drive_base.launch.py b/system_modes_examples/launch/drive_base.launch.py index 2a4cdef..4036980 100644 --- a/system_modes_examples/launch/drive_base.launch.py +++ b/system_modes_examples/launch/drive_base.launch.py @@ -21,11 +21,12 @@ def generate_launch_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')]) + 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 5bad474..ea44a41 100644 --- a/system_modes_examples/launch/manipulator.launch.py +++ b/system_modes_examples/launch/manipulator.launch.py @@ -21,11 +21,12 @@ def generate_launch_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')]) + 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')]) From 888b5a814c668f899bc7db512d3e9f3d0be0de8b Mon Sep 17 00:00:00 2001 From: Arne Nordmann Date: Mon, 3 Aug 2020 14:34:57 +0200 Subject: [PATCH 06/15] Use specific versions of ros-tooling Signed-off-by: Arne Nordmann --- .github/workflows/lint.yaml | 18 +++++++++--------- .github/workflows/test.yaml | 7 ++++--- 2 files changed, 13 insertions(+), 12 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 36ba7f1..9a578ad 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, macos-latest, windows-latest] 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 From 8ea52a239fbc0dbc3236800f93db706938d94652 Mon Sep 17 00:00:00 2001 From: Arne Nordmann Date: Wed, 23 Sep 2020 12:01:22 +0200 Subject: [PATCH 07/15] 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 08/15] 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 09/15] 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 10/15] 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 11/15] 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 12/15] 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 13/15] 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 From 5540c5eb91fc4272d63131892993e181e6240166 Mon Sep 17 00:00:00 2001 From: Arne Nordmann Date: Fri, 16 Oct 2020 10:38:49 +0200 Subject: [PATCH 14/15] Updates ROS actions Signed-off-by: Arne Nordmann --- .github/workflows/lint.yaml | 6 +++--- .github/workflows/test.yaml | 6 +++--- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/.github/workflows/lint.yaml b/.github/workflows/lint.yaml index 6a1a2e2..422c83a 100644 --- a/.github/workflows/lint.yaml +++ b/.github/workflows/lint.yaml @@ -8,7 +8,7 @@ jobs: runs-on: ubuntu-18.04 steps: - uses: actions/checkout@v2 - - uses: ros-tooling/setup-ros@0.0.25 + - uses: ros-tooling/setup-ros@0.0.26 - uses: ros-tooling/action-ros2-lint@0.0.6 with: linter: xmllint @@ -23,7 +23,7 @@ jobs: linter: [cppcheck, cpplint, uncrustify] steps: - uses: actions/checkout@v2 - - uses: ros-tooling/setup-ros@0.0.25 + - uses: ros-tooling/setup-ros@0.0.26 - uses: ros-tooling/action-ros2-lint@0.0.6 with: linter: ${{ matrix.linter }} @@ -38,7 +38,7 @@ jobs: linter: [flake8, pep257] steps: - uses: actions/checkout@v2 - - uses: ros-tooling/setup-ros@0.0.25 + - uses: ros-tooling/setup-ros@0.0.26 - uses: ros-tooling/action-ros2-lint@0.0.6 with: linter: ${{ matrix.linter }} diff --git a/.github/workflows/test.yaml b/.github/workflows/test.yaml index 9a578ad..95d3e0f 100644 --- a/.github/workflows/test.yaml +++ b/.github/workflows/test.yaml @@ -1,4 +1,4 @@ -name: Test system modes +name: Build and test system modes on: pull_request: push: @@ -13,8 +13,8 @@ jobs: matrix: os: [ubuntu-18.04, macos-latest, windows-latest] steps: - - uses: ros-tooling/setup-ros@0.0.25 - - uses: ros-tooling/action-ros-ci@0.0.15 + - uses: ros-tooling/setup-ros@0.0.26 + - uses: ros-tooling/action-ros-ci@0.0.19 with: package-name: system_modes system_modes_examples target-ros2-distro: foxy From e013b03c958ff82875507b8ac7533bc6091c0c20 Mon Sep 17 00:00:00 2001 From: "Nordmann Arne (CR/ADT3)" Date: Tue, 9 Feb 2021 16:23:13 +0100 Subject: [PATCH 15/15] Use more recent github actions (ros, codecov) Signed-off-by: Nordmann Arne (CR/ADT3) --- .github/workflows/ci.yaml | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 5aa4d9d..dffad17 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -5,9 +5,6 @@ on: branches: [ master, dev ] pull_request: branches: [ master, dev ] - schedule: - # Run once per day to detect broken dependencies. - - cron: '17 6 * * *' jobs: build: @@ -19,15 +16,16 @@ jobs: ros_distribution: [ foxy, rolling ] steps: - uses: actions/checkout@v2 - - uses: ros-tooling/setup-ros@0.0.26 + - uses: ros-tooling/setup-ros@0.1.2 with: required-ros-distributions: ${{ matrix.ros_distribution }} - - uses : ros-tooling/action-ros-ci@0.1.0 + - uses : ros-tooling/action-ros-ci@0.1.1 with: package-name: "system_modes system_modes_examples" target-ros2-distro: ${{ matrix.ros_distribution }} + vcs-repo-file-url: "" colcon-mixin-name: coverage-gcc colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml - - uses: codecov/codecov-action@v1 + - uses: codecov/codecov-action@v1.2.1 with: file: ros_ws/lcov/total_coverage.info