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

added accessability features (choosing ip, robot id at startup, changing max speed), remove obstacle code

Browse Source
This commit is contained in:
Simon Pirkelmann 2020-01-12 16:11:26 +01:00
parent c2fad13698
commit aab1b78e8f
1 changed files with 60 additions and 42 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

102
remote_control/position_controller.py
View File

@ -30,8 +30,12 @@ from marker_pos_angle.msg import id_pos_angle
from collections import OrderedDict
from argparse import ArgumentParser
class Robot:
def __init__(self, id, ip=None):
def __init__(self, id, ip):
self.pos = None
self.orient = None
@ -122,20 +126,18 @@ def f_ode(t, x, u):
return dx
class RemoteController:
def __init__(self):
def __init__(self, id, ip):
self.robots = [Robot(11, '192.168.1.103')]
#self.robots = [Robot(14, '192.168.1.102')]
self.anim_stopped = False
#self.robots = [Robot(14, '192.168.1.103')]
#self.robots = [Robot(15, '192.168.1.102')]
self.robots = [Robot(id, ip)]
self.robot_ids = {}
for r in self.robots:
self.robot_ids[r.id] = r
obst = [Obstacle(12, 0.275), Obstacle(10, 0.275), Obstacle(14, 0.275), Obstacle(15, 0.275)]
self.obstacles = {}
for r in obst:
self.obstacles[r.id] = r
self.track = Track()
@ -198,13 +200,6 @@ class RemoteController:
self.track_line_inner, = self.ax.plot([], [])
self.track_line_outer, = self.ax.plot([], [])
self.circles = []
for o in self.obstacles:
self.circles.append(patch.Circle((0.0, 0.0), radius=0.1, fill=False, color='red', linestyle='--'))
for s in self.circles:
self.ax.add_artist(s)
self.ax.set_xlabel('x-position')
self.ax.set_ylabel('y-position')
self.ax.grid()
@ -231,6 +226,7 @@ class RemoteController:
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()
@ -251,10 +247,13 @@ class RemoteController:
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, self.circles[0], self.circles[1],\
self.circles[2], self.circles[3],
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:
@ -306,21 +305,11 @@ class RemoteController:
else:
self.line_ol.set_data([],[])
i = 0
obst_keys = self.obstacles.keys()
for s in self.circles:
o = self.obstacles[obst_keys[i]]
i = i + 1
if o.pos is not None:
s.center = o.pos
s.radius = o.radius
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, self.circles[0], self.circles[1],self.circles[2],self.circles[3],
self.line_x, self.line_y,
def measurement_callback(self, data):
#print(data)
@ -352,11 +341,6 @@ class RemoteController:
finally:
self.mutex.release()
# detect obstacles
if data.id in self.obstacles.keys():
obst = (data.x, data.y)
self.obstacles[data.id].pos = obst
# detect track
if data.id in self.track.inner.keys() or data.id in self.track.outer.keys():
self.track.set_id(data)
@ -369,7 +353,8 @@ class RemoteController:
markers_out = self.track.outer.values()
# find targets:
lamb = 0.2
# generate waypoints
lamb = 0.5
j = 0
for i in range(0,4):
p = np.array(markers_in[i]) + lamb * (np.array(markers_out[i]) - np.array(markers_in[i]))
@ -384,6 +369,8 @@ class RemoteController:
auto_control = False
current_target = 0
control_scaling = 0.3
while True:
@ -422,8 +409,26 @@ class RemoteController:
auto_control = not auto_control
if auto_control:
self.target = targets[current_target]
elif event.key == pygame.K_PLUS:
control_scaling += 0.1
control_scaling = min(control_scaling, 1.0)
elif event.key == pygame.K_MINUS:
control_scaling -= 0.1
control_scaling = max(control_scaling, 0.3)
elif event.key == pygame.K_ESCAPE:
print("quit!")
self.controlling = False
if self.rc_socket:
self.rc_socket.send('(0.0,0.0)\n')
self.anim_stopped = True
return
elif event.key == pygame.QUIT:
sys.exit(0)
print("quit!")
self.controlling = False
if self.rc_socket:
self.rc_socket.send('(0.0,0.0)\n')
self.anim_stopped = True
return
if self.controlling:
@ -439,10 +444,13 @@ class RemoteController:
tmpc_start = time.time()
# solve mpc open loop problem
res = self.ols.solve(x_pred, self.target, self.obstacles, self.track)
res = self.ols.solve(x_pred, self.target, [], self.track)
us1 = res[0]
us2 = res[1]
#us1 = res[0]
#us2 = res[1]
us1 = res[0] * control_scaling
us2 = res[1] * control_scaling
print("u = {}", (us1, us2))
# save open loop trajectories for plotting
self.mutex.acquire()
@ -493,9 +501,18 @@ class RemoteController:
def main(args):
parser = ArgumentParser()
parser.add_argument('id', metavar='id', type=str, help='marker id of the controlled robot')
parser.add_argument('ip', metavar='ip', type=str, help='ip address of the controlled robot')
args = parser.parse_args()
marker_id = int(args.id)
ip = args.ip
rospy.init_node('controller_node', anonymous=True)
rc = RemoteController()
rc = RemoteController(marker_id, ip)
pygame.init()
@ -508,10 +525,11 @@ def main(args):
pass
#threading.Thread(target=rc.input_handling).start()
threading.Thread(target=rc.controller).start()
controller_thread = threading.Thread(target=rc.controller)
controller_thread.start()
rc.ani()
if __name__ == '__main__':
main(sys.argv)
main(sys.argv)