class Controller(object): def __init__(self, wheel_base, steer_ratio, max_lat_accel, max_steer_angle): # TODO: Implement self.wheel_base = wheel_base self.steer_ratio = steer_ratio self.max_lat_accel = max_lat_accel self.max_steer_angle = max_steer_angle self.min_speed = 0 self.yaw_controller = YawController(self.wheel_base, self.steer_ratio, self.min_speed, self.max_lat_accel, self.max_steer_angle) self.pid_controller = PID() self.prev_time_secs = None self.prev_steering_angle = 0.0 def control(self, linear_velocity, angular_velocity, current_linear_velocity, current_angular_velocity, is_dbw_enabled, time_secs): if is_dbw_enabled == False: return 0., 0., 0. if self.prev_time_secs == None: self.prev_time_secs = time_secs return 1., 0., 0. delta_t = (time_secs - self.prev_time_secs) / 1000000000.0 #rospy.logwarn("delta_t: %s", delta_t) self.prev_time_secs = time_secs #error = angular_velocity.z - current_angular_velocity.z #p_val = self.pid_controller.step(error, 0.02) #rospy.logwarn("p_val: %s", p_val) #angular_velocity.z = p_val desired_steering_angle = self.get_steering_angle( linear_velocity, angular_velocity) steering_angle = self.yaw_controller.get_steering( linear_velocity.x, angular_velocity.z, current_linear_velocity.x) error = steering_angle - desired_steering_angle rospy.logwarn("angle, desired, error: %s, %s, %s", steering_angle, desired_steering_angle, error) p_val = self.pid_controller.step(error, 0.02) self.prev_steering_angle = steering_angle # Return throttle, brake, steer return 1., 0., steering_angle - p_val def get_steering_angle(self, linear_velocity, angular_velocity): radius = linear_velocity.x / angular_velocity.z if abs( angular_velocity.z) > 0. else 0. steering_angle = self.yaw_controller.get_angle( radius) if abs(radius) > 0. else 0. if steering_angle < -1: steering_angle = -1 elif steering_angle > 1: steering_angle = 1 return steering_angle
class Controller(object): def __init__(self, brake_deadband, decel_limit, accel_limit, wheel_radius, wheel_base, steer_ratio, max_lat_accel, max_steer_angle): self.brake_deadband = brake_deadband self.decel_limit = decel_limit self.accel_limit = accel_limit self.steer_ratio = steer_ratio # Is pid controller need to init. self.need_init = True # max and min speed, m/s min_speed = 0.001 max_speed = 120 / 3.6 # TODO: find the best pid parameters. speed_kp, speed_ki, speed_kd = 1, 0, -0.2 steer_kp, steer_ki, steer_kd = 1, 0, -0.2 self.pid_speed = PID(speed_kp, speed_ki, speed_kd, mn=decel_limit, mx=accel_limit) self.pid_steer = PID(steer_kp, steer_ki, steer_kd, mn=-22 * math.pi / 180, mx=22 * math.pi / 180) self.yaw_ctl = YawController(wheel_base, steer_ratio, min_speed, max_lat_accel, max_steer_angle) # For interal debug. self.debug_dict = {} def distance(self, p1, p2): x, y, z = p1.x - p2.x, p1.y - p2.y, p1.z - p2.z return math.sqrt(x * x + y * y + z * z) def vector_length(self, v): return math.sqrt(v.x**2 + v.y**2 + v.z**2) def control(self, target_vl, target_va, current_vl, current_va, dbw_enabled): """ target_vl: target linear velocity. target_va: target angular velocity. current_vl: current linear velocity. current_va: current angular velocity. current_va is always going to 0. dbw_enabled: dbw state. Caculate the steering angle and accelerate from target velocity and current velocity. """ if not dbw_enabled: self.need_init = True return 0, 0, 0 if self.need_init: self.pid_speed.reset() self.pid_steer.reset() self.need_init = False rospy.loginfo("Init pid control.") # TODO: delta time between current to next waypoint. Neet to check. sample_time = 1.0 / 50 # Differance between current and target. error_velocity_linear = self.vector_length( target_vl) - self.vector_length(current_vl) error_velocity_angular = target_va.z throttle = self.pid_speed.step(error_velocity_linear, sample_time) angular = self.pid_steer.step(error_velocity_angular, sample_time) # Translate the wheel angle to steer angle. if abs(angular) > 0: current_vl_len = self.vector_length(current_vl) steering = self.yaw_ctl.get_angle( max(current_vl_len, self.yaw_ctl.min_speed) / angular) else: steering = 0.0 # A negative throttle means we need to brake. if throttle < 0: brake = -throttle throttle = 0.11 else: brake = 0 # self.log("state: ", round(self.vector_length(target_vl)), round(self.vector_length(current_vl)), # round(throttle, 2), round(brake, 2), round(steering, 2), round(angular, 2)) return throttle, brake, steering def log(self, key_word, *msg): """ print the log msg with a 1s interal. """ if not self.debug_dict.has_key(key_word): self.debug_dict[key_word] = 0 if self.debug_dict[key_word] == 50: rospy.logwarn(str(key_word) + "\n" + str(msg)) self.debug_dict[key_word] = 0 self.debug_dict[key_word] += 1