Merge branch 'main' into JRA/ZED-Wrapper

Author: jbrhm

.github/workflows/ci.yml

@@ -7,6 +7,7 @@ on:
   push:
     branches:
       - main
+      - integration
 jobs:
   ci:
     name: CI
@@ -19,11 +20,14 @@ jobs:
     steps:
       - uses: actions/checkout@v4
         with:
+          # Re-enable when we put the simulator in the CI pipeline.
           # lfs: "true"
           # This makes sure that $GITHUB_WORKSPACE is the catkin workspace path
           path: "src/mrover"
       - name: Style Check
         run: . /home/mrover/ros2_ws/src/mrover/venv/bin/activate && cd $GITHUB_WORKSPACE/src/mrover/ && ./style.sh
+      - name: Compiler Cache
+        uses: hendrikmuhs/[email protected]
       - name: Build
         if: github.event.pull_request.draft == false
         run: . /opt/ros/humble/setup.sh && . /home/mrover/ros2_ws/src/mrover/venv/bin/activate && cd $GITHUB_WORKSPACE/src/mrover/ && ./build.sh

CMakeLists.txt

@@ -318,6 +318,7 @@ install(PROGRAMS
         localization/basestation_gps_driver.py
         superstructure/superstructure.py
         scripts/debug_course_publisher.py
+		scripts/visualizer.py
 
         # starter project sources
         starter_project/autonomy/src/localization.py

ansible/roles/ci/tasks/main.yml

@@ -64,7 +64,7 @@
       # - neovim
       # - sudo
       - cmake
-      # - ccache # Caches intermediate build files, speeds up compilation over time
+      - ccache # Caches intermediate build files, speeds up compilation over time
       - ninja-build # Faster than make
       # - tmux
       # - htop

config/localization.yaml

@@ -15,6 +15,6 @@ gps_linearization:
     rover_frame: "base_link"
 
     # Exception, these are in degrees
-    ref_lat: 38.4225202
-    ref_lon: -110.7844653
+    ref_lat: 42.293195
+    ref_lon: -83.7096706
     ref_alt: 0.0

config/simulator.yaml

@@ -23,15 +23,15 @@ simulator:
       tag_0:
         type: urdf
         uri: package://mrover/urdf/world/tag_0.urdf.xacro
-        position: [ -2.0, -2.0, 0.7 ]
+        position: [ 11.0, 3.0, 0.7 ]
       tag_1:
         type: urdf
         uri: package://mrover/urdf/world/tag_1.urdf.xacro
         position: [ 15.0, -14.0, 2.4 ]
       hammer:
         type: urdf
         uri: package://mrover/urdf/world/hammer.urdf.xacro
-        position: [ 5.0, 2.0, 0.7 ]
+        position: [ -2.0, -2.0, 0.7 ]
       bottle:
         type: urdf
         uri: package://mrover/urdf/world/bottle.urdf.xacro
@@ -58,4 +58,4 @@ simulator:
     ref_lon: -110.7844653
     ref_alt: 0.0
     world_frame: "map"
-    rover_frame: "base_link"
+    rover_frame: "sim_base_link"

perception/tag_detector/README.md

@@ -0,0 +1,161 @@
+#!/usr/bin/env python3
+
+from __future__ import annotations
+import signal
+import graphviz  # type: ignore
+import time
+from PyQt5.QtWidgets import *  # type: ignore
+from PyQt5.QtCore import *  # type: ignore
+from PyQt5.QtGui import QPainter  # type: ignore
+from PyQt5.QtSvg import QSvgRenderer  # type:ignore
+
+import rclpy
+import rclpy.time
+import rclpy.logging
+from rclpy.node import Node
+from rclpy.executors import ExternalShutdownException
+import sys
+from mrover.msg import StateMachineStructure, StateMachineStateUpdate
+from threading import Lock
+from dataclasses import dataclass
+from typing import Optional, List, Dict
+import threading
+
+
+STRUCTURE_TOPIC = "nav_structure"
+STATUS_TOPIC = "nav_state"
+
+
+@dataclass
+class State:
+    name: str
+    children: List[State]
+
+
+class StateMachine:
+    def __init__(self):
+        self.states: Dict[str, State] = {}
+        self.structure: Optional[StateMachineStructure] = None
+        self.mutex: Lock = Lock()
+        self.cur_active: str = ""
+        self.previous_state: str = ""
+        self.needs_redraw: bool = True
+
+    def set_active_state(self, active_state):
+        """
+        sets the state specified to be active (thread safe)
+        """
+        with self.mutex:
+            if active_state != self.cur_active and active_state in self.states:
+                self.previous_state = self.cur_active
+                self.cur_active = active_state
+                self.needs_redraw = True
+                now = rclpy.time.Time()
+                rclpy.logging.get_logger("Visualizer").info(
+                    f"Current time: {now} Previous state: {self.previous_state} Current State: { self.cur_active}"
+                )
+
+    def _rebuild(self, structure: StateMachineStructure):
+        """
+        rebuilds the state dictionary with a new structure message
+        """
+        self.states = {child.origin: State(child.origin, []) for child in structure.transitions}
+        for transition in structure.transitions:
+            origin = transition.origin
+            for to in transition.destinations:
+                self.states[origin].children.append(self.states[to])
+        self.needs_redraw = True
+
+    def check_rebuild(self, structure: StateMachineStructure):
+        """
+        checks if the structure passed as input matches the structure already represented (thread safe)
+        """
+        with self.mutex:
+            if structure == self.structure:
+                return False
+            else:
+                self._rebuild(structure)
+                self.structure = structure
+
+    def container_status_callback(self, status: StateMachineStateUpdate):
+        self.set_active_state(status.state)
+
+    def container_structure_callback(self, structure: StateMachineStructure):
+        self.check_rebuild(structure)
+
+
+class GUI(QWidget):  # type: ignore
+    def __init__(self, state_machine_instance, *args, **kwargs):
+        super().__init__(*args, **kwargs)
+        self.label: QLabel = QLabel()  # type: ignore
+        self.timer: QTimer = QTimer()  # type: ignore
+        self.renderer: QSvgRenderer = QSvgRenderer()
+        self.timer.timeout.connect(self.update)
+        self.timer.start(1)
+        self.graph: Optional[graphviz.Digraph] = None
+        self.img = None
+        self.state_machine: StateMachine = state_machine_instance
+        self.viz = Node("Visualizer")
+
+        self.viz.create_subscription(
+            StateMachineStructure, STRUCTURE_TOPIC, self.state_machine.container_structure_callback, 1
+        )
+
+        self.viz.create_subscription(
+            StateMachineStateUpdate, STATUS_TOPIC, self.state_machine.container_status_callback, 1
+        )
+
+    def paintEvent(self, event):
+        painter = QPainter(self)
+        if self.img is not None:
+            self.renderer.load(self.img)
+        self.resize(self.renderer.defaultSize())
+        self.renderer.render(painter)
+
+    def update(self):
+        rclpy.spin_once(self.viz)
+        with self.state_machine.mutex:
+            if self.graph is None or self.state_machine.needs_redraw:
+                self.graph = graphviz.Digraph(comment="State Machine Diagram", format="svg")
+                for state_name in self.state_machine.states.keys():
+                    color = "red" if self.state_machine.cur_active == state_name else "black"
+                    self.graph.node(state_name, color=color)
+
+                for state in self.state_machine.states.values():
+                    for child in state.children:
+                        self.graph.edge(state.name, child.name)
+
+                self.state_machine.needs_redraw = False
+
+        self.img = self.graph.pipe()
+        self.repaint()
+
+
+def main():
+    try:
+        rclpy.init()
+
+        signal.signal(signal.SIGINT, signal.SIG_DFL)
+        state_machine = StateMachine()
+
+        print("Node Created...")
+
+        print("Subscriptions Created...")
+
+        print("Node Created...")
+
+        print("Subscriptions Created...")
+
+        app = QApplication([])  # type: ignore
+        g = GUI(state_machine)
+        g.show()
+        app.exec_()
+
+    except KeyboardInterrupt:
+        pass
+    except ExternalShutdownException:
+        sys.exit(1)
+
+
+if __name__ == "__main__":
+    main()

scripts/visualizer.py

@@ -42,10 +42,10 @@ def generate_launch_description():
             # ===========
             # Navigation
             # ===========
-            # navigation_node,
+            navigation_node,
             # ============
             # Localization
             # ============
-            # localization_node
+            localization_node,
         ]
     )

