Merge pull request #148 from kas-lab/suave_extended

Add mission metrics, battery monitor, recharge battery action, and adjust to new metacontrol version

CHANGELOG.md

@@ -5,6 +5,43 @@ All notable changes to this project will be documented in this file.
 The format is based on [Keep a Changelog](http://keepachangelog.com/)
 and this project adheres to [Semantic Versioning](http://semver.org/).
 
+## Unreleased
+
+### Added
+
+1. Added battery monitor node
+
+2. Added recharge battery task
+
+3. Added qa_comparison_operator to water_visibility in suave.owl
+
+4. Documentation with sphinx
+
+5. CI to build sphinx documentation
+
+6. Added mission metrics node
+
+### Changed
+
+1. Ardusub, mavros, ros_gz, mc_mros_reasoner, mc_mdl_tomasys, mros_ontology verions
+
+2. Removed unused code
+
+3. Mission config default parameters
+
+
+### Fixed
+
+1. README.md
+
+2. Fix task bridge callbacks
+
+## 1.2.1 [SEAMS Publication]
+
+### Fixed
+
+1. Minor bugs
+
 ## 1.2.0 [SEAMS Publication]
 
 Getting repository camera-ready

suave/launch/suave.launch.py

@@ -32,6 +32,13 @@ def generate_launch_description():
         parameters=[mission_config],
     )
 
+    battery_monitor_node = Node(
+        package='suave',
+        executable='battery_monitor',
+        name='battery_monitor',
+        parameters=[mission_config],
+    )
+
     pipeline_detection_wv_node = Node(
         package='suave',
         executable='pipeline_detection_wv',
@@ -56,6 +63,12 @@ def generate_launch_description():
         output='screen',
     )
 
+    recharge_battery_node = Node(
+        package='suave',
+        executable='recharge_battery',
+        output='screen',
+    )
+
     recover_thrusters_node = Node(
         package='suave',
         executable='recover_thrusters'
@@ -80,10 +93,12 @@ def generate_launch_description():
     return LaunchDescription([
         task_bridge_arg,
         water_visibility_node,
+        battery_monitor_node,
         pipeline_detection_wv_node,
         thruster_monitor_node,
         spiral_search_node,
         follow_pipeline_node,
+        recharge_battery_node,
         recover_thrusters_node,
         task_bridge_node,
         system_modes_launch,

suave/setup.py

@@ -27,8 +27,10 @@
     # tests_require=['pytest'],
     entry_points={
         'console_scripts': [
+            'battery_monitor = suave.battery_monitor:main',
             'pipeline_detection = suave.pipeline_node:main',
             'pipeline_detection_wv = suave.pipeline_detection_wv:main',
+            'recharge_battery = suave.recharge_battery_lc:main',
             'spiral_search = suave.spiral_search_lc:main',
             'follow_pipeline = suave.follow_pipeline_lc:main',
             'thruster_monitor = suave.thruster_monitor:main',

suave/suave/battery_monitor.py

@@ -0,0 +1,106 @@
+# Copyright 2023 Gustavo Rezende Silva
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from diagnostic_msgs.msg import DiagnosticArray
+from diagnostic_msgs.msg import DiagnosticStatus
+from diagnostic_msgs.msg import KeyValue
+from mavros_msgs.msg import State
+from std_srvs.srv import Trigger
+from std_msgs.msg import Bool
+
+import rclpy
+from rclpy.node import Node
+import sys
+
+
+class BatteryMonitor(Node):
+
+    def __init__(self):
+        super().__init__('battery_monitor')
+
+        self.declare_parameter('qa_publishing_period', 1.0)
+        self.declare_parameter('discharge_time', 200.0)
+
+        self.qa_publishing_period = self.get_parameter(
+            'qa_publishing_period').value
+
+        self.diagnostics_publisher = self.create_publisher(
+            DiagnosticArray, '/diagnostics', 10)
+
+        self.battery_recharged_pub = self.create_publisher(
+            Bool, '/battery_monitor/recharge/complete', 10)
+
+        self.battery_level = 1.0
+
+        self.get_interpolated_path_srv = self.create_service(
+            Trigger,
+            'battery_monitor/recharge',
+            self.recharge_battery_cb)
+
+        self.mavros_state_sub = self.create_subscription(
+            State, 'mavros/state', self.status_cb, 10)
+
+    def status_cb(self, msg):
+        if msg.mode == "GUIDED":
+            self.last_time = self.get_clock().now().to_msg().sec
+            self.qa_publisher_timer = self.create_timer(
+                self.qa_publishing_period, self.qa_publisher_cb)
+            self.destroy_subscription(self.mavros_state_sub)
+
+    def qa_publisher_cb(self):
+        discharge_time = self.get_parameter('discharge_time').value
+
+        current_time = self.get_clock().now().to_msg().sec
+        dt = current_time - self.last_time
+
+        if dt > 0:
+            self.last_time = current_time
+
+        v = self.battery_level - (1/discharge_time)*dt
+        if v < 0.0:
+            v = 0.0
+        self.battery_level = v
+
+        key_value = KeyValue()
+        key_value.key = "battery_level"
+        key_value.value = str(self.battery_level)
+
+        status_msg = DiagnosticStatus()
+        status_msg.level = DiagnosticStatus.OK
+        status_msg.name = ""
+        status_msg.message = "QA status"
+        status_msg.values.append(key_value)
+
+        diag_msg = DiagnosticArray()
+        diag_msg.header.stamp = self.get_clock().now().to_msg()
+        diag_msg.status.append(status_msg)
+
+        self.diagnostics_publisher.publish(diag_msg)
+
+    def recharge_battery_cb(self, req, res):
+        self.battery_level = 1.0
+        res.success = True
+        self.battery_recharged_pub.publish(Bool(data=True))
+        return res
+
+
+def main():
+    print("Starting battery_monitor observer node")
+
+    rclpy.init(args=sys.argv)
+
+    battery_monitor = BatteryMonitor()
+    rclpy.spin(battery_monitor)
+
+    rclpy.shutdown()

suave/suave/follow_pipeline_lc.py

@@ -1,8 +1,22 @@
-#!/usr/bin/env python
+# Copyright 2023 Gustavo Rezende Silva
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
 import math
 import rclpy
 import threading
 
+from rclpy.callback_groups import MutuallyExclusiveCallbackGroup
 from rclpy.lifecycle import Node
 from rclpy.lifecycle import State
 from rclpy.lifecycle import TransitionCallbackReturn
@@ -14,30 +28,46 @@
 from std_msgs.msg import Float32
 
 
+def spin_srv(executor):
+    try:
+        executor.spin()
+    except rclpy.executors.ExternalShutdownException:
+        pass
+
+
 class PipelineFollowerLC(Node):
 
     def __init__(self, node_name, **kwargs):
         super().__init__(node_name, **kwargs)
-        self.trigger_configure()
         self.abort_follow = False
         self.distance_inspected = 0
+        self.first_inspection = True
+        self.ardusub = None
+        self.trigger_configure()
 
     def on_configure(self, state: State) -> TransitionCallbackReturn:
         self.get_logger().info("on_configure() is called.")
-        self.ardusub = BlueROVGazebo('bluerov_pipeline_follower')
-        self.thread = threading.Thread(
-            target=rclpy.spin, args=(self.ardusub, ), daemon=True)
-        self.thread.start()
 
         self.get_path_timer = self.create_rate(5)
         self.get_path_service = self.create_client(
-            GetPath, 'pipeline/get_path')
+            GetPath,
+            'pipeline/get_path',
+            callback_group=MutuallyExclusiveCallbackGroup())
 
         self.pipeline_inspected_pub = self.create_lifecycle_publisher(
             Bool, 'pipeline/inspected', 10)
 
         self.pipeline_distance_inspected_pub = self.create_publisher(
             Float32, 'pipeline/distance_inspected', 10)
+
+        if self.ardusub is None:
+            self.ardusub = BlueROVGazebo('bluerov_pipeline_follower')
+            executor = rclpy.executors.SingleThreadedExecutor()
+            executor.add_node(self.ardusub)
+            self.thread = threading.Thread(
+                target=spin_srv, args=(executor, ), daemon=True)
+            self.thread.start()
+        self.get_logger().info("on_configure() completed")
         return TransitionCallbackReturn.SUCCESS
 
     def on_activate(self, state: State) -> TransitionCallbackReturn:
@@ -46,18 +76,28 @@ def on_activate(self, state: State) -> TransitionCallbackReturn:
             self.get_logger().info(
                 'pipeline/get_path service is not available')
             return TransitionCallbackReturn.FAILURE
+
+        if self.first_inspection is True:
+            self.pipe_path = self.call_service(
+                self.get_path_service, GetPath.Request())
+            if self.pipe_path is None:
+                return TransitionCallbackReturn.FAILURE
+            self.pipe_path = self.pipe_path.path.poses
+            self.first_inspection = False
+
         if self.executor is None:
             self.get_logger().info('Executor is None')
             return TransitionCallbackReturn.FAILURE
-        else:
-            self.executor.create_task(self.follow_pipeline)
-            self.abort_follow = False
+
+        self.follow_task = self.executor.create_task(self.follow_pipeline)
+        self.abort_follow = False
 
         return super().on_activate(state)
 
     def on_deactivate(self, state: State) -> TransitionCallbackReturn:
         self.get_logger().info("on_deactivate() is called.")
         self.abort_follow = True
+        self.follow_task.cancel()
         return super().on_deactivate(state)
 
     def on_cleanup(self, state: State) -> TransitionCallbackReturn:
@@ -77,16 +117,11 @@ def on_shutdown(self, state: State) -> TransitionCallbackReturn:
     def follow_pipeline(self):
         self.get_logger().info("Follow pipeline started")
 
-        pipe_path = self.get_path_service.call_async(GetPath.Request())
-
-        timer = self.ardusub.create_rate(5)  # Hz
-        while not pipe_path.done():
-            if self.abort_follow is True:
-                return
-            timer.sleep()
-        last_point = None
+        timer = self.create_rate(0.5)  # Hz
+        self.last_point = None
         self.distance_inspected = 0
-        for gz_pose in pipe_path.result().path.poses:
+        while len(self.pipe_path) > 0:
+            gz_pose = self.pipe_path.pop(0)
             if self.abort_follow is True:
                 return
             setpoint = self.ardusub.setpoint_position_gz(
@@ -95,25 +130,20 @@ def follow_pipeline(self):
             count = 0
             while not self.ardusub.check_setpoint_reached_xy(setpoint, 0.5):
                 if self.abort_follow is True:
-                    self.distance_inspected += self.calc_distance(
-                        last_point, self.ardusub.local_pos)
-                    dist = Float32()
-                    dist.data = self.distance_inspected
-                    self.pipeline_distance_inspected_pub.publish(dist)
                     return
                 if count > 10:
                     setpoint = self.ardusub.setpoint_position_gz(
                         gz_pose, fixed_altitude=True)
                 count += 1
                 timer.sleep()
 
-            if last_point is not None:
+            if self.last_point is not None:
                 self.distance_inspected += self.calc_distance(
-                    last_point, setpoint)
+                    self.last_point, setpoint)
                 dist = Float32()
                 dist.data = self.distance_inspected
                 self.pipeline_distance_inspected_pub.publish(dist)
-            last_point = setpoint
+            self.last_point = setpoint
 
         pipe_inspected = Bool()
         pipe_inspected.data = True
@@ -125,6 +155,19 @@ def calc_distance(self, pose1, pose2):
             (pose1.pose.position.x - pose2.pose.position.x)**2 +
             (pose1.pose.position.y - pose2.pose.position.y)**2)
 
+    def call_service(self, cli, request):
+        if cli.wait_for_service(timeout_sec=5.0) is False:
+            self.get_logger().error(
+                'service not available {}'.format(cli.srv_name))
+            return None
+        future = cli.call_async(request)
+        self.executor.spin_until_future_complete(future, timeout_sec=5.0)
+        if future.done() is False:
+            self.get_logger().error(
+                'Future not completed {}'.format(cli.srv_name))
+            return None
+        return future.result()
+
 
 def main():
     rclpy.init()