HW1 Part1.2

Author: hhxjqm

GEMstack/knowledge/defaults/computation_graph.yaml

@@ -49,6 +49,6 @@ components:
   - motion_planning:
       inputs: all
       outputs: trajectory
-  - trajectory_tracking:
-      inputs: [vehicle, trajectory]
-      outputs: 
+  - signaline:
+        inputs: intent
+        outputs: []

GEMstack/onboard/planning/blink_component.py

@@ -1,6 +1,7 @@
 from ..component import Component
 from ..interface.gem import GEMInterface,GEMVehicleCommand,GEMVehicleReading
 import time
+from ...state import AllState, VehicleIntentEnum
 
 TURN_OFF = 0
 TURN_LEFT = 1
@@ -19,7 +20,7 @@ def __init__(self, vehicle_interface : GEMInterface):
     def rate(self):
         """Requested update frequency, in Hz"""
         return 0.5
-
+    
     def initialize(self):
         """Run first"""
         print("BlinkDistress Component Initialized.")
@@ -32,28 +33,31 @@ def cleanup(self):
         self.send_turn_command(TURN_OFF)
         # pass
 
-    def update(self):
+    def update(self, intent):
         """Run in a loop"""
         # we need to set up a GEMVehicleCommand which encapsulates all commands that will be
         # sent to the drive-by-wire system, simultaneously.  To avoid doing arbitrary things
         # to the vehicle, let's maintain the current values (e.g., accelerator, brake pedal,
         # steering angle) from its current readings.
-        current_time = time.time()
-        if current_time - self.last_update_time >= 2:  # Change signal every 2 seconds
-            if self.turn_state == TURN_OFF:
-                self.turn_state = TURN_LEFT
-            elif self.turn_state == TURN_LEFT:
-                self.turn_state = TURN_RIGHT
-            else:
-                self.turn_state = TURN_OFF
+        if intent.intent == VehicleIntentEnum.HALTING:
+            current_time = time.time()
+            if current_time - self.last_update_time >= 2:  # Change signal every 2 seconds
+                if self.turn_state == TURN_OFF:
+                    self.turn_state = TURN_LEFT
+                elif self.turn_state == TURN_LEFT:
+                    self.turn_state = TURN_RIGHT
+                else:
+                    self.turn_state = TURN_OFF
 
-            self.send_turn_command(self.turn_state)
-            self.last_update_time = current_time
-        
+                self.send_turn_command(self.turn_state)
+                self.last_update_time = current_time
+        else:
+            self.send_turn_command(TURN_OFF)
+            
          # Read vehicle sensor data
         vehicle_reading = self.vehicle_interface.get_reading()
         print(f"Vehicle Speed: {vehicle_reading.speed:.2f} m/s")
-        print(f"Acceleration: {vehicle_reading.accelerator_pedal_position - vehicle_reading.brake_pedal_position:.2f} m/s²")
+        print(f"Acceleration: {vehicle_reading.accelerator_pedal_position - vehicle_reading.brake_pedal_position:.2f} m/s²")
         # command = self.vehicle_interface.command_from_reading()
         # TODO: alter command to execute turn signals, then uncomment line below to send
         # the command to vehicle
@@ -80,5 +84,4 @@ def send_turn_command(self, turn_signal):
 
     def healthy(self):
         """Returns True if the element is in a stable state."""
-        return True
-       
+        return True

homework/blink.py

@@ -109,7 +109,7 @@ def run_ros_loop(node):
     termination_reason = "undetermined"
     try:
         while not rospy.is_shutdown() and not node.done() and node.healthy():
-            node.update()
+            node.update(node.state_inputs())
             rate.sleep()
         if node.done():
             termination_reason = "Node done"

launch/blink_launch.yaml

@@ -9,14 +9,16 @@ recovery:
         state_estimation : GNSSStateEstimator
         perception_normalization : StandardPerceptionNormalizer
     planning: 
-        trajectory_tracking : blink_component.BlinkDistress
+        # trajectory_tracking : blink_component.BlinkDistress
+        signaling : blink_component.BlinkDistress
 # Driving behavior for the GEM vehicle.  Runs perception and planner but doesn't execute anything (no controller).
 drive: 
     perception:
         state_estimation : GNSSStateEstimator
         perception_normalization : StandardPerceptionNormalizer
     planning:
-        trajectory_tracking : blink_component.BlinkDistress
+        # trajectory_tracking : blink_component.BlinkDistress
+        signaling : blink_component.BlinkDistress
 log:
     # Specify the top-level folder to save the log files.  Default is 'logs'
     folder : 'logs' 
@@ -61,4 +63,4 @@ variants:
                     state_estimation : OmniscientStateEstimator
             drive: 
                 perception:
-                    state_estimation : OmniscientStateEstimator
+                    state_estimation : OmniscientStateEstimator