added accessability features (choosing ip, robot id at startup, changing max speed), remove obstacle code
This commit is contained in:
parent
c2fad13698
commit
aab1b78e8f
|
|
@ -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]))
|
||||
|
|
@ -385,6 +370,8 @@ class RemoteController:
|
|||
auto_control = False
|
||||
current_target = 0
|
||||
|
||||
control_scaling = 0.3
|
||||
|
||||
while True:
|
||||
|
||||
# open loop controller
|
||||
|
|
@ -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,7 +525,8 @@ 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()
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue
Block a user