robot driving to position determined by mouse click

remote_control/mpc_controller.py

@@ -22,7 +22,6 @@ class MPCController:
     def move_to_pos(self, target_pos, robot, near_target_counter=5):
         near_target = 0
         while near_target < near_target_counter:
-
             while not self.estimator.event_queue.empty():
                 event = self.estimator.event_queue.get()
                 print("event: ", event)
@@ -56,7 +55,6 @@ class MPCController:
                         print("quit!")
                         self.controlling = False
                         robot.send_cmd()
-                        self.anim_stopped = True
                         return
 
             x_pred = self.get_measurement(robot.id)
@@ -98,5 +96,77 @@ class MPCController:
                     time.sleep(self.dt)
                 self.t = time.time()    # save time the most recent control was applied
 
+    def interactive_control(self, robots):
+        controlled_robot_number = 0
+        robot = robots[controlled_robot_number]
+
+        target_pos = np.array([0.0, 0.0, 0.0])
+
+        running = True
+        while running:
+            # handle events from opencv window
+            while not self.estimator.event_queue.empty():
+                event = self.estimator.event_queue.get()
+                print("event: ", event)
+                if event[0] == 'click':
+                    target_pos = event[1]
+                elif event[0] == 'key':
+                    key = event[1]
+
+                    if key == 32:   # arrow up
+                        self.controlling = not self.controlling
+                        if not self.controlling:
+                            print("disable control")
+                            robot.send_cmd()    # stop robot
+                        else:
+                            print("enable control")
+                            self.t = time.time()
+                    elif key == 48:     # 0
+                        target_pos = np.array([0.0, 0.0, 0.0])    # TODO: use center of board for target pos
+                    elif key == 43:     # +
+                        self.control_scaling += 0.1
+                        self.control_scaling = min(self.control_scaling, 1.0)
+                        print("control scaling = ", self.control_scaling)
+                    elif key == 45:     # -
+                        self.control_scaling -= 0.1
+                        self.control_scaling = max(self.control_scaling, 0.1)
+                        print("control scaling = ", self.control_scaling)
+                    elif key == 9:     # TAB
+                        # switch controlled robot
+                        robot.send_cmd()  # stop current robot
+                        controlled_robot_number = (controlled_robot_number + 1) % len(robots)
+                        robot = robots[controlled_robot_number]
+                        print(f"controlled robot: {robot.id}")
+                    elif key == 113 or key == 27:    # q or ESCAPE
+                        print("quit!")
+                        self.controlling = False
+                        robot.send_cmd()
+                        return
+
+            if self.controlling:
+                # measure state
+                x_pred = self.get_measurement(robot.id)
+
+                #print(np.linalg.norm(x_pred-target_pos))
+
+                # solve mpc open loop problem
+                res = self.ols.solve(x_pred, target_pos)
+
+                us1 = res[0] * self.control_scaling
+                us2 = res[1] * self.control_scaling
+
+                dt_mpc = time.time() - self.t
+                if dt_mpc < self.dt:  # wait until next control can be applied
+                    time.sleep(self.dt - dt_mpc)
+
+                # send controls to the robot
+                for i in range(0, self.mstep):  # option to use multistep mpc if len(range) > 1
+                    u1 = us1[i]
+                    u2 = us2[i]
+                    robot.send_cmd(u1, u2)
+                    if i < self.mstep:
+                        time.sleep(self.dt)
+                    self.t = time.time()  # save time the most recent control was applied
+
     def get_measurement(self, robot_id):
         return np.array(self.estimator.get_robot_state_estimate(robot_id))

remote_control/mpc_test.py

