Patrol mission

README.md

@@ -122,7 +122,7 @@ This pilot has been tested on different platforms. Above we show how to run the
 1. **Launch turtlebot3 world in gazebo sim**
 
     ```console
-      export GAZEBO_MODEL_PATH=$GAZEBO_MODEL_PATH:[ros2_ws]/src/turtlebot3/turtlebot3_simulations/turtlebot3_gazebo/models
+      export GAZEBO_MODEL_PATH=$GAZEBO_MODEL_PATH:[ros2_ws]/src/turtlebot3/turtlebot3_simulations/turtlebot3_gazebo/models:[ros2_ws]/src/Pilot-URJC/pilot_urjc_bringup/worlds/models
       export TURTLEBOT3_MODEL=${TB3_MODEL}
       ros2 launch pilot_urjc_bringup tb3_sim_launch.py
     ```

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/CMakeLists.txt

@@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.5)
 project(laser_resender)
 
 set(CMAKE_CXX_STANDARD 17)
-set(CMAKE_BUILD_TYPE DEBUG)
+#set(CMAKE_BUILD_TYPE DEBUG)
 
 find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
@@ -22,6 +22,12 @@ set(dependencies
 add_executable(laser_resender_node src/laser_resender_node.cpp)
 ament_target_dependencies(laser_resender_node ${dependencies})
 
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  find_package(ament_lint_common REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
 install(TARGETS
   laser_resender_node
   ARCHIVE DESTINATION lib

laser_resender/package.xml

@@ -18,6 +18,9 @@
   <depend>sensor_msgs</depend>
   <depend>lifecycle_msgs</depend>
 
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
   <export>
     <build_type>ament_cmake</build_type>
   </export>

laser_resender/src/laser_resender_node.cpp

@@ -20,76 +20,121 @@
 #include "rclcpp_lifecycle/lifecycle_node.hpp"
 #include "sensor_msgs/msg/laser_scan.hpp"
 
-// Execute: 
-//  ros2 lifecycle list /lifecycle_node_example
-//  ros2 lifecycle set /lifecycle_node_example configure
+// Execute:
+//  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;
 using namespace std::placeholders;
 
-
 class LaserResender : public rclcpp_lifecycle::LifecycleNode
 {
 public:
   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));
+    sub_ = create_subscription<sensor_msgs::msg::LaserScan>(
+      "/scan",
+      rclcpp::SensorDataQoS(), std::bind(&LaserResender::scan_cb, this, _1));
   }
 
   using CallbackReturnT =
     rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
 
   CallbackReturnT on_configure(const rclcpp_lifecycle::State & state)
   {
-    RCLCPP_INFO(get_logger(), "[%s] Configuring from [%s] state...", get_name(), state.label().c_str());   
+    RCLCPP_INFO(
+      get_logger(), "[%s] Configuring from [%s] state...",
+      get_name(),
+      state.label().c_str());
     return CallbackReturnT::SUCCESS;
   }
 
-  CallbackReturnT on_activate(const rclcpp_lifecycle::State & state) 
+  CallbackReturnT on_activate(const rclcpp_lifecycle::State & state)
   {
-    RCLCPP_INFO(get_logger(), "[%s] Activating from [%s] state...", get_name(), state.label().c_str());
+    RCLCPP_INFO(
+      get_logger(), "[%s] Activating from [%s] state...",
+      get_name(),
+      state.label().c_str());
     pub_->on_activate();
     return CallbackReturnT::SUCCESS;
   }
 
-  CallbackReturnT on_deactivate(const rclcpp_lifecycle::State & state) 
+  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) 
+  CallbackReturnT on_cleanup(const rclcpp_lifecycle::State & state)
   {
-    RCLCPP_INFO(get_logger(), "[%s] Cleanning Up from [%s] state...", get_name(), state.label().c_str());
+    RCLCPP_INFO(
+      get_logger(), "[%s] Cleanning Up from [%s] state...",
+      get_name(),
+      state.label().c_str());
     return CallbackReturnT::SUCCESS;
   }
 
-  CallbackReturnT on_shutdown(const rclcpp_lifecycle::State & state) 
+  CallbackReturnT on_shutdown(const rclcpp_lifecycle::State & state)
   {
-    RCLCPP_INFO(get_logger(), "[%s] Shutting Down from [%s] state...", get_name(), state.label().c_str());
+    RCLCPP_INFO(
+      get_logger(), "[%s] Shutting Down from [%s] state...",
+      get_name(),
+      state.label().c_str());
     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);
+      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[])
@@ -101,4 +146,4 @@ int main(int argc, char * argv[])
   rclcpp::shutdown();
 
   return 0;
-}
+}

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__':