Hw1.3

GEMstack/knowledge/defaults/computation_graph.yaml

@@ -52,3 +52,6 @@ components:
   - trajectory_tracking:
       inputs: [vehicle, trajectory]
       outputs: 
+  - signaling:
+      inputs: [intent] 
+      outputs: 

GEMstack/onboard/planning/blink_component.py

@@ -38,6 +38,9 @@ def update(self):
         # 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.
+
+        #if Allstate.intent.intent == "HALTING" :
+            #return
         current_time = time.time()
         if current_time - self.last_update_time >= 2:  # Change signal every 2 seconds
             if self.turn_state == TURN_OFF:

homework/blink.py

@@ -6,7 +6,6 @@
 TURN_NONE = 1
 TURN_LEFT = 2
 TURN_HAZARD = 3 
-TURN_AROUND = 4
 # For message format, see
 # https://github.com/astuff/astuff_sensor_msgs/blob/3.3.0/pacmod_msgs/msg/PacmodCmd.msg
 
@@ -48,19 +47,20 @@ def cleanup(self):
     def get_dir_distress(self):
         pass
 
-    def update(self):
+    def update(self, Allstate):
         """Run in a loop"""
         # TODO: Implement your control loop here
         # You will need to publish a PacmodCmd() to /pacmod/as_rx/turn_cmd.  Read the documentation to see
         # what the data in the message indicates.
         #self.turn_cmd = PacmodCmd()
         # TODO change to actual direction in Part 2
-        if self.turn_cmd.ui16_cmd == TURN_NONE:
-            self.turn_cmd.ui16_cmd = TURN_LEFT
-        elif self.turn_cmd.ui16_cmd == TURN_LEFT:
-            self.turn_cmd.ui16_cmd = TURN_RIGHT
-        else:
-            self.turn_cmd.ui16_cmd = TURN_NONE
+        if Allstate.intent.intent == "HALTING":
+            if self.turn_cmd.ui16_cmd == TURN_NONE:
+                self.turn_cmd.ui16_cmd = TURN_LEFT
+            elif self.turn_cmd.ui16_cmd == TURN_LEFT:
+                self.turn_cmd.ui16_cmd = TURN_RIGHT
+            else:
+                self.turn_cmd.ui16_cmd = TURN_NONE
         self.turn_blink_pub.publish(self.turn_cmd)
 
         pass
@@ -97,7 +97,7 @@ def accel_callback(self, msg):
         rospy.loginfo(f"Output: {msg.output} (Final Output to Vehicle)")
         rospy.loginfo("---------------------------\n")
 
-def run_ros_loop(node):
+def run_ros_loop(node, Allstate):
     """Executes the event loop of a node using ROS.  `node` should support
     rate(), initialize(), cleanup(), update(), and done().  You should not modify
     this code.
@@ -111,7 +111,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, Allstate)
             rate.sleep()
         if node.done():
             termination_reason = "Node done"

homework/blink_launch.yaml

@@ -9,14 +9,14 @@ recovery:
         state_estimation : GNSSStateEstimator
         perception_normalization : StandardPerceptionNormalizer
     planning: 
-        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
+        signaling: blink_component.BlinkDistress
 log:
     # Specify the top-level folder to save the log files.  Default is 'logs'
     folder : 'logs'