forked from Telos4/RoboRally
improved grid logic for robots + general cleanup
This commit is contained in:
parent
32892e5dcf
commit
e3753f7644
|
|
@ -1,21 +1,13 @@
|
||||||
## License: Apache 2.0. See LICENSE file in root directory.
|
|
||||||
## Copyright(c) 2015-2017 Intel Corporation. All Rights Reserved.
|
|
||||||
|
|
||||||
###############################################
|
|
||||||
## Open CV and Numpy integration ##
|
|
||||||
###############################################
|
|
||||||
|
|
||||||
import pyrealsense2 as rs
|
import pyrealsense2 as rs
|
||||||
import numpy as np
|
import numpy as np
|
||||||
import cv2
|
import cv2
|
||||||
from cv2 import aruco
|
from cv2 import aruco
|
||||||
from shapely.geometry import LineString
|
from shapely.geometry import LineString
|
||||||
|
|
||||||
import time
|
|
||||||
|
|
||||||
DS5_product_ids = ["0AD1", "0AD2", "0AD3", "0AD4", "0AD5", "0AF6", "0AFE", "0AFF", "0B00", "0B01", "0B03", "0B07","0B3A"]
|
DS5_product_ids = ["0AD1", "0AD2", "0AD3", "0AD4", "0AD5", "0AF6", "0AFE", "0AFF", "0B00", "0B01", "0B03", "0B07","0B3A"]
|
||||||
|
|
||||||
def find_device_that_supports_advanced_mode() :
|
|
||||||
|
def find_device_that_supports_advanced_mode():
|
||||||
ctx = rs.context()
|
ctx = rs.context()
|
||||||
ds5_dev = rs.device()
|
ds5_dev = rs.device()
|
||||||
devices = ctx.query_devices()
|
devices = ctx.query_devices()
|
||||||
|
|
@ -26,6 +18,7 @@ def find_device_that_supports_advanced_mode() :
|
||||||
return dev
|
return dev
|
||||||
raise Exception("No device that supports advanced mode was found")
|
raise Exception("No device that supports advanced mode was found")
|
||||||
|
|
||||||
|
|
||||||
class ArucoEstimator:
|
class ArucoEstimator:
|
||||||
grid_columns = 10
|
grid_columns = 10
|
||||||
grid_rows = 8
|
grid_rows = 8
|
||||||
|
|
@ -36,9 +29,6 @@ class ArucoEstimator:
|
||||||
'd': 3
|
'd': 3
|
||||||
}
|
}
|
||||||
|
|
||||||
robot_marker_ids = [12]
|
|
||||||
|
|
||||||
robot_marker_estimates = dict([(id, None) for id in robot_marker_ids])
|
|
||||||
angles = []
|
angles = []
|
||||||
|
|
||||||
corner_estimates = {
|
corner_estimates = {
|
||||||
|
|
@ -48,8 +38,11 @@ class ArucoEstimator:
|
||||||
'd': (None, 0)
|
'd': (None, 0)
|
||||||
}
|
}
|
||||||
|
|
||||||
def __init__(self):
|
def __init__(self, robot_marker_ids=[]):
|
||||||
if True: # check if realsense camera is connected
|
self.robot_marker_ids = robot_marker_ids
|
||||||
|
self.robot_marker_estimates = dict([(id, None) for id in self.robot_marker_ids])
|
||||||
|
|
||||||
|
if False: # check if realsense camera is connected
|
||||||
# Configure depth and color streams
|
# Configure depth and color streams
|
||||||
self.pipeline = rs.pipeline()
|
self.pipeline = rs.pipeline()
|
||||||
config = rs.config()
|
config = rs.config()
|
||||||
|
|
@ -75,9 +68,6 @@ class ArucoEstimator:
|
||||||
self.cv_camera = cv2.VideoCapture(0)
|
self.cv_camera = cv2.VideoCapture(0)
|
||||||
self.pipeline = None
|
self.pipeline = None
|
||||||
|
|
||||||
# array containing pose estimates for each marker
|
|
||||||
estimates = {}
|
|
||||||
|
|
||||||
def run_tracking(self):
|
def run_tracking(self):
|
||||||
try:
|
try:
|
||||||
while True:
|
while True:
|
||||||
|
|
@ -100,16 +90,27 @@ class ArucoEstimator:
|
||||||
corners, ids, rejectedImgPoints = aruco.detectMarkers(gray, aruco_dict, parameters=parameters)
|
corners, ids, rejectedImgPoints = aruco.detectMarkers(gray, aruco_dict, parameters=parameters)
|
||||||
frame = aruco.drawDetectedMarkers(color_image.copy(), corners, ids)
|
frame = aruco.drawDetectedMarkers(color_image.copy(), corners, ids)
|
||||||
|
|
||||||
if ids is not None:
|
if False:#ids is not None:
|
||||||
for id, c in zip(ids, corners):
|
for id, c in zip(ids, corners):
|
||||||
res = aruco.estimatePoseSingleMarkers(c, 0.10, self.camera_matrix, self.dist_coeffs)
|
res = aruco.estimatePoseSingleMarkers(c, 0.10, self.camera_matrix, self.dist_coeffs)
|
||||||
rvecs = res[0]
|
rvecs = res[0][0][0]
|
||||||
tvecs = res[1]
|
tvecs = res[1][0][0]
|
||||||
|
|
||||||
self.update_estimate(id, rvecs, tvecs)
|
self.update_estimate(id, rvecs, tvecs)
|
||||||
|
|
||||||
frame = self.draw_grid_lines(frame, corners, ids)
|
frame = self.draw_grid_lines(frame, corners, ids)
|
||||||
frame = self.draw_robot_pos(frame, corners, ids)
|
frame = self.draw_robot_pos(frame, corners, ids)
|
||||||
|
else:
|
||||||
|
# pretent we detected some markers
|
||||||
|
pass
|
||||||
|
self.update_estimate(0, rvec=np.array([0,0,0]), tvec=np.array([-1, 1, 0]))
|
||||||
|
self.update_estimate(1, rvec=np.array([0, 0, 0]), tvec=np.array([1, 1.5, 0]))
|
||||||
|
self.update_estimate(2, rvec=np.array([0, 0, 0]), tvec=np.array([1, -1.5, 0]))
|
||||||
|
self.update_estimate(3, rvec=np.array([0, 0, 0]), tvec=np.array([-1, -1, 0]))
|
||||||
|
#import random
|
||||||
|
#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]))
|
||||||
|
|
||||||
# Show images
|
# Show images
|
||||||
cv2.namedWindow('RealSense', cv2.WINDOW_AUTOSIZE)
|
cv2.namedWindow('RealSense', cv2.WINDOW_AUTOSIZE)
|
||||||
|
|
@ -117,16 +118,10 @@ class ArucoEstimator:
|
||||||
cv2.waitKey(1)
|
cv2.waitKey(1)
|
||||||
|
|
||||||
finally:
|
finally:
|
||||||
|
if self.pipeline is not None:
|
||||||
# Stop streaming
|
# Stop streaming
|
||||||
self.pipeline.stop()
|
self.pipeline.stop()
|
||||||
|
|
||||||
|
|
||||||
# import matplotlib.pyplot as plt
|
|
||||||
|
|
||||||
# plt.plot(playboard.angles)
|
|
||||||
# plt.show()
|
|
||||||
|
|
||||||
|
|
||||||
def update_estimate(self, id, rvec, tvec):
|
def update_estimate(self, id, rvec, tvec):
|
||||||
# update the marker estimate with new data
|
# update the marker estimate with new data
|
||||||
if id in self.corner_marker_ids.values():
|
if id in self.corner_marker_ids.values():
|
||||||
|
|
@ -135,7 +130,7 @@ class ArucoEstimator:
|
||||||
corner = next(filter(lambda key: self.corner_marker_ids[key] == id, self.corner_marker_ids.keys())) # get corresponding corner to the detected marker
|
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][0]
|
||||||
n_estimates = self.corner_estimates[corner][1]
|
n_estimates = self.corner_estimates[corner][1]
|
||||||
tvec_proj = tvec[0][0][0:2] # projection to get rid of z coordinate
|
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
|
tvec_proj = np.array((tvec_proj[0], -tvec_proj[1])) # flip y coordinate
|
||||||
if old_estimate is not None:
|
if old_estimate is not None:
|
||||||
new_estimate = (n_estimates * old_estimate + tvec_proj) / (n_estimates + 1) # weighted update
|
new_estimate = (n_estimates * old_estimate + tvec_proj) / (n_estimates + 1) # weighted update
|
||||||
|
|
@ -145,17 +140,17 @@ class ArucoEstimator:
|
||||||
elif id in self.robot_marker_ids:
|
elif id in self.robot_marker_ids:
|
||||||
# for robot markers we extract x and y position as well as the angle
|
# for robot markers we extract x and y position as well as the angle
|
||||||
# here we could also implement a filter
|
# here we could also implement a filter
|
||||||
x = tvec[0][0][0]
|
x = tvec[0]
|
||||||
y = -tvec[0][0][1] # flip y coordinate
|
y = -tvec[1] # flip y coordinate
|
||||||
|
|
||||||
# compute angle
|
# compute angle
|
||||||
rot_mat, _ = cv2.Rodrigues(rvec[0][0])
|
rot_mat, _ = cv2.Rodrigues(rvec)
|
||||||
pose_mat = cv2.hconcat((rot_mat, tvec[0][0]))
|
pose_mat = cv2.hconcat((rot_mat, tvec))
|
||||||
_, _, _, _, _, _, euler_angles = cv2.decomposeProjectionMatrix(pose_mat)
|
_, _, _, _, _, _, euler_angles = cv2.decomposeProjectionMatrix(pose_mat)
|
||||||
angle = -euler_angles[2][0] * np.pi / 180.0
|
angle = -euler_angles[2][0] * np.pi / 180.0
|
||||||
|
|
||||||
self.angles.append(angle)
|
self.angles.append(angle)
|
||||||
self.robot_marker_estimates[id[0]] = (x, y, angle)
|
self.robot_marker_estimates[id] = (x, y, angle)
|
||||||
|
|
||||||
def all_corners_detected(self):
|
def all_corners_detected(self):
|
||||||
# checks if all corner markers have been detected at least once
|
# checks if all corner markers have been detected at least once
|
||||||
|
|
@ -308,4 +303,4 @@ class ArucoEstimator:
|
||||||
|
|
||||||
|
|
||||||
if __name__ == "__main__":
|
if __name__ == "__main__":
|
||||||
playboard = Board()
|
estimator = ArucoEstimator()
|
||||||
|
|
|
||||||
|
|
@ -52,10 +52,21 @@ def myreceive(sock):
|
||||||
|
|
||||||
|
|
||||||
class Robot:
|
class Robot:
|
||||||
def __init__(self, id, ip, grid_pos = (0,0,0)):
|
# dictionary mapping the current orientation and a turn command to the resulting orientation
|
||||||
self.pos = None
|
resulting_orientation = {
|
||||||
self.orient = None
|
'^': {'turn left': '<', 'turn right': '>', 'turn around': 'v'},
|
||||||
self.grid_pos = grid_pos
|
'>': {'turn left': '^', 'turn right': 'v', 'turn around': '<'},
|
||||||
|
'v': {'turn left': '>', 'turn right': '<', 'turn around': '^'},
|
||||||
|
'<': {'turn left': 'v', 'turn right': '^', 'turn around': '>'},
|
||||||
|
}
|
||||||
|
|
||||||
|
# dictionary mapping an orientation to its opposite
|
||||||
|
opposites = {'^': 'v', '>': '<', 'v': '^', '<': '>'}
|
||||||
|
|
||||||
|
def __init__(self, id, ip, x=0, y=0, orientation='>'):
|
||||||
|
self.x = x
|
||||||
|
self.y = y
|
||||||
|
self.orientation = orientation
|
||||||
|
|
||||||
self.id = id
|
self.id = id
|
||||||
|
|
||||||
|
|
@ -75,17 +86,52 @@ class Robot:
|
||||||
self.u1 = 0.0
|
self.u1 = 0.0
|
||||||
self.u2 = 0.0
|
self.u2 = 0.0
|
||||||
|
|
||||||
|
def get_neighbor_coordinates(self, direction):
|
||||||
|
# get the coordinates of the neighboring tile in the given direction
|
||||||
|
if direction == '^':
|
||||||
|
return (self.x, self.y - 1)
|
||||||
|
elif direction == '>':
|
||||||
|
return (self.x + 1, self.y)
|
||||||
|
elif direction == 'v':
|
||||||
|
return (self.x, self.y + 1)
|
||||||
|
elif direction == '<':
|
||||||
|
return (self.x - 1, self.y)
|
||||||
|
else:
|
||||||
|
print("error: unknown direction")
|
||||||
|
sys.exit(1)
|
||||||
|
|
||||||
|
def move(self, type):
|
||||||
|
if type == 'forward':
|
||||||
|
target_tile = self.get_neighbor_coordinates(self.orientation)
|
||||||
|
self.x = target_tile[0]
|
||||||
|
self.y = target_tile[1]
|
||||||
|
elif type == 'backward':
|
||||||
|
opposite_orientation = Robot.opposites[self.orientation]
|
||||||
|
target_tile = self.get_neighbor_coordinates(opposite_orientation)
|
||||||
|
self.x = target_tile[0]
|
||||||
|
self.y = target_tile[1]
|
||||||
|
elif 'turn' in type:
|
||||||
|
self.orientation = Robot.resulting_orientation[self.orientation][type]
|
||||||
|
else:
|
||||||
|
print("error: invalid move")
|
||||||
|
sys.exit(1)
|
||||||
|
|
||||||
def connect(self):
|
def connect(self):
|
||||||
# connect to robot
|
# connect to robot
|
||||||
try:
|
try:
|
||||||
print("connecting to robot {} with ip {} ...".format(self.id, self.ip))
|
print("connecting to robot {} with ip {} ...".format(self.id, self.ip))
|
||||||
self.socket.connect((self.ip, 1234)) # connect to robot
|
#self.socket.connect((self.ip, 1234)) # connect to robot
|
||||||
print("connected!")
|
print("connected!")
|
||||||
except socket.error:
|
except socket.error:
|
||||||
print("could not connect to robot {} with ip {}".format(self.id, self.ip))
|
print("could not connect to robot {} with ip {}".format(self.id, self.ip))
|
||||||
sys.exit(1)
|
sys.exit(1)
|
||||||
|
|
||||||
|
def __str__(self):
|
||||||
|
return self.__repr__()
|
||||||
|
|
||||||
|
def __repr__(self):
|
||||||
|
return f"x: {self.x}, y: {self.y}, orienation: {self.orientation}"
|
||||||
|
|
||||||
def f_ode(t, x, u):
|
def f_ode(t, x, u):
|
||||||
# dynamical model of the two-wheeled robot
|
# dynamical model of the two-wheeled robot
|
||||||
# TODO: find exact values for these parameters
|
# TODO: find exact values for these parameters
|
||||||
|
|
@ -110,9 +156,6 @@ def f_ode(t, x, u):
|
||||||
|
|
||||||
class RemoteController:
|
class RemoteController:
|
||||||
def __init__(self, id, ip):
|
def __init__(self, id, ip):
|
||||||
|
|
||||||
self.anim_stopped = False
|
|
||||||
|
|
||||||
# self.robots = #[Robot(11, '192.168.1.11', (6, -3, np.pi)), Robot(12, '192.168.1.12', (6, -3, np.pi)),
|
# self.robots = #[Robot(11, '192.168.1.11', (6, -3, np.pi)), Robot(12, '192.168.1.12', (6, -3, np.pi)),
|
||||||
# Robot(13, '192.168.1.13', (6, -3, np.pi)), Robot(14, '192.168.1.14', (6, -2, np.pi))]
|
# Robot(13, '192.168.1.13', (6, -3, np.pi)), Robot(14, '192.168.1.14', (6, -2, np.pi))]
|
||||||
#self.robots = [Robot(13, '192.168.1.13', (6, -3, np.pi))]
|
#self.robots = [Robot(13, '192.168.1.13', (6, -3, np.pi))]
|
||||||
|
|
@ -120,8 +163,6 @@ class RemoteController:
|
||||||
#self.robots = [Robot(11, '192.168.1.11', (6,-3,0)), Robot(14, '192.168.1.14', (6,3,0))]
|
#self.robots = [Robot(11, '192.168.1.11', (6,-3,0)), Robot(14, '192.168.1.14', (6,3,0))]
|
||||||
#self.robots = [Robot(11, '192.168.1.11'), Robot(14, '192.168.1.14')]
|
#self.robots = [Robot(11, '192.168.1.11'), Robot(14, '192.168.1.14')]
|
||||||
self.robots = [Robot(12, '192.168.1.12')]
|
self.robots = [Robot(12, '192.168.1.12')]
|
||||||
#self.robots = [Robot(15, '192.168.1.102')]
|
|
||||||
#self.robots = [Robot(id, ip)]
|
|
||||||
|
|
||||||
self.robot_ids = {}
|
self.robot_ids = {}
|
||||||
for r in self.robots:
|
for r in self.robots:
|
||||||
|
|
@ -136,7 +177,6 @@ class RemoteController:
|
||||||
for robot in self.robots:
|
for robot in self.robots:
|
||||||
robot.connect()
|
robot.connect()
|
||||||
|
|
||||||
#self.comm_socket.bind((socket.gethostname(), 1337))
|
|
||||||
self.comm_socket.bind(('', 1337))
|
self.comm_socket.bind(('', 1337))
|
||||||
self.comm_socket.listen(5)
|
self.comm_socket.listen(5)
|
||||||
|
|
||||||
|
|
@ -147,15 +187,12 @@ class RemoteController:
|
||||||
self.ts = np.array([])
|
self.ts = np.array([])
|
||||||
self.xs = []
|
self.xs = []
|
||||||
|
|
||||||
|
|
||||||
# variable for mpc open loop
|
# variable for mpc open loop
|
||||||
self.ol_x = None
|
self.ol_x = None
|
||||||
self.ol_y = None
|
self.ol_y = None
|
||||||
|
|
||||||
self.mutex = threading.Lock()
|
|
||||||
|
|
||||||
# ROS subscriber for detected markers
|
# ROS subscriber for detected markers
|
||||||
self.estimator = opencv_viewer_example.ArucoEstimator()
|
self.estimator = opencv_viewer_example.ArucoEstimator(self.robot_ids.keys())
|
||||||
|
|
||||||
self.estimator_thread = threading.Thread(target=self.estimator.run_tracking)
|
self.estimator_thread = threading.Thread(target=self.estimator.run_tracking)
|
||||||
self.estimator_thread.start()
|
self.estimator_thread.start()
|
||||||
|
|
@ -173,41 +210,13 @@ class RemoteController:
|
||||||
# integrator
|
# integrator
|
||||||
self.r = scipy.integrate.ode(f_ode)
|
self.r = scipy.integrate.ode(f_ode)
|
||||||
self.omega_max = 5.0
|
self.omega_max = 5.0
|
||||||
self.control_scaling = 0.1
|
self.control_scaling = 0.2
|
||||||
#self.omega_max = 13.32
|
#self.omega_max = 13.32
|
||||||
|
|
||||||
|
|
||||||
def measurement_callback(self, data):
|
|
||||||
#print(data)
|
|
||||||
|
|
||||||
# detect robots
|
|
||||||
if data.id in self.robot_ids:
|
|
||||||
r = self.robot_ids[data.id]
|
|
||||||
|
|
||||||
r.pos = (data.x, data.y) # only x and y component are important for us
|
|
||||||
r.euler = data.angle
|
|
||||||
|
|
||||||
# save measured position and angle for plotting
|
|
||||||
measurement = np.array([r.pos[0], r.pos[1], r.euler])
|
|
||||||
if r.tms_0 is None:
|
|
||||||
r.tms_0 = time.time()
|
|
||||||
r.xm_0 = measurement
|
|
||||||
|
|
||||||
self.mutex.acquire()
|
|
||||||
try:
|
|
||||||
r.tms = np.array([0.0])
|
|
||||||
r.xms = measurement
|
|
||||||
finally:
|
|
||||||
self.mutex.release()
|
|
||||||
else:
|
|
||||||
self.mutex.acquire()
|
|
||||||
try:
|
|
||||||
r.tms = np.vstack((r.tms, time.time() - r.tms_0))
|
|
||||||
r.xms = np.vstack((r.xms, measurement))
|
|
||||||
finally:
|
|
||||||
self.mutex.release()
|
|
||||||
|
|
||||||
def controller(self):
|
def controller(self):
|
||||||
|
print("waiting until all markers are detected...")
|
||||||
|
while not self.estimator.all_corners_detected():
|
||||||
|
pass
|
||||||
print("starting control")
|
print("starting control")
|
||||||
|
|
||||||
running = True
|
running = True
|
||||||
|
|
@ -276,43 +285,12 @@ class RemoteController:
|
||||||
|
|
||||||
def mpc_control(self, robot_id, cmd):
|
def mpc_control(self, robot_id, cmd):
|
||||||
robot = self.robot_ids[robot_id] # get robot to be controlled
|
robot = self.robot_ids[robot_id] # get robot to be controlled
|
||||||
grid_pos = robot.grid_pos # grid position of the robot
|
|
||||||
|
|
||||||
print("old grid pos for robot {}: {}".format(robot_id, grid_pos))
|
print("robot grid pos before move: ", robot)
|
||||||
|
robot.move(cmd)
|
||||||
|
print("robot grid pos after move: ", robot)
|
||||||
|
|
||||||
# compute new grid position and orientation
|
self.target = self.estimator.get_pos_from_grid_point(robot.x, robot.y, robot.orientation)
|
||||||
if cmd == 'forward':
|
|
||||||
new_x = int(round(grid_pos[0] + 1 * np.cos(grid_pos[2])))
|
|
||||||
new_y = int(round(grid_pos[1] + 1 * np.sin(grid_pos[2])))
|
|
||||||
new_angle = grid_pos[2]
|
|
||||||
elif cmd == 'backward':
|
|
||||||
new_x = int(round(grid_pos[0] - 1 * np.cos(grid_pos[2])))
|
|
||||||
new_y = int(round(grid_pos[1] - 1 * np.sin(grid_pos[2])))
|
|
||||||
new_angle = grid_pos[2]
|
|
||||||
elif cmd == 'turn left':
|
|
||||||
new_x = grid_pos[0]
|
|
||||||
new_y = grid_pos[1]
|
|
||||||
new_angle = np.unwrap([0, grid_pos[2] + np.pi / 2])[1]
|
|
||||||
elif cmd == 'turn right':
|
|
||||||
new_x = grid_pos[0]
|
|
||||||
new_y = grid_pos[1]
|
|
||||||
new_angle = np.unwrap([0, grid_pos[2] - np.pi / 2])[1]
|
|
||||||
elif cmd == 'turn around':
|
|
||||||
new_x = grid_pos[0]
|
|
||||||
new_y = grid_pos[1]
|
|
||||||
new_angle = np.unwrap([0, grid_pos[2] + np.pi])[1]
|
|
||||||
else:
|
|
||||||
print("unknown command!")
|
|
||||||
sys.exit(1)
|
|
||||||
if new_x != grid_pos[0] and new_y != grid_pos[1]:
|
|
||||||
print("problem detected!")
|
|
||||||
|
|
||||||
grid_pos = (new_x, new_y, new_angle)
|
|
||||||
print("new grid pos for robot {}: {}\n".format(robot_id, grid_pos))
|
|
||||||
|
|
||||||
target_pos = self.estimator.get_pos_from_grid_point(new_x, new_y)
|
|
||||||
self.target = np.array((target_pos[0], target_pos[1], grid_pos[2]))
|
|
||||||
#np.array((0.25 * grid_pos[0], 0.25 * grid_pos[1], grid_pos[2]))
|
|
||||||
|
|
||||||
self.pid = False
|
self.pid = False
|
||||||
self.mpc = True
|
self.mpc = True
|
||||||
|
|
@ -358,8 +336,7 @@ class RemoteController:
|
||||||
return
|
return
|
||||||
|
|
||||||
if self.mpc:
|
if self.mpc:
|
||||||
#x_pred = self.get_measurement_prediction(robot_id)
|
x_pred = self.get_measurement(robot_id)
|
||||||
x_pred = self.get_measurement()
|
|
||||||
|
|
||||||
tmpc_start = time.time()
|
tmpc_start = time.time()
|
||||||
|
|
||||||
|
|
@ -380,14 +357,6 @@ class RemoteController:
|
||||||
us2 = res[1] * self.control_scaling
|
us2 = res[1] * self.control_scaling
|
||||||
#print("u = {}", (us1, us2))
|
#print("u = {}", (us1, us2))
|
||||||
|
|
||||||
# save open loop trajectories for plotting
|
|
||||||
self.mutex.acquire()
|
|
||||||
try:
|
|
||||||
self.ol_x = res[2]
|
|
||||||
self.ol_y = res[3]
|
|
||||||
finally:
|
|
||||||
self.mutex.release()
|
|
||||||
|
|
||||||
tmpc_end = time.time()
|
tmpc_end = time.time()
|
||||||
#print("---------------- mpc solution took {} seconds".format(tmpc_end - tmpc_start))
|
#print("---------------- mpc solution took {} seconds".format(tmpc_end - tmpc_start))
|
||||||
dt_mpc = time.time() - self.t
|
dt_mpc = time.time() - self.t
|
||||||
|
|
@ -399,57 +368,19 @@ class RemoteController:
|
||||||
us2 = [0] * self.mstep
|
us2 = [0] * self.mstep
|
||||||
|
|
||||||
near_target += 1
|
near_target += 1
|
||||||
robot.grid_pos = grid_pos
|
|
||||||
|
|
||||||
# send controls to the robot
|
# send controls to the robot
|
||||||
for i in range(0, self.mstep): # option to use multistep mpc if len(range) > 1
|
for i in range(0, self.mstep): # option to use multistep mpc if len(range) > 1
|
||||||
u1 = us1[i]
|
u1 = us1[i]
|
||||||
u2 = us2[i]
|
u2 = us2[i]
|
||||||
if self.robot_ids[robot_id].socket:
|
if self.robot_ids[robot_id].socket:
|
||||||
self.robot_ids[robot_id].socket.send('({},{})\n'.format(u1, u2).encode())
|
#self.robot_ids[robot_id].socket.send('({},{})\n'.format(u1, u2).encode())
|
||||||
if i < self.mstep:
|
if i < self.mstep:
|
||||||
time.sleep(self.dt)
|
time.sleep(self.dt)
|
||||||
self.t = time.time() # save time the most recent control was applied
|
self.t = time.time() # save time the most recent control was applied
|
||||||
|
|
||||||
|
def get_measurement(self, robot_id):
|
||||||
def get_measurement_prediction(self, robot_id):
|
return np.array(self.estimator.get_robot_state_estimate(robot_id))
|
||||||
# get measurement
|
|
||||||
self.mutex.acquire()
|
|
||||||
try:
|
|
||||||
window = 3
|
|
||||||
last_measurement = copy.deepcopy(self.robot_ids[robot_id].xms[-window:])
|
|
||||||
#print("last_measurements = {}".format(last_measurement))
|
|
||||||
#print("mean = {}".format(np.mean(last_measurement, axis=0)))
|
|
||||||
last_measurement = np.mean(last_measurement, axis=0)
|
|
||||||
last_time = copy.deepcopy(self.robot_ids[robot_id].tms[-1])
|
|
||||||
finally:
|
|
||||||
self.mutex.release()
|
|
||||||
|
|
||||||
# prediction of state at time the mpc will terminate
|
|
||||||
self.r.set_f_params(np.array([self.robot_ids[robot_id].u1 * self.omega_max, self.robot_ids[robot_id].u2 * self.omega_max]))
|
|
||||||
|
|
||||||
self.r.set_initial_value(last_measurement, last_time)
|
|
||||||
|
|
||||||
x_pred = self.r.integrate(self.r.t + self.dt)
|
|
||||||
|
|
||||||
return x_pred
|
|
||||||
|
|
||||||
def get_measurement_old(self):
|
|
||||||
self.mutex.acquire()
|
|
||||||
try:
|
|
||||||
last_measurement = copy.deepcopy(self.xms[-1:])
|
|
||||||
finally:
|
|
||||||
self.mutex.release()
|
|
||||||
return last_measurement[0]
|
|
||||||
|
|
||||||
def get_measurement(self):
|
|
||||||
return np.array(self.estimator.get_robot_state_estimate(12))
|
|
||||||
|
|
||||||
def pos_getter(self):
|
|
||||||
while True:
|
|
||||||
x_pred = self.get_measurement_prediction()
|
|
||||||
|
|
||||||
print("pos = ", x_pred)
|
|
||||||
|
|
||||||
def main(args):
|
def main(args):
|
||||||
parser = ArgumentParser()
|
parser = ArgumentParser()
|
||||||
|
|
@ -460,9 +391,6 @@ def main(args):
|
||||||
marker_id = int(args.id)
|
marker_id = int(args.id)
|
||||||
ip = args.ip
|
ip = args.ip
|
||||||
|
|
||||||
|
|
||||||
#rospy.init_node('controller_node', anonymous=True)
|
|
||||||
|
|
||||||
rc = RemoteController(marker_id, ip)
|
rc = RemoteController(marker_id, ip)
|
||||||
|
|
||||||
pygame.init()
|
pygame.init()
|
||||||
|
|
@ -471,21 +399,7 @@ def main(args):
|
||||||
screenwidth = 640
|
screenwidth = 640
|
||||||
pygame.display.set_mode([screenwidth, screenheight])
|
pygame.display.set_mode([screenwidth, screenheight])
|
||||||
|
|
||||||
# print("waiting until track is completely detected")
|
rc.controller()
|
||||||
# while not rc.track.track_complete:
|
|
||||||
# pass
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
time.sleep(1)
|
|
||||||
|
|
||||||
#threading.Thread(target=rc.input_handling).start()
|
|
||||||
controller_thread = threading.Thread(target=rc.controller)
|
|
||||||
controller_thread.start()
|
|
||||||
|
|
||||||
#time.sleep(10)
|
|
||||||
#rc.ani()
|
|
||||||
|
|
||||||
|
|
||||||
if __name__ == '__main__':
|
if __name__ == '__main__':
|
||||||
main(sys.argv)
|
main(sys.argv)
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue
Block a user