コード例 #1
0
import colour
from colour import RED, WHITE, ORANGE, YELLOW, GREEN, BLUE
import prm
import motion
import client

if __name__ == "__main__":

    N = 9  #Number of samples for PRM
    k = 10  #Number of nearest neighbours for PRM
    W = 640  #Width of arena
    H = 480  #Height of arena
    Pad = 50  #Padding around edges
    manual = True
    graph = prm.initGraph(N, H, W, Pad, k, manual)
    cap = cv2.VideoCapture(0)
    client.createConnections()  #Creates three connections to servers
    #PID Stuff

    k_angle = [0.15, 0.025, 0.1]
    k_linear = [0.002, 0.0001, 0.005]
    I = 0
    Il = 0
    last = 0
    lastl = 0
    angle = 0
    distance = 0
    tol = 0.25
    toll = 0.01
    turnVel = 3
コード例 #2
0
N = 100
k = 10
wallPad = 100
h = 640
w = 480

directory = "footage/"
format = ".avi"
timestamp = time.ctime(time.time()).split(" ")[4].split(":")
filename = "output-"+timestamp[0]+"-"+timestamp[1]+"-"+timestamp[2]
print(timestamp)

fourcc = cv2.VideoWriter_fourcc('M','J','P','G')
out = cv2.VideoWriter(directory+filename+format, fourcc, 20.0, (int(cap.get(3)),int(cap.get(4))))

graph = prm.initGraph(N,w,h,wallPad,k)
start = prm.Node(np.random.randint(0,w-wallPad),np.random.randint(0,h-wallPad),N)
goal = prm.Node(w/2,h/2,N+1)
enemy = prm.Obstacle(w/2,h/2,60)
ball = prm.Obstacle(np.random.randint(0,w-wallPad),np.random.randint(0,h-wallPad),10)

font = cv2.FONT_HERSHEY_SIMPLEX
topLeft = (20,50)
fontScale = 1
fontColor = (255,255,255)
lineType = 2



while(True):
    # Capture frame-by-frame
コード例 #3
0
# server names for turtlebots which we connect to
servers = ["10.199.61.21", "10.199.26.14"]
# ports in which the messages pass through
ports = [12007, 12006]
# sockets for each port/server connection
sockets = [0, 1]

if __name__ == "__main__":

    N = 100  # Number of samples for PRM
    k = 10  # Number of nearest neighbours for PRM
    W = 640  # Width of arena
    H = 480  # Height of arena
    Pad = 80  # Padding around edges
    graph = prm.initGraph(N, W, H, Pad, k)
    cap = cv2.VideoCapture(0)
    # turtlebot server 1
    client.create_connections(servers[0], ports[0], sockets[0])
    # turtlebot server 2
    # client.create_connections(servers[1], ports[1], sockets[1])

    # PID Stuff
    prev_angle = 0
    prev_distance = 0
    # kp, ki, kd
    k_angle = [0.15, 0.005, 0]
    k_distance = [0.001, 0.00005, 0]

    # integral control
    K_i_angle = 0