System modes inference

dependencies.repos

@@ -14,3 +14,8 @@ repositories:
     url: https://github.com/MROS-RobMoSys-ITP/pointcloud_to_laserscan
     version: managed_node
 
+  utils/system_modes:
+    type: git
+    url: https://github.com/micro-ROS/system_modes
+    version: feature/rules
+

laser_resender/src/laser_resender_node.cpp

@@ -21,8 +21,9 @@
 #include "sensor_msgs/msg/laser_scan.hpp"
 
 // Execute: 
-//  ros2 lifecycle list /lifecycle_node_example
-//  ros2 lifecycle set /lifecycle_node_example configure
+//  ros2 lifecycle list /laser_resender
+//  ros2 lifecycle get /laser_resender
+//  ros2 lifecycle set /laser_resender configure
 
 using rcl_interfaces::msg::ParameterType;
 using std::placeholders::_1;
@@ -35,6 +36,7 @@ class LaserResender : public rclcpp_lifecycle::LifecycleNode
   LaserResender()
   : rclcpp_lifecycle::LifecycleNode("laser_resender")
   {
+    declare_parameter("node_name");
     pub_ = create_publisher<sensor_msgs::msg::LaserScan>("/mros_scan", rclcpp::SensorDataQoS());
     sub_ = create_subscription<sensor_msgs::msg::LaserScan>
       ("/scan", rclcpp::SensorDataQoS(), std::bind(&LaserResender::scan_cb, this, _1));
@@ -58,8 +60,13 @@ class LaserResender : public rclcpp_lifecycle::LifecycleNode
 
   CallbackReturnT on_deactivate(const rclcpp_lifecycle::State & state) 
   {
-    RCLCPP_INFO(get_logger(), "[%s] Deactivating from [%s] state...", get_name(), state.label().c_str());
-    return CallbackReturnT::SUCCESS;
+    if (all_zero_error_)
+    {
+      return CallbackReturnT::ERROR;
+    } else {
+      RCLCPP_INFO(get_logger(), "[%s] Deactivating from [%s] state...", get_name(), state.label().c_str());
+      return CallbackReturnT::SUCCESS;
+    } 
   }
 
   CallbackReturnT on_cleanup(const rclcpp_lifecycle::State & state) 
@@ -74,22 +81,47 @@ class LaserResender : public rclcpp_lifecycle::LifecycleNode
     return CallbackReturnT::SUCCESS;
   }
 
-    CallbackReturnT on_error(const rclcpp_lifecycle::State & state) 
+  CallbackReturnT on_error(const rclcpp_lifecycle::State & state) 
   {
-    RCLCPP_INFO(get_logger(), "[%s] Shutting Down from [%s] state...", get_name(), state.label().c_str());
+    RCLCPP_ERROR(get_logger(), "[%s] Error processing from [%s] state...", get_name(), state.label().c_str());
+
     return CallbackReturnT::SUCCESS;
   }
 
   void scan_cb(sensor_msgs::msg::LaserScan::ConstSharedPtr laser_scan)
   {
-    if (get_current_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) {
-      pub_->publish(*laser_scan);
+    if (get_current_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) 
+    {
+      all_zero_error_ = true;
+      for (auto range : laser_scan->ranges)
+      {
+        if (range != 0.0)
+        {
+          all_zero_error_ = false;
+          break;
+        }
+      }
+
+      if (!all_zero_error_) 
+      {
+        pub_->publish(*laser_scan);
+      } else {
+        RCLCPP_WARN(get_logger(),
+          "[%s] ALL-ZEROS. It has to go to error processing state", get_name());
+        trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE);
+      }
     }
+
+    /* if (get_current_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED)
+    {
+      trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE);
+    } */
   }
 
 private:
   rclcpp_lifecycle::LifecyclePublisher<sensor_msgs::msg::LaserScan>::SharedPtr pub_;
   rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr sub_;
+  bool all_zero_error_;
 };
 
 int main(int argc, char * argv[])

metacontroller_pilot/metacontroller_pilot/metacontroller_sim.py

@@ -1,38 +1,20 @@
 #!/usr/bin/env python
 
-# Software License Agreement (BSD License)
+# Copyright 2020 Intelligent Robotics Lab
 #
-# Copyright (c) 2020, Intelligent Robotics Core S.L.
-# All rights reserved.
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
 #
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
+#     http://www.apache.org/licenses/LICENSE-2.0
 #
-#  * Redistributions of source code must retain the above copyright
-#    notice, this list of conditions and the following disclaimer.
-#  * Redistributions in binary form must reproduce the above
-#    copyright notice, this list of conditions and the following
-#    disclaimer in the documentation and/or other materials provided
-#    with the distribution.
-#  * Neither the name of Willow Garage, Inc. nor the names of its
-#    contributors may be used to endorse or promote products derived
-#    from this software without specific prior written permission.
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
 #
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
-# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-# Author: Lorena Bajo Rebollo - lorena.bajo@urjc.es
+# Author: jginesclavero - jonatan.gines@urjc.es
 
 import argparse
 import functools
@@ -47,66 +29,45 @@
 from rqt_gui_py.plugin import Plugin
 from std_msgs.msg import Float32, Header
 from system_modes.srv import ChangeMode
-
+from rcl_interfaces.msg import Log
 
 class Metacontroller(Node):
-    def __init__(self, node_name, mode_name):
+    def __init__(self):
         super().__init__('metacontroller')
