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 @@ + + + + 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/__init__.py b/SubjuGator/command/subjugator_missions/subjugator_missions/__init__.py index ae0fabb87..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 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..594a721df --- /dev/null +++ b/SubjuGator/command/subjugator_missions/subjugator_missions/hydrothermal_vent.py @@ -0,0 +1,114 @@ +#!/usr/bin/env python3 +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 circumnavigate it on a given orientation + +fprint = text_effects.FprintFactory(title="HYDROTHERMAL VENT", msg_color="green").fprint + + +# 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 + + # First, move down the necessary depth to reach the buoy + + self.send_feedback("Submerging to buoy depth") + 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 + 0.1) + 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 + ) + + yaw_angle2 = np.deg2rad(90) + + CCW = await self.nh.get_param("/strategy/orientation/CCW") + + 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_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)) + + else: + 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(0.7)) + + # async def run(self, args): + + # 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) diff --git a/SubjuGator/command/subjugator_missions/subjugator_missions/pose_editor.py b/SubjuGator/command/subjugator_missions/subjugator_missions/pose_editor.py index ee2562491..dc6512046 100644 --- a/SubjuGator/command/subjugator_missions/subjugator_missions/pose_editor.py +++ b/SubjuGator/command/subjugator_missions/subjugator_missions/pose_editor.py @@ -445,6 +445,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: """ Returns a pose editor with the orientation yawed to the left by some @@ -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( diff --git a/SubjuGator/simulation/subjugator_gazebo/worlds/robosub_2022.world b/SubjuGator/simulation/subjugator_gazebo/worlds/robosub_2022.world index 642fbb197..6495aac53 100644 --- a/SubjuGator/simulation/subjugator_gazebo/worlds/robosub_2022.world +++ b/SubjuGator/simulation/subjugator_gazebo/worlds/robosub_2022.world @@ -835,6 +835,108 @@ -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 + 1 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