Merge pull request #14 from cornell-cup/script-feature

Script feature

python-interface/src/received/line_follower.py

@@ -0,0 +1,20 @@
+from cup_minibot_prepend import CupMiniBot
+from .sensor.Sensor import Sensor
+bot = CupMiniBot()
+left = Sensor(bot, 'left')
+right = Sensor(bot, 'right')
+center = Sensor(bot, 'center')
+
+end = False
+while(not end):
+
+    while(center.read()==1):
+        bot.move_foward(100)
+    while(center.read() != 1):
+        if(left.read() == 1):
+            bot.turn_clockwise(100)
+        else if(right.read() == 1):
+            bot.turn_counter_clockwise(100)
+        else:
+            end = True
+            break

python-interface/src/received/line_follower_demo.py

@@ -0,0 +1,24 @@
+#COPY AND PASTE THIS CODE INTO THE GUI TERMINAL
+
+# bot.move_forward(70)
+# s = GPIOSensor(bot, 'right', 1)
+# s1 = GPIOSensor(bot, 'left', 2)
+# s2 = GPIOSensor(bot, 'center', 3)
+# state = 0
+# while(True):
+#     data = s.readAll()
+#     j = json.loads(data)
+#     center = j['center']['data']
+#     right = j['right']['data']
+#     left = j['left']['data']
+#
+#     if left != 1 and right != 1:
+#         bot.move_forward(70)
+#     else:
+#         if left and right:
+#             bot.move_forward(70)
+#             state = 1
+#         elif right:
+#             bot.turn_clockwise(70)
+#         elif left:
+#             bot.turn_counter_clockwise(70)

script.py

@@ -0,0 +1,168 @@
+import socket
+import sys
+import time
+import json
+
+HOST = "localhost"
+PORT = 11111
+
+sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+
+class SimMiniBot:
+    """
+    Abstract simulator class defining the base functions of the MiniBot. More customized MiniBots may
+    subclass this.
+    """
+    def __init__(self):
+        server_address = (HOST, PORT)
+        sock.connect(server_address)
+
+    def move_forward(self, power):
+        """
+        Moves the bot forward at a percentage of its full power
+
+        :param power The percentage of the bot's power to use from 0-100
+        :return True if the action is supported
+        """
+        message = "FORWARD:" + str(power) + '\n'
+        sock.sendall(message)
+        return
+
+    def move_backward(self, power):
+        """
+        Moves the bot backward at a percentage of its full power
+
+        :param power The percentage of the bot's power to use from 0-100
+        :return True if the action is supported
+        """
+        message = "BACKWARD:" + str(power) + '\n'
+        sock.sendall(message)
+        return
+
+    def turn_clockwise(self, power):
+        """
+        Moves the bot clockwise  at a percentage of its full power
+
+        :param power The percentage of the bot's power to use from 0-100
+        :return True if the action is supported
+        """
+        message = "RIGHT:" + str(power) + '\n'
+        sock.sendall(message)
+        return
+
+    def turn_counter_clockwise(self, power):
+        """
+        Moves the bot counter-clockwise at a percentage of its full power
+
+        :param power The percentage of the bot's power to use from 0-100
+        :return True if the action is supported
+        """
+        message = "LEFT:" + str(power) + '\n'
+        sock.sendall(message)
+        return
+
+    def set_wheel_power(self, front_left, front_right, back_left, back_right):
+        """
+        Sets the power of the bot's wheels as a percentage from -100 to 100. If a wheel
+        specified does not exist, the power for that wheel is ignored.
+        :param front_left power to deliver to the front_left wheel
+        :param front_right power to deliver to the front_right wheel
+        :param back_left power to deliver to the back_left wheel
+        :param back_right power to deliver to the back_right wheel
+        :return True if the action is supported
+        """
+        message = "WHEELS:" + str(front_left) + ',' + str(front_right) + ',' + str(back_left) + ',' \
+                  + str(back_right) + '\n';
+        sock.sendall(message)
+        return
+
+    def wait(self, t):
+        """
+        Waits for a duration in seconds.
+        :param t The duration in seconds
+        """
+        message = "WAIT:" + str(t) + '\n'
+        sock.sendall(message)
+        time.sleep(t)
+        return
+
+    def stop(self):
+        """
+        Waits for a duration in seconds.
+        :param t The duration in seconds
+        """
+        message = "STOP:0" + '\n'
+        sock.sendall(message)
+        return
+
+    def register_sensor(self, name):
+        """
+        Registers a new sensor.
+        :param name The name of the sensor
+        """
+        message = "REGISTER:" + name + '\n'
+        sock.sendall(message)
+        return
+
+    def kill(self):
+        """
+        Kills TCP connection
+        """
+        message = "KILL:-1" + '\n'
+        sock.sendall(message)
+        sock.close()
+        return
+
+
+class Sensor:
+    def __init__(self, bot, name):
+        self.name = name
+        bot.register_sensor(name)
+
+    def read(self):
+        return "Invalid: Abstract Sensor Class Reading"
+
+
+class GPIOSensor(Sensor):
+    def __init__(self, bot, name, pin_number):
+        Sensor.__init__(self, bot, name)
+        self.pin_number = pin_number
+
+    def readAll(self):
+        """
+        Get All Sensor Data
+        """
+        message = "GET:ALL" + '\n'
+        sock.sendall(message)
+        result = sock.recv(1024)
+        return result
+
+    def read(self):
+        """
+        Get Sensor Data
+        """
+        message = "GET:" + self.name + '\n'
+        sock.sendall(message)
+        result = sock.recv(1024)
+        return result
+
+bot = SimMiniBot()
+bot.move_forward(70)
+s = GPIOSensor(bot, 'bot1', 1)
+state = 0
+while(True):
+ data = s.readAll()
+ j = json.loads(data)
+ center = j['center']['data']
+ right = j['right']['data']
+ left = j['left']['data']
+
+ if left and right:
+   bot.move_forward(70)
+ elif right:
+   bot.turn_clockwise(70)
+ elif left:
+   bot.turn_counter_clockwise(70)
+ else:
+   if state == 0:
+     bot.move_forward(70)

