added accessability features (choosing ip, robot id at startup, changing max speed), remove obstacle code

This commit is contained in:
Simon Pirkelmann 2020-01-12 16:11:26 +01:00
parent c2fad13698
commit aab1b78e8f

View File

@ -30,8 +30,12 @@ from marker_pos_angle.msg import id_pos_angle
from collections import OrderedDict from collections import OrderedDict
from argparse import ArgumentParser
class Robot: class Robot:
def __init__(self, id, ip=None): def __init__(self, id, ip):
self.pos = None self.pos = None
self.orient = None self.orient = None
@ -122,20 +126,18 @@ def f_ode(t, x, u):
return dx return dx
class RemoteController: class RemoteController:
def __init__(self): def __init__(self, id, ip):
self.robots = [Robot(11, '192.168.1.103')] self.anim_stopped = False
#self.robots = [Robot(14, '192.168.1.102')]
#self.robots = [Robot(14, '192.168.1.103')]
#self.robots = [Robot(15, '192.168.1.102')]
self.robots = [Robot(id, ip)]
self.robot_ids = {} self.robot_ids = {}
for r in self.robots: for r in self.robots:
self.robot_ids[r.id] = r self.robot_ids[r.id] = r
obst = [Obstacle(12, 0.275), Obstacle(10, 0.275), Obstacle(14, 0.275), Obstacle(15, 0.275)]
self.obstacles = {}
for r in obst:
self.obstacles[r.id] = r
self.track = Track() self.track = Track()
@ -198,13 +200,6 @@ class RemoteController:
self.track_line_inner, = self.ax.plot([], []) self.track_line_inner, = self.ax.plot([], [])
self.track_line_outer, = 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='--'))
for s in self.circles:
self.ax.add_artist(s)
self.ax.set_xlabel('x-position') self.ax.set_xlabel('x-position')
self.ax.set_ylabel('y-position') self.ax.set_ylabel('y-position')
self.ax.grid() self.ax.grid()
@ -231,6 +226,7 @@ class RemoteController:
def ani(self): def ani(self):
print("starting animation") print("starting animation")
self.ani = anim.FuncAnimation(self.fig, init_func=self.ani_init, func=self.ani_update, interval=10, blit=True) self.ani = anim.FuncAnimation(self.fig, init_func=self.ani_init, func=self.ani_update, interval=10, blit=True)
plt.ion() plt.ion()
@ -251,10 +247,13 @@ class RemoteController:
self.track_line_outer.set_data(self.track.outer_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,\ return self.line, self.line_sim, self.dirm, self.dirs, self.line_ol,\
self.track_line_inner, self.track_line_outer, self.line_x,self.line_y, self.circles[0], self.circles[1],\ self.track_line_inner, self.track_line_outer, self.line_x,self.line_y,
self.circles[2], self.circles[3],
def ani_update(self, frame): def ani_update(self, frame):
if self.anim_stopped:
self.ani.event_source.stop()
sys.exit(0)
#print("plotting") #print("plotting")
self.mutex.acquire() self.mutex.acquire()
try: try:
@ -306,21 +305,11 @@ class RemoteController:
else: else:
self.line_ol.set_data([],[]) self.line_ol.set_data([],[])
i = 0
obst_keys = self.obstacles.keys()
for s in self.circles:
o = self.obstacles[obst_keys[i]]
i = i + 1
if o.pos is not None:
s.center = o.pos
s.radius = o.radius
finally: finally:
self.mutex.release() self.mutex.release()
return self.line, self.line_sim, self.dirm, self.dirs, self.line_ol, self.track_line_inner, self.track_line_outer,\ return self.line, self.line_sim, self.dirm, self.dirs, self.line_ol, self.track_line_inner, self.track_line_outer,\
self.line_x, self.line_y, self.circles[0], self.circles[1],self.circles[2],self.circles[3], self.line_x, self.line_y,
def measurement_callback(self, data): def measurement_callback(self, data):
#print(data) #print(data)
@ -352,11 +341,6 @@ class RemoteController:
finally: finally:
self.mutex.release() self.mutex.release()
# detect obstacles
if data.id in self.obstacles.keys():
obst = (data.x, data.y)
self.obstacles[data.id].pos = obst
# detect track # detect track
if data.id in self.track.inner.keys() or data.id in self.track.outer.keys(): if data.id in self.track.inner.keys() or data.id in self.track.outer.keys():
self.track.set_id(data) self.track.set_id(data)
@ -369,7 +353,8 @@ class RemoteController:
markers_out = self.track.outer.values() markers_out = self.track.outer.values()
# find targets: # find targets:
lamb = 0.2 # generate waypoints
lamb = 0.5
j = 0 j = 0
for i in range(0,4): for i in range(0,4):
p = np.array(markers_in[i]) + lamb * (np.array(markers_out[i]) - np.array(markers_in[i])) p = np.array(markers_in[i]) + lamb * (np.array(markers_out[i]) - np.array(markers_in[i]))
@ -385,6 +370,8 @@ class RemoteController:
auto_control = False auto_control = False
current_target = 0 current_target = 0
control_scaling = 0.3
while True: while True:
# open loop controller # open loop controller
@ -422,8 +409,26 @@ class RemoteController:
auto_control = not auto_control auto_control = not auto_control
if auto_control: if auto_control:
self.target = targets[current_target] self.target = targets[current_target]
elif event.key == pygame.K_PLUS:
control_scaling += 0.1
control_scaling = min(control_scaling, 1.0)
elif event.key == pygame.K_MINUS:
control_scaling -= 0.1
control_scaling = max(control_scaling, 0.3)
elif event.key == pygame.K_ESCAPE:
print("quit!")
self.controlling = False
if self.rc_socket:
self.rc_socket.send('(0.0,0.0)\n')
self.anim_stopped = True
return
elif event.key == pygame.QUIT: elif event.key == pygame.QUIT:
sys.exit(0) print("quit!")
self.controlling = False
if self.rc_socket:
self.rc_socket.send('(0.0,0.0)\n')
self.anim_stopped = True
return
if self.controlling: if self.controlling:
@ -439,10 +444,13 @@ class RemoteController:
tmpc_start = time.time() tmpc_start = time.time()
# solve mpc open loop problem # solve mpc open loop problem
res = self.ols.solve(x_pred, self.target, self.obstacles, self.track) res = self.ols.solve(x_pred, self.target, [], self.track)
us1 = res[0] #us1 = res[0]
us2 = res[1] #us2 = res[1]
us1 = res[0] * control_scaling
us2 = res[1] * control_scaling
print("u = {}", (us1, us2))
# save open loop trajectories for plotting # save open loop trajectories for plotting
self.mutex.acquire() self.mutex.acquire()
@ -493,9 +501,18 @@ class RemoteController:
def main(args): def main(args):
parser = ArgumentParser()
parser.add_argument('id', metavar='id', type=str, help='marker id of the controlled robot')
parser.add_argument('ip', metavar='ip', type=str, help='ip address of the controlled robot')
args = parser.parse_args()
marker_id = int(args.id)
ip = args.ip
rospy.init_node('controller_node', anonymous=True) rospy.init_node('controller_node', anonymous=True)
rc = RemoteController() rc = RemoteController(marker_id, ip)
pygame.init() pygame.init()
@ -508,7 +525,8 @@ def main(args):
pass pass
#threading.Thread(target=rc.input_handling).start() #threading.Thread(target=rc.input_handling).start()
threading.Thread(target=rc.controller).start() controller_thread = threading.Thread(target=rc.controller)
controller_thread.start()
rc.ani() rc.ani()