Telos4
/
RoboRally
1
0
Fork 1
Code Issues Pull Requests Releases Wiki Activity

some cleanup

master
Simon Pirkelmann 2020-11-17 21:23:22 +01:00
parent 35b6c3bdfd
commit edc3e8d290
6 changed files with 26 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
remote_control/aruco_estimator.py
View File

@ -10,6 +10,7 @@ from queue import Queue
import aruco
class ArucoEstimator:
corner_marker_ids = {
'a': 0,
@ -43,7 +44,7 @@ class ArucoEstimator:
# Configure depth and color streams
self.pipeline = rs.pipeline()
config = rs.config()
#config.enable_stream(rs.stream.color, 1920, 1080, rs.format.bgr8, 30)
# config.enable_stream(rs.stream.color, 1920, 1080, rs.format.bgr8, 30)
config.enable_stream(rs.stream.color, 1280, 720, rs.format.bgr8, 30)
# Start streaming
@ -69,10 +70,6 @@ class ArucoEstimator:
# create detector and get parameters
self.detector = aruco.MarkerDetector()
# self.detector.setDictionary('ARUCO_MIP_36h12')
#self.detector.setDictionary('ARUCO_MIP_16h3')
# self.detector.setDictionary('ARUCO')
#self.detector.setDetectionMode(aruco.DM_NORMAL, 0.05)
self.detector.setDetectionMode(aruco.DM_VIDEO_FAST, 0.05)
self.detector_params = self.detector.getParameters()
@ -114,8 +111,6 @@ class ArucoEstimator:
target_x = x1 + alpha * (x3 - x1)
target_y = y1 + beta * (y3 - y1)
target = np.array([target_x, target_y])
#print(f"target = ({target_x},{target_y})")
else:
print("not all markers have been detected!")
target = np.array([px, -py])

11
remote_control/controller.py
View File

@ -1,6 +1,7 @@
import threading
import time
class ControllerBase:
def __init__(self, control_rate=0.1):
self.robot = None
@ -20,7 +21,7 @@ class ControllerBase:
self.apply_control(control)
time.sleep(self.control_rate)
self.apply_control((0.0,0.0)) # stop robot
self.apply_control((0.0, 0.0)) # stop robot
def set_target_position(self, target_pos):
self.target_pos = target_pos
@ -29,16 +30,18 @@ class ControllerBase:
if self.robot is not None:
return self.robot.get_measurement()
else:
raise Exception("error: controller cannot get measurement!\n there is no robot attached to the controller!")
raise Exception("error: controller cannot get measurement!\n"
" there is no robot attached to the controller!")
def compute_control(self, state):
return (0.0, 0.0)
return 0.0, 0.0
def apply_control(self, control):
if self.robot is not None:
self.robot.send_cmd(u1=control[0], u2=control[1])
else:
raise Exception("error: controller cannot apply control!\n there is no robot attached to the controller!")
raise Exception("error: controller cannot apply control!\n"
" there is no robot attached to the controller!")
def attach_robot(self, robot):
self.robot = robot

2
remote_control/event_listener.py
View File

@ -22,7 +22,7 @@ class EventListener:
self.event_socket.settimeout(0.1)
# check if we receive data from the event server
response = self.event_socket.recv(1024)
if not 'error' in str(response):
if 'error' not in str(response):
print("... connected! -> start listening for events")
self.event_socket.settimeout(None)
# if so we start the event thread

14
remote_control/measurement_server.py
View File

@ -8,12 +8,12 @@ from aruco_estimator import ArucoEstimator
class MeasurementHandler(socketserver.BaseRequestHandler):
def handle(self) -> None:
self.data = self.request.recv(1024).strip()
data = self.request.recv(1024).strip()
cur_thread = threading.current_thread()
print(f"current thread {cur_thread}")
if 'events' in self.data.decode():
if 'events' in data.decode():
self.request.sendall('subscribed to events\n'.encode())
# send input events
while True:
@ -26,24 +26,27 @@ class MeasurementHandler(socketserver.BaseRequestHandler):
else:
# send robot position
try:
marker_id = int(self.data)
marker_id = int(data)
except ValueError:
marker_id = None
if marker_id is not None:
if marker_id in self.server.estimator.robot_marker_estimates:
while True:
self.request.sendall((json.dumps(self.server.estimator.robot_marker_estimates[marker_id]) + '\n').encode())
self.request.sendall((json.dumps(self.server.estimator.robot_marker_estimates[marker_id])
+ '\n').encode())
time.sleep(1.0 / self.server.max_measurements_per_second)
else:
self.request.sendall("error: unknown robot marker id\n".encode())
else:
self.request.sendall("error: data not understood. expected <robot marker id (int)> or 'events'\n".encode())
self.request.sendall("error: data not understood. "
"expected <robot marker id (int)> or 'events'\n".encode())
return
class MeasurementServer(socketserver.ThreadingMixIn, socketserver.TCPServer):
allow_reuse_address = True
def __init__(self, server_address, RequestHandlerClass, estimator, max_measurements_per_second=30):
super().__init__(server_address, RequestHandlerClass)
self.estimator = estimator
@ -52,6 +55,7 @@ class MeasurementServer(socketserver.ThreadingMixIn, socketserver.TCPServer):
def handle_error(self, request, client_address):
print("an error occurred -> terminating connection")
if __name__ == "__main__":
aruco_estimator = ArucoEstimator(use_realsense=True, robot_marker_ids=[11, 12, 13, 14])
estimator_thread = threading.Thread(target=aruco_estimator.run_tracking, kwargs={'invert_grayscale': True})

6
remote_control/pid_controller.py
View File

@ -4,6 +4,7 @@ import time
from controller import ControllerBase
class PIDController(ControllerBase):
def __init__(self):
super().__init__()
@ -47,7 +48,6 @@ class PIDController(ControllerBase):
else:
dt = time.time() - self.t
#print(f"x_pred = {x_pred}\ntarget = {target_pos}\nerror = {np.linalg.norm(target_pos - x_pred)}\n")
if self.mode == 'angle':
# compute angle such that robot faces to target point
target_angle = self.target_pos[2]
@ -56,7 +56,6 @@ class PIDController(ControllerBase):
e_angle = angles_unwrapped[0] - angles_unwrapped[1] # angle difference
p = self.P_angle * e_angle
# i += self.I * dt * e # right Riemann sum
self.i += self.I_angle * dt * (e_angle + self.e_angle_old)/2.0 # trapezoidal rule
d = self.D_angle * (e_angle - self.e_angle_old)/dt
@ -113,6 +112,5 @@ class PIDController(ControllerBase):
else:
u1 = 0.0
u2 = 0.0
#print(f"u = ({u1}, {u2})")
self.t = time.time() # save time when the most recent control was applied
return (u1, u2)
return u1, u2

14
remote_control/robot.py
View File

@ -25,10 +25,10 @@ class Robot:
self.measurement_server = measurement_server
self.measurement_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM) # TCP socket
self.measurement_thread = threading.Thread(target=self.receive_measurements)
self.measurement_thread.daemon = True # mark thread as daemon -> it terminates automatically when program shuts down
# mark thread as daemon -> it terminates automatically when program shuts down
self.measurement_thread.daemon = True
self.receiving = False
def connect(self):
# connect to robot
try:
@ -49,7 +49,7 @@ class Robot:
self.measurement_socket.settimeout(0.1)
# check if we receive data from the measurement server
response = self.measurement_socket.recv(1024)
if not 'error' in str(response):
if 'error' not in str(response):
print("... connected! -> start listening for measurements")
self.measurement_socket.settimeout(None)
# if so we start the measurement thread
@ -89,17 +89,13 @@ class Robot:
self.receiving = False
print(f"measurement server stopped sending data for robot {self.id}")
def stop_measurement_thread(self):
self.receiving = False
self.measurement_thread.join()
def get_measurement(self):
return (self.t_last_measurement, self.x, self.y, self.angle)
return self.t_last_measurement, self.x, self.y, self.angle
def __str__(self):
connection_state = '' if self.connected else 'not'
state = self.get_measurement()
last_measurement = f'last measurement = {state}' if not None in state else 'no measurements available'
last_measurement = f'last measurement = {state}' if None not in state else 'no measurements available'
return f"Robot {self.id}: ip = {self.ip}:{self.port} ({connection_state} connected) " + last_measurement
def __repr__(self):