+        self.rosout_sub_ = self.create_subscription(
+          Log,
+          "/rosout",
+          self.rosout_cb, 1)
+        self.current_mode = 'NORMAL'
+    def change_mode(self, node_name, mode_name):
         cli = self.create_client(ChangeMode, '/'+node_name+'/change_mode')
         while not cli.wait_for_service(timeout_sec=1.0):
             print('service not available, waiting again...')
         req = ChangeMode.Request()
+        req.node_name = node_name
         req.mode_name = mode_name
 
         future = cli.call_async(req)
         rclpy.spin_until_future_complete(self, future)
         if future.result() is not None:
             self.get_logger().info('Mode change completed')
-            sys.exit()
         else:
             self.get_logger().error('Exception while calling service: %r' % future.exception())
 
-def main(args=None):
-    print ("------------------------------")
-    print ("Specify the option number:")
-    print ("------------------------------")
-    print (" 0) Normal")
-    print (" 1) Degraded (Navigate with pointcloud)")
-    print (" 2) Performance")
-    print (" 3) Energy saving")
-    print (" 4) Slow")
-    print ("------------------------------")
-
-    option = input()
-    if option == "0":
-        print ("Normal") 
-        mode_name = 'NORMAL'
-    elif option == "1":
-        print ("Degraded (Navigate with pointcloud).") 
-        mode_name = 'DEGRADED'
-    elif option == "2":
-        print ("Performance") 
-        mode_name = 'PERFORMANCE'
-    elif option == "3":
-        print ("Energy saving.") 
-        mode_name = 'ENERGY_SAVING'
-    elif option == "4":
-        print ("Slow.")
-        mode_name = 'SLOW'
-    else:
-        print("Invalid option.")
-        sys.exit()
+    def rosout_cb(self, msg):
+        if msg.level == 40 and msg.function == "on_error":
+            if msg.name == "battery_contingency_sim" and self.current_mode == 'NORMAL':
+                self.current_mode = 'ENERGY_SAVING'
+                self.get_logger().info('Battery low detected, solving contingency...')
+                self.change_mode("pilot", self.current_mode)
 
+def main(args=None):
     rclpy.init(args=args)
-    node_name = "pilot"
-    node = Metacontroller(node_name, mode_name)
-
-    try:
-        rclpy.spin(node)
-    except KeyboardInterrupt:
-        pass
-
-    node.destroy_node()
+    node = Metacontroller()
+    node.change_mode("pilot", '__DEFAULT__')
+    node.change_mode("pilot", 'NORMAL')
+    rclpy.spin(node)
+    node.destroy()
     rclpy.shutdown()
 
 if __name__ == '__main__':

metacontroller_pilot/metacontroller_pilot/simple_mode_manager.py

@@ -0,0 +1,113 @@
+#!/usr/bin/env python
+
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2020, Intelligent Robotics Core S.L.
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+#  * Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+#  * Redistributions in binary form must reproduce the above
+#    copyright notice, this list of conditions and the following
+#    disclaimer in the documentation and/or other materials provided
+#    with the distribution.
+#  * Neither the name of Willow Garage, Inc. nor the names of its
+#    contributors may be used to endorse or promote products derived
+#    from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+# Author: Lorena Bajo Rebollo - lorena.bajo@urjc.es
+
+import argparse
+import functools
+import re
+import time
+import sys
+
+import rclpy
+import rclpy.logging
+from rclpy.node import Node
+
+from rqt_gui_py.plugin import Plugin
+from std_msgs.msg import Float32, Header
+from system_modes.srv import ChangeMode
+
+
+class Metacontroller(Node):
+    def __init__(self, node_name, mode_name):
+        super().__init__('metacontroller')
+        cli = self.create_client(ChangeMode, '/'+node_name+'/change_mode')
+        while not cli.wait_for_service(timeout_sec=1.0):
+            print('service not available, waiting again...')
+        req = ChangeMode.Request()
+        req.mode_name = mode_name
+
+        future = cli.call_async(req)
+        rclpy.spin_until_future_complete(self, future)
+        if future.result() is not None:
+            self.get_logger().info('Mode change completed')
+            sys.exit()
+        else:
+            self.get_logger().error('Exception while calling service: %r' % future.exception())
+
+def main(args=None):
+    print ("------------------------------")
+    print ("Specify the option number:")
+    print ("------------------------------")
+    print (" 0) Normal")
+    print (" 1) Degraded (Navigate with pointcloud)")
+    print (" 2) Performance")
+    print (" 3) Energy saving")
+    print (" 4) Slow")
+    print ("------------------------------")
+
+    option = input()
+    if option == "0":
+        print ("Normal") 
+        mode_name = 'NORMAL'
+    elif option == "1":
+        print ("Degraded (Navigate with pointcloud).") 
+        mode_name = 'DEGRADED'
+    elif option == "2":
+        print ("Performance") 
+        mode_name = 'PERFORMANCE'
+    elif option == "3":
+        print ("Energy saving.") 
+        mode_name = 'ENERGY_SAVING'
+    elif option == "4":
+        print ("Slow.")
+        mode_name = 'SLOW'
+    else:
+        print("Invalid option.")
+        sys.exit()
+
+    rclpy.init(args=args)
+    node_name = "pilot"
+    node = Metacontroller(node_name, mode_name)
+
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+
+    node.destroy_node()
+    rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()

metacontroller_pilot/setup.py

@@ -32,6 +32,7 @@
     entry_points={
         'console_scripts': [
             'metacontroller = metacontroller_pilot.metacontroller_sim:main',
+            'mode_manager = metacontroller_pilot.simple_mode_manager:main',
         ],
     },
 )