merge auton starter project

CMakeLists.txt

@@ -277,6 +277,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

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"

scripts/visualizer.py

@@ -98,11 +98,17 @@ 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):
@@ -138,10 +144,20 @@ 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()

starter_project/autonomy/launch/starter_project.launch.py

@@ -31,21 +31,22 @@ 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
+    ])

starter_project/autonomy/src/localization.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
@@ -60,7 +61,6 @@ def spherical_to_cartesian(spherical_coord: np.ndarray, reference_coord: np.ndar
         """
         # TODO
 
-
 def main():
     # initialize the node
     rclpy.init()
@@ -71,8 +71,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/navigation/context.py

@@ -5,15 +5,16 @@
 
 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
 
@@ -34,7 +35,6 @@ def send_drive_stop(self):
         # TODO: tell the rover to stop
         pass
 
-
 @dataclass
 class Environment:
     """
@@ -55,25 +55,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/drive_state.py

@@ -2,29 +2,26 @@
 
 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(State):
+    def on_enter(self, context) -> None:
+        pass
 
-class DriveState(BaseState):
-    def __init__(self, context: Context):
-        super().__init__(
-            context,
-            # TODO:
-            add_outcomes=["TODO: add outcomes here"],
-        )
+    def on_exit(self, context) -> None:
+        pass
 
-    def evaluate(self, ud):
+    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

starter_project/autonomy/src/navigation/navigation_starter_project.py

@@ -1,35 +1,34 @@
 #!/usr/bin/env python3
 
-import signal
 import sys
-import threading
 
 # ros and state machine imports
 import rclpy
 from rclpy.executors import ExternalShutdownException
-
+from rclpy.node import Node
 from state_machine.state_machine import StateMachine
 from state_machine.state_publisher_server import StatePublisher
 
 # navigation specific imports
 from context import Context
 from drive_state import DriveState
-from state import DoneState, Off
+from state import DoneState, FailState
 from tag_seek import TagSeekState
 
 
-class Navigation(threading.Thread):
+class Navigation(Node):
     state_machine: StateMachine
-    context: Context
+    ctx: Context
     state_machine_server: StatePublisher
 
-    def __init__(self, context: Context):
-        super().__init__()
-        self.name = "NavigationThread"
-        self.state_machine = StateMachine(outcomes=["terminated"])
-        self.context = context
-        self.state_machine_server = StatePublisher(self, self.state_machine, "/SM_ROOT")
-        self.state_machine = StateMachine[Context]
+    def __init__(self, ctx: Context):
+        super().__init__("navigation")
+
+        self.get_logger().info("Starting...")
+
+        self.ctx = ctx
+
+        self.state_machine = StateMachine[Context](DriveState(), "NavigationStateMachine", self.ctx, self.get_logger())
 
         # TODO: add DriveState and its transitions here
 
@@ -38,19 +37,18 @@ def __init__(self, context: Context):
             DoneState(),
             [DoneState()],
         )
-        # TODO: add TagSeekState and its transitions here
 
-    def run(self):
-        self.state_machine.execute()
+        # FailState and its transitions
+        self.state_machine.add_transitions(
+            FailState(),
+            [FailState()],
+        )
+
+        # TODO: add TagSeekState and its transitions here
 
-    def stop(self):
-        self.sis.stop()
-        # Requests current state to go into 'terminated' to cleanly exit state machine
-        self.state_machine.request_preempt()
-        # Wait for smach thread to terminate
-        self.join()
-        self.context.rover.send_drive_stop()
+        self.state_machine_server = StatePublisher(self, self.state_machine, "nav_structure", 1, "nav_state", 10)
 
+        self.create_timer(1 / 60, self.state_machine.update)
 
 def main():
     try:
@@ -59,6 +57,7 @@ def main():
         # context and navigation objects
         context = Context()
         navigation = Navigation(context)
+        context.setup(navigation)
 
         rclpy.spin(navigation)
         rclpy.shutdown()