Telos4
/
RoboRally
1
0
Fork 1
Code Issues Pull Requests Releases Wiki Activity

added plot for race track

simple_control
Simon Pirkelmann 2019-07-04 10:54:50 +02:00
parent 9c222b1099
commit 24cf87e9dd
2 changed files with 86 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
remote_control/casadi_opt.py
View File

@ -138,7 +138,7 @@ class OpenLoopSolver:
#return
def solve(self, x0, target, obstacles):
def solve(self, x0, target, obstacles, track):
tstart = time.time()
# alternative solution using multiple shooting (way faster!)
self.opti = Opti() # Optimization problem
@ -250,8 +250,11 @@ class OpenLoopSolver:
tstart = time.time()
if self.use_warmstart and self.opti_x0 is not None:
self.opti.set_initial(self.opti.lam_g, self.opti_lam_g0)
self.opti.set_initial(self.opti.x, self.opti_x0)
try:
self.opti.set_initial(self.opti.lam_g, self.opti_lam_g0)
self.opti.set_initial(self.opti.x, self.opti_x0)
except RuntimeError:
print("could not set warmstart")
sol = self.opti.solve() # actual solve
tend = time.time()
print("solving the problem took {} seconds".format(tend - tstart))

85
remote_control/position_controller.py
View File

@ -20,12 +20,16 @@ import matplotlib.pyplot as plt
import matplotlib.animation as anim
import matplotlib.patches as patch
from shapely.geometry import Polygon
import time
from casadi_opt import OpenLoopSolver
from marker_pos_angle.msg import id_pos_angle
from collections import OrderedDict
class Robot:
def __init__(self, id, ip=None):
self.pos = None
@ -44,6 +48,57 @@ class Obstacle:
self.pos = None
self.radius = radius
class Track:
def __init__(self):
# ids in clockwise direction
self.inner = OrderedDict()
self.inner[5] = None
self.inner[8] = None
self.inner[7] = None
self.inner[1] = None
self.outer = OrderedDict()
self.outer[11] = None
self.outer[2] = None
self.outer[4] = None
self.outer[0] = None
self.track_complete = False
self.inner_poly = None
self.outer_poly = None
def set_id(self, d):
if not self.track_complete:
if d.id in self.inner:
print("Detected marker {} at pos {}".format(d.id, (d.x,d.y)))
self.inner[d.id] = (d.x, d.y)
elif d.id in self.outer:
print("Detected marker {} at pos {}".format(d.id, (d.x, d.y)))
self.outer[d.id] = (d.x, d.y)
else:
print("Unknown marker!")
else:
return
if not None in self.inner.values() and not None in self.outer.values():
print("Track marker positions detected!")
self.track_complete = True
self.inner_poly = Polygon(self.inner.values())
self.outer_poly = Polygon(self.outer.values())
def plot_track(self):
if self.track_complete:
plt.figure(2)
x_in, y_in = self.inner_poly.exterior.xy
x_out, y_out = self.outer_poly.exterior.xy
plt.plot(x_in, y_in)
plt.plot(x_out, y_out)
else:
print("plot is not complete yet!")
def f_ode(t, x, u):
# dynamical model of the two-wheeled robot
# TODO: find exact values for these parameters
@ -69,18 +124,20 @@ def f_ode(t, x, u):
class RemoteController:
def __init__(self):
self.robots = [Robot(3, '192.168.1.103')]
self.robots = [Robot(15, '192.168.1.103')]
self.robot_ids = {}
for r in self.robots:
self.robot_ids[r.id] = r
obst = [Obstacle(6, 0.175), Obstacle(5, 0.175), Obstacle(8, 0.175)]
obst = [Obstacle(1, 0.175), Obstacle(5, 0.175), Obstacle(7, 0.175)]
self.obstacles = {}
for r in obst:
self.obstacles[r.id] = r
self.track = Track()
# connect to robot
self.rc_socket = socket.socket()
#self.rc_socket = None
@ -131,6 +188,9 @@ class RemoteController:
self.dirm, = self.ax.plot([], [])
self.dirs, = self.ax.plot([], [])
self.track_line_inner, = self.ax.plot([], [])
self.track_line_outer, = self.ax.plot([], [])
self.circles = []
for o in self.obstacles:
self.circles.append(patch.Circle((0.0, 0.0), radius=0.1, fill=False, color='red', linestyle='--'))
@ -154,6 +214,7 @@ class RemoteController:
def ani(self):
print("starting animation")
self.ani = anim.FuncAnimation(self.fig, init_func=self.ani_init, func=self.ani_update, interval=10, blit=True)
plt.ion()
plt.show(block=True)
@ -163,7 +224,11 @@ class RemoteController:
self.ax.set_ylim(-2, 2)
self.ax.set_aspect('equal', adjustable='box')
return self.line, self.line_sim, self.dirm, self.dirs, self.line_ol, self.circles[0], self.circles[1],self.circles[2],
self.track_line_inner.set_data(self.track.inner_poly.exterior.xy)
self.track_line_outer.set_data(self.track.outer_poly.exterior.xy)
return self.line, self.line_sim, self.dirm, self.dirs, self.line_ol,\
self.track_line_inner, self.track_line_outer, self.circles[0], self.circles[1],self.circles[2],
def ani_update(self, frame):
#print("plotting")
@ -224,9 +289,11 @@ class RemoteController:
finally:
self.mutex.release()
return self.line, self.line_sim, self.dirm, self.dirs, self.line_ol, self.circles[0], self.circles[1],self.circles[2],
return self.line, self.line_sim, self.dirm, self.dirs, self.line_ol, self.track_line_inner, self.track_line_outer, self.circles[0], self.circles[1],self.circles[2],
def measurement_callback(self, data):
#print(data)
# detect robots
if data.id in self.robot_ids:
r = self.robot_ids[data.id]
@ -259,6 +326,10 @@ class RemoteController:
obst = (data.x, data.y)
self.obstacles[data.id].pos = obst
# detect track
if data.id in self.track.inner.keys() or data.id in self.track.outer.keys():
self.track.set_id(data)
def controller(self):
print("starting control")
while True:
@ -291,7 +362,7 @@ class RemoteController:
tmpc_start = time.time()
# solve mpc open loop problem
res = self.ols.solve(x_pred, self.target, self.obstacles)
res = self.ols.solve(x_pred, self.target, self.obstacles, self.track)
us1 = res[0]
us2 = res[1]
@ -349,6 +420,10 @@ def main(args):
screenwidth = 640
pygame.display.set_mode([screenwidth, screenheight])
print("waiting until track is completely detected")
while not rc.track.track_complete:
pass
#threading.Thread(target=rc.input_handling).start()
threading.Thread(target=rc.controller).start()