@@ -0,0 +1,53 @@
+import sys
+import socket
+import threading
+import time
+
+from robot import Robot
+
+from mpc_controller import MPCController
+
+import opencv_viewer_example
+
+
+class RemoteController:
+    def __init__(self):
+        self.robots = [Robot(12, '192.168.1.12')]
+
+        self.robot_ids = {}
+        for r in self.robots:
+            self.robot_ids[r.id] = r
+
+        # socket for movement commands
+        self.comm_socket = socket.socket()
+        self.comm_socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
+
+        for robot in self.robots:
+            robot.connect()
+
+        self.comm_socket.bind(('', 1337))
+        self.comm_socket.listen(5)
+
+        self.t = time.time()
+
+        # start thread for marker position detection
+        self.estimator = opencv_viewer_example.ArucoEstimator(self.robot_ids.keys())
+        self.estimator_thread = threading.Thread(target=self.estimator.run_tracking)
+        self.estimator_thread.start()
+
+        self.controller = MPCController(self.estimator)
+
+    def run(self):
+        print("waiting until all markers are detected...")
+        while not (self.estimator.all_corners_detected() and self.estimator.all_robots_detected()):
+            pass
+        print("starting control")
+        self.controller.interactive_control(robots=self.robots)
+
+def main(args):
+    rc = RemoteController()
+    rc.run()
+
+
+if __name__ == '__main__':
+    main(sys.argv)

remote_control/opencv_viewer_example.py

@@ -5,21 +5,6 @@ from cv2 import aruco
 from shapely.geometry import LineString
 from queue import Queue
 
-DS5_product_ids = ["0AD1", "0AD2", "0AD3", "0AD4", "0AD5", "0AF6", "0AFE", "0AFF", "0B00", "0B01", "0B03", "0B07","0B3A"]
-
-
-def find_device_that_supports_advanced_mode():
-    ctx = rs.context()
-    ds5_dev = rs.device()
-    devices = ctx.query_devices()
-    for dev in devices:
-        if dev.supports(rs.camera_info.product_id) and str(dev.get_info(rs.camera_info.product_id)) in DS5_product_ids:
-            if dev.supports(rs.camera_info.name):
-                print("Found device that supports advanced mode:", dev.get_info(rs.camera_info.name))
-            return dev
-    raise Exception("No device that supports advanced mode was found")
-
-
 class ArucoEstimator:
     grid_columns = 10
     grid_rows = 8
@@ -33,19 +18,21 @@ class ArucoEstimator:
     angles = []
 
     corner_estimates = {
-        'a': (None, 0),     # (estimate, n_estimates)
+        'a': {'pixel_coordinate': None, 'real_world_estimate': None, 'n_estimates': 0 },
-        'b': (None, 0),
+        'b': {'pixel_coordinate': None, 'real_world_estimate': None, 'n_estimates': 0 },
-        'c': (None, 0),
+        'c': {'pixel_coordinate': None, 'real_world_estimate': None, 'n_estimates': 0 },
-        'd': (None, 0)
+        'd': {'pixel_coordinate': None, 'real_world_estimate': None, 'n_estimates': 0 },
     }
 
-    def __init__(self, robot_marker_ids=[]):
+    def __init__(self, robot_marker_ids=None, use_realsense=True):
+        if robot_marker_ids is None:
+            robot_marker_ids = []
         self.robot_marker_ids = robot_marker_ids
         self.robot_marker_estimates = dict([(id, None) for id in self.robot_marker_ids])
 
         self.event_queue = Queue()
 
-        if False:   # check if realsense camera is connected
+        if use_realsense:   # check if realsense camera is connected
             # Configure depth and color streams
             self.pipeline = rs.pipeline()
             config = rs.config()
@@ -71,10 +58,33 @@ class ArucoEstimator:
             self.cv_camera = cv2.VideoCapture(0)
             self.pipeline = None
 
-    def mouse_callback(self, event, x, y, flags, param):
+    def mouse_callback(self, event, px, py, flags, param):
         if event == 1:
