Hydrothermal vent

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

SubjuGator/command/subjugator_missions/subjugator_missions/hydrothermal_vent.py

@@ -0,0 +1,56 @@
+#!/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
+
+
+# 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
+
+        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(f"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)
+        # 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):
+            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))
+
+        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().set_roll_pitch_yaw(0, 0, -yaw_angle))
+
+        await self.go(self.move().up(self.buoy_pos[1]))
+
+

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
+

SubjuGator/simulation/subjugator_gazebo/worlds/robosub_2022.world

@@ -937,5 +937,107 @@
       </link>
       <pose>-0.957126 1.98002 0 0 -0 0</pose>
     </model>
+    <model name='Red_buoy'>
+    <link name='link_0'>
+      <inertial>
+        <mass>0.0131332</mass>
+        <inertia>
+          <ixx>7.311e-05</ixx>
+          <ixy>0</ixy>
+          <ixz>0</ixz>
+          <iyy>7.311e-05</iyy>
+          <iyz>0</iyz>
+          <izz>7.311e-05</izz>
+        </inertia>
+        <pose>0 0 0 0 -0 0</pose>
+      </inertial>
+      <pose>0 0 0 0 -0 0</pose>
+      <gravity>1</gravity>
+      <self_collide>0</self_collide>
+      <kinematic>0</kinematic>
+      <enable_wind>0</enable_wind>
+      <visual name='visual'>
+        <pose>3 4 -2 0 -0 0</pose>
+        <geometry>
+          <sphere>
+            <radius>0.15</radius>
+          </sphere>
+        </geometry>
+        <material>
+          <lighting>1</lighting>
+          <script>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+            <name>Gazebo/Grey</name>
+          </script>
+          <shader type='pixel'>
+            <normal_map>__default__</normal_map>
+          </shader>
+          <ambient>1 0 0 1</ambient>
+          <diffuse>1 0 0 1</diffuse>
+          <specular>1 0 0 1</specular>
+          <emissive>1 0 0 1</emissive>
+        </material>
+        <transparency>0</transparency>
+        <cast_shadows>1</cast_shadows>
+      </visual>
+      <collision name='collision'>
+        <laser_retro>0</laser_retro>
+        <max_contacts>10</max_contacts>
+        <pose>0 0 0 0 -0 0</pose>
+        <geometry>
+          <sphere>
+            <radius>0.117968</radius>
+          </sphere>
+        </geometry>
+        <surface>
+          <friction>
+            <ode>
+              <mu>1</mu>
+              <mu2>1</mu2>
+              <fdir1>0 0 0</fdir1>
+              <slip1>0</slip1>
+              <slip2>0</slip2>
+            </ode>
+            <torsional>
+              <coefficient>1</coefficient>
+              <patch_radius>0</patch_radius>
+              <surface_radius>0</surface_radius>
+              <use_patch_radius>1</use_patch_radius>
+              <ode>
+                <slip>0</slip>
+              </ode>
+            </torsional>
+          </friction>
+          <bounce>
+            <restitution_coefficient>0</restitution_coefficient>
+            <threshold>1e+06</threshold>
+          </bounce>
+          <contact>
+            <collide_without_contact>0</collide_without_contact>
+            <collide_without_contact_bitmask>1</collide_without_contact_bitmask>
+            <collide_bitmask>1</collide_bitmask>
+            <ode>
+              <soft_cfm>0</soft_cfm>
+              <soft_erp>0.2</soft_erp>
+              <kp>1e+13</kp>
+              <kd>1</kd>
+              <max_vel>0.01</max_vel>
+              <min_depth>0</min_depth>
+            </ode>
+            <bullet>
+              <split_impulse>1</split_impulse>
+              <split_impulse_penetration_threshold>-0.01</split_impulse_penetration_threshold>
+              <soft_cfm>0</soft_cfm>
+              <soft_erp>0.2</soft_erp>
+              <kp>1e+13</kp>
+              <kd>1</kd>
+            </bullet>
+          </contact>
+        </surface>
+      </collision>
+    </link>
+    <static>1</static>
+    <allow_auto_disable>1</allow_auto_disable>
+  </model>
   </world>
 </sdf>