starter_project/autonomy/launch/starter_project.launch.py

@@ -6,6 +6,7 @@
 # library for interacting with ROS and TF tree
 import rclpy
 from rclpy.node import Node
+import rclpy.time
 import tf2_ros
 
 # ROS message types we need to use
@@ -71,8 +72,7 @@ def main():
     # let the callback functions run asynchronously in the background
     rclpy.spin(localization)
 
-    # TODO (ali): shutdown maybe?
-    # rclpy.shutdown()
+    rclpy.shutdown()
 
 
 if __name__ == "__main__":

starter_project/autonomy/src/localization.py

@@ -5,15 +5,17 @@
 
 import numpy as np
 import rclpy
+from rclpy.publisher import Publisher
+from rclpy.subscription import Subscription
 from rclpy.node import Node
 import tf2_ros
 from geometry_msgs.msg import Twist
 from mrover.msg import StarterProjectTag
 
-# import sys
-# TODO (ali): python relative imports are the bane of my existence
-# sys.path.append('..')
-# print(sys.path)
+import sys
+import os
+
+sys.path.append(os.getcwd() + "/starter_project/autonomy/src")
 from util.SE3 import SE3
 from visualization_msgs.msg import Marker
 
@@ -55,25 +57,30 @@ def get_fid_data(self) -> Optional[StarterProjectTag]:
         if it exists, otherwise returns None
         """
         # TODO: return either None or your position message
-        pass
 
 
-class Context(Node):
+class Context:
+    node: Node
     tf_buffer: tf2_ros.Buffer
     tf_listener: tf2_ros.TransformListener
-    vel_cmd_publisher: rclpy.Publisher
-    vis_publisher: rclpy.Publisher
-    fid_listener: rclpy.Subscriber
+    vel_cmd_publisher: Publisher
+    vis_publisher: Publisher
+    fid_listener: Subscription
 
     # Use these as the primary interfaces in states
     rover: Rover
     env: Environment
+    disable_requested: bool
 
-    def __init__(self):
+    def setup(self, node: Node):
+        self.node = node
         self.tf_buffer = tf2_ros.Buffer()
-        self.tf_listener = tf2_ros.TransformListener(self.tf_buffer)
-        self.vel_cmd_publisher = self.create_publisher(Twist, "cmd_vel", queue_size=1)
-        self.vis_publisher = self.create_publisher("nav_vis", Marker)
+
+        self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self.node)
+        self.vel_cmd_publisher = node.create_publisher(Twist, "cmd_vel", 1)
+        self.vis_publisher = node.create_publisher(Marker, "nav_vis", 1)
         self.rover = Rover(self)
         self.env = Environment(self, None)
-        self.fid_listener = self.create_subscription(StarterProjectTag, "/tag", self.env.receive_fid_data)
+        self.disable_requested = False
+
+        self.fid_listener = node.create_subscription(StarterProjectTag, "/tag", self.env.receive_fid_data, 1)

starter_project/autonomy/src/navigation/context.py

@@ -2,29 +2,27 @@
 
 from context import Context
 from drive import get_drive_command
-from state import BaseState
+from state_machine.state import State
+from tag_seek import TagSeekState
 
 
-class DriveState(BaseState):
-    def __init__(self, context: Context):
-        super().__init__(
-            context,
-            # TODO:
-            add_outcomes=["TODO: add outcomes here"],
-        )
+class DriveState(State):
+    def on_enter(self, context) -> None:
+        pass
+
+    def on_exit(self, context) -> None:
+        pass
 
-    def evaluate(self, ud):
-        target = np.array([5.5, 2.0, 0.0])
+    def on_loop(self, context) -> State:
+        target = np.array([8.0, 2.0, 0.0])
 
-        # TODO: get the rover's pose, if it doesn't exist stay in DriveState (with outcome "driving_to_point")
+        # TODO: get the rover's pose, if it doesn't exist stay in DriveState (return self)
 
         # TODO: get the drive command and completion status based on target and pose
         # (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 (with outcome "reached_point")
+        # TODO: if we are finished getting to the target, go to TagSeekState
 
         # TODO: send the drive command to the rover
 
-        # TODO: tell smach to stay in the DriveState by returning with outcome "driving_to_point"
-
-        pass
+        # TODO: tell state machine to stay in the DriveState by returning self