From 0e3c018be58eeac89f1929f2466c92d929c5e69a Mon Sep 17 00:00:00 2001 From: Gustavo Date: Sun, 10 Dec 2023 00:07:43 +0100 Subject: [PATCH 1/7] =?UTF-8?q?=E2=9C=A8=20add=20battery=5Fmonitor=20&=20r?= =?UTF-8?q?echarge=5Fbattery?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- suave/launch/suave.launch.py | 15 +++ suave/setup.py | 2 + suave/suave/battery_monitor.py | 106 ++++++++++++++++++++ suave/suave/recharge_battery_lc.py | 152 +++++++++++++++++++++++++++++ 4 files changed, 275 insertions(+) create mode 100644 suave/suave/battery_monitor.py create mode 100644 suave/suave/recharge_battery_lc.py diff --git a/suave/launch/suave.launch.py b/suave/launch/suave.launch.py index 0c7bcc8..61f9d0b 100644 --- a/suave/launch/suave.launch.py +++ b/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, diff --git a/suave/setup.py b/suave/setup.py index 9362d2c..31d67d3 100644 --- a/suave/setup.py +++ b/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', diff --git a/suave/suave/battery_monitor.py b/suave/suave/battery_monitor.py new file mode 100644 index 0000000..a8d1634 --- /dev/null +++ b/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() diff --git a/suave/suave/recharge_battery_lc.py b/suave/suave/recharge_battery_lc.py new file mode 100644 index 0000000..68ec3d7 --- /dev/null +++ b/suave/suave/recharge_battery_lc.py @@ -0,0 +1,152 @@ +# 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 rclpy +import threading + +from rclpy.callback_groups import MutuallyExclusiveCallbackGroup +from rclpy.executors import MultiThreadedExecutor +from rclpy.lifecycle import Node +from rclpy.lifecycle import State +from rclpy.lifecycle import TransitionCallbackReturn + +from geometry_msgs.msg import Point +from geometry_msgs.msg import Pose +from std_srvs.srv import Trigger + +from suave.bluerov_gazebo import BlueROVGazebo + + +def check_lc_active(func): + def inner(*args, **kwargs): + if args[0].active is True: + return func(*args, **kwargs) + return inner + + +class RechargeBattery(Node): + """Recharge battery node.""" + + def __init__(self, node_name, **kwargs): + super().__init__(node_name, **kwargs) + self.recharge_timer_period = 5.0 + self.active = False + self.cli_group = MutuallyExclusiveCallbackGroup() + self.trigger_configure() + + def on_configure(self, state: State) -> TransitionCallbackReturn: + self.get_logger().info(self.get_name() + ': on_configure() is called.') + + self.declare_parameter( + 'recharge_station_gz_pos', [-3.0, -2.0, -19.5]) + + self.recharge_battery_cli = self.create_client( + Trigger, + 'battery_monitor/recharge', + callback_group=self.cli_group + ) + + self.ardusub = BlueROVGazebo('bluerov_recharge') + + self.thread = threading.Thread( + target=rclpy.spin, args=(self.ardusub, ), daemon=True) + self.thread.start() + + self.recharge_cb_timer = self.create_timer( + self.recharge_timer_period, self.recharge_cb) + + self.get_logger().info(self.get_name() + ': on_configure() completed.') + return TransitionCallbackReturn.SUCCESS + + @check_lc_active + def recharge_cb(self): + recharge_station_gz_pos = self.get_parameter( + 'recharge_station_gz_pos').get_parameter_value() + recharge_station_pos = recharge_station_gz_pos.double_array_value + station_pose = Pose(position=Point( + x=recharge_station_pos[0], + y=recharge_station_pos[1], + z=recharge_station_pos[2], + ) + ) + + setpoint = self.ardusub.setpoint_position_gz( + station_pose, + fixed_altitude=True) + + if self.ardusub.check_setpoint_reached_xy(setpoint, 0.5): + self.call_service(self.recharge_battery_cli, Trigger.Request()) + return + + def on_activate(self, state: State) -> TransitionCallbackReturn: + self.get_logger().info(self.get_name() + ': on_activate() is called.') + self.active = True + self.get_logger().info( + self.get_name() + ': on_activate() is completed.') + return super().on_activate(state) + + def on_deactivate(self, state: State) -> TransitionCallbackReturn: + self.get_logger().info("on_deactivate() is called.") + self.active = False + return super().on_deactivate(state) + + def on_cleanup(self, state: State) -> TransitionCallbackReturn: + self.active = False + self.thread.join() + self.ardusub.destroy_node() + self.destroy_timer(self.recharge_cb_timer) + self.get_logger().info('on_cleanup() is called.') + return TransitionCallbackReturn.SUCCESS + + def on_shutdown(self, state: State) -> TransitionCallbackReturn: + self.get_logger().info('on_shutdown() is called.') + self.thread.join() + self.ardusub.destroy_node() + self.destroy_timer(self.recharge_cb_timer) + return super().on_shutdown(state) + + 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(args=None): + rclpy.init(args=args) + try: + executor = MultiThreadedExecutor() + lc_node = RechargeBattery('recharge_battery') + executor.add_node(lc_node) + try: + executor.spin() + except (KeyboardInterrupt, rclpy.executors.ExternalShutdownException): + executor.shutdown() + lc_node.destroy_node() + finally: + executor.shutdown() + lc_node.destroy_node() + finally: + rclpy.shutdown() + + +if __name__ == '__main__': + main() From e42908fef1aeb638f62f4edc8ba69134ecc000c6 Mon Sep 17 00:00:00 2001 From: Gustavo Date: Sun, 10 Dec 2023 00:10:57 +0100 Subject: [PATCH 2/7] =?UTF-8?q?=F0=9F=94=A5=20remove=20unused=20code?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- suave/suave/follow_pipeline_lc.py | 97 +++++++++++++------ suave/suave/spiral_search_lc.py | 3 - suave/suave/thruster_monitor.py | 16 ++- .../suave_missions/mission_planner.py | 3 - 4 files changed, 83 insertions(+), 36 deletions(-) diff --git a/suave/suave/follow_pipeline_lc.py b/suave/suave/follow_pipeline_lc.py index 5557e6b..3f2547c 100755 --- a/suave/suave/follow_pipeline_lc.py +++ b/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,11 +130,6 @@ 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( @@ -107,13 +137,13 @@ def follow_pipeline(self): 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() diff --git a/suave/suave/spiral_search_lc.py b/suave/suave/spiral_search_lc.py index 069c056..23688c1 100755 --- a/suave/suave/spiral_search_lc.py +++ b/suave/suave/spiral_search_lc.py @@ -4,13 +4,11 @@ from rcl_interfaces.msg import ParameterDescriptor, SetParametersResult from rclpy.lifecycle import Node -from rclpy.lifecycle import Publisher from rclpy.lifecycle import State from rclpy.lifecycle import TransitionCallbackReturn from rclpy.timer import Timer from suave.bluerov_gazebo import BlueROVGazebo -import std_msgs.msg import threading import math @@ -74,7 +72,6 @@ def publish(self): self.old_spiral_altitude = self.spiral_altitude fov = math.pi/3 - pipe_z = 0.5 spiral_width = 2.0*self.spiral_altitude*math.tan(fov/2) altitude_bug = False diff --git a/suave/suave/thruster_monitor.py b/suave/suave/thruster_monitor.py index 8052d10..13b9d20 100644 --- a/suave/suave/thruster_monitor.py +++ b/suave/suave/thruster_monitor.py @@ -1,10 +1,20 @@ -#!/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 sys import rclpy -import threading from rclpy.node import Node -from rclpy.duration import Duration from diagnostic_msgs.msg import DiagnosticArray from diagnostic_msgs.msg import DiagnosticStatus diff --git a/suave_missions/suave_missions/mission_planner.py b/suave_missions/suave_missions/mission_planner.py index 313d08e..6c824e3 100644 --- a/suave_missions/suave_missions/mission_planner.py +++ b/suave_missions/suave_missions/mission_planner.py @@ -1,8 +1,5 @@ import csv -import rclpy from pathlib import Path -from datetime import datetime -from rclpy.callback_groups import MutuallyExclusiveCallbackGroup from rclpy.node import Node from suave_msgs.srv import Task From a387194ca999d2d7100cfcb3bc25f7b2a31ed390 Mon Sep 17 00:00:00 2001 From: Gustavo Date: Sun, 10 Dec 2023 00:11:15 +0100 Subject: [PATCH 3/7] =?UTF-8?q?=E2=9A=A1=20adjust=20parameters?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- suave/suave/thruster_monitor.py | 2 ++ suave/suave/water_visibility_observer.py | 6 ++++-- suave_missions/config/mission_config.yaml | 4 ++-- 3 files changed, 8 insertions(+), 4 deletions(-) diff --git a/suave/suave/thruster_monitor.py b/suave/suave/thruster_monitor.py index 13b9d20..fc60e53 100644 --- a/suave/suave/thruster_monitor.py +++ b/suave/suave/thruster_monitor.py @@ -41,6 +41,8 @@ def __init__(self): self.mavros_state_sub = self.create_subscription( State, 'mavros/state', self.status_cb, 10) + self.last_event_time = self.get_clock().now().to_msg().sec + def status_cb(self, msg): if msg.mode == 'GUIDED': self.last_event_time = self.get_clock().now().to_msg().sec diff --git a/suave/suave/water_visibility_observer.py b/suave/suave/water_visibility_observer.py index 3da04d8..3d7b325 100755 --- a/suave/suave/water_visibility_observer.py +++ b/suave/suave/water_visibility_observer.py @@ -16,8 +16,8 @@ class WaterVisibilityObserver(Node): def __init__(self): super().__init__('water_visibility') - self.declare_parameter('qa_publishing_period', 0.2) - self.declare_parameter('water_visibility_period', 100) + self.declare_parameter('qa_publishing_period', 1.0) + self.declare_parameter('water_visibility_period', 150) self.declare_parameter('water_visibility_min', 1.25) self.declare_parameter('water_visibility_max', 3.75) self.declare_parameter('water_visibility_sec_shift', 0.0) @@ -31,6 +31,8 @@ def __init__(self): self.mavros_state_sub = self.create_subscription( State, 'mavros/state', self.status_cb, 10) + self.initial_time = self.get_clock().now().to_msg().sec + def status_cb(self, msg): if msg.mode == "GUIDED": self.initial_time = self.get_clock().now().to_msg().sec diff --git a/suave_missions/config/mission_config.yaml b/suave_missions/config/mission_config.yaml index b279147..cbb9d92 100644 --- a/suave_missions/config/mission_config.yaml +++ b/suave_missions/config/mission_config.yaml @@ -7,7 +7,7 @@ /water_visibility_observer_node: ros__parameters: - water_visibility_period: 80 #Water visibility period in seconds + water_visibility_period: 120 #Water visibility period in seconds water_visibility_min: 1.25 #Minimum value for water visibility water_visibility_max: 3.75 #Maximum value for water visibility water_visibility_sec_shift: 0.0 #Water visibility seconds shift to left @@ -15,7 +15,7 @@ /thruster_monitor: ros__parameters: thruster_events: # Thruster events, format: (thruster number (1 to 6), failure or recovery, delta time in seconds(from the last event)) - - (1,failure,35) + - (1,failure,60) /task_bridge: ros__parameters: From a0991985e1dff526897aea8ded8b9b8b2f1c3c1c Mon Sep 17 00:00:00 2001 From: Gustavo Date: Fri, 19 Jan 2024 18:09:49 -0300 Subject: [PATCH 4/7] match suave_reasoner w/ new metacontrol version --- suave_metacontrol/suave_metacontrol/suave_reasoner.py | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/suave_metacontrol/suave_metacontrol/suave_reasoner.py b/suave_metacontrol/suave_metacontrol/suave_reasoner.py index 9d3e4b3..11e1d01 100755 --- a/suave_metacontrol/suave_metacontrol/suave_reasoner.py +++ b/suave_metacontrol/suave_metacontrol/suave_reasoner.py @@ -21,8 +21,9 @@ def analyze(self): try: objectives = self.search_objectives() for objective in objectives: - if objective not in objectives_in_error and \ - str(objective.typeF.name) == "f_generate_search_path": + if str(objective.name) not in objectives_in_error and \ + self.get_function_name_from_objective_id(objective.name) == \ + "f_generate_search_path": measured_water_visibility = get_measured_qa( 'water_visibility', self.tomasys) current_fd = get_current_function_design( @@ -31,8 +32,9 @@ def analyze(self): for qa in current_fd.hasQAestimation: if str(qa.isQAtype.name) == 'water_visibility' \ and (qa.hasValue - measured_water_visibility) < 0: - objectives_in_error.append(objective) - objective.o_status = "IN_ERROR_NFR" + objectives_in_error.append(str(objective.name)) + self.update_objective_status( + objective, 'IN_ERROR_NFR') except Exception as err: self.logger.info( "In Custom Analyze, exception returned: {}".format(err)) From 5012576aef25a7819b45d00cdf07af83a892fd7a Mon Sep 17 00:00:00 2001 From: Gustavo Date: Fri, 19 Jan 2024 18:10:55 -0300 Subject: [PATCH 5/7] =?UTF-8?q?=E2=9C=A8=20add=20mission=5Fmetrics.py?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- suave_missions/setup.py | 1 + .../suave_missions/mission_metrics.py | 235 ++++++++++++++++++ 2 files changed, 236 insertions(+) create mode 100644 suave_missions/suave_missions/mission_metrics.py diff --git a/suave_missions/setup.py b/suave_missions/setup.py index a560dec..4bdb44d 100644 --- a/suave_missions/setup.py +++ b/suave_missions/setup.py @@ -30,6 +30,7 @@ 'suave_missions.const_dist_mission:main', 'time_constrained_mission = ' + 'suave_missions.time_constrained_mission:main', + 'mission_metrics = suave_missions.mission_metrics:main', ], }, ) diff --git a/suave_missions/suave_missions/mission_metrics.py b/suave_missions/suave_missions/mission_metrics.py new file mode 100644 index 0000000..584488e --- /dev/null +++ b/suave_missions/suave_missions/mission_metrics.py @@ -0,0 +1,235 @@ +# Copyright 2024 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. +"""module used to save metrics.""" +import csv +import os +import sys +from datetime import datetime +from pathlib import Path + +import rclpy +from rclpy.callback_groups import MutuallyExclusiveCallbackGroup +from rclpy.node import Node + +from geometry_msgs.msg import Pose +from mavros_msgs.msg import State +from std_msgs.msg import Bool +from std_msgs.msg import Float32 +from std_srvs.srv import Empty + + +class MissionMetrics(Node): + """ROS node used to save mission metrics. + + ROS params: `result_path`, `result_filename`, `adaptation_manager`, + `mission_name`. + + """ + + def __init__(self, node_name: str = 'suave_metrics'): + super().__init__(node_name) + + self.declare_parameter('result_path', '~/suave/results') + self.declare_parameter('result_filename', 'mission_results') + self.declare_parameter('adaptation_manager', 'none') + self.declare_parameter('mission_name', 'inspection') + # self.declare_parameter('metrics_header', ['']) + + #: path where results must be saved + self.result_path = self.get_parameter('result_path').value + #: file name where results must be saved + self.result_filename = self.get_parameter('result_filename').value + #: adaptation manager name, e.g., none, random, metacontrol + self.adaptation_manager = self.get_parameter( + 'adaptation_manager').value + + self.mission_name = self.get_parameter('mission_name').value + self.mission_name += ' ' + self.adaptation_manager + + self.metrics_header = [ + 'mission_name', + 'datetime', + 'initial pos (x,y)', + 'time_mission (s)', + 'time search (s)', + 'distance inspected (m)'] + + self.mission_start_time = None + self.pipeline_detected_time = None + + # TODO: get this automatically + self.initial_x = 0.0 + self.initial_y = 0.0 + self.first_gz_pose = True + + self.distance_inspected = 0.0 + + self.state_sub = self.create_subscription( + State, + 'mavros/state', + self.status_cb, + 10, + callback_group=MutuallyExclusiveCallbackGroup() + ) + + self.pipeline_detected_sub = self.create_subscription( + Bool, + 'pipeline/detected', + self.pipeline_detected_cb, + 10, + callback_group=MutuallyExclusiveCallbackGroup()) + + self.pipeline_inspected_sub = self.create_subscription( + Bool, + 'pipeline/inspected', + self.pipeline_inspected_cb, + 10, + callback_group=MutuallyExclusiveCallbackGroup()) + + self.pipeline_distance_inspected_sub = self.create_subscription( + Float32, + 'pipeline/distance_inspected', + self.distance_inspected_cb, + 1, + callback_group=MutuallyExclusiveCallbackGroup()) + + self.gazebo_pos_sub = self.create_subscription( + Pose, + 'model/bluerov2/pose', + self.gazebo_pos_cb, + 10, + callback_group=MutuallyExclusiveCallbackGroup()) + + self.save_mission_results_srv = self.create_service( + Empty, + 'mission_metrics/save', + self.save_mission_results_cb, + callback_group=MutuallyExclusiveCallbackGroup() + ) + + def status_cb(self, msg): + if msg.mode == 'GUIDED': + self.mission_start_time = self.get_clock().now() + self.destroy_subscription(self.state_sub) + + def pipeline_detected_cb(self, msg): + if msg.data is True: + self.pipeline_detected_time = self.get_clock().now() + self.destroy_subscription(self.pipeline_detected_sub) + + def pipeline_inspected_cb(self, msg): + if msg.data is True: + self.save_mission_results() + self.destroy_subscription(self.pipeline_inspected_sub) + + def distance_inspected_cb(self, msg): + self.distance_inspected = msg.data + + def gazebo_pos_cb(self, msg): + self.gazebo_pos = msg + if self.first_gz_pose is True: + self.first_gz_pose = False + self.initial_x = msg.position.x + self.initial_y = msg.position.y + self.destroy_subscription(self.gazebo_pos_sub) + + def save_mission_results_cb( + self, req: Empty.Request, res: Empty.Response) -> None: + self.save_mission_results() + self.destroy_subscription(self.gazebo_pos_sub) + self.destroy_subscription(self.pipeline_detected_sub) + self.destroy_subscription(self.pipeline_inspected_sub) + self.destroy_subscription(self.pipeline_distance_inspected_sub) + return res + + def save_mission_results(self) -> None: + """Process mission data and save it into a .csv file.""" + detection_time_delta = -1 + if self.pipeline_detected_time is not None: + detection_time_delta = \ + self.pipeline_detected_time - self.mission_start_time + detection_time_delta = detection_time_delta.to_msg().sec + + mission_time_delta = \ + self.get_clock().now() - self.mission_start_time + + self.get_logger().info( + 'Time elapsed to detect pipeline: {} seconds'.format( + detection_time_delta)) + self.get_logger().info( + 'Time elapsed to complete mission: {} seconds'.format( + mission_time_delta.to_msg().sec)) + + mission_data = [ + self.mission_name, + datetime.now().strftime("%b-%d-%Y-%H-%M-%S"), + '({0}, {1})'.format( + round(self.initial_x, 2), round(self.initial_y, 2)), + mission_time_delta.to_msg().sec, + detection_time_delta, + self.distance_inspected + ] + + self.save_metrics(mission_data) + # TODO: make this a parameter + os.system("touch ~/suave_ws/mission.done") + + def save_metrics(self, data: list[str | float | int]) -> None: + """Save data into a .csv file. + + Create folder if `result_path` folder does not exist. + Create file if `result_file` does not exist and insert header. + Append data if `result_file` exist. + + :param data: array with data to be saved + """ + result_path = Path(self.result_path).expanduser() + + if result_path.is_dir() is False: + result_path.mkdir(parents=True) + + result_file = result_path / (self.result_filename + '.csv') + if result_file.is_file() is False: + result_file.touch() + self.append_csv(result_file, self.metrics_header) + + self.append_csv(result_file, data) + + def append_csv(self, file_path: str, + data: list[str | float | int]) -> None: + """Append array to .csv file. + + :param file_path: file path + :param data: data to be saved + """ + with open(file_path, 'a') as file: + writer = csv.writer(file) + writer.writerow(data) + + +def main(): + rclpy.init(args=sys.argv) + + metrics_node = MissionMetrics() + + mt_executor = rclpy.executors.MultiThreadedExecutor() + mt_executor.add_node(metrics_node) + mt_executor.spin() + + metrics_node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() From d5a957baa0e7f3a173d91b61fd5dd559c50ed5e4 Mon Sep 17 00:00:00 2001 From: Gustavo Date: Fri, 19 Jan 2024 18:11:39 -0300 Subject: [PATCH 6/7] =?UTF-8?q?=F0=9F=94=A5=20rm=20metric=20code=20from=20?= =?UTF-8?q?mission=20code?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- suave_missions/config/mission_config.yaml | 5 +- suave_missions/launch/mission.launch.py | 24 +++++-- .../suave_missions/inspection_mission.py | 70 ------------------- .../suave_missions/mission_planner.py | 18 ----- .../time_constrained_mission.py | 70 ++++++------------- 5 files changed, 43 insertions(+), 144 deletions(-) diff --git a/suave_missions/config/mission_config.yaml b/suave_missions/config/mission_config.yaml index cbb9d92..74dfe62 100644 --- a/suave_missions/config/mission_config.yaml +++ b/suave_missions/config/mission_config.yaml @@ -1,7 +1,10 @@ +/mission_metrics: + ros__parameters: + result_path: "~/suave/results" #Path to save results + /mission_node: ros__parameters: time_limit: 300 #Time limit for a time_constrained mission - result_path: "~/suave/results" #Path to save results f_generate_search_path_mode: "fd_spiral_low" #Default mode for f_generate_search_path when no manager is used f_follow_pipeline_mode: "fd_follow_pipeline" #Default mode for f_follow_pipeline when no manager is used diff --git a/suave_missions/launch/mission.launch.py b/suave_missions/launch/mission.launch.py index 082c57f..b5f7c8e 100644 --- a/suave_missions/launch/mission.launch.py +++ b/suave_missions/launch/mission.launch.py @@ -46,27 +46,36 @@ def generate_launch_description(): 'mission_config.yaml' ) - mission_node = Node( + mission_metrics_node = Node( package='suave_missions', - executable=mission_type, - name='mission_node', + executable='mission_metrics', + name='mission_metrics', parameters=[mission_config, { 'adaptation_manager': adaptation_manager, + 'mission_name': mission_type, }], condition=LaunchConfigurationEquals('result_filename', '') ) - mission_node_filename_override = Node( + mission_metrics_node_override = Node( package='suave_missions', - executable=mission_type, - name='mission_node', + executable='mission_metrics', + name='mission_metrics', parameters=[mission_config, { 'adaptation_manager': adaptation_manager, + 'mission_name': mission_type, 'result_filename': result_filename, }], condition=LaunchConfigurationNotEquals('result_filename', '') ) + mission_node = Node( + package='suave_missions', + executable=mission_type, + name='mission_node', + parameters=[mission_config], + ) + pkg_suave_path = get_package_share_directory('suave') suave_launch_path = os.path.join( pkg_suave_path, @@ -105,8 +114,9 @@ def generate_launch_description(): adaptation_manager_arg, mission_type_arg, result_filename_arg, + mission_metrics_node, + mission_metrics_node_override, mission_node, - mission_node_filename_override, suave_launch, suave_metacontrol_launch, suave_random_launch, diff --git a/suave_missions/suave_missions/inspection_mission.py b/suave_missions/suave_missions/inspection_mission.py index 9f7ac59..3c8e2b6 100644 --- a/suave_missions/suave_missions/inspection_mission.py +++ b/suave_missions/suave_missions/inspection_mission.py @@ -1,6 +1,3 @@ -import os -from datetime import datetime -from geometry_msgs.msg import Pose from mavros_msgs.msg import State from mavros_msgs.srv import CommandBool from mavros_msgs.srv import SetMode @@ -12,22 +9,6 @@ class InspectionMission(MissionPlanner): def __init__(self, node_name='inspection_mission'): super().__init__(node_name) - self.declare_parameter('adaptation_manager', 'none') - self.adaptation_manager = self.get_parameter( - 'adaptation_manager').value - - self.mission_name = 'inspection mission ' + str( - self.adaptation_manager) - self.metrics_header = [ - 'mission_name', - 'datetime', - 'initial pos (x,y)', - 'time_mission (s)', - 'time_search (s)'] - - # TODO: get this automatically - self.initial_x = 0.0 - self.initial_y = 0.0 self.status = State() self.state_sub = self.create_subscription( @@ -59,16 +40,6 @@ def __init__(self, node_name='inspection_mission'): self.set_mode_service = self.create_client( SetMode, 'mavros/set_mode') - self.gazebo_pos_sub = self.create_subscription( - Pose, - 'model/bluerov2/pose', - self.gazebo_pos_cb, - 10, - callback_group=MutuallyExclusiveCallbackGroup()) - - self.pipeline_detected_time = None - self.first_gz_pose = True - def perform_mission(self): self.get_logger().info("Pipeline inspection mission starting!!") self.timer = self.create_rate(1) @@ -92,38 +63,6 @@ def perform_mission(self): self.perform_task('search_pipeline', lambda: self.pipeline_detected) self.perform_task('inspect_pipeline', lambda: self.pipeline_inspected) - self.mission_completed_time = self.get_clock().now() - self.exit_mission() - - def exit_mission(self): - detection_time_delta = -1 - if self.pipeline_detected_time is not None: - detection_time_delta = \ - self.pipeline_detected_time - self.mission_start_time - detection_time_delta = detection_time_delta.to_msg().sec - - mission_time_delta = \ - self.mission_completed_time - self.mission_start_time - - self.get_logger().info( - 'Time elapsed to detect pipeline: {} seconds'.format( - detection_time_delta)) - self.get_logger().info( - 'Time elapsed to complete mission: {} seconds'.format( - mission_time_delta.to_msg().sec)) - - mission_data = [ - self.mission_name, - datetime.now().strftime("%b-%d-%Y-%H-%M-%S"), - '({0}, {1})'.format( - round(self.initial_x, 2), round(self.initial_y, 2)), - mission_time_delta.to_msg().sec, - detection_time_delta] - - self.save_metrics(mission_data) - # TODO: make this a parameter - os.system("touch ~/suave_ws/mission.done") - def status_cb(self, msg): self.status = msg @@ -145,12 +84,3 @@ def set_mode(self, mode): req = SetMode.Request() req.custom_mode = mode return self.call_service(self.set_mode_service, req) - - def gazebo_pos_cb(self, msg): - self.gazebo_pos = msg - if self.first_gz_pose is True: - self.first_gz_pose = False - self.initial_x = msg.position.x - self.initial_y = msg.position.y - - self.destroy_subscription(self.gazebo_pos_sub) diff --git a/suave_missions/suave_missions/mission_planner.py b/suave_missions/suave_missions/mission_planner.py index 6c824e3..e67edaa 100644 --- a/suave_missions/suave_missions/mission_planner.py +++ b/suave_missions/suave_missions/mission_planner.py @@ -43,24 +43,6 @@ def call_service(self, cli, request): self.get_logger().info("Waiting for future to complete") return future.result() - def save_metrics(self, data): - result_path = Path(self.result_path).expanduser() - - if result_path.is_dir() is False: - result_path.mkdir(parents=True) - - result_file = result_path / (self.result_filename + '.csv') - if result_file.is_file() is False: - result_file.touch() - self.append_csv(result_file, self.metrics_header) - - self.append_csv(result_file, data) - - def append_csv(self, file_path, data): - with open(file_path, 'a') as file: - writer = csv.writer(file) - writer.writerow(data) - def perform_mission(self): self.get_logger().warning("No mission defined!!!") diff --git a/suave_missions/suave_missions/time_constrained_mission.py b/suave_missions/suave_missions/time_constrained_mission.py index eb52503..f184e7a 100755 --- a/suave_missions/suave_missions/time_constrained_mission.py +++ b/suave_missions/suave_missions/time_constrained_mission.py @@ -1,12 +1,10 @@ #!/usr/bin/env python import sys import rclpy -import os -from datetime import datetime from rclpy.callback_groups import MutuallyExclusiveCallbackGroup from rclpy.executors import MultiThreadedExecutor -from std_msgs.msg import Float32 +from std_srvs.srv import Empty from suave_missions.inspection_mission import InspectionMission @@ -16,31 +14,18 @@ def __init__(self, node_name='time_contrained_mission'): if self.result_filename == 'mission_results': self.result_filename = 'time_mission_results' - self.mission_name = 'time constrained ' + str(self.adaptation_manager) - self.metrics_header = [ - 'mission_name', - 'datetime', - 'initial pos (x,y)', - 'time budget (s)', - 'time search (s)', - 'distance inspected (m)'] - self.declare_parameter('time_limit', 300) self.time_limit = self.get_parameter('time_limit').value - self.pipeline_distance_inspected_sub = self.create_subscription( - Float32, - 'pipeline/distance_inspected', - self.distance_inspected_cb, - 1, + self.time_monitor_timer = self.create_timer( + 0.5, + self.time_monitor_cb, callback_group=MutuallyExclusiveCallbackGroup()) - self.distance_inspected = 0.0 - self.time_monitor_timer = self.create_timer(0.5, self.time_monitor_cb) - - def distance_inspected_cb(self, msg): - if self.abort_mission is False: - self.distance_inspected = msg.data + self.save_mission_results_cli = self.create_client( + Empty, + 'mission_metrics/save', + callback_group=MutuallyExclusiveCallbackGroup()) def time_monitor_cb(self): if self.mission_start_time is not None: @@ -48,33 +33,22 @@ def time_monitor_cb(self): elapsed_time = current_time - self.mission_start_time if elapsed_time.to_msg().sec >= self.time_limit: self.abort_mission = True + self.call_service( + self.save_mission_results_cli, Empty.Request()) self.time_monitor_timer.destroy() - def exit_mission(self): - detection_time_delta = -1 - if self.pipeline_detected_time is not None: - detection_time_delta = \ - self.pipeline_detected_time - self.mission_start_time - detection_time_delta = detection_time_delta.to_msg().sec - - self.get_logger().info( - 'Time elapsed to detect pipeline: {} seconds'.format( - detection_time_delta)) - self.get_logger().info( - 'Distance inspected: {} meters'.format( - self.distance_inspected)) - - mission_metrics = [ - self.mission_name, - datetime.now().strftime("%b-%d-%Y-%H-%M-%S"), - '({0}, {1})'.format( - round(self.initial_x, 2), round(self.initial_y, 2)), - self.time_limit, - detection_time_delta, - self.distance_inspected] - - self.save_metrics(mission_metrics) - os.system("touch ~/suave_ws/mission.done") + 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(): From 731c410a7c5da944fbf53ead7fa7eb2ffc5960e8 Mon Sep 17 00:00:00 2001 From: Gustavo Date: Fri, 19 Jan 2024 18:26:51 -0300 Subject: [PATCH 7/7] =?UTF-8?q?=F0=9F=93=9D=20Update=20changelog?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- CHANGELOG.md | 37 +++++++++++++++++++++++++++++++++++++ 1 file changed, 37 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 29e156a..ec4863b 100644 --- a/CHANGELOG.md +++ b/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