src/main/java/minibot/BaseHTTPInterface.java

@@ -14,7 +14,9 @@
 import com.google.gson.JsonObject;
 import com.google.gson.JsonParser;
 import simulator.physics.PhysicalObject;
+import simulator.simbot.ColorIntensitySensor;
 import simulator.simbot.SimBotConnection;
+import simulator.simbot.SimBotSensorCenter;
 import spark.route.RouteOverview;
 
 import java.util.ArrayList;
@@ -88,13 +90,19 @@ public static void main(String[] args) {
             }
                else {
                 SimBotConnection sbc = new SimBotConnection();
-                newBot = new SimBot(sbc, name);
-
-                PhysicalObject po = new PhysicalObject("TESTBOT", 50, simvs.getWorld(), 0.4f, 0.0f, 1f, 1f, true);
+                PhysicalObject po = new PhysicalObject("simbot", 50, simvs.getWorld(), 0.0f, 0.0f, 1f, 3.6f, true);
+                SimBot simbot;
+                simbot = new SimBot(sbc, name, po);
+                newBot = simbot;
 
                 ArrayList<PhysicalObject> pObjs = new ArrayList<>();
                 pObjs.add(po);
                 simvs.processPhysicalObjects(pObjs);
+
+                // Color sensor TODO put somewhere nice
+                ColorIntensitySensor colorSensorL = new ColorIntensitySensor((SimBotSensorCenter) simbot.getSensorCenter(),"right",simbot, 5);
+                ColorIntensitySensor colorSensorR = new ColorIntensitySensor((SimBotSensorCenter) simbot.getSensorCenter(),"left",simbot, -5);
+                ColorIntensitySensor colorSensorM = new ColorIntensitySensor((SimBotSensorCenter) simbot.getSensorCenter(),"center",simbot, 0);
             }
 
             return BaseStation.getInstance().getBotManager().addBot(newBot);