-            print(event)
+            if self.all_corners_detected():
-            self.event_queue.put(('click', (x,y)))
+                # inter/extrapolate from clicked point to marker position
+                px1 = self.corner_estimates['a']['pixel_coordinate'][0]
+                px3 = self.corner_estimates['c']['pixel_coordinate'][0]
+                py1 = self.corner_estimates['a']['pixel_coordinate'][1]
+                py3 = self.corner_estimates['c']['pixel_coordinate'][1]
+
+                x1 = self.corner_estimates['a']['real_world_estimate'][0]
+                x3 = self.corner_estimates['c']['real_world_estimate'][0]
+                y1 = self.corner_estimates['a']['real_world_estimate'][1]
+                y3 = self.corner_estimates['c']['real_world_estimate'][1]
+
+                alpha = (px - px1) / (px3 - px1)
+                beta = (py - py1) / (py3 - py1)
+
+                print(f"alpha = {alpha}, beta = {beta}")
+
+                target_x = x1 + alpha * (x3 - x1)
+                target_y = y1 + beta * (y3 - y1)
+                target = np.array([target_x, target_y, 0])
+
+                print(f"target = ({target_x},{target_y})")
+                self.event_queue.put(('click', target))
+            else:
+                print("not all markers have been detected!")
 
     def run_tracking(self):
         cv2.namedWindow('RoboRally', cv2.WINDOW_AUTOSIZE)
@@ -102,13 +112,14 @@ class ArucoEstimator:
                 corners, ids, rejectedImgPoints = aruco.detectMarkers(gray, aruco_dict, parameters=parameters)
                 frame = aruco.drawDetectedMarkers(color_image.copy(), corners, ids)
 
-                if False:#ids is not None:
+                if ids is not None:
-                    for id, c in zip(ids, corners):
+                    for id, corner in zip(ids, corners):
-                        res = aruco.estimatePoseSingleMarkers(c, 0.10, self.camera_matrix, self.dist_coeffs)
+                        corner_pixel_coord = np.mean(corner[0], axis=0)
+                        res = aruco.estimatePoseSingleMarkers(corner, 0.10, self.camera_matrix, self.dist_coeffs)
                         rvecs = res[0][0][0]
                         tvecs = res[1][0][0]
 
-                        self.update_estimate(id, rvecs, tvecs)
+                        self.update_estimate(id[0], corner_pixel_coord, rvecs, tvecs)
 
                     frame = self.draw_grid_lines(frame, corners, ids)
                     frame = self.draw_robot_pos(frame, corners, ids)
@@ -123,6 +134,8 @@ class ArucoEstimator:
                     #self.update_estimate(12, rvec=np.array([0.0, 0.0, 0.0]), tvec=np.array([-1.0 + random.random() * 2, -1.0 + random.random() * 2, 0]))
                     self.update_estimate(12, rvec=np.array([0.0, 0.0, 0.0]),
                                          tvec=np.array([1.2, 0.42, 0]))
+                    self.update_estimate(13, rvec=np.array([0.0, 0.0, 0.0]),
+                                         tvec=np.array([-1.2, -0.42, 0]))
 
                 # Show images
                 cv2.imshow('RoboRally', frame)
@@ -139,21 +152,25 @@ class ArucoEstimator:
                 # Stop streaming
                 self.pipeline.stop()
 
-    def update_estimate(self, id, rvec, tvec):
+    def update_estimate(self, id, pixel_coord, rvec, tvec):
         # update the marker estimate with new data
         if id in self.corner_marker_ids.values():
             # for corner markers we compute the mean of all measurements s.t. the position stabilizes over time
             # (we assume the markers do not move)
             corner = next(filter(lambda key: self.corner_marker_ids[key] == id, self.corner_marker_ids.keys()))  # get corresponding corner to the detected marker
-            old_estimate = self.corner_estimates[corner][0]
+            old_estimate = self.corner_estimates[corner]['real_world_estimate']
-            n_estimates = self.corner_estimates[corner][1]
+            n_estimates = self.corner_estimates[corner]['n_estimates']
+
             tvec_proj = tvec[0:2] # projection to get rid of z coordinate
             tvec_proj = np.array((tvec_proj[0], -tvec_proj[1])) # flip y coordinate
             if old_estimate is not None:
                 new_estimate = (n_estimates * old_estimate + tvec_proj) / (n_estimates + 1)  # weighted update
             else:
                 new_estimate = tvec_proj # first estimate
