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

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.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 = {}
         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.track_line_inner, self.track_line_outer, self.line_x,self.line_y,
-               self.circles[2], self.circles[3],
 
     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]))
@@ -385,6 +370,8 @@ class RemoteController:
         auto_control = False
         current_target = 0
                 
+        control_scaling = 0.3
+
         while True:
 
             # open loop controller
@@ -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]
+                #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
                 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,7 +525,8 @@ 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()