diff --git a/docker-compose.yml b/docker-compose.yml index 3f03148..01893cd 100644 --- a/docker-compose.yml +++ b/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 diff --git a/mrobosub_bringup/launch/bringup_sim_launch.xml b/mrobosub_bringup/launch/bringup_sim_launch.xml index 425ab17..a687d3c 100644 --- a/mrobosub_bringup/launch/bringup_sim_launch.xml +++ b/mrobosub_bringup/launch/bringup_sim_launch.xml @@ -1,8 +1,8 @@ - - - + + + diff --git a/mrobosub_bringup/mrobosub_bringup/quartermaster.py b/mrobosub_bringup/mrobosub_bringup/quartermaster.py index 59db701..20aaf69 100755 --- a/mrobosub_bringup/mrobosub_bringup/quartermaster.py +++ b/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 diff --git a/mrobosub_gnc/mrobosub_gnc/passthrough_dof_controller.py b/mrobosub_gnc/mrobosub_gnc/passthrough_dof_controller.py index be6e109..cd257b1 100755 --- a/mrobosub_gnc/mrobosub_gnc/passthrough_dof_controller.py +++ b/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() diff --git a/mrobosub_planning/mrobosub_planning/common_states.py b/mrobosub_planning/mrobosub_planning/common_states.py index 8d834db..bee39ae 100644 --- a/mrobosub_planning/mrobosub_planning/common_states.py +++ b/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 diff --git a/mrobosub_planning/mrobosub_planning/standard_run.py b/mrobosub_planning/mrobosub_planning/standard_run.py index ac94676..dda5afd 100644 --- a/mrobosub_planning/mrobosub_planning/standard_run.py +++ b/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 } diff --git a/mrobosub_sim/mrobosub_sim/hal.py b/mrobosub_sim/mrobosub_sim/hal.py index c3098a8..db83370 100755 --- a/mrobosub_sim/mrobosub_sim/hal.py +++ b/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()