-            self.corner_estimates[corner] = (new_estimate, n_estimates + 1)
+            self.corner_estimates[corner]['real_world_estimate'] = new_estimate
+            self.corner_estimates[corner]['n_estimates'] = n_estimates + 1
+            self.corner_estimates[corner]['pixel_coordinate'] = pixel_coord
+
         elif id in self.robot_marker_ids:
             # for robot markers we extract x and y position as well as the angle
             # here we could also implement a filter
@@ -171,7 +188,11 @@ class ArucoEstimator:
 
     def all_corners_detected(self):
         # checks if all corner markers have been detected at least once
-        return not any([estimate[0] is None for estimate in self.corner_estimates.values()])
+        return not any([estimate['n_estimates'] == 0 for estimate in self.corner_estimates.values()])
+
+    def all_robots_detected(self):
+        # checks if all robot markers have been detected at least once
+        return not any([estimate is None for estimate in self.robot_marker_estimates.values()])
 
     def get_pos_from_grid_point(self, x, y, orientation=None):
         """
@@ -187,10 +208,10 @@ class ArucoEstimator:
         assert self.all_corners_detected()
 
         # compute column line
-        a = self.corner_estimates['a'][0]
+        a = self.corner_estimates['a']['real_world_estimate']
-        b = self.corner_estimates['b'][0]
+        b = self.corner_estimates['b']['real_world_estimate']
-        c = self.corner_estimates['c'][0]
+        c = self.corner_estimates['c']['real_world_estimate']
-        d = self.corner_estimates['d'][0]
+        d = self.corner_estimates['d']['real_world_estimate']
         x_frac = (x + 0.5) / self.grid_columns
         y_frac = (y + 0.5) / self.grid_rows
 
@@ -239,8 +260,8 @@ class ArucoEstimator:
 
     def print_corner_estimates(self):
         for key, value in self.corner_estimates.items():
-            if value[0] is not None:
+            if value['n_estimates'] != 0:
-                print("corner marker {} at pos {}".format(key, value[0]))
+                print("corner marker {} at pos {}".format(key, value['real_world_estimate']))
         print()
 
     def draw_corner_line(self, frame, corner_1, corner_2, corner_coords_dict):
@@ -278,7 +299,7 @@ class ArucoEstimator:
             for x in range(1,self.grid_columns):
                 column_line_top = a + x / self.grid_columns * vab
                 column_line_bottom = d + x / self.grid_columns * vdc
-                frame = cv2.line(frame, tuple(column_line_top), tuple(column_line_bottom), color=(255, 0, 0),
+                frame = cv2.line(frame, tuple(column_line_top), tuple(column_line_bottom), color=(0, 255, 0),
                          thickness=1)
 
             vad = d - a
@@ -286,7 +307,7 @@ class ArucoEstimator:
             for y in range(1, self.grid_rows):
                 row_line_top = a + y / self.grid_rows * vad
                 row_line_bottom = b + y / self.grid_rows * vbc
-                frame = cv2.line(frame, tuple(row_line_top), tuple(row_line_bottom), color=(255, 0, 0),
+                frame = cv2.line(frame, tuple(row_line_top), tuple(row_line_bottom), color=(0, 255, 0),
                              thickness=1)
 
         return frame

remote_control/robot.py

@@ -0,0 +1,35 @@
+import socket
+import sys
+
+
+class Robot:
+    def __init__(self, marker_id, ip):
+        self.id = marker_id
+        self.pos = None
+        self.euler = None
+
+        self.ip = ip
+        self.socket = socket.socket()
+
+        # currently active control
+        self.u1 = 0.0
+        self.u2 = 0.0
+
+    def connect(self):
+        # connect to robot
+        try:
+            print("connecting to robot {} with ip {} ...".format(self.id, self.ip))
+            self.socket.connect((self.ip, 1234))  # connect to robot
+            print("connected!")
+        except socket.error:
+            print("could not connect to robot {} with ip {}".format(self.id, self.ip))
+            sys.exit(1)
+
+    def send_cmd(self, u1=0.0, u2=0.0):
+        if self.socket:
+            try:
+                self.socket.send(f'({u1},{u2})\n'.encode())
+            except BrokenPipeError:
+                # print(f"error: connection to robot {self.id} with ip {self.ip} lost")
+                pass
+