class LongController(object): def __init__(self, vehicle_mass, brake_deadband, decel_limit, accel_limit, wheel_radius): self.n_wheel_accel = 2 # 2 wheel drive self.n_wheel_deccel = 4 # 4 wheel brake self.vehicle_mass = vehicle_mass self.brake_deadband = brake_deadband self.decel_limit = decel_limit self.accel_limit = accel_limit self.wheel_radius = wheel_radius self.max_force = vehicle_mass * accel_limit # max throttle force self.min_force = vehicle_mass * decel_limit # max brake force (negative!) self.force_brake_deadband = -1.0 * vehicle_mass * brake_deadband # This is from the simulator!!!- perhaps not valid for real vehicle # # throttle at 0.25 pedel position # v_start: 3.12 km/h v_end: 20.50 km/h --> delta_v = 4.83 m/s # t_start: 50.85 s t_end: 62.15 s -> delta_t = 11.3 s # --> accel: 0.43 m/s^2 # # throttle at 0.50 pedel position # v_start: 3.72 km/h v_end: 21.00 km/h --> delta_v = 4.80 m/s # t_start: 696.17 s t_end: 698.87 s -> delta_t = 2.7 s # --> accel: 1.78 m/s^2 # # throttle at 0.40 pedel position # v_start: 3.24 km/h v_end: 20.00 km/h --> delta_v = 4.66 m/s # t_start: 94.71 s t_end: 100.61 s -> delta_t = 2.7 s # --> accel: 0.79 m/s^2 # acceleration is not linear # guess for vehicle # throttle 1.0 --> accel 3 m/s^2 self.max_accel = 3.0 # self.ref_force_throttle = vehicle_mass * self.max_accel # force at 100% throttle position # init low pass filter for acceleration self.accel_LowPassFilter = LowPassFilter(FILT_TAU_ACCEL, TS) self.last_spd = 0.0 self.last_target_accel = 0.0 self.last_expected_spd = 0.0 self.last_brake_actv = False # brake active self.last_force = 0.0 # init PID self.accel_PID = PID(kp=2.0 * vehicle_mass * wheel_radius, ki=0.015 * vehicle_mass * wheel_radius, kd=0.2 * vehicle_mass * wheel_radius) def control(self, target_spd, current_spd, delta_t): # calulate filter acceleration of vehicle accel_raw = (current_spd - self.last_spd) / delta_t accel_filt = self.accel_LowPassFilter.filt(accel_raw) # calculate speed error spd_err = target_spd - current_spd # calulate target acceleration from speed error # could be tuned if target_spd > 0.05: k_accel = 0.2 else: k_accel = 1.0 if (abs(spd_err) < 1.0): target_accel = k_accel * spd_err else: target_accel = k_accel * spd_err * spd_err * spd_err # limit jerk accel_change_limit = LONG_JERK_LIMIT * delta_t target_accel = max( min(target_accel, self.last_target_accel + accel_change_limit), self.last_target_accel - accel_change_limit) # check for min and max allowed acceleration target_accel = max(min(target_accel, self.accel_limit), self.decel_limit) trq = 0.0 force_feedForward = 0.0 force_PID = 0.0 # calculate an expected speed from last target values # this can be used in PID to large errors, when target_spd jumps # it's smooth and near the expected behavior expected_spd = self.last_expected_spd + target_accel * delta_t expected_spd = max(min(target_spd, expected_spd), 0.0) if target_spd > expected_spd: expected_spd = max(current_spd, expected_spd, self.last_expected_spd + 0.1 * delta_t) elif target_spd < expected_spd: expected_spd = min(current_spd, expected_spd, self.last_expected_spd - 0.1 * delta_t) # assert valid range expected_spd = max(min(target_spd, expected_spd), 0.0) if target_spd > 0.05 or current_spd > 1.0: # overall resistance (tuned in simulator) force_resistance = 4 * current_spd * current_spd + 40 * current_spd + 40 # calculate force from target_accel using mass # F = m * a force_feedForward = target_accel * self.vehicle_mass + force_resistance # use PID to get better control performance expected_err = expected_spd - current_spd force_PID = self.accel_PID.step(expected_err, delta_t, mn=MIN_NUM, mx=MAX_NUM) # calulate overall torque force = force_feedForward + force_PID else: # hold vehicle force = min(self.last_force, self.min_force / 2) self.accel_PID.freeze() # calulate throttle # published throttle is a percentage [0 ... 1]!!! if force > 0.: throttle = force / self.ref_force_throttle * 1.0 brake = 0.0 self.last_brake_actv = False elif self.last_brake_actv: throttle = 0.0 brake = -force * self.wheel_radius / self.n_wheel_accel self.last_brake_actv = True elif force < self.force_brake_deadband or (force < 0. and target_spd < 1.0): throttle = 0.0 brake = -force * self.wheel_radius / self.n_wheel_accel self.last_brake_actv = True else: throttle = 0.0 brake = 0.0 self.last_brake_actv = False # output for debug #rospy.logwarn("accel_raw: %f" % accel_raw + "; accel_filt: %f" % accel_filt + "; target_accel: %f" % target_accel + "; current_spd: %f" % current_spd) #rospy.logwarn("target_spd: {0:.2f} km/h, current_spd: {1:.2f} km/h, expected_spd: {2:.2f} km/h, target_accel: {3:.2f} m/ss, accel: {4:.2f} m/ss, throttle: {5:.2f}, brake: {6:.2f} Nm, force_PID: {7:.2f} N".format(target_spd * 3.6, current_spd * 3.6, expected_spd*3.6, target_accel, accel_filt, throttle, brake, force_PID)) # write values for next iteration self.last_spd = current_spd self.last_target_accel = target_accel self.last_expected_spd = expected_spd self.last_force = force return throttle, brake def reset(self, current_spd): self.accel_PID.reset() self.last_spd = current_spd self.last_target_accel = 0.0 self.last_expected_spd = current_spd self.last_force = 0.0
class Controller(object): def __init__(self, vehicle_mass, fuel_capacity, brake_deadband, decel_limit, accel_limit, wheel_radius, wheel_base, steer_ratio, max_lat_accel, max_steer_angle): self.vehicle_mass = vehicle_mass self.fuel_capacity = fuel_capacity self.brake_deadband = brake_deadband self.decel_limit = decel_limit self.accel_limit = accel_limit self.wheel_radius = wheel_radius self.wheel_base = wheel_base self.steer_ratio = steer_ratio self.max_lat_accel = max_lat_accel self.min_steer_angle = -max_steer_angle self.max_steer_angle = max_steer_angle self.min_force = vehicle_mass * accel_limit # max brake force self.force_brake_deadband = -1.0 * vehicle_mass * brake_deadband self.vehicle_accel_sensitivity = vehicle_mass * wheel_radius # force at 100% throttle position self.ref_force_throttle = vehicle_mass * 3.0 # store all the previsous steer, speed and acceleration values self.last_speed = 0.0 self.last_target_accel = 0.0 self.last_expected_speed = 0.0 self.last_force = 0.0 self.last_steer = 0.0 # brakes are not applied initially self.last_brake_applied = False # init feed forward yaw-rate control self.yawControl = YawController(wheel_base, steer_ratio, MIN_SPEED, max_lat_accel, max_steer_angle) # initialize acceleration and steering PID self.accel_PID = PID(kp=2.0 * self.vehicle_accel_sensitivity, ki=0.015 * self.vehicle_accel_sensitivity, kd=0.2 * self.vehicle_accel_sensitivity) self.steer_PID = PID(kp=0.05, ki=0.0005, kd=0.5) def control(self, waypoints, pose, current_speed, target_speed, target_yaw, delta_t): throttle, brake = self.control_speed(current_speed, target_speed, delta_t) steer = self.control_steer(waypoints, pose, current_speed, target_speed, target_yaw, delta_t) return throttle, brake, steer def control_speed(self, current_speed, target_speed, delta_t): if target_speed > 0.05 or current_speed > 1.0: # overall resistance (tuned in simulator) force_resistance = 4 * current_speed**2 + 40 * current_speed + 40 # use PID to get better control performance CTE = self.speed_CTE(current_speed, target_speed, delta_t) force_PID = self.accel_PID.step(CTE, delta_t) # calculate force from target_accel using mass: F = m * a force_feedForward = self.last_target_accel * self.vehicle_mass + force_resistance # calulate overall torque force = force_feedForward + force_PID else: # apply minimum force if vehicle is near the target speed force = min(self.last_force, self.min_force / 2) self.accel_PID.freeze() # Initialiaze the throttle, brake and last_brake_applied throttle = 0.0 brake = 0.0 self.last_brake_applied = False # calulate throttle # published throttle is a percentage [0 ... 1]!!! if force > 0.0: throttle = force / self.ref_force_throttle elif force < self.force_brake_deadband or target_speed < 1.0: brake = -force * self.wheel_radius / NUM_WHEEL_ACCELERATION self.last_brake_applied = True # store last_speed and last_force for next iteration self.last_speed = current_speed self.last_force = force return throttle, brake def speed_CTE(self, current_speed, target_speed, delta_t): # calculate speed error speed_error = target_speed - current_speed # calulate target acceleration from speed error target_accel = 0.3 if target_speed > 0.05 else 1.0 if abs(speed_error) < 1.0: target_accel *= speed_error else: target_accel *= speed_error**3 # check for min and max allowed acceleration target_accel = max(min(target_accel, self.accel_limit), self.decel_limit) self.last_target_accel = target_accel # calculate CTE and return it return speed_error def control_steer(self, waypoints, pose, current_speed, target_speed, target_yaw, delta_t): if current_speed < MIN_SPEED: self.steer_PID.freeze() return self.last_steer # feed forward control to drive curvature of road steer_feedForward = self.yawControl.get_steering( target_speed, target_yaw, current_speed) # limit steering angle steer_feedForward = max(min(steer_feedForward, self.max_steer_angle), self.min_steer_angle) # PID control CTE = self.steer_CTE(waypoints, pose) steer_PID = self.steer_PID.step(CTE, delta_t) # steering command steer = steer_feedForward + steer_PID self.last_steer = steer return steer def steer_CTE(self, waypoints, pose): # transfrom waypoints into vehicle coordinates car_theta = 2 * np.arccos(pose.orientation.w) # Constraining the angle in [-pi, pi) if car_theta > np.pi: car_theta = -(2 * np.pi - car_theta) num_waypoints = len(waypoints) car_x = pose.position.x car_y = pose.position.y # transform waypoints in vehicle coordiantes wp_car_coords_x = np.zeros(num_waypoints) wp_car_coords_y = np.zeros(num_waypoints) for idx, waypoint in enumerate(waypoints): wp_x = waypoint.pose.pose.position.x wp_y = waypoint.pose.pose.position.y wp_car_coords_x[idx] = (wp_y - car_y) * math.sin(car_theta) - ( car_x - wp_x) * math.cos(car_theta) wp_car_coords_y[idx] = (wp_y - car_y) * math.cos(car_theta) - ( wp_x - car_x) * math.sin(car_theta) # get waypoint which should be used for controller input# # get the first waypoint in front of car. The waypoints are already in order as # they are coming out of /waypoint_updater topic viewRange = 10.0 first_wp_idx = -1 last_wp_idx = -1 first_wp_found = False for idx, wp_car_coord_x in enumerate(wp_car_coords_x): if not first_wp_found and wp_car_coord_x >= 0.0: first_wp_idx = idx first_wp_found = True elif wp_car_coord_x >= viewRange: last_wp_idx = idx break # Check if we have enough waypoints to move forward We need over 5 waypoints if first_wp_idx < 0 or last_wp_idx - first_wp_idx < 4: rospy.logerr("twist_controller: Invalid Waypoints") return 0.0 # Interpolate waypoints (already transformed to vehicle coordinates) to a polynomial of 3rd degree coeffs = np.polyfit(wp_car_coords_x[first_wp_idx:last_wp_idx + 1], wp_car_coords_y[first_wp_idx:last_wp_idx + 1], 3) # distance to track is polynomial at car's position x = 0 return np.poly1d(coeffs)(0.0) def reset(self, current_speed): self.steer_PID.reset() self.accel_PID.reset() self.last_speed = current_speed self.last_target_accel = 0.0 self.last_expected_speed = current_speed self.last_force = 0.0
class LatController(object): def __init__(self, wheel_base, steer_ratio, min_speed, max_lat_accel, max_steer_angle): self.wheel_base = wheel_base self.steer_ratio = steer_ratio self.min_speed = min_speed self.max_lat_accel = max_lat_accel self.min_angle = -max_steer_angle self.max_angle = max_steer_angle # init feed forward yaw-rate control self.yawControl = YawController(wheel_base, steer_ratio, min_speed, max_lat_accel, max_steer_angle) # init PID self.steer_PID = PID(kp=0.05, ki=0.0005, kd=0.5) self.last_steer = 0.0 def control(self, target_spd, target_yawRate, current_spd, waypoints, pose, delta_t): steer = self.last_steer if current_spd > self.min_speed: # feed forward control to drive curvature of road steer_feedForward = self.yawControl.get_steering( target_spd, target_yawRate, current_spd) # limit steering angle steer_feedForward = max(min(steer_feedForward, self.max_angle), self.min_angle) # PID control CTE = self.calc_CTE(waypoints, pose) steer_PID = self.steer_PID.step( CTE, delta_t, mn=self.min_angle - steer_feedForward, mx=self.max_angle - steer_feedForward) # steering command steer = steer_feedForward + steer_PID self.last_steer = steer else: self.steer_PID.freeze() return steer def reset(self): self.steer_PID.reset() def transfromWPcarCoord(self, waypoints, pose): n = len(waypoints) # get car's x and y position and heading angle car_x = pose.position.x car_y = pose.position.y # get orientation s = pose.orientation.w # quaternion scalar v1 = pose.orientation.x # vector part 1 v2 = pose.orientation.y # vector part 2 v3 = pose.orientation.z # vector part 3 # now obtaining orientation of the car (assuming rotation about z: [0;0;1]) car_theta = 2 * np.arccos(s) # Constraining the angle in [-pi, pi) if car_theta > np.pi: car_theta = -(2 * np.pi - car_theta) #car_theta = pose.orientation.z # transform waypoints in vehicle coordiantes wp_carCoord_x = np.zeros(n) wp_carCoord_y = np.zeros(n) for i in range(n): wp_x = waypoints[i].pose.pose.position.x wp_y = waypoints[i].pose.pose.position.y wp_carCoord_x[i] = (wp_y - car_y) * math.sin(car_theta) - ( car_x - wp_x) * math.cos(car_theta) wp_carCoord_y[i] = (wp_y - car_y) * math.cos(car_theta) - ( wp_x - car_x) * math.sin(car_theta) return wp_carCoord_x, wp_carCoord_y def calc_CTE(self, waypoints, pose): # transfrom waypoints into vehicle coordinates wp_carCoord_x, wp_carCoord_y = self.transfromWPcarCoord( waypoints, pose) # get waypoint which should be used for controller input viewRange = 10.0 n_points_min = 5 idxFirstWP, idxLastWP = self.findWPs(wp_carCoord_x, viewRange, n_points_min) # do some checks if enough waypoints in the front of the car are available if (idxFirstWP < 0): rospy.logerr( "dbw_node: No waypoint in front of car for lateral control received!" ) return 0.0 elif (idxLastWP < 0): rospy.logerr( "dbw_node: Not enough waypoints for a view range of %f m!" % viewRange) return 0.0 elif (idxLastWP + 1 - idxFirstWP < n_points_min): rospy.logerr("dbw_node: Resolution of waypoints not high enough!") return 0.0 else: # Interpolate waypoints (already transformed to vehicle coordinates) to a polynomial of 3rd degree coeffs = np.polyfit(wp_carCoord_x[idxFirstWP:idxLastWP + 1], wp_carCoord_y[idxFirstWP:idxLastWP + 1], 3) p = np.poly1d(coeffs) # distance to track is polynomial at car's position x = 0 CTE = p(0.0) return CTE def findWPs(self, wp_carCoord_x, viewRange, n_points_min): # this function return first waypoint in front of car # it is asumed that wayponts are already ordered (by waypoint_updater) # if return value is negative then no point in front of car is found! # transfrom waypoint in vehcile coordnates idxFirstWP = -1 idxLastWP = -1 n_pts = len(wp_carCoord_x) flagFirstWP = False for i in range(n_pts): if not flagFirstWP and wp_carCoord_x[i] >= 0.0: idxFirstWP = i flagFirstWP = True elif wp_carCoord_x[i] >= viewRange: idxLastWP = i break # if view range to less than check if enough points are available #rospy.logwarn("idxFirstWP: %f" % idxFirstWP + "idxLastWP: %f" % idxLastWP ) #if idxFirstWP>=0 and idxLastWP<0 and n_pts-idxFirstWP >= n_points_min: # idxLastWP = n_pts-1 # zero based indexing return idxFirstWP, idxLastWP