Add modular states

docker-compose.yml

@@ -7,6 +7,7 @@ services:
     privileged: true
     ports:
       - "10000:10000"
+      - "1817-1818:1817-1818"
     volumes:
       - .:/home/ubuntu/ros2_ws/src/
       - /tmp/.X11-unix:/tmp/.X11-unix

mrobosub_bringup/launch/bringup_sim_launch.xml

@@ -1,8 +1,8 @@
 <launch>
     <include file="$(find-pkg-share mrobosub_gnc)/launch/gnc_launch.xml"/>
-    <include file="$(find-pkg-share mrobosub_monitoring)/launch/monitoring_launch.xml"/>
-    <include file="$(find-pkg-share mrobosub_localization)/launch/localization_launch.xml"/>
-    <include file="$(find-pkg-share mrobosub_perception)/launch/perception_sim_launch.xml"/>
+    <!-- <include file="$(find-pkg-share mrobosub_monitoring)/launch/monitoring_launch.xml"/> -->
+    <!-- <include file="$(find-pkg-share mrobosub_localization)/launch/localization_launch.xml"/> -->
+    <!-- <include file="$(find-pkg-share mrobosub_perception)/launch/perception_sim_launch.xml"/> -->
     <include file="$(find-pkg-share mrobosub_fcu)/launch/fcu_launch.xml"/>
     <include file="$(find-pkg-share mrobosub_sim)/launch/sim_hal_launch.xml"/>
 </launch>

mrobosub_bringup/mrobosub_bringup/quartermaster.py

@@ -11,7 +11,7 @@
 from std_srvs.srv import SetBool, Trigger
 from rclpy.executors import SingleThreadedExecutor
 
-from mrobosub_lib    import Node
+from mrobosub_lib.mrobosub_lib import Node
 from .               import constants as const
 from .launch_manager import LaunchManager
 
@@ -93,7 +93,7 @@ def pub_on_led(self, val: bool):
         b = Bool()
         b.data = val
         self.led_on_pub.publish(b)
-        self.led_states.on = val
+        self.led_states.led_on = val
 
 
     def pub_led(self, led: str, val: bool):
@@ -181,7 +181,7 @@ def call_soft_stop_srv(self):
             return
 
         request = Trigger.Request()
-        self.thruster_mixing_future = self.soft_stop_srv.call_async(request)
+        self.soft_stop_future = self.soft_stop_srv.call_async(request)
 
         def callback(future):
             try: 
@@ -196,7 +196,7 @@ def callback(future):
         self.soft_stop_future.add_done_callback(callback)
 
 
-   def timer_callback(self):
+    def timer_callback(self):
         # In order to visually ensure quartermaster has started, turn on the LED
         self.pub_on_led(True)
 
@@ -211,8 +211,8 @@ def timer_callback(self):
         # We want the LED showing that we have triggered a specific hall effect sensor to turn off
         # after 5 seconds.
         if self.timeout_disabled_led is not None:
-            self.pub_led(timeout_disable, False)
-            self.timeout_disable = None
+            self.pub_led(self.timeout_disabled_led, False)
+            self.timeout_disabled_led = None
 
         # We don't want to advance the state machine if both are false. Only on the rising edge.
         if (self.hall_effect_triggered.strange is False) and (self.hall_effect_triggered.charm is False):
@@ -224,10 +224,10 @@ def timer_callback(self):
 
         if self.hall_effect_triggered.strange:
             self.pub_strange_led(True)
-            self.timeout_disable = "strange"
+            self.timeout_disabled_led = "strange"
         elif self.hall_effect_triggered.charm:
             self.pub_charm_led(True)
-            self.timeout_disable = "charm"
+            self.timeout_disabled_led = "charm"
 
         # Zero hall effects.
         self.hall_effect_triggered.strange = False

mrobosub_gnc/mrobosub_gnc/passthrough_dof_controller.py

@@ -30,10 +30,10 @@ def target_twist_callback(self, target_twist: Float64):
         self.pub_output_dof(target_twist.data)
 
     def pub_output_dof(self, output: float):
-        self.output_pub.publish(output)
+        self.output_pub.publish(Float64(data=output))
 
     def destroy_node(self):
-        self.output_pub.publish(0)
+        self.output_pub.publish(Float64(data=0.0))
         super().destroy_node()        
 
 

mrobosub_planning/mrobosub_planning/common_states.py

