add pickup function to autonomous driving scripts

code/decision.py

@@ -63,7 +63,10 @@ def decision_step(Rover):
     # Just to make the rover do something 
     # even if no modifications have been made to the code
     else:
-        Rover.throttle = Rover.throttle_set
-
+        Rover.throttle = 0#Rover.throttle_set
+        Rover.steer = 0
+        Rover.brake = 0
+        if Rover.near_sample == 1 and Rover.vel == 0:
+            Rover.pick_up = True
     return Rover
 

code/drive_rover.py

@@ -71,7 +71,9 @@ def __init__(self):
         # obstacles and rock samples
         self.worldmap = np.zeros((200, 200, 3), dtype=np.float) 
         self.samples_pos = None # To store the actual sample positions
-        self.samples_found = None # To count the number of samples found
+        self.samples_found = 0 # To count the number of samples found
+        self.near_sample = False # Set to True if within reach of a rock sample
+        self.pick_up = False # Set to True to trigger rock pickup
 # Initialize our rover 
 Rover = RoverState()
 
@@ -81,7 +83,6 @@ def __init__(self):
 def telemetry(sid, data):
     if data:
         global Rover
-
         # Initialize / update Rover with current telemetry
         Rover, image = update_rover(Rover, data)
 
@@ -97,7 +98,12 @@ def telemetry(sid, data):
             # The action step!  Send commands to the rover!
             commands = (Rover.throttle, Rover.brake, Rover.steer)
             send_control(commands, out_image_string1, out_image_string2)
-
+
+            # If we are in a state where want to pickup a rock send pickup command
+            if Rover.pick_up:
+                send_pickup()
+                # Reset Rover flags
+                Rover.pick_up = False
         # In case of invalid telemetry, send null commands
         else:
 
@@ -140,6 +146,14 @@ def send_control(commands, image_string1, image_string2):
         data,
         skip_sid=True)
 
+# Define a function to send the "pickup" command 
+def send_pickup():
+    print("Picking up")
+    pickup = {}
+    sio.emit(
+        "pickup",
+        pickup,
+        skip_sid=True)
 
 if __name__ == '__main__':
     parser = argparse.ArgumentParser(description='Remote Driving')

code/supporting_functions.py

@@ -35,9 +35,11 @@ def update_rover(Rover, data):
       Rover.throttle = np.float(data["throttle"])
       # The current steering angle
       Rover.steer = np.float(data["steering_angle"])
+      # Near sample flag
+      Rover.near_sample = np.int(data["near_sample"])
 
       print('speed =',Rover.vel, 'position =', Rover.pos, 'throttle =', 
-      Rover.throttle, 'steer_angle =', Rover.steer)
+      Rover.throttle, 'steer_angle =', Rover.steer, 'near_sample', Rover.near_sample, data["picking_up"])
 
       # Get the current image from the center camera of the rover
       imgString = data["image"]