diff --git a/scripts/visualizer.py b/scripts/visualizer.py index 298f023f..5aae7a5d 100755 --- a/scripts/visualizer.py +++ b/scripts/visualizer.py @@ -98,17 +98,11 @@ def __init__(self, state_machine_instance, *args, **kwargs): self.viz = Node("Visualizer") self.viz.create_subscription( - StateMachineStructure, - STRUCTURE_TOPIC, - self.state_machine.container_structure_callback, - 1 + StateMachineStructure, STRUCTURE_TOPIC, self.state_machine.container_structure_callback, 1 ) self.viz.create_subscription( - StateMachineStateUpdate, - STATUS_TOPIC, - self.state_machine.container_status_callback, - 1 + StateMachineStateUpdate, STATUS_TOPIC, self.state_machine.container_status_callback, 1 ) def paintEvent(self, event): @@ -144,20 +138,14 @@ def main(): signal.signal(signal.SIGINT, signal.SIG_DFL) state_machine = StateMachine() -<<<<<<< HEAD print("Node Created...") print("Subscriptions Created...") -======= - print("Node Created...") - print("Subscriptions Created...") - ->>>>>>> nav_fixes app = QApplication([]) # type: ignore g = GUI(state_machine) g.show() diff --git a/starter_project/autonomy/launch/starter_project.launch.py b/starter_project/autonomy/launch/starter_project.launch.py index 4b383395..f8b24620 100644 --- a/starter_project/autonomy/launch/starter_project.launch.py +++ b/starter_project/autonomy/launch/starter_project.launch.py @@ -31,22 +31,21 @@ def generate_launch_description(): condition=LaunchConfigurationEquals("rviz", "true"), ) - return LaunchDescription([ - launch_include_sim, - rviz_node, - - # ========== - # Perception - # ========== - perception_node, - - # =========== - # Navigation - # =========== - navigation_node, - - # ============ - # Localization - # ============ - localization_node - ]) + return LaunchDescription( + [ + launch_include_sim, + rviz_node, + # ========== + # Perception + # ========== + perception_node, + # =========== + # Navigation + # =========== + navigation_node, + # ============ + # Localization + # ============ + localization_node, + ] + ) diff --git a/starter_project/autonomy/src/localization.py b/starter_project/autonomy/src/localization.py index d46d695d..35e9687c 100755 --- a/starter_project/autonomy/src/localization.py +++ b/starter_project/autonomy/src/localization.py @@ -61,6 +61,7 @@ def spherical_to_cartesian(spherical_coord: np.ndarray, reference_coord: np.ndar """ # TODO + def main(): # initialize the node rclpy.init() diff --git a/starter_project/autonomy/src/navigation/context.py b/starter_project/autonomy/src/navigation/context.py index d4c48242..24f39368 100644 --- a/starter_project/autonomy/src/navigation/context.py +++ b/starter_project/autonomy/src/navigation/context.py @@ -14,7 +14,8 @@ import sys import os -sys.path.append(os.getcwd() + '/starter_project/autonomy/src') + +sys.path.append(os.getcwd() + "/starter_project/autonomy/src") from util.SE3 import SE3 from visualization_msgs.msg import Marker @@ -35,6 +36,7 @@ def send_drive_stop(self): # TODO: tell the rover to stop pass + @dataclass class Environment: """ diff --git a/starter_project/autonomy/src/navigation/drive_state.py b/starter_project/autonomy/src/navigation/drive_state.py index 8ea8434e..ab6291d8 100644 --- a/starter_project/autonomy/src/navigation/drive_state.py +++ b/starter_project/autonomy/src/navigation/drive_state.py @@ -5,6 +5,7 @@ from state_machine.state import State from tag_seek import TagSeekState + class DriveState(State): def on_enter(self, context) -> None: pass @@ -21,7 +22,7 @@ def on_loop(self, context) -> State: # (HINT: use get_drive_command(), with completion_thresh set to 0.7 and turn_in_place_thresh set to 0.2) # TODO: if we are finished getting to the target, go to TagSeekState - + # TODO: send the drive command to the rover # TODO: tell state machine to stay in the DriveState by returning self diff --git a/starter_project/autonomy/src/navigation/navigation_starter_project.py b/starter_project/autonomy/src/navigation/navigation_starter_project.py index 1882e0b2..73941e33 100755 --- a/starter_project/autonomy/src/navigation/navigation_starter_project.py +++ b/starter_project/autonomy/src/navigation/navigation_starter_project.py @@ -50,6 +50,7 @@ def __init__(self, ctx: Context): self.create_timer(1 / 60, self.state_machine.update) + def main(): try: # TODO: init a node called "navigation" diff --git a/starter_project/autonomy/src/navigation/state.py b/starter_project/autonomy/src/navigation/state.py index 0c29028e..9dc034e1 100644 --- a/starter_project/autonomy/src/navigation/state.py +++ b/starter_project/autonomy/src/navigation/state.py @@ -6,6 +6,7 @@ from context import Context + class DoneState(State): def on_enter(self, context) -> None: pass @@ -18,7 +19,8 @@ def on_loop(self, context) -> State: cmd_vel = Twist() context.rover.send_drive_command(cmd_vel) return self - + + class FailState(State): def on_enter(self, context) -> None: pass @@ -30,4 +32,4 @@ def on_loop(self, context) -> State: # Stop rover cmd_vel = Twist() context.rover.send_drive_command(cmd_vel) - return self \ No newline at end of file + return self