Hydrothermal vent

SubjuGator/command/subjugator_launch/launch/sub8.launch

@@ -30,6 +30,10 @@
       <rosparam file="$(find subjugator_launch)/config/course_geometry.yaml" />
   </group>
 
+<!-- Parameters for competition strategy -->
+  <group ns="strategy">
+      <rosparam file="$(find subjugator_missions)/config/strategy.yaml" />
+  </group>
   <include if="$(eval environment != 'dynsim')" file="$(find subjugator_launch)/launch/subsystems/cameras.launch">
     <arg name="environment" value="$(arg environment)" />
     <arg name="simulate_cams" value="$(arg simulate_cams)" />

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

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

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)

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(

SubjuGator/simulation/subjugator_gazebo/worlds/robosub_2022.world

@@ -835,6 +835,108 @@
       </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>
     <model name='buoys_2'>
       <model name='Bootlegger_Buoy'>
         <static>1</static>