@@ -143,6 +151,11 @@ public static void main(String[] args) {
             String name = commandInfo.get("name").getAsString();
             String script = commandInfo.get("script").getAsString();
 
+            ((SimBot)BaseStation.getInstance()
+                    .getBotManager()
+                    .getBotByName(name)
+                    .orElseThrow(NoSuchElementException::new)).resetServer();
+
             return BaseStation.getInstance()
                     .getBotManager()
                     .getBotByName(name)

src/main/java/simulator/baseinterface/SimulatorVisionSystem.java

@@ -10,6 +10,7 @@
 import java.util.ArrayList;
 import java.util.HashSet;
 import java.util.Set;
+import java.util.concurrent.ConcurrentHashMap;
 
 import org.jbox2d.dynamics.World;
 
@@ -20,7 +21,7 @@ public class SimulatorVisionSystem extends VisionSystem {
      * @param o A coordinate specifying the origin of the VisionSystem
      */
     private static SimulatorVisionSystem instance;
-    private volatile HashSet<VisionObject> set;
+    private volatile Set<VisionObject> set;
     private HashSet<PhysicalObject> po_set;
     private World world;
     private long before;
@@ -29,7 +30,7 @@ public class SimulatorVisionSystem extends VisionSystem {
 
     public SimulatorVisionSystem() {
         super(new VisionCoordinate(0, 0, 0));
-        set = new HashSet<>();
+        set = ConcurrentHashMap.newKeySet();
         world = new World(new Vec2(0f, 0f));
         po_set = new HashSet<>();
         before = System.nanoTime();
@@ -57,7 +58,7 @@ public Set<VisionObject> getAllObjects() {
      * @param pObjs The data sent from simulator
      */
     public void processPhysicalObjects(ArrayList<PhysicalObject> pObjs) {
-        HashSet<VisionObject> newSet = new HashSet<>();
+        Set<VisionObject> newSet = ConcurrentHashMap.newKeySet();
         for(PhysicalObject obj: pObjs) {
             VisionCoordinate vc = new VisionCoordinate(obj.getX(),obj.getY(), 0.0);
             VisionObject vo = new VisionObject(this,obj.getID(),vc);

src/main/java/simulator/simbot/ColorIntensitySensor.java

@@ -0,0 +1,88 @@
+package simulator.simbot;
+
+import basestation.BaseStation;
+import basestation.vision.VisionCoordinate;
+import basestation.vision.VisionObject;
+import com.google.gson.JsonObject;
+
+import javax.imageio.ImageIO;
+import java.awt.image.BufferedImage;
+import java.io.File;
+import java.io.IOException;
+import java.util.List;
+
+/**
+ * A sensor that measures color intensity based on an image provided to the simulator.
+ */
+public class ColorIntensitySensor extends Sensor {
+
+    transient BufferedImage img;
+    private transient SimBot parent;
+    private double lateralOffset;
+
+    public ColorIntensitySensor(SimBotSensorCenter myCenter, String name, SimBot parent, double lateralOffset) {
+        super(myCenter, name);
+        this.parent = parent;
+        try {
+            img = ImageIO.read(new File("./src/main/resources/public/img/line.png"));
+        } catch (IOException e) {
+            e.printStackTrace();
+        }
+        this.lateralOffset = lateralOffset;
+    }
+
+    public JsonObject read() {
+        // Get the x,y coordinates of the bot.
+        // TODO: Make it so we can do this line, not the one below. VisionCoordinate parentCoordinate = BaseStation.getInstance().getVisionManager().getBotCoordinate(parent);
+        List<VisionObject> allLocs = BaseStation.getInstance().getVisionManager().getAllLocationData();
+        JsonObject jo = new JsonObject();
+
+        if (allLocs.size() == 0) {
+            jo.addProperty("data",-1);
+            return jo;
+        }
+
+        VisionCoordinate parentCoordinate = allLocs.get(0).coord;
+
+        if (parentCoordinate == null) {
+            jo.addProperty("data",-1);
+            return jo;
+        }
+
+
+        int[] transformed = transformToPixels(parentCoordinate, this.lateralOffset);
+
+        // Get pixel values
+        if(img==null) {
+            System.err.println("null image");
+            jo.addProperty("data", -1);
+            return jo;
+        }
+        int rgb = img.getRGB(transformed[0], transformed[1]);
+        if (rgb <= -16777216) {
+            rgb=1;
+        } else {
+            rgb=0;
+        }
+        jo.addProperty("data",rgb);
+
+
+        return jo;
+    }
+
+    private int[] transformToPixels(VisionCoordinate vc, double lateralOffset) {
+        double X_SCALE = 100;
+        double Y_SCALE = 100;
+        double medial_offset = 9.5; //offset along the medial axis of the robot, positive values indicate a "forwards" offset. 62.5 corresponds with offsetting it to the front of the bot
+        double lateral_offset = lateralOffset; //offset along lateral axis of robot, positive values indicate a "leftwards" offset, 0 indicates that there is no left/right offset
+        int[] ret = new int[2];
+
+        double angle = vc.getThetaOrZero();
+        double angle_offset = Math.atan(lateral_offset/medial_offset)+angle; //intermediate step for calculating coordinates
+        double total_offset = Math.sqrt(medial_offset*medial_offset+lateral_offset*lateral_offset); //intermediate step for calculating coordinates
+
+        ret[0] = (int) Math.floor(vc.x * X_SCALE + total_offset*Math.cos(angle_offset));
+        ret[1] = (int) Math.floor(vc.y * Y_SCALE + total_offset*Math.sin(angle_offset));
+        return ret;
+    }
+}