def stanley_steering_simulation(waypts):
'''Stanley steering simulation.
Args:
waypts: The waypoints in which the robot should move through.
'''
# Vehicle parameters
v = 0.4 # Speed
delta_max = 15 * math.pi / 180 # max steering angle
k = 0.25 # Gain
l = 0.238 # Car length
# Initial conditions
x0 = np.array([0, 45 * math.pi / 180, 0])
# Setup the simulation time
Tmax = 60*1.5
dt = 0.001
T = np.arange(0, Tmax, dt)
# Setup simulation
xd = np.zeros((len(T) - 1, 3)) # Derivative of state
x = np.zeros((len(T), 3)) # State
x[0, :] = x0 # Initial condition
delta = np.zeros((len(T), 1))
p = np.zeros((len(T), 2))
p[0, :] = [0, 0] # Initial position
waypt = 0
for i in xrange(len(T) - 1):
# Steering calculation
steering = controller.stanley_steering(
waypts[waypt:waypt + 3],
p[i, :],
x[i, 1],
v,
k,
)
waypt = waypt + steering['waypt']
delta[i] = max(-delta_max, min(delta_max, steering['angle']))
# PLEASE MAKE SURE THIS IS CORRECT AND ALL
# State derivatives
xd[i, 0] = v * np.sin(x[i, 1] - delta[i])
xd[i, 1] = -(v * np.sin(delta[i])) / l
# State update
x[i + 1, 0] = x[i, 0] + dt * xd[i, 0]
x[i + 1, 1] = x[i, 1] + dt * xd[i, 1]
# Position update
p[i + 1, 0] = p[i, 0] + dt * v * np.cos(x[i, 1] - delta[i])
p[i + 1, 1] = p[i, 1] + dt * v * np.sin(x[i, 1] - delta[i])
## Plotting
# Trajectory
pyplot.figure(1)
pyplot.clf()
pyplot.hold(True)
pyplot.plot(x0[0] * np.sin(x0[1]), x0[0] * np.cos(x0[1]))
pyplot.plot(p[:, 0], p[:, 1], 'r')
pyplot.plot([pt[0] for pt in waypts], [pt[1] for pt in waypts], 'go')
for t in xrange(0, len(T), 300):
draw.drawbox(p[t, 0], p[t, 1], x[t, 1], .3, 1);
pyplot.xlabel('x (m)')
pyplot.ylabel('y (m)')
pyplot.axis('equal')
pyplot.show()
t=msg.header.stamp.to_sec(),
prev_t=ekf_data['prev_t'],
**EKF_CONSTS_GPS) # Q, H, R, G, mu_p
csv_writers['ekf'].writerow([msg.header.stamp, ekf_data['mu'], ekf_data['S'], msg.latitude, msg.longitude, msg.track, 'GPS'])
csv_writers['ekf_est'].writerow(ekf_data['mu'])
elif callback_type == GPS_STATUS:
# GPS Status Message Processing
rospy.loginfo("GPS:I got: [%c] as number of visible satellites",
msg.satellites_visible)
# Steering controller
if not pid_data['prev_velocity'] is None:
steering_angle = controller.stanley_steering(
waypts,
(ekf_data['mu'][0], ekf_data['mu'][1]),
ekf_data['mu'][2],
pid_data['prev_velocity'],
0.25)
steering_angle['angle'] = max(-1.0/4, min(1.0/4, steering_angle['angle']))
# Set the motor commands with stiction and steering angle correction
vel_cmd.linear.x = max(
- 100,
min(100,
(pid_data['linear_velocity_cmd'] + VELOCITY_STICTION_OFFSET) if
pid_data['linear_velocity_cmd'] > 0 else
(pid_data['linear_velocity_cmd'] - VELOCITY_STICTION_OFFSET) if
(pid_data['linear_velocity_cmd'] < 0) else 0))
vel_cmd.angular.z = max(-100, min(100, steering_angle['angle'] * 400 - 3))
# Publish velocity command
