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