coupled robot control with new position estimator
parent
36f3ccbd6c
commit
149e9b3536
|
|
@ -0,0 +1,280 @@
|
|||
## 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 numpy as np
|
||||
import cv2
|
||||
from cv2 import aruco
|
||||
from shapely.geometry import LineString
|
||||
|
||||
import time
|
||||
|
||||
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
|
||||
corner_marker_ids = {
|
||||
'a': 0,
|
||||
'b': 1,
|
||||
'c': 2,
|
||||
'd': 3
|
||||
}
|
||||
|
||||
robot_marker_ids = [12]
|
||||
|
||||
robot_marker_estimates = dict([(id, None) for id in robot_marker_ids])
|
||||
angles = []
|
||||
|
||||
corner_estimates = {
|
||||
'a': (None, 0), # (estimate, n_estimates)
|
||||
'b': (None, 0),
|
||||
'c': (None, 0),
|
||||
'd': (None, 0)
|
||||
}
|
||||
|
||||
def __init__(self):
|
||||
if True: # check if realsense camera is connected
|
||||
# Configure depth and color streams
|
||||
self.pipeline = rs.pipeline()
|
||||
config = rs.config()
|
||||
# config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 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)
|
||||
# config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)
|
||||
|
||||
# Start streaming
|
||||
self.pipeline.start(config)
|
||||
|
||||
camera_intrinsics = self.pipeline.get_active_profile().get_stream(
|
||||
rs.stream.color).as_video_stream_profile().get_intrinsics()
|
||||
self.camera_matrix = np.zeros((3, 3))
|
||||
self.camera_matrix[0][0] = camera_intrinsics.fx
|
||||
self.camera_matrix[1][1] = camera_intrinsics.fy
|
||||
self.camera_matrix[0][2] = camera_intrinsics.ppx
|
||||
self.camera_matrix[1][2] = camera_intrinsics.ppy
|
||||
self.dist_coeffs = np.array(camera_intrinsics.coeffs)
|
||||
# more info: https://dev.intelrealsense.com/docs/projection-in-intel-realsense-sdk-20
|
||||
else:
|
||||
# use other camera
|
||||
self.cv_camera = cv2.VideoCapture(0)
|
||||
self.pipeline = None
|
||||
|
||||
# array containing pose estimates for each marker
|
||||
estimates = {}
|
||||
|
||||
def run_tracking(self):
|
||||
try:
|
||||
while True:
|
||||
if self.pipeline:
|
||||
frames = self.pipeline.wait_for_frames()
|
||||
color_frame = frames.get_color_frame()
|
||||
if not color_frame:
|
||||
continue
|
||||
# Convert images to numpy arrays
|
||||
color_image = np.asanyarray(color_frame.get_data())
|
||||
else:
|
||||
# Capture frame-by-frame
|
||||
ret, color_image = self.cv_camera.read()
|
||||
|
||||
gray = cv2.cvtColor(color_image, cv2.COLOR_BGR2GRAY)
|
||||
|
||||
# aruco_dict = aruco.Dictionary_get(aruco.DICT_5X5_250)
|
||||
aruco_dict = aruco.Dictionary_get(aruco.DICT_ARUCO_ORIGINAL)
|
||||
parameters = aruco.DetectorParameters_create()
|
||||
corners, ids, rejectedImgPoints = aruco.detectMarkers(gray, aruco_dict, parameters=parameters)
|
||||
frame = aruco.drawDetectedMarkers(color_image.copy(), corners, ids)
|
||||
|
||||
if ids is not None:
|
||||
for id, c in zip(ids, corners):
|
||||
res = aruco.estimatePoseSingleMarkers(c, 0.10, self.camera_matrix, self.dist_coeffs)
|
||||
rvecs = res[0]
|
||||
tvecs = res[1]
|
||||
|
||||
self.update_estimate(id, rvecs, tvecs)
|
||||
|
||||
frame = self.draw_grid_lines(frame, corners, ids)
|
||||
frame = self.draw_robot_pos(frame, corners, ids)
|
||||
|
||||
# Show images
|
||||
cv2.namedWindow('RealSense', cv2.WINDOW_AUTOSIZE)
|
||||
cv2.imshow('RealSense', frame)
|
||||
cv2.waitKey(1)
|
||||
|
||||
finally:
|
||||
# Stop streaming
|
||||
self.pipeline.stop()
|
||||
|
||||
|
||||
# import matplotlib.pyplot as plt
|
||||
|
||||
# plt.plot(playboard.angles)
|
||||
# plt.show()
|
||||
|
||||
|
||||
def update_estimate(self, id, 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]
|
||||
n_estimates = self.corner_estimates[corner][1]
|
||||
tvec_proj = tvec[0][0][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)
|
||||
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
|
||||
x = tvec[0][0][0]
|
||||
y = -tvec[0][0][1] # flip y coordinate
|
||||
|
||||
# compute angle
|
||||
rot_mat, _ = cv2.Rodrigues(rvec[0][0])
|
||||
pose_mat = cv2.hconcat((rot_mat, tvec[0][0]))
|
||||
_, _, _, _, _, _, euler_angles = cv2.decomposeProjectionMatrix(pose_mat)
|
||||
angle = -euler_angles[2][0] * np.pi / 180.0
|
||||
|
||||
self.angles.append(angle)
|
||||
self.robot_marker_estimates[id[0]] = (x, y, angle)
|
||||
|
||||
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()])
|
||||
|
||||
def get_pos_from_grid_point(self, x, y):
|
||||
# returns the position for the given grid point based on the current corner estimates
|
||||
assert x >= 0 and x < self.grid_columns
|
||||
assert y >= 0 and y < self.grid_rows
|
||||
assert self.all_corners_detected()
|
||||
|
||||
# compute column line
|
||||
a = self.corner_estimates['a'][0]
|
||||
b = self.corner_estimates['b'][0]
|
||||
c = self.corner_estimates['c'][0]
|
||||
d = self.corner_estimates['d'][0]
|
||||
vab = b - a
|
||||
vdc = c - d
|
||||
column_line_top = a + (x + 0.5)/self.grid_columns * vab
|
||||
column_line_bottom = d + (x + 0.5) / self.grid_columns * vdc
|
||||
|
||||
vad = d - a
|
||||
vbc = c - b
|
||||
row_line_top = a + (y + 0.5)/self.grid_rows * vad
|
||||
row_line_bottom = b + (y + 0.5) / self.grid_rows * vbc
|
||||
|
||||
column_line = LineString([column_line_top, column_line_bottom])
|
||||
row_line = LineString([row_line_top, row_line_bottom])
|
||||
|
||||
int_pt = column_line.intersection(row_line)
|
||||
point_of_intersection = np.array([int_pt.x, int_pt.y])
|
||||
|
||||
return point_of_intersection
|
||||
|
||||
def get_grid_point_from_pos(self):
|
||||
# return the nearest grid point for the given position estimate
|
||||
pass
|
||||
|
||||
def print_corner_estimates(self):
|
||||
for key, value in self.corner_estimates.items():
|
||||
if value[0] is not None:
|
||||
print("corner marker {} at pos {}".format(key, value[0]))
|
||||
print()
|
||||
|
||||
def draw_corner_line(self, frame, corner_1, corner_2, corner_coords_dict):
|
||||
# draws a line between the given markers onto the given frame
|
||||
if corner_1 in corner_coords_dict and corner_2 in corner_coords_dict:
|
||||
frame = cv2.line(frame, corner_coords_dict[corner_1], corner_coords_dict[corner_2], color=(0, 0, 255),
|
||||
thickness=2)
|
||||
return frame
|
||||
|
||||
def draw_grid_lines(self, frame, corners, ids):
|
||||
# draws a grid onto the given frame
|
||||
board_corners_pixel_coords = {}
|
||||
for corner, id in self.corner_marker_ids.items():
|
||||
try:
|
||||
ind, _ = np.where(ids == id) # find index
|
||||
ind = ind[0]
|
||||
board_corners_pixel_coords[corner] = tuple(np.mean(corners[ind][0], axis=0))
|
||||
except IndexError:
|
||||
pass
|
||||
|
||||
frame = self.draw_corner_line(frame, 'a', 'b', board_corners_pixel_coords)
|
||||
frame = self.draw_corner_line(frame, 'b', 'c', board_corners_pixel_coords)
|
||||
frame = self.draw_corner_line(frame, 'c', 'd', board_corners_pixel_coords)
|
||||
frame = self.draw_corner_line(frame, 'd', 'a', board_corners_pixel_coords)
|
||||
|
||||
if set(board_corners_pixel_coords.keys()) == set(self.corner_marker_ids.keys()): # all markers have been detected
|
||||
# compute column line
|
||||
a = np.array(board_corners_pixel_coords['a'])
|
||||
b = np.array(board_corners_pixel_coords['b'])
|
||||
c = np.array(board_corners_pixel_coords['c'])
|
||||
d = np.array(board_corners_pixel_coords['d'])
|
||||
|
||||
vab = b - a
|
||||
vdc = c - d
|
||||
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),
|
||||
thickness=1)
|
||||
|
||||
vad = d - a
|
||||
vbc = c - b
|
||||
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),
|
||||
thickness=1)
|
||||
|
||||
return frame
|
||||
|
||||
def get_robot_state_estimate(self, id):
|
||||
if id in self.robot_marker_estimates:
|
||||
if self.robot_marker_estimates[id] is not None:
|
||||
return self.robot_marker_estimates[id]
|
||||
else:
|
||||
print(f"error: no estimate available for robot {id}")
|
||||
else:
|
||||
print(f"error: invalid robot id {id}")
|
||||
|
||||
def draw_robot_pos(self, frame, corners, ids):
|
||||
# draws information about the robot positions onto the given frame
|
||||
robot_corners_pixel_coords = {}
|
||||
for id, estimate in self.robot_marker_estimates.items():
|
||||
try:
|
||||
ind, _ = np.where(ids == id) # find index
|
||||
ind = ind[0]
|
||||
robot_corners_pixel_coords[id] = tuple(np.mean(corners[ind][0], axis=0))
|
||||
except IndexError:
|
||||
pass
|
||||
|
||||
for id, coord in robot_corners_pixel_coords.items():
|
||||
x = self.robot_marker_estimates[id][0]
|
||||
y = self.robot_marker_estimates[id][1]
|
||||
angle = self.robot_marker_estimates[id][2]
|
||||
frame = cv2.putText(frame, "pos = ({:5.3f}, {:5.3f}), ang = {:5.3f}".format(x, y, angle), coord, cv2.FONT_HERSHEY_SIMPLEX, 0.50, (0,255,0))
|
||||
return frame
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
playboard = Board()
|
||||
|
|
@ -26,34 +26,36 @@ import time
|
|||
|
||||
from casadi_opt import OpenLoopSolver
|
||||
|
||||
from marker_pos_angle.msg import id_pos_angle
|
||||
#from marker_pos_angle.msg import id_pos_angle
|
||||
|
||||
from collections import OrderedDict
|
||||
|
||||
from argparse import ArgumentParser
|
||||
|
||||
import opencv_viewer_example
|
||||
|
||||
MSGLEN = 64
|
||||
def myreceive(sock):
|
||||
chunks = []
|
||||
bytes_recd = 0
|
||||
while bytes_recd < MSGLEN:
|
||||
chunk = sock.recv(min(MSGLEN - bytes_recd, 2048))
|
||||
chunk = sock.recv(1)
|
||||
if chunk == b'':
|
||||
raise RuntimeError("socket connection broken")
|
||||
chunks.append(chunk)
|
||||
bytes_recd = bytes_recd + len(chunk)
|
||||
|
||||
if chunk[-1] == '\n':
|
||||
if chunks[-1] == b'\n':
|
||||
break
|
||||
|
||||
return b''.join(chunks)
|
||||
|
||||
|
||||
class Robot:
|
||||
def __init__(self, id, ip):
|
||||
def __init__(self, id, ip, grid_pos = (0,0,0)):
|
||||
self.pos = None
|
||||
self.orient = None
|
||||
self.grid_pos = (0,0,0)
|
||||
self.grid_pos = grid_pos
|
||||
|
||||
self.id = id
|
||||
|
||||
|
|
@ -111,8 +113,13 @@ class RemoteController:
|
|||
|
||||
self.anim_stopped = False
|
||||
|
||||
self.robots = [Robot(11, '192.168.1.11'), Robot(14, '192.168.1.14')]
|
||||
#self.robots = [Robot(11, '192.168.1.101')]
|
||||
# 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))]
|
||||
#self.robots = [Robot(13, '192.168.1.13', (6, -3, np.pi))]
|
||||
#self.robots = []
|
||||
#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(12, '192.168.1.12')]
|
||||
#self.robots = [Robot(15, '192.168.1.102')]
|
||||
#self.robots = [Robot(id, ip)]
|
||||
|
||||
|
|
@ -148,42 +155,14 @@ class RemoteController:
|
|||
self.mutex = threading.Lock()
|
||||
|
||||
# ROS subscriber for detected markers
|
||||
marker_sub = rospy.Subscriber("/marker_id_pos_angle", id_pos_angle, self.measurement_callback)
|
||||
self.estimator = opencv_viewer_example.ArucoEstimator()
|
||||
|
||||
self.estimator_thread = threading.Thread(target=self.estimator.run_tracking)
|
||||
self.estimator_thread.start()
|
||||
|
||||
# pid parameters
|
||||
self.controlling = False
|
||||
|
||||
# animation
|
||||
self.fig = plt.figure()
|
||||
self.ax = self.fig.add_subplot(2,2,1)
|
||||
self.ax2 = self.fig.add_subplot(2, 2, 2)
|
||||
self.ax3 = self.fig.add_subplot(2, 2, 4)
|
||||
|
||||
self.xdata, self.ydata = [], []
|
||||
self.line, = self.ax.plot([],[], color='grey', linestyle=':')
|
||||
self.line_sim, = self.ax.plot([], [])
|
||||
self.line_ol, = self.ax.plot([],[], color='green', linestyle='--')
|
||||
self.dirm, = self.ax.plot([], [])
|
||||
self.dirs, = self.ax.plot([], [])
|
||||
|
||||
self.line_x, = self.ax2.plot([],[])
|
||||
self.line_y, = self.ax3.plot([], [])
|
||||
|
||||
self.track_line_inner, = self.ax.plot([], [])
|
||||
self.track_line_outer, = self.ax.plot([], [])
|
||||
|
||||
self.ax.set_xlabel('x-position')
|
||||
self.ax.set_ylabel('y-position')
|
||||
self.ax.grid()
|
||||
|
||||
self.ax2.set_xlabel('Zeit t')
|
||||
self.ax2.set_ylabel('x-position')
|
||||
self.ax2.grid()
|
||||
|
||||
self.ax3.set_xlabel('Zeit t')
|
||||
self.ax3.set_ylabel('y-position')
|
||||
self.ax3.grid()
|
||||
|
||||
self.mstep = 2
|
||||
self.ols = OpenLoopSolver(N=20, T=1.0)
|
||||
self.ols.setup()
|
||||
|
|
@ -194,96 +173,10 @@ class RemoteController:
|
|||
# integrator
|
||||
self.r = scipy.integrate.ode(f_ode)
|
||||
self.omega_max = 5.0
|
||||
self.control_scaling = 0.3
|
||||
self.control_scaling = 0.1
|
||||
#self.omega_max = 13.32
|
||||
|
||||
|
||||
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()
|
||||
plt.show(block=True)
|
||||
|
||||
def ani_init(self):
|
||||
self.ax.set_xlim(-2, 2)
|
||||
self.ax.set_ylim(-2, 2)
|
||||
self.ax.set_aspect('equal', adjustable='box')
|
||||
|
||||
self.ax2.set_ylim(-2, 2)
|
||||
self.ax2.set_xlim(0, 10)
|
||||
|
||||
self.ax3.set_ylim(-2, 2)
|
||||
self.ax3.set_xlim(0, 10)
|
||||
|
||||
self.track_line_inner.set_data(self.track.inner_poly.exterior.xy)
|
||||
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,
|
||||
|
||||
def ani_update(self, frame):
|
||||
if self.anim_stopped:
|
||||
self.ani.event_source.stop()
|
||||
sys.exit(0)
|
||||
|
||||
#print("plotting")
|
||||
self.mutex.acquire()
|
||||
try:
|
||||
# copy data for plot from global arrays
|
||||
if self.tms is not None:
|
||||
tm_local = deepcopy(self.tms)
|
||||
xm_local = deepcopy(self.xms)
|
||||
|
||||
if len(tm_local) > 0:
|
||||
# plot path of the robot
|
||||
self.line.set_data(xm_local[:,0], xm_local[:,1])
|
||||
|
||||
# compute and plot direction the robot is facing
|
||||
a = xm_local[-1, 0]
|
||||
b = xm_local[-1, 1]
|
||||
|
||||
a2 = a + np.cos(xm_local[-1, 2]) * 0.2
|
||||
b2 = b + np.sin(xm_local[-1, 2]) * 0.2
|
||||
|
||||
self.dirm.set_data(np.array([a, a2]), np.array([b, b2]))
|
||||
|
||||
n_plot = 300
|
||||
if len(tm_local) > n_plot:
|
||||
# plot x and y coordinate
|
||||
self.line_x.set_data(tm_local[-n_plot:] - (tm_local[-1] - 10), xm_local[-n_plot:,0])
|
||||
self.line_y.set_data(tm_local[-n_plot:] - (tm_local[-1] - 10), xm_local[-n_plot:, 1])
|
||||
|
||||
ts_local = deepcopy(self.ts)
|
||||
xs_local = deepcopy(self.xs)
|
||||
|
||||
if len(ts_local) > 0:
|
||||
# plot simulated path of the robot
|
||||
self.line_sim.set_data(xs_local[:,0], xs_local[:,1])
|
||||
|
||||
# compute and plot direction the robot is facing
|
||||
a = xs_local[-1, 0]
|
||||
b = xs_local[-1, 1]
|
||||
|
||||
a2 = a + np.cos(xs_local[-1, 2]) * 0.2
|
||||
b2 = b + np.sin(xs_local[-1, 2]) * 0.2
|
||||
|
||||
self.dirs.set_data(np.array([a, a2]), np.array([b, b2]))
|
||||
|
||||
ol_x_local = deepcopy(self.ol_x)
|
||||
ol_y_local = deepcopy(self.ol_y)
|
||||
|
||||
if ol_x_local is not None:
|
||||
self.line_ol.set_data(ol_x_local, ol_y_local)
|
||||
else:
|
||||
self.line_ol.set_data([],[])
|
||||
|
||||
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,
|
||||
|
||||
def measurement_callback(self, data):
|
||||
#print(data)
|
||||
|
||||
|
|
@ -330,7 +223,7 @@ class RemoteController:
|
|||
|
||||
inputs = data.split(b',')
|
||||
cmd = inputs[0]
|
||||
cmd = cmd.strip()
|
||||
cmd = cmd.strip().decode()
|
||||
|
||||
if len(inputs) > 1:
|
||||
if cmd in self.valid_cmds:
|
||||
|
|
@ -341,13 +234,14 @@ class RemoteController:
|
|||
clientsocket.sendall(b'Could not read robot id!\n')
|
||||
|
||||
if robot_id in self.robot_ids:
|
||||
if cmd == 'get position':
|
||||
if cmd == b'get position':
|
||||
clientsocket.sendall(bytes('{}\n'.format(self.robot_ids[robot_id].grid_pos)))
|
||||
elif cmd == 'set position':
|
||||
elif cmd == b'set position':
|
||||
try:
|
||||
pos_data = ",".join(inputs[2:])
|
||||
new_grid_pos = tuple(map(lambda x: int(x[1]) if x[0] < 2 else float(x[1]), enumerate(pos_data.strip().strip('()').split(','))))
|
||||
self.robot_ids[robot_id].grid_pos = new_grid_pos
|
||||
clientsocket.sendall(b'OK\n')
|
||||
except IndexError as e:
|
||||
print("could not set grid position!")
|
||||
clientsocket.sendall(bytes(
|
||||
|
|
@ -366,7 +260,7 @@ class RemoteController:
|
|||
else:
|
||||
clientsocket.sendall(b'Invalid command!\n')
|
||||
else: # len(inputs) <= 1
|
||||
if 'quit' in inputs[0]:
|
||||
if b'quit' in inputs[0]:
|
||||
clientsocket.close()
|
||||
self.comm_socket.close()
|
||||
connected = False
|
||||
|
|
@ -375,8 +269,6 @@ class RemoteController:
|
|||
print("could not process command!")
|
||||
clientsocket.sendall(b'Could not process request!\n')
|
||||
|
||||
|
||||
|
||||
except RuntimeError:
|
||||
print("disconnected")
|
||||
connected = False
|
||||
|
|
@ -418,7 +310,9 @@ class RemoteController:
|
|||
grid_pos = (new_x, new_y, new_angle)
|
||||
print("new grid pos for robot {}: {}\n".format(robot_id, grid_pos))
|
||||
|
||||
self.target = np.array((0.25 * grid_pos[0], 0.25 * grid_pos[1], grid_pos[2]))
|
||||
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.mpc = True
|
||||
|
|
@ -464,7 +358,8 @@ class RemoteController:
|
|||
return
|
||||
|
||||
if self.mpc:
|
||||
x_pred = self.get_measurement_prediction(robot_id)
|
||||
#x_pred = self.get_measurement_prediction(robot_id)
|
||||
x_pred = self.get_measurement()
|
||||
|
||||
tmpc_start = time.time()
|
||||
|
||||
|
|
@ -474,7 +369,8 @@ class RemoteController:
|
|||
#print("error pos = ", error_pos)
|
||||
#print("error_pos = {}, error_ang = {}".format(error_pos, error_ang))
|
||||
|
||||
if error_pos > 0.075 or error_ang > 0.35:
|
||||
#if error_pos > 0.075 or error_ang > 0.35:
|
||||
if error_pos > 0.05 or error_ang > 0.2:
|
||||
# solve mpc open loop problem
|
||||
res = self.ols.solve(x_pred, self.target)
|
||||
|
||||
|
|
@ -510,7 +406,7 @@ class RemoteController:
|
|||
u1 = us1[i]
|
||||
u2 = us2[i]
|
||||
if self.robot_ids[robot_id].socket:
|
||||
self.robot_ids[robot_id].socket.send('({},{})\n'.format(u1, u2))
|
||||
self.robot_ids[robot_id].socket.send('({},{})\n'.format(u1, u2).encode())
|
||||
if i < self.mstep:
|
||||
time.sleep(self.dt)
|
||||
self.t = time.time() # save time the most recent control was applied
|
||||
|
|
@ -538,7 +434,7 @@ class RemoteController:
|
|||
|
||||
return x_pred
|
||||
|
||||
def get_measurement(self):
|
||||
def get_measurement_old(self):
|
||||
self.mutex.acquire()
|
||||
try:
|
||||
last_measurement = copy.deepcopy(self.xms[-1:])
|
||||
|
|
@ -546,6 +442,9 @@ class RemoteController:
|
|||
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()
|
||||
|
|
@ -562,7 +461,7 @@ def main(args):
|
|||
ip = args.ip
|
||||
|
||||
|
||||
rospy.init_node('controller_node', anonymous=True)
|
||||
#rospy.init_node('controller_node', anonymous=True)
|
||||
|
||||
rc = RemoteController(marker_id, ip)
|
||||
|
||||
|
|
@ -576,6 +475,10 @@ def main(args):
|
|||
# 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()
|
||||
|
|
|
|||
Loading…
Reference in New Issue