def animate_pure_pursuit(car_model, x, y, steering, target_ind):
""" Single animation update for pure pursuit."""
plt.cla() #clear last plot/frame
plot_trajectory(car_model, x, y)
# plot pure pursuit current target
plt.plot(cx[target_ind], cy[target_ind], "xg", label="target")
# plot car current state
plot_car(car_model.x, car_model.y, car_model.yaw, steering)
plt.pause(0.001)
def animate_pure_pursuit(car_model, x, y, steering, color, target_ind):
""" Single animation update for pure pursuit."""
plot_trajectory(car_model, x, y, color)
# plot pure pursuit current target
plt.plot(cx[target_ind], cy[target_ind], "xg", label="target")
# plot car current state
plot_car(car_model.x,
car_model.y,
car_model.yaw,
steering,
color)
def main():
rospy.init_node('SVEA_sim_keyboard')
# initialize simulated model and control interface
simple_bicycle_model = SimpleBicycleState(*init_state, dt=dt)
ctrl_interface = ControlInterface(vehicle_name).start()
rospy.sleep(0.5)
# start background simulation thread
simulator = SimSVEA(vehicle_name, simple_bicycle_model, dt)
simulator.start()
rospy.sleep(0.5)
# log data
x = []
y = []
yaw = []
v = []
# hook callback for updating keyboard commands
rospy.Subscriber("/key_vel", Twist, update_key_teleop)
# simualtion + animation loop
r = rospy.Rate(30) #SVEA cars operate on 30Hz!
while not rospy.is_shutdown():
# pass keyboard commands to ctrl interface
ctrl_interface.send_control(steering, velocity)
# log data
x.append(simple_bicycle_model.x)
y.append(simple_bicycle_model.y)
yaw.append(simple_bicycle_model.yaw)
v.append(simple_bicycle_model.v)
# update for animation
if show_animation:
plt.cla()
plt.plot(x, y, "-b", label="trajectory")
plot_car(simple_bicycle_model.x, simple_bicycle_model.y,
simple_bicycle_model.yaw, steering)
plt.axis("equal")
plt.grid(True)
plt.title("Heading[deg]: " +
str(round(math.degrees(simple_bicycle_model.yaw), 4)) +
" | Speed[m/s]:" + str(round(simple_bicycle_model.v, 4)))
plt.pause(0.001)
# sleep so loop runs at 30Hz
r.sleep()
def main():
rospy.init_node('SVEA_sim_teleop')
# initialize simulated model and control interface
simple_bicycle_model = SimpleBicycleState(*init_state, dt=dt)
ctrl_interface = ControlInterfaceWTeleop(vehicle_name).start()
rospy.sleep(0.5)
# start background simulation thread
simulator = SimSVEA(vehicle_name, simple_bicycle_model, dt)
simulator.start()
rospy.sleep(0.5)
# log data
x = []
y = []
yaw = []
v = []
# simualtion + animation loop
r = rospy.Rate(30) #SVEA cars operate on 30Hz!
while not rospy.is_shutdown():
# trigger controller to send teleop commands to car
ctrl_interface.send_control()
# log data
x.append(simple_bicycle_model.x)
y.append(simple_bicycle_model.y)
yaw.append(simple_bicycle_model.yaw)
v.append(simple_bicycle_model.v)
# update for animation
if show_animation:
plt.cla()
plt.plot(x, y, "-b", label="trajectory")
plot_car(simple_bicycle_model.x, simple_bicycle_model.y,
simple_bicycle_model.yaw)
plt.axis("equal")
plt.grid(True)
plt.title("Heading[deg]: " +
str(round(math.degrees(simple_bicycle_model.yaw), 4)) +
" | Speed[m/s]:" + str(round(simple_bicycle_model.v, 4)))
plt.pause(0.001)
# sleep so loop runs at 30Hz
r.sleep()