@@ -1,4 +1,5 @@
 from typing import Optional, Union
+
 from mrobosub_planning.umrsm import State, Outcome
 from mrobosub_planning.abstract_states import TimedState
 import rclpy
@@ -39,6 +40,47 @@ def handle_once_timedout(self) -> TimedOut:
         return self.TimedOut()
 
 
+class AlignToYaw(TimedState):
+    target_yaw = 0.0
+    yaw_threshold = 2.0
+
+    class Aligned(Outcome):
+        pass
+
+    class TimedOut(Outcome):
+        pass
+
+    def handle_if_not_timedout(self) -> Union[Aligned, None]:
+        self.io_node.set_target_pose_yaw(self.target_yaw)
+
+        if self.io_node.is_yaw_within_threshold(self.yaw_threshold):
+            return self.Aligned()
+        return None
+
+    def handle_once_timedout(self) -> TimedOut:
+        return self.TimedOut()
+
+class Surge(TimedState):
+    class Surged(Outcome):
+        pass
+
+    class TimedOut(Outcome):
+        pass
+    distance = 10
+    velocity = 2
+    timeout: float = distance/velocity
+
+
+    def handle_if_not_timedout(self) -> Union[Surged, None]:
+        self.timeout = self.distance/self.velocity
+        self.io_node.set_target_twist_surge(self.velocity)
+
+        return None
+
+    def handle_once_timedout(self) -> Surged:
+        return self.Surged()
+
+
 class Stop(State):
     class Surfaced(Outcome):
         pass

mrobosub_planning/mrobosub_planning/standard_run.py

@@ -1,12 +1,57 @@
-from mrobosub_planning.common_states import Start, Submerge, Surface, Stop
+#!/usr/bin/env python
+from mrobosub_planning.common_states import Outcome, Start, Submerge, Surface, Stop, Surge, AlignToYaw
 from mrobosub_planning.umrsm import TransitionMap
 
+def make_move_forward():
+    class SurgeExtender(Surge):
+        class Surged(Outcome):
+            pass
+        class TimedOut(Outcome):
+            pass
+
+    class AlignExtender(AlignToYaw):
+        class Aligned(Outcome):
+            pass
+        class TimedOut(Outcome):
+            pass
+
+    return SurgeExtender, AlignExtender
+
+Surge1, Align1 = make_move_forward()
+Surge2, Align2 = make_move_forward()
+Surge3, Align3 = make_move_forward()
+Surge4, Align4 = make_move_forward()
+Surge5, Align5 = make_move_forward()
+Surge6, _ = make_move_forward()
+
 
 transitions: TransitionMap = {
     Start.Complete: Submerge.with_params(target_heave=1.),
 
-    Submerge.Submerged: Surface,
+    Submerge.Submerged: Surge1.with_params(distance=20.),  # object distance
     Submerge.TimedOut: Surface,
 
+    Surge1.Surged: Align1.with_params(target_yaw=45.),
+    Align1.Aligned: Surge2.with_params(distance=10.),
+    Align1.TimedOut: Surface,
+
+    Surge2.Surged: Align2.with_params(target_yaw=315.),
+    Align2.Aligned: Surge3.with_params(distance=10.),
+    Align2.TimedOut: Surface,
+
+    Surge3.Surged: Align3.with_params(target_yaw=225.),
+    Align3.Aligned: Surge4.with_params(distance=10.),
+    Align3.TimedOut: Surface,
+
+    Surge4.Surged: Align4.with_params(target_yaw=135.),
+    Align4.Aligned: Surge5.with_params(distance=10.),
+    Align4.TimedOut: Surface,
+
+    Surge5.Surged: Align5.with_params(target_yaw=180.),
+    Align5.Aligned: Surge6.with_params(distance=20.),
+    Align5.TimedOut: Surface,
+
+    Surge6.Surged: Surface,
+
     Surface.Surfaced: Stop
 }

mrobosub_sim/mrobosub_sim/hal.py

@@ -56,7 +56,7 @@ def unpack(data: bytes) -> "SensorData":
 
         dvl = Dvl()
         dvl.header = header
-        dvl.velocity = Vector3(x=vals[1], y=vals[2], z=vals[3])
+        dvl.velocity = np.array((vals[1], vals[2], vals[3]))
 
         imu_ins = ImuINS()
         imu_ins.header.stamp = node.get_clock().now().to_msg()