Merge pull request #115 from krishauser/A1_control

A1 control

Author: alo-20

GEMstack/knowledge/defaults/computation_graph.yaml

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

GEMstack/knowledge/defaults/current.yaml

@@ -13,12 +13,12 @@ control:
         brake_speed : 2.0
     pure_pursuit:
         lookahead: 2.0
-        lookahead_scale: 3.0
+        lookahead_scale: 2.0
         crosstrack_gain: 0.5
         desired_speed: trajectory
     longitudinal_control:
         pid_p: 1.0
-        pid_i: 0.1
+        pid_i: 0.05
         pid_d: 0.0
 
 #configure the simulator, if using

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:

GEMstack/onboard/planning/pure_pursuit.py

@@ -164,7 +164,7 @@ def compute(self, state : VehicleState, component : Component = None):
                 print("Feedforward accel: " + str(feedforward_accel) + " m/s^2")
         else:
             #decay speed when crosstrack error is high
-            desired_speed *= np.exp(-abs(ct_error)*1.0)
+            desired_speed *= np.exp(-abs(ct_error)*0.8)
         if desired_speed > self.speed_limit:
             desired_speed = self.speed_limit 
         output_accel = self.pid_speed.advance(e = desired_speed - speed, t = t, feedforward_term=feedforward_accel)

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' 

tuning logs/2025-02-13_21-10-39 (fine tuning)/PurePursuitTrajectoryTracker_debug.csv

@@ -0,0 +1,19 @@
+events:
+- description: Ctrl+C pressed, switching to recovery mode
+  time: 1739502705.4840224
+  vehicle_time: 1739502704.3390243
+- description: Mission execution ended
+  time: 1739502707.669565
+  vehicle_time: 1739502706.5190222
+exit_reason: normal exit
+git_branch: A1_control
+git_commit_id: b5cb190b3f6c6cf17344817eca71aa0a5d350394
+pipelines:
+- name: drive
+  time: 1739502639.5759368
+  vehicle_time: 1739502638.629087
+- name: recovery
+  time: 1739502705.4858768
+  vehicle_time: 1739502704.3390243
+start_time: 1739502639.3786135
+start_time_human_readable: '2025-02-13 21:10:39'