diff --git a/remote_control/position_controller.py b/remote_control/position_controller.py index 3c5b318..19ce046 100644 --- a/remote_control/position_controller.py +++ b/remote_control/position_controller.py @@ -30,8 +30,12 @@ from marker_pos_angle.msg import id_pos_angle from collections import OrderedDict +from argparse import ArgumentParser + + + class Robot: - def __init__(self, id, ip=None): + def __init__(self, id, ip): self.pos = None self.orient = None @@ -122,20 +126,18 @@ def f_ode(t, x, u): return dx class RemoteController: - def __init__(self): + def __init__(self, id, ip): - self.robots = [Robot(11, '192.168.1.103')] - #self.robots = [Robot(14, '192.168.1.102')] + self.anim_stopped = False + + #self.robots = [Robot(14, '192.168.1.103')] + #self.robots = [Robot(15, '192.168.1.102')] + self.robots = [Robot(id, ip)] self.robot_ids = {} for r in self.robots: 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() @@ -198,13 +200,6 @@ class RemoteController: 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='--')) - - for s in self.circles: - self.ax.add_artist(s) - self.ax.set_xlabel('x-position') self.ax.set_ylabel('y-position') self.ax.grid() @@ -231,6 +226,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() @@ -251,10 +247,13 @@ class RemoteController: 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.line_x,self.line_y, self.circles[0], self.circles[1],\ - self.circles[2], self.circles[3], + self.track_line_inner, self.track_line_outer, self.line_x,self.line_y, def ani_update(self, frame): + if self.anim_stopped: + self.ani.event_source.stop() + sys.exit(0) + #print("plotting") self.mutex.acquire() try: @@ -306,21 +305,11 @@ class RemoteController: else: 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: self.mutex.release() 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): #print(data) @@ -352,11 +341,6 @@ class RemoteController: finally: 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 if data.id in self.track.inner.keys() or data.id in self.track.outer.keys(): self.track.set_id(data) @@ -369,7 +353,8 @@ class RemoteController: markers_out = self.track.outer.values() # find targets: - lamb = 0.2 + # generate waypoints + lamb = 0.5 j = 0 for i in range(0,4): p = np.array(markers_in[i]) + lamb * (np.array(markers_out[i]) - np.array(markers_in[i])) @@ -384,6 +369,8 @@ class RemoteController: auto_control = False current_target = 0 + + control_scaling = 0.3 while True: @@ -422,8 +409,26 @@ class RemoteController: auto_control = not auto_control if auto_control: 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: - 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: @@ -439,10 +444,13 @@ class RemoteController: tmpc_start = time.time() # 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] - us2 = res[1] + #us1 = res[0] + #us2 = res[1] + us1 = res[0] * control_scaling + us2 = res[1] * control_scaling + print("u = {}", (us1, us2)) # save open loop trajectories for plotting self.mutex.acquire() @@ -493,9 +501,18 @@ class RemoteController: 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) - rc = RemoteController() + rc = RemoteController(marker_id, ip) pygame.init() @@ -508,10 +525,11 @@ def main(args): pass #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() if __name__ == '__main__': - main(sys.argv) \ No newline at end of file + main(sys.argv)