class Controller(object):
def __init__(self, *args, **kwargs):
# TODO: Implement
rospy.loginfo('TwistController: Start init')
self.sampling_rate = kwargs["sampling_rate"]
self.decel_limit = kwargs["decel_limit"]
self.accel_limit = kwargs["accel_limit"]
self.wheel_base = kwargs["wheel_base"]
self.steer_ratio = kwargs["steer_ratio"]
self.max_lat_accel = kwargs["max_lat_accel"]
self.max_steer_angle = kwargs["max_steer_angle"]
self.brake_deadband = kwargs["brake_deadband"]
self.vehicle_mass = kwargs["vehicle_mass"]
self.fuel_capacity = kwargs["fuel_capacity"]
self.wheel_radius = kwargs["wheel_radius"]
self.delta_t = 1 / self.sampling_rate
self.brake_torque_const = (self.vehicle_mass + self.fuel_capacity \
* GAS_DENSITY) * self.wheel_radius
self.past_vel_linear = 0.0
self.current_accel = 0.0
self.low_pass_filter_accel = LowPassFilter(TAU, self.delta_t)
self.pid_vel_linear = PID(PVEL, IVEL, DVEL, self.decel_limit,
self.accel_limit)
self.accel_pid = PID(PACC, IACC, DACC, 0.0, 0.75)
self.yaw_controller = YawController(
wheel_base=self.wheel_base,
steer_ratio=self.steer_ratio,
min_speed=5.0,
max_lat_accel=self.max_lat_accel,
max_steer_angle=self.max_steer_angle)
rospy.loginfo('TwistController: Complete init')
rospy.loginfo('TwistController: Steer ratio = ' +
str(self.steer_ratio))
def control(self, linear_vel, angular_vel, current_linear_vel):
# TODO: Change the arg, kwarg list to suit your needs
# Return throttle, brake, steer
throttle = 0.
brake = 0.
steer = 0.
cte = linear_vel - current_linear_vel
accel = self.sampling_rate * (self.past_vel_linear -
current_linear_vel)
self.past_vel_linear = current_linear_vel
self.lowpass_accel.filt(accel)
self.current_accel = self.lowpass_accel.get()
accel_des = sef.pid_linvel.step(cte, self.delta_t)
if desired_accel > 0.0:
if desired_accel < self.accel_limit:
throttle = self.accel_pid.step(
desired_accel - self.current_accel, self.delta_t)
else:
throttle = self.accel_pid.step(
self.accel_limit - self.current_accel, self.delta_t)
brake = 0.0
else:
throttle = 0.0
self.accel_pid.reset()
if abs(desired_accel) > self.brake_deadband:
if abs(desired_accel) > abs(self.decel_limit):
brake = abs(self.decel_limit) * self.brake_torque_const
else:
brake = abs(desired_accel) * self.brake_torque_const
steer = self.yaw_controller.get_steer(linear_vel, angular_vel,
linear_vel)
if abs(steer) <> 0.0:
rospy.loginfo('Veer: steer = ' + str(steer) + ', required = ' +
str(required_vel_angular))
return throttle, brake, steer
def set_controllers(self):
self.pid_throttle = PID(0.25, 0.0, 0.0, 0.0, 1.0)
self.pid_brake = PID(0.25, 0.0, 0.0, 0.0, 1.0)
self.lowpass_steer = PID(0.25, 0.0, 0.0, 0.0, 1.0)
def reset(self):
self.pid_linvel.reset()
