From 0e1401ea8c1acbb755f992af91c1a28d2fa52cb2 Mon Sep 17 00:00:00 2001 From: Thomas Canro Date: Wed, 20 Mar 2024 11:11:10 -0400 Subject: [PATCH 01/10] Added hydrothermal vent file with basic functionality --- .../subjugator_missions/__init__.py | 1 + .../subjugator_missions/hydrothermal_vent.py | 40 +++++++++++++++++++ mil_common/axros | 2 +- 3 files changed, 42 insertions(+), 1 deletion(-) create mode 100644 SubjuGator/command/subjugator_missions/subjugator_missions/hydrothermal_vent.py diff --git a/SubjuGator/command/subjugator_missions/subjugator_missions/__init__.py b/SubjuGator/command/subjugator_missions/subjugator_missions/__init__.py index ae0fabb87..8119c9d9b 100644 --- a/SubjuGator/command/subjugator_missions/subjugator_missions/__init__.py +++ b/SubjuGator/command/subjugator_missions/subjugator_missions/__init__.py @@ -18,3 +18,4 @@ from .surface import Surface from .torpedos_test import TorpedosTest from .vampire_slayer import VampireSlayer +from .hydrothermal_vent import HydrothermalVent diff --git a/SubjuGator/command/subjugator_missions/subjugator_missions/hydrothermal_vent.py b/SubjuGator/command/subjugator_missions/subjugator_missions/hydrothermal_vent.py new file mode 100644 index 000000000..154b1ebdf --- /dev/null +++ b/SubjuGator/command/subjugator_missions/subjugator_missions/hydrothermal_vent.py @@ -0,0 +1,40 @@ +#!/usr/bin/env python3 +from .sub_singleton import SubjuGatorMission +import numpy as np +import math + +# This mission will direct the sub towards a red buoy, and continously circumnavigate +# the buoy in a CCW motion +class HydrothermalVent(SubjuGatorMission): + buoy_pos = [1, 2, 4] + async def run(self, args): + pitch_angle = np.arctan(self.buoy_pos[2]/self.buoy_pos[1]) + yaw_angle = np.arctan(self.buoy_pos[2]/self.buoy_pos[0]) + self.send_feedback(f"Rotating towards Buoy with yaw {math.degrees(yaw_angle)} and pitch {math.degrees(pitch_angle)}") + + rotate = self.move().set_roll_pitch_yaw(0, pitch_angle, yaw_angle) + # Multiply quaternarion by 1? (makes sure angles dont overwrite) + await self.go(rotate) + + # does this actually go forward relative to orientation + self.send_feedback(f"Traveling forward to buoy") + await self.go( + self.move() + .forward(self.buoy_pos[2] - 2) # don't reach the buoy, remain 2 units away + ) + + # rotate 90 degrees: + await self.go(self.move().set_roll_pitch_yaw(0, 90, 0)) + + # continuosly rotate around the buoy + while True: + sub_current_pos = await self.tx_pose() + delta_x = sub_current_pos[0] - self.buoy_pos[0] + delta_y = sub_current_pos[1] - self.buoy_pos[1] + yaw_angle = np.arctan2(delta_y, delta_x) # Calculate angle based on position difference + + await self.go(self.move().forward(0.2)) + await self.go(self.move().set_roll_pitch_yaw(0, 0, math.degrees(yaw_angle))) + + + diff --git a/mil_common/axros b/mil_common/axros index 8cdf13186..a4951d333 160000 --- a/mil_common/axros +++ b/mil_common/axros @@ -1 +1 @@ -Subproject commit 8cdf131866af08edf0bbe3ac9020e1da47b3e432 +Subproject commit a4951d33356c349856532ddef5c97c6ec6810c28 From 9b6f18c7d193a00a167fe7687603cba2d21ddee4 Mon Sep 17 00:00:00 2001 From: Thomas Canro Date: Thu, 21 Mar 2024 23:53:58 -0400 Subject: [PATCH 02/10] Added functionality for pose editing and setting up circumvention --- .../subjugator_missions/hydrothermal_vent.py | 61 ++++++++++++++----- .../subjugator_missions/pose_editor.py | 17 ++++-- 2 files changed, 58 insertions(+), 20 deletions(-) diff --git a/SubjuGator/command/subjugator_missions/subjugator_missions/hydrothermal_vent.py b/SubjuGator/command/subjugator_missions/subjugator_missions/hydrothermal_vent.py index 154b1ebdf..4d6b49aa7 100644 --- a/SubjuGator/command/subjugator_missions/subjugator_missions/hydrothermal_vent.py +++ b/SubjuGator/command/subjugator_missions/subjugator_missions/hydrothermal_vent.py @@ -6,35 +6,64 @@ # This mission will direct the sub towards a red buoy, and continously circumnavigate # the buoy in a CCW motion class HydrothermalVent(SubjuGatorMission): - buoy_pos = [1, 2, 4] async def run(self, args): - pitch_angle = np.arctan(self.buoy_pos[2]/self.buoy_pos[1]) - yaw_angle = np.arctan(self.buoy_pos[2]/self.buoy_pos[0]) - self.send_feedback(f"Rotating towards Buoy with yaw {math.degrees(yaw_angle)} and pitch {math.degrees(pitch_angle)}") + self.buoy_pos = np.array([3, 2, 4]) # Convert buoy position to numpy array + # pitch_angle = np.arctan(self.buoy_pos[2]/self.buoy_pos[1]) + # yaw_angle = np.arctan(self.buoy_pos[2]/self.buoy_pos[0]) + # self.send_feedback(f"Rotating towards Buoy with yaw {math.degrees(yaw_angle)} and pitch {math.degrees(pitch_angle)}") + + # rotate = self.move().set_roll_pitch_yaw(0, pitch_angle, yaw_angle) + # await self.go(rotate) + + # # does this actually go forward relative to orientation + # self.send_feedback(f"Traveling forward to buoy") + # await self.go( + # self.move() + # .forward(self.buoy_pos[2] - 2) # don't reach the buoy, remain 2 units away + # ) + + + # self.send_feedback(f"Undoing pitch") + # rotate = self.move().set_roll_pitch_yaw(0, -pitch_angle, 0) + # await self.go(rotate) - rotate = self.move().set_roll_pitch_yaw(0, pitch_angle, yaw_angle) - # Multiply quaternarion by 1? (makes sure angles dont overwrite) - await self.go(rotate) + # # rotate 90 degrees: + # self.send_feedback(f"Rotating 90 degrees") + # rotate = self.move().set_roll_pitch_yaw(0, 0, np.radians(90)) + # await self.go(rotate) - # does this actually go forward relative to orientation + # First, move down the necessary depth to reach the buoy + + self.send_feedback("Submerging to buoy depth") + await self.go(self.move().down(self.buoy_pos[1])) + yaw_angle = np.arctan(self.buoy_pos[2]/self.buoy_pos[0]) + self.send_feedback(f"Rotating towards Buoy with yaw {math.degrees(yaw_angle)}") + rotate = self.move().set_roll_pitch_yaw(0, 0, yaw_angle) + await self.go(rotate) + self.send_feedback(f"Traveling forward to buoy") await self.go( self.move() .forward(self.buoy_pos[2] - 2) # don't reach the buoy, remain 2 units away ) - + # rotate 90 degrees: - await self.go(self.move().set_roll_pitch_yaw(0, 90, 0)) + self.send_feedback("Rotating 90 degrees") + rotate = self.move().set_roll_pitch_yaw(0, 0, np.radians(90)) + await self.go(rotate) # continuosly rotate around the buoy while True: - sub_current_pos = await self.tx_pose() - delta_x = sub_current_pos[0] - self.buoy_pos[0] - delta_y = sub_current_pos[1] - self.buoy_pos[1] - yaw_angle = np.arctan2(delta_y, delta_x) # Calculate angle based on position difference + sub_current_pos = await self.tx_pose() # Assuming tx_pose() returns a tuple + + # Figure out why this is not working: - await self.go(self.move().forward(0.2)) - await self.go(self.move().set_roll_pitch_yaw(0, 0, math.degrees(yaw_angle))) + #sub_current_pos = np.array(sub_current_pos[:2]) # Extract position information and convert to numpy array + # delta = sub_current_pos - self.buoy_pos # Calculate position difference + #yaw_angle = np.arctan2(delta[1], delta[0]) # Calculate angle based on position difference + #rotate = self.move().set_roll_pitch_yaw(0, 0, yaw_angle) + #await self.go(self.move().forward(1)) + #await self.go(rotate) diff --git a/SubjuGator/command/subjugator_missions/subjugator_missions/pose_editor.py b/SubjuGator/command/subjugator_missions/subjugator_missions/pose_editor.py index ee2562491..870b6cb1f 100644 --- a/SubjuGator/command/subjugator_missions/subjugator_missions/pose_editor.py +++ b/SubjuGator/command/subjugator_missions/subjugator_missions/pose_editor.py @@ -444,6 +444,14 @@ def turn_vec_towards(self, body_vec, towards_point): def turn_vec_towards_rel(self, body_vec, towards_rel_point) -> PoseEditor: return self.set_orientation(triad((UP, towards_rel_point), (UP, body_vec))) + + def set_roll_pitch_yaw(self, roll: float, pitch: float, yaw: float) -> PoseEditor: + return self.set_orientation( + transformations.quaternion_multiply( + transformations.quaternion_from_euler(roll, pitch, yaw), + self.orientation, + ), + ) def yaw_left(self, angle: float) -> PoseEditor: """ @@ -617,8 +625,8 @@ def as_PoseTwist( def as_PoseTwistStamped( self, - linear: Sequence[float] = [0, 0, 0], - angular: Sequence[float] = [0, 0, 0], + linear: Sequence[int] = [0, 0, 0], + angular: Sequence[int] = [0, 0, 0], ) -> PoseTwistStamped: """ Returns a :class:`~mil_msgs.msg.PoseTwist` message class with the pose @@ -639,8 +647,8 @@ def as_PoseTwistStamped( def as_MoveToGoal( self, - linear: Sequence[float] = [0, 0, 0], - angular: Sequence[float] = [0, 0, 0], + linear: Sequence[int] = [0, 0, 0], + angular: Sequence[int] = [0, 0, 0], **kwargs, ) -> MoveToGoal: return MoveToGoal( @@ -679,3 +687,4 @@ def linear_tolerance(self) -> int: @property def angular_tolerance(self) -> int: return 0 + \ No newline at end of file From 486cdb3d8c5f3dd33459cf7daa455825469d1dee Mon Sep 17 00:00:00 2001 From: Thomas Canro Date: Thu, 21 Mar 2024 23:55:00 -0400 Subject: [PATCH 03/10] fixed comments --- .../subjugator_missions/hydrothermal_vent.py | 23 ------------------- 1 file changed, 23 deletions(-) diff --git a/SubjuGator/command/subjugator_missions/subjugator_missions/hydrothermal_vent.py b/SubjuGator/command/subjugator_missions/subjugator_missions/hydrothermal_vent.py index 4d6b49aa7..3747a5b86 100644 --- a/SubjuGator/command/subjugator_missions/subjugator_missions/hydrothermal_vent.py +++ b/SubjuGator/command/subjugator_missions/subjugator_missions/hydrothermal_vent.py @@ -8,29 +8,6 @@ class HydrothermalVent(SubjuGatorMission): async def run(self, args): self.buoy_pos = np.array([3, 2, 4]) # Convert buoy position to numpy array - # pitch_angle = np.arctan(self.buoy_pos[2]/self.buoy_pos[1]) - # yaw_angle = np.arctan(self.buoy_pos[2]/self.buoy_pos[0]) - # self.send_feedback(f"Rotating towards Buoy with yaw {math.degrees(yaw_angle)} and pitch {math.degrees(pitch_angle)}") - - # rotate = self.move().set_roll_pitch_yaw(0, pitch_angle, yaw_angle) - # await self.go(rotate) - - # # does this actually go forward relative to orientation - # self.send_feedback(f"Traveling forward to buoy") - # await self.go( - # self.move() - # .forward(self.buoy_pos[2] - 2) # don't reach the buoy, remain 2 units away - # ) - - - # self.send_feedback(f"Undoing pitch") - # rotate = self.move().set_roll_pitch_yaw(0, -pitch_angle, 0) - # await self.go(rotate) - - # # rotate 90 degrees: - # self.send_feedback(f"Rotating 90 degrees") - # rotate = self.move().set_roll_pitch_yaw(0, 0, np.radians(90)) - # await self.go(rotate) # First, move down the necessary depth to reach the buoy From 9f3144cc525e665f95abcf883ba7b3b39b679a39 Mon Sep 17 00:00:00 2001 From: Thomas Canro Date: Fri, 5 Apr 2024 19:42:53 -0400 Subject: [PATCH 04/10] Finalized hydrothermal vent mission functionality --- .../subjugator_missions/hydrothermal_vent.py | 37 +++++++++++-------- 1 file changed, 22 insertions(+), 15 deletions(-) diff --git a/SubjuGator/command/subjugator_missions/subjugator_missions/hydrothermal_vent.py b/SubjuGator/command/subjugator_missions/subjugator_missions/hydrothermal_vent.py index 3747a5b86..e20ba06ef 100644 --- a/SubjuGator/command/subjugator_missions/subjugator_missions/hydrothermal_vent.py +++ b/SubjuGator/command/subjugator_missions/subjugator_missions/hydrothermal_vent.py @@ -5,6 +5,9 @@ # This mission will direct the sub towards a red buoy, and continously circumnavigate # the buoy in a CCW motion + + +# Computer vision aspect still required to detect the red buoy. For now, random position used class HydrothermalVent(SubjuGatorMission): async def run(self, args): self.buoy_pos = np.array([3, 2, 4]) # Convert buoy position to numpy array @@ -21,26 +24,30 @@ async def run(self, args): self.send_feedback(f"Traveling forward to buoy") await self.go( self.move() - .forward(self.buoy_pos[2] - 2) # don't reach the buoy, remain 2 units away + .forward(self.buoy_pos[2] - 1) # don't reach the buoy, remain 1 meter away ) - + yaw_angle2 = np.deg2rad(90) # rotate 90 degrees: - self.send_feedback("Rotating 90 degrees") - rotate = self.move().set_roll_pitch_yaw(0, 0, np.radians(90)) + self.send_feedback("Rotating 90 degrees left") + rotate = self.move().yaw_left(yaw_angle2) await self.go(rotate) + + self.send_feedback("Circumnaviganting the buoy") + await self.go(self.move().forward(0.5)) + for i in range(0, 3): + rotate = self.move().yaw_right(yaw_angle2) + await self.go(rotate) + await self.go(self.move().forward(1)) + rotate = self.move().yaw_right(yaw_angle2) + await self.go(rotate) + await self.go(self.move().forward(0.5)) - # continuosly rotate around the buoy - while True: - sub_current_pos = await self.tx_pose() # Assuming tx_pose() returns a tuple - - # Figure out why this is not working: + self.send_feedback("Returning to origin") + await self.go(self.move().yaw_left(yaw_angle2)) + await self.go(self.move().forward(self.buoy_pos[2] - 1)) - #sub_current_pos = np.array(sub_current_pos[:2]) # Extract position information and convert to numpy array - # delta = sub_current_pos - self.buoy_pos # Calculate position difference - #yaw_angle = np.arctan2(delta[1], delta[0]) # Calculate angle based on position difference - #rotate = self.move().set_roll_pitch_yaw(0, 0, yaw_angle) - #await self.go(self.move().forward(1)) - #await self.go(rotate) + await self.go(self.move().set_roll_pitch_yaw(0, 0, -yaw_angle)) + await self.go(self.move().up(self.buoy_pos[1])) From f2b211df604367e1c937530246926551f54ecd1d Mon Sep 17 00:00:00 2001 From: Thomas Canro Date: Tue, 9 Apr 2024 17:43:41 -0400 Subject: [PATCH 05/10] added the 3D model to gazebo and adapted the mission so it works once simulated --- .../subjugator_missions/hydrothermal_vent.py | 21 ++-- .../worlds/robosub_2022.world | 102 ++++++++++++++++++ 2 files changed, 114 insertions(+), 9 deletions(-) diff --git a/SubjuGator/command/subjugator_missions/subjugator_missions/hydrothermal_vent.py b/SubjuGator/command/subjugator_missions/subjugator_missions/hydrothermal_vent.py index e20ba06ef..701844971 100644 --- a/SubjuGator/command/subjugator_missions/subjugator_missions/hydrothermal_vent.py +++ b/SubjuGator/command/subjugator_missions/subjugator_missions/hydrothermal_vent.py @@ -10,21 +10,24 @@ # Computer vision aspect still required to detect the red buoy. For now, random position used class HydrothermalVent(SubjuGatorMission): async def run(self, args): - self.buoy_pos = np.array([3, 2, 4]) # Convert buoy position to numpy array + self.buoy_pos = np.array([3, 4, -2]) # Convert buoy position to numpy array # First, move down the necessary depth to reach the buoy self.send_feedback("Submerging to buoy depth") - await self.go(self.move().down(self.buoy_pos[1])) - yaw_angle = np.arctan(self.buoy_pos[2]/self.buoy_pos[0]) + if (self.buoy_pos[2] < 0): + await self.go(self.move().down(abs(self.buoy_pos[2] + 0.3))) + else: + await self.go(self.move().up(self.buoy_pos[2] - 0.3)) + yaw_angle = np.arctan(self.buoy_pos[1]/self.buoy_pos[0]) self.send_feedback(f"Rotating towards Buoy with yaw {math.degrees(yaw_angle)}") - rotate = self.move().set_roll_pitch_yaw(0, 0, yaw_angle) + rotate = self.move().set_roll_pitch_yaw(0, 0, yaw_angle + 0.1) await self.go(rotate) self.send_feedback(f"Traveling forward to buoy") await self.go( self.move() - .forward(self.buoy_pos[2] - 1) # don't reach the buoy, remain 1 meter away + .forward(np.sqrt(np.square(self.buoy_pos[0]) + np.square(self.buoy_pos[1])) - 0.7) # don't reach the buoy, remain 0.5 meter away ) yaw_angle2 = np.deg2rad(90) # rotate 90 degrees: @@ -33,18 +36,18 @@ async def run(self, args): await self.go(rotate) self.send_feedback("Circumnaviganting the buoy") - await self.go(self.move().forward(0.5)) + await self.go(self.move().forward(0.7)) for i in range(0, 3): rotate = self.move().yaw_right(yaw_angle2) await self.go(rotate) - await self.go(self.move().forward(1)) + await self.go(self.move().forward(1.4)) rotate = self.move().yaw_right(yaw_angle2) await self.go(rotate) - await self.go(self.move().forward(0.5)) + await self.go(self.move().forward(0.7)) self.send_feedback("Returning to origin") await self.go(self.move().yaw_left(yaw_angle2)) - await self.go(self.move().forward(self.buoy_pos[2] - 1)) + await self.go(self.move().forward(np.sqrt(np.square(self.buoy_pos[0]) + np.square(self.buoy_pos[1])) - 1)) await self.go(self.move().set_roll_pitch_yaw(0, 0, -yaw_angle)) diff --git a/SubjuGator/simulation/subjugator_gazebo/worlds/robosub_2022.world b/SubjuGator/simulation/subjugator_gazebo/worlds/robosub_2022.world index 3058401f4..08284ac20 100644 --- a/SubjuGator/simulation/subjugator_gazebo/worlds/robosub_2022.world +++ b/SubjuGator/simulation/subjugator_gazebo/worlds/robosub_2022.world @@ -937,5 +937,107 @@ -0.957126 1.98002 0 0 -0 0 + + + + 0.0131332 + + 7.311e-05 + 0 + 0 + 7.311e-05 + 0 + 7.311e-05 + + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + 0 + 0 + 0 + + 3 4 -2 0 -0 0 + + + 0.15 + + + + 1 + + + __default__ + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + 0 + 1 + + + 0 + 10 + 0 0 0 0 -0 0 + + + 0.117968 + + + + + + 1 + 1 + 0 0 0 + 0 + 0 + + + 1 + 0 + 0 + 1 + + 0 + + + + + 0 + 1e+06 + + + 0 + 1 + 1 + + 0 + 0.2 + 1e+13 + 1 + 0.01 + 0 + + + 1 + -0.01 + 0 + 0.2 + 1e+13 + 1 + + + + + + 1 + 1 + From c696eba7098a5be291efb3629be64f464241d4bb Mon Sep 17 00:00:00 2001 From: Thomas Canro Date: Tue, 16 Apr 2024 18:06:50 -0400 Subject: [PATCH 06/10] pre-commit changes --- .../subjugator_missions/__init__.py | 2 +- .../subjugator_missions/hydrothermal_vent.py | 34 +++++++++++-------- .../subjugator_missions/pose_editor.py | 5 ++- mil_common/axros | 2 +- 4 files changed, 24 insertions(+), 19 deletions(-) diff --git a/SubjuGator/command/subjugator_missions/subjugator_missions/__init__.py b/SubjuGator/command/subjugator_missions/subjugator_missions/__init__.py index 8119c9d9b..8a42eae11 100644 --- a/SubjuGator/command/subjugator_missions/subjugator_missions/__init__.py +++ b/SubjuGator/command/subjugator_missions/subjugator_missions/__init__.py @@ -5,6 +5,7 @@ from .ball_drop_test import BallDropTest from .dracula_grab import DraculaGrabber from .gripper_test import GripperTest +from .hydrothermal_vent import HydrothermalVent from .move import Move from .pinger import Pinger from .pose_editor import PoseEditor @@ -18,4 +19,3 @@ from .surface import Surface from .torpedos_test import TorpedosTest from .vampire_slayer import VampireSlayer -from .hydrothermal_vent import HydrothermalVent diff --git a/SubjuGator/command/subjugator_missions/subjugator_missions/hydrothermal_vent.py b/SubjuGator/command/subjugator_missions/subjugator_missions/hydrothermal_vent.py index 701844971..bda9af000 100644 --- a/SubjuGator/command/subjugator_missions/subjugator_missions/hydrothermal_vent.py +++ b/SubjuGator/command/subjugator_missions/subjugator_missions/hydrothermal_vent.py @@ -1,9 +1,11 @@ #!/usr/bin/env python3 -from .sub_singleton import SubjuGatorMission -import numpy as np import math -# This mission will direct the sub towards a red buoy, and continously circumnavigate +import numpy as np + +from .sub_singleton import SubjuGatorMission + +# This mission will direct the sub towards a red buoy, and continuously circumnavigate # the buoy in a CCW motion @@ -15,26 +17,28 @@ async def run(self, args): # First, move down the necessary depth to reach the buoy self.send_feedback("Submerging to buoy depth") - if (self.buoy_pos[2] < 0): + if self.buoy_pos[2] < 0: await self.go(self.move().down(abs(self.buoy_pos[2] + 0.3))) else: await self.go(self.move().up(self.buoy_pos[2] - 0.3)) - yaw_angle = np.arctan(self.buoy_pos[1]/self.buoy_pos[0]) + yaw_angle = np.arctan(self.buoy_pos[1] / self.buoy_pos[0]) self.send_feedback(f"Rotating towards Buoy with yaw {math.degrees(yaw_angle)}") rotate = self.move().set_roll_pitch_yaw(0, 0, yaw_angle + 0.1) - await self.go(rotate) - - self.send_feedback(f"Traveling forward to buoy") + await self.go(rotate) + + self.send_feedback("Traveling forward to buoy") await self.go( - self.move() - .forward(np.sqrt(np.square(self.buoy_pos[0]) + np.square(self.buoy_pos[1])) - 0.7) # don't reach the buoy, remain 0.5 meter away + self.move().forward( + np.sqrt(np.square(self.buoy_pos[0]) + np.square(self.buoy_pos[1])) + - 0.7, + ), # don't reach the buoy, remain 0.5 meter away ) yaw_angle2 = np.deg2rad(90) # rotate 90 degrees: self.send_feedback("Rotating 90 degrees left") rotate = self.move().yaw_left(yaw_angle2) await self.go(rotate) - + self.send_feedback("Circumnaviganting the buoy") await self.go(self.move().forward(0.7)) for i in range(0, 3): @@ -47,10 +51,12 @@ async def run(self, args): self.send_feedback("Returning to origin") await self.go(self.move().yaw_left(yaw_angle2)) - await self.go(self.move().forward(np.sqrt(np.square(self.buoy_pos[0]) + np.square(self.buoy_pos[1])) - 1)) + await self.go( + self.move().forward( + np.sqrt(np.square(self.buoy_pos[0]) + np.square(self.buoy_pos[1])) - 1, + ), + ) await self.go(self.move().set_roll_pitch_yaw(0, 0, -yaw_angle)) await self.go(self.move().up(self.buoy_pos[1])) - - diff --git a/SubjuGator/command/subjugator_missions/subjugator_missions/pose_editor.py b/SubjuGator/command/subjugator_missions/subjugator_missions/pose_editor.py index 870b6cb1f..dc6512046 100644 --- a/SubjuGator/command/subjugator_missions/subjugator_missions/pose_editor.py +++ b/SubjuGator/command/subjugator_missions/subjugator_missions/pose_editor.py @@ -444,10 +444,10 @@ def turn_vec_towards(self, body_vec, towards_point): def turn_vec_towards_rel(self, body_vec, towards_rel_point) -> PoseEditor: return self.set_orientation(triad((UP, towards_rel_point), (UP, body_vec))) - + def set_roll_pitch_yaw(self, roll: float, pitch: float, yaw: float) -> PoseEditor: return self.set_orientation( - transformations.quaternion_multiply( + transformations.quaternion_multiply( transformations.quaternion_from_euler(roll, pitch, yaw), self.orientation, ), @@ -687,4 +687,3 @@ def linear_tolerance(self) -> int: @property def angular_tolerance(self) -> int: return 0 - \ No newline at end of file diff --git a/mil_common/axros b/mil_common/axros index a86842c93..a4951d333 160000 --- a/mil_common/axros +++ b/mil_common/axros @@ -1 +1 @@ -Subproject commit a86842c93370a62c0dad3fba0e6ef250137edf21 +Subproject commit a4951d33356c349856532ddef5c97c6ec6810c28 From 70e8a7e81608d57deb88215b8434e30fedb588ed Mon Sep 17 00:00:00 2001 From: Thomas Canro Date: Tue, 23 Apr 2024 12:46:06 -0400 Subject: [PATCH 07/10] center function --- .../subjugator_missions/hydrothermal_vent.py | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/SubjuGator/command/subjugator_missions/subjugator_missions/hydrothermal_vent.py b/SubjuGator/command/subjugator_missions/subjugator_missions/hydrothermal_vent.py index bda9af000..d7dc4aa81 100644 --- a/SubjuGator/command/subjugator_missions/subjugator_missions/hydrothermal_vent.py +++ b/SubjuGator/command/subjugator_missions/subjugator_missions/hydrothermal_vent.py @@ -60,3 +60,13 @@ async def run(self, args): await self.go(self.move().set_roll_pitch_yaw(0, 0, -yaw_angle)) await self.go(self.move().up(self.buoy_pos[1])) + + # This function is supposed to center the buoy based on position given in terms of xy position of the center of the red buoy, with + # respect to the center of the camera, as well as the distance from the buoy + async def center_bouy(self, buoy_pos_c): + while (abs(buoy_pos_c[0]) > 0.01) and (abs(buoy_pos_c[1]) > 0.01): + await self.go(self.move().depth(buoy_pos_c[1])) + yaw_angle = np.arctan(buoy_pos_c[0] / buoy_pos_c[2]) + await self.go(self.move().set_roll_pitch_yaw(0, 0, yaw_angle)) + + buoy_pos_c = buoy_pos_c # this will be replaced with the computer vision call to check the position of the buoy From d6ec6f06e1919dd582cc69dcd0ddf2ac31435ee9 Mon Sep 17 00:00:00 2001 From: Thomas Canro Date: Thu, 23 May 2024 18:59:53 -0400 Subject: [PATCH 08/10] Added the option to circumnavigate in either CW or CCW orientation, and a function to later center the sub with respect to the buoy, once computer vision data is ready --- .../subjugator_missions/hydrothermal_vent.py | 82 +++++++++++++------ 1 file changed, 59 insertions(+), 23 deletions(-) diff --git a/SubjuGator/command/subjugator_missions/subjugator_missions/hydrothermal_vent.py b/SubjuGator/command/subjugator_missions/subjugator_missions/hydrothermal_vent.py index d7dc4aa81..a2dcbef5a 100644 --- a/SubjuGator/command/subjugator_missions/subjugator_missions/hydrothermal_vent.py +++ b/SubjuGator/command/subjugator_missions/subjugator_missions/hydrothermal_vent.py @@ -2,16 +2,21 @@ import math import numpy as np +from mil_misc_tools import text_effects +from sensor_msgs.msg import CameraInfo from .sub_singleton import SubjuGatorMission -# This mission will direct the sub towards a red buoy, and continuously circumnavigate -# the buoy in a CCW motion +# This mission will direct the sub towards a red buoy, and circumnavigate it on a given orientation + +fprint = text_effects.FprintFactory(title="HYDROTHERMAL VENT", msg_color="green").fprint +CW = True # Computer vision aspect still required to detect the red buoy. For now, random position used class HydrothermalVent(SubjuGatorMission): async def run(self, args): + self.buoy_pos = np.array([3, 4, -2]) # Convert buoy position to numpy array # First, move down the necessary depth to reach the buoy @@ -33,38 +38,69 @@ async def run(self, args): - 0.7, ), # don't reach the buoy, remain 0.5 meter away ) + yaw_angle2 = np.deg2rad(90) - # rotate 90 degrees: - self.send_feedback("Rotating 90 degrees left") - rotate = self.move().yaw_left(yaw_angle2) - await self.go(rotate) - self.send_feedback("Circumnaviganting the buoy") - await self.go(self.move().forward(0.7)) - for i in range(0, 3): + if CW: + self.send_feedback("Circumnaviganting the buoy on a CW orientation") + + rotate = self.move().yaw_left(yaw_angle2) + await self.go(rotate) + await self.go(self.move().forward(0.7)) + for i in range(0, 3): + rotate = self.move().yaw_right(yaw_angle2) + await self.go(rotate) + await self.go(self.move().forward(1.4)) rotate = self.move().yaw_right(yaw_angle2) await self.go(rotate) - await self.go(self.move().forward(1.4)) - rotate = self.move().yaw_right(yaw_angle2) - await self.go(rotate) - await self.go(self.move().forward(0.7)) + await self.go(self.move().forward(0.7)) + else: + self.send_feedback("Circumnaviganting the buoy on a CCW orientation") - self.send_feedback("Returning to origin") - await self.go(self.move().yaw_left(yaw_angle2)) - await self.go( - self.move().forward( - np.sqrt(np.square(self.buoy_pos[0]) + np.square(self.buoy_pos[1])) - 1, - ), - ) + rotate = self.move().yaw_right(yaw_angle2) + await self.go(rotate) + await self.go(self.move().forward(0.7)) + for i in range(0, 3): + rotate = self.move().yaw_left(yaw_angle2) + await self.go(rotate) + await self.go(self.move().forward(1.4)) + rotate = self.move().yaw_left(yaw_angle2) + await self.go(rotate) + await self.go(self.move().forward(0.7)) - await self.go(self.move().set_roll_pitch_yaw(0, 0, -yaw_angle)) + # async def run(self, args): - await self.go(self.move().up(self.buoy_pos[1])) + # buoy_rad_and_center = [20, 2, 3] # arbitrary numbers to test functionality + # # center buoy + # self.center_bouy(np.array(buoy_rad_and_center[1], buoy_rad_and_center[2])) + # # go forward until desired distance is reached + # while (buoy_rad_and_center[0] < 40): + # await self.go(self.move().forward) # This function is supposed to center the buoy based on position given in terms of xy position of the center of the red buoy, with # respect to the center of the camera, as well as the distance from the buoy + + @classmethod async def center_bouy(self, buoy_pos_c): - while (abs(buoy_pos_c[0]) > 0.01) and (abs(buoy_pos_c[1]) > 0.01): + fprint("Enabling cam_ray publisher") + + await self.nh.sleep(1) + + fprint("Connecting camera") + + cam_info_sub_r = self.nh.subscribe( + "/camera/front/right/camera_info", + CameraInfo, + ) + await cam_info_sub_r.setup() + + fprint("Obtaining cam info message") + cam_info_r = await cam_info_sub_r.get_next_message() + cam_center = np.array([cam_info_r.width / 2, cam_info_r.height / 2]) + # fprint(f"Cam center: {cam_center}") + while ((abs(buoy_pos_c[0] - cam_center[0])) > 0.01) and ( + (abs(buoy_pos_c[1]) - cam_center[1]) > 0.01 + ): await self.go(self.move().depth(buoy_pos_c[1])) yaw_angle = np.arctan(buoy_pos_c[0] / buoy_pos_c[2]) await self.go(self.move().set_roll_pitch_yaw(0, 0, yaw_angle)) From 2df9224e2b14ee1f8b57e494843b01c27ca24e8c Mon Sep 17 00:00:00 2001 From: Thomas Canro Date: Sun, 9 Jun 2024 17:50:59 -0400 Subject: [PATCH 09/10] Added parameters file and included it in the hydrothermal vent mission --- .../subjugator_missions/config/strategy.yaml | 31 +++++++ .../subjugator_missions/hydrothermal_vent.py | 86 ++++++++++--------- 2 files changed, 77 insertions(+), 40 deletions(-) create mode 100644 SubjuGator/command/subjugator_missions/config/strategy.yaml diff --git a/SubjuGator/command/subjugator_missions/config/strategy.yaml b/SubjuGator/command/subjugator_missions/config/strategy.yaml new file mode 100644 index 000000000..737af2158 --- /dev/null +++ b/SubjuGator/command/subjugator_missions/config/strategy.yaml @@ -0,0 +1,31 @@ +--- +#################################### +# Overall variables +#################################### + +# Which team the sub is playing for. +# - Two possible values: red, blue. +team: red + +#################################### +# Start Gate task +#################################### +start_gate: + with_style: true + +#################################### +# Hydrothermal Vent (red buoy) task +# - CCW or CW rotation +#################################### +orientation: + CCW: true +#################################### +# Ocean Temperatures (bins) task +#################################### + # This task is based off the overall + # team the sub is playing for +#################################### +# Mapping (torpedoes) task +#################################### +torpedoes: + color: red diff --git a/SubjuGator/command/subjugator_missions/subjugator_missions/hydrothermal_vent.py b/SubjuGator/command/subjugator_missions/subjugator_missions/hydrothermal_vent.py index a2dcbef5a..594a721df 100644 --- a/SubjuGator/command/subjugator_missions/subjugator_missions/hydrothermal_vent.py +++ b/SubjuGator/command/subjugator_missions/subjugator_missions/hydrothermal_vent.py @@ -10,11 +10,44 @@ # This mission will direct the sub towards a red buoy, and circumnavigate it on a given orientation fprint = text_effects.FprintFactory(title="HYDROTHERMAL VENT", msg_color="green").fprint -CW = True # Computer vision aspect still required to detect the red buoy. For now, random position used class HydrothermalVent(SubjuGatorMission): + + async def center_bouy(self, buoy_pos_c): + """This function is supposed to center the buoy based on position given in terms of xy position of the center of the + red buoy, with respect to the center of the camera, as well as the distance from the buoy + + Args: + buoy_pos_c (float array): buoy position with respect to the center of the sub camera + """ + + fprint("Enabling cam_ray publisher") + + await self.nh.sleep(1) + + fprint("Connecting camera") + + cam_info_sub_r = self.nh.subscribe( + "/camera/front/right/camera_info", + CameraInfo, + ) + await cam_info_sub_r.setup() + + fprint("Obtaining cam info message") + cam_info_r = await cam_info_sub_r.get_next_message() + cam_center = np.array([cam_info_r.width / 2, cam_info_r.height / 2]) + # fprint(f"Cam center: {cam_center}") + while ((abs(buoy_pos_c[0] - cam_center[0])) > 0.01) and ( + (abs(buoy_pos_c[1]) - cam_center[1]) > 0.01 + ): + await self.go(self.move().depth(buoy_pos_c[1])) + yaw_angle = np.arctan(buoy_pos_c[0] / buoy_pos_c[2]) + await self.go(self.move().set_roll_pitch_yaw(0, 0, yaw_angle)) + + buoy_pos_c = buoy_pos_c # this will be replaced with the computer vision call to check the position of the buoy + async def run(self, args): self.buoy_pos = np.array([3, 4, -2]) # Convert buoy position to numpy array @@ -41,30 +74,33 @@ async def run(self, args): yaw_angle2 = np.deg2rad(90) - if CW: - self.send_feedback("Circumnaviganting the buoy on a CW orientation") + CCW = await self.nh.get_param("/strategy/orientation/CCW") - rotate = self.move().yaw_left(yaw_angle2) + if CCW: + self.send_feedback("Circumnaviganting the buoy on a CCW orientation") + + rotate = self.move().yaw_right(yaw_angle2) await self.go(rotate) await self.go(self.move().forward(0.7)) for i in range(0, 3): - rotate = self.move().yaw_right(yaw_angle2) + rotate = self.move().yaw_left(yaw_angle2) await self.go(rotate) await self.go(self.move().forward(1.4)) - rotate = self.move().yaw_right(yaw_angle2) + rotate = self.move().yaw_left(yaw_angle2) await self.go(rotate) await self.go(self.move().forward(0.7)) + else: - self.send_feedback("Circumnaviganting the buoy on a CCW orientation") + self.send_feedback("Circumnaviganting the buoy on a CW orientation") - rotate = self.move().yaw_right(yaw_angle2) + rotate = self.move().yaw_left(yaw_angle2) await self.go(rotate) await self.go(self.move().forward(0.7)) for i in range(0, 3): - rotate = self.move().yaw_left(yaw_angle2) + rotate = self.move().yaw_right(yaw_angle2) await self.go(rotate) await self.go(self.move().forward(1.4)) - rotate = self.move().yaw_left(yaw_angle2) + rotate = self.move().yaw_right(yaw_angle2) await self.go(rotate) await self.go(self.move().forward(0.7)) @@ -76,33 +112,3 @@ async def run(self, args): # # go forward until desired distance is reached # while (buoy_rad_and_center[0] < 40): # await self.go(self.move().forward) - - # This function is supposed to center the buoy based on position given in terms of xy position of the center of the red buoy, with - # respect to the center of the camera, as well as the distance from the buoy - - @classmethod - async def center_bouy(self, buoy_pos_c): - fprint("Enabling cam_ray publisher") - - await self.nh.sleep(1) - - fprint("Connecting camera") - - cam_info_sub_r = self.nh.subscribe( - "/camera/front/right/camera_info", - CameraInfo, - ) - await cam_info_sub_r.setup() - - fprint("Obtaining cam info message") - cam_info_r = await cam_info_sub_r.get_next_message() - cam_center = np.array([cam_info_r.width / 2, cam_info_r.height / 2]) - # fprint(f"Cam center: {cam_center}") - while ((abs(buoy_pos_c[0] - cam_center[0])) > 0.01) and ( - (abs(buoy_pos_c[1]) - cam_center[1]) > 0.01 - ): - await self.go(self.move().depth(buoy_pos_c[1])) - yaw_angle = np.arctan(buoy_pos_c[0] / buoy_pos_c[2]) - await self.go(self.move().set_roll_pitch_yaw(0, 0, yaw_angle)) - - buoy_pos_c = buoy_pos_c # this will be replaced with the computer vision call to check the position of the buoy From 259ecf0cfb7425bb08edf53759b21e0d1631154a Mon Sep 17 00:00:00 2001 From: Thomas Canro Date: Sun, 9 Jun 2024 18:47:12 -0400 Subject: [PATCH 10/10] added the strategy file to the ros parameters, in the sub8.launch file --- SubjuGator/command/subjugator_launch/launch/sub8.launch | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/SubjuGator/command/subjugator_launch/launch/sub8.launch b/SubjuGator/command/subjugator_launch/launch/sub8.launch index 247fe1e62..cf51ef574 100644 --- a/SubjuGator/command/subjugator_launch/launch/sub8.launch +++ b/SubjuGator/command/subjugator_launch/launch/sub8.launch @@ -30,6 +30,10 @@ + + + +