got mpc controller running again

remote_control/controller.py

@@ -21,7 +21,8 @@ class ControllerBase:
                 if self.controlling:
                     self.apply_control(control)
 
-        self.apply_control((0.0, 0.0))   # stop robot
+        if self.robot is not None:
+            self.robot.send_cmd(u1=0, u2=0)   # stop robot
 
     def set_target_position(self, target_pos):
         self.target_pos = target_pos

remote_control/mpc_test.py

@@ -3,17 +3,32 @@ import socket
 import threading
 import time
 
-from robot import Robot
+from robot import Robot, ControlledRobot
 
 from mpc_controller import MPCController
 
-from aruco_estimator import ArucoEstimator
+# HOST, PORT = "localhost", 42424
+#
+# robot_id = 12
+#
+# sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+# sock.connect((HOST, PORT))
+# sock.sendall(f"{robot_id}\n".encode())     # request data for robot with given id
+# #sock.sendall(f"events\n".encode())        # request events
+# receiving = True
+# while receiving:
+#     received = str(sock.recv(1024), "utf-8")
+#     print("Received: {}".format(received))
+#     receiving = len(received) > 0
 
 
 class RemoteController:
     def __init__(self):
         self.robots = []
-        #self.robots = [Robot(11, '192.168.1.11'), Robot(13, '192.168.1.13'), Robot(14, '192.168.1.14')]
+        #self.robots = [Robot(11, '192.168.1.11')] #, Robot(13, '192.168.1.13'), Robot(14, '192.168.1.14')]
+        #self.robots = [Robot(12, '10.10.11.91'), Robot(13, '10.10.11.90'), Robot(14, '10.10.11.89')]
+        #self.robots = [Robot(14, '10.10.11.89')]
+        self.robots = [ControlledRobot(13, '192.168.1.13')]
 
         self.robot_ids = {}
         for r in self.robots:
@@ -32,18 +47,17 @@ class RemoteController:
         self.t = time.time()
 
         # start thread for marker position detection
-        self.estimator = ArucoEstimator(self.robot_ids.keys())
-        self.estimator_thread = threading.Thread(target=self.estimator.run_tracking)
-        self.estimator_thread.start()
-
-        self.controller = MPCController(self.estimator)
+        self.controller = MPCController()
+        self.robots[0].attach_controller(self.controller)
 
     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("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)
+        #self.controller.interactive_control(robots=self.robots)
+        self.controller.start()
+        pass
 
 
 def main(args):
@@ -53,3 +67,13 @@ def main(args):
 
 if __name__ == '__main__':
     main(sys.argv)
+
+# to use the program, first start measurement_server.py, which launches the Aruco marker detection GUI and streams
+# marker positions and GUI events via socket
+# next, launch this program (mpc_test.py) in debug mode and set a breakpoint after self.controller.start()
+# you can now set the target position for the controlled robot via move_to_pos(), e.g.
+# self.robots[0].move_to_pos([0.0,0,0], True)
+
+# TODO
+# figure out how to get clicked positions from measurement server and drive robot there
+# also document and improve program structure

remote_control/robot.py

@@ -1,7 +1,9 @@
 import socket
 import threading
 import json
-
+import numpy as np
+import time
+import math
 
 class Robot:
     def __init__(self, marker_id, ip, measurement_server=('127.0.0.1', 42424)):
@@ -123,8 +125,26 @@ class ControlledRobot(Robot):
         self.controller = controller
         self.controller.attach_robot(self)
 
-    def move_to_pos(self, target_pos):
+    def move_to_pos(self, target_pos, blocking=False):
         if self.controller is not None:
+            if not blocking:
                 self.controller.set_target_position(target_pos)
+            else:   # only return after the robot has reached the target
+                self.stop_control()
+                self.controller.set_target_position(target_pos)
+                self.start_control()
+
+                close_to_target = False
+                while not close_to_target:
+                    current_pos = np.array([self.x, self.y, self.angle])
+                    v = target_pos - current_pos
+                    angles_unwrapped = np.unwrap([current_pos[2], target_pos[2]])  # unwrap angle to avoid jump in data
+                    e_angle = angles_unwrapped[0] - angles_unwrapped[1]  # angle difference
+                    e_pos = np.linalg.norm(v[0:2])
+                    print(f"e_pos = {e_pos}, e_ang = {e_angle}")
+                    close_to_target = e_pos < 0.05 and abs(e_angle) < 0.1
+                    time.sleep(0.1)
+                print("target reached!")
+                self.stop_control()
         else:
             raise Exception("Error: Cannot move to position: there is not controller attached to the robot!")