class Controller(object):
def __init__(self):
self.steering_low_pass_filter = LowPassFilter(0.80)
self.throttle_low_pass_filter = LowPassFilter(0.80)
self.throttle_pid = PID(0.5, -0.0002, 0, 0, 1)
def control(self, twist_cmd, current_velocity):
velocity_diff = twist_cmd.twist.linear.x - current_velocity.twist.linear.x
#rospy.loginfo("twist_controller current velocity, end_velocity, diff = %s, %s, %s", current_velocity.twist.linear.x, twist_cmd.twist.linear.x, velocity_diff)
if velocity_diff > 0:
throttle = (velocity_diff * 0.5 * ACCEL_LIMIT) - 0.02
#throttle = self.throttle_pid.step(velocity_diff,0) - 0.02
if throttle > 1:
throttle = 1
brake = 0.0
elif velocity_diff < 0:
throttle = 0.0 - 0.02
#throttle = 0.0
brake = velocity_diff * 0.1 * BRAKE_LIMIT
else:
throttle = 0.0 - 0.02
brake = 0.0
throttle = self.throttle_low_pass_filter.filter(throttle)
yaw_controller = YawController()
steering = yaw_controller.get_steering(twist_cmd.twist.linear.x,
twist_cmd.twist.angular.z,
current_velocity.twist.linear.x)
steering = self.steering_low_pass_filter.filter(steering)
#rospy.loginfo("twist_controller current velocity, end_velocity, throttle = %s, %s, %s, %s, %s", current_velocity.twist.linear.x, twist_cmd.twist.linear.x,throttle, brake, steering)
return throttle, brake, steering
class ThrottleBrakeController():
def __init__(self, car_mass, wheel_radius, deceleration_limit):
self.lowpass_filter = LowPassFilter(tau=0.5, ts=.02)
self.car_mass = car_mass
self.deceleration_limit = deceleration_limit
self.wheel_radius = wheel_radius
self.last_time = rospy.get_time()
kp = 1.5
ki = 0.001
kd = 0.5
self.throttle_controller_pid = PID(kp, ki, kd)
def control(self, current_speed, target_speed):
current_speed = self.lowpass_filter.filter(current_speed)
error_vel = target_speed - current_speed
self.last_velocity = current_speed
current_time = rospy.get_time()
sample_time = current_time - self.last_time
self.last_time = current_time
throttle = self.throttle_controller_pid.step(error_vel, sample_time)
brake = 0.0
if target_speed == 0.0 and current_speed < 0.1:
throttle = 0
brake = 700
elif throttle < 0.1 and error_vel < 0.0:
throttle = 0.0
decel = max(error_vel, self.deceleration_limit)
brake = min(700, abs(decel) * self.car_mass * self.wheel_radius)
return throttle, brake
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.yaw_controller = YawController(wheel_base, steer_ratio, 0.1,
max_lat_accel, max_steer_angle)
kp = 0.3
ki = 0.1
kd = 0.0
mn = 0.0 # Mininum throttle value
mx = 0.2 #Maximum throttle value
self.throttle_controller = PID(kp, ki, kd, mn, mx)
tau = 0.5 # 1/(2pi*tau) = cutoff frequency
ts = 0.02 #Sample time
self.vel_lpf = LowPassFilter(tau, ts)
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.last_time = rospy.get_time()
self.last_steering = 0.
def control(self, current_vel, current_angular_vel, dbw_enabled,
linear_vel, angular_vel):
if not dbw_enabled:
self.throttle_controller.reset()
return 0., 0., 0.
current_vel = self.vel_lpf.filter(current_vel)
steering = self.yaw_controller.get_steering(linear_vel, angular_vel,
current_vel)
if (current_angular_vel == angular_vel):
steering = self.last_steering
# rospy.logwarn("use previous steering: %f, %f", current_angular_vel, angular_vel)
self.last_steering = steering
vel_error = linear_vel - current_vel
self.last_vel = current_vel
current_time = rospy.get_time()
sample_time = current_time - self.last_time
self.lat_time = current_time
throttle = self.throttle_controller.step(vel_error, sample_time)
brake = 0
if linear_vel == 0. and current_vel < 0.1:
throttle = 0
brake = 700 #N*m to hold the car in place if we are stopped at a light, Acceleration ~c1m/s^2
elif throttle < .1 and vel_error < 0:
throttle = 0
decel = max(vel_error, self.decel_limit)
brake = abs(decel) * self.vehicle_mass * self.wheel_radius
return throttle, brake, steering
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,
max_throttle):
# Yaw controller to controller the turning
self.vehicle_min_velocity = .1
self.yaw_controller = YawController(
wheel_base=wheel_base,
steer_ratio=steer_ratio,
min_speed=self.vehicle_min_velocity,
max_lat_accel=max_lat_accel,
max_steer_angle=max_steer_angle)
self.steering_controller = PID(
kp=.0, ki=.001, kd=.1, min=-.2, max=.2)
# Throttle controller
self.throttle_controller = PID(
kp=.8, ki=.001, kd=.1, min=0, max=max_throttle)
# Remove high frequency noise on the velocity
self.low_pass = LowPassFilter(tau=.5, ts=.02)
# Assume the tank is full
fuel_mass = fuel_capacity * GAS_DENSITY
self.total_mass = vehicle_mass + fuel_mass
self.decel_limit = decel_limit
self.wheel_radius = wheel_radius
self.last_time = rospy.get_time()
def control(
self,
current_velocity,
linear_velocity,
angular_velocity,
cte):
current_velocity = self.low_pass.filter(current_velocity)
# Calculate the velocity error
dv = linear_velocity - current_velocity
# Update time
current_time = rospy.get_time()
dt = current_time - self.last_time
self.last_time = current_time
# Calculate the predictive path and correct for the cross track error
steering = self.yaw_controller.get_steering(
linear_velocity,
angular_velocity,
current_velocity)
steering -= self.steering_controller.step(cte, dt)
# Calculate the throttle and brake
throttle = self.throttle_controller.step(dv, dt)
brake = 0.
if linear_velocity < 0. and current_velocity < self.vehicle_min_velocity:
# Keep car in place when it is stopped
throttle = 0.
brake = 700.
elif throttle < .1 and dv < 0.:
throttle = 0.
decel = max(dv, self.decel_limit)
brake = abs(decel) * self.total_mass * self.wheel_radius
return throttle, brake, steering
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):
# Instantiate steering angle controller - YawController
self.yaw_controller = YawController(wheel_base, steer_ratio, MIN_SPEED,
max_lat_accel, max_steer_angle)
# Instantiate throttle controller - PIDController
self.throttle_controller = PID(PID_PARAMS.get('Kp'),
PID_PARAMS.get('Ki'),
PID_PARAMS.get('Kd'),
PID_PARAMS.get('Min_Throttle'),
PID_PARAMS.get('Max_Throttle'))
# Instantiate a low pass filter for averaging the velocity inputs
self.velocity_lowPassFilter = LowPassFilter(
LPF_PARAMS.get('tau'), LPF_PARAMS.get('Sample_Time'))
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.last_time = rospy.get_time()
self.last_velocity = None
pass
def update_lastknowngood_states(self, current_velocity, current_time):
self.last_velocity = current_velocity
self.last_time = current_time
def compute_throttle_params(self, linear_velocity, current_velocity,
current_time):
velocity_error = linear_velocity - current_velocity
if not self.last_time is None:
sample_time = current_time - self.last_time
else:
sample_time = current_time
rospy.loginfo("Sample time: {}".format(sample_time))
rospy.loginfo("Velocity error: {}".format(velocity_error))
throttle = self.throttle_controller.step(velocity_error, sample_time)
rospy.loginfo("Updated throttle: {}".format(throttle))
brake = 0
if linear_velocity == 0.0 and current_velocity < MIN_SPEED:
throttle = 0
brake = MAX_BRAKE # Nm- to hold the car in place if we are stopped at light, accel = 1 m/s^2
elif throttle < MIN_SPEED and velocity_error < 0:
throttle = 0
decel = max(velocity_error, self.decel_limit)
brake = min(MAX_BRAKE,
abs(decel) * self.vehicle_mass *
self.wheel_radius) # torque Nm
return throttle, brake
def control(self, current_velocity, current_angular_velocity, dbw_enabled,
linear_velocity, angular_velocity):
if not dbw_enabled:
self.throttle_controller.reset()
return 0., 0., 0.
# Smoothen the velocity input using low pass filter
current_velocity = self.velocity_lowPassFilter.filter(current_velocity)
# Use yaw controller to get the desired steering angle given the linear and angular velocity
# from the car simulator (via dbw node) and the current step's velocity
steering = self.yaw_controller.get_steering(linear_velocity,
angular_velocity,
current_velocity,
current_angular_velocity)
rospy.loginfo("Updated steering: {}".format(steering))
current_time = rospy.get_time()
# Use throttle controller to get the updated acceleration
throttle, brake = self.compute_throttle_params(linear_velocity,
current_velocity,
current_time)
#rospy.loginfo("Updated throttle: {}".format(throttle))
rospy.loginfo("Updated brake: {}".format(brake))
self.update_lastknowngood_states(current_velocity, current_time)
return throttle, brake, steering
class Controller(object):
"""Twist controller"""
# same params passing from dwb_node.py
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.yaw_controller = YawController(wheel_base, steer_ratio, 0.1,
max_lat_accel, max_steer_angle)
# play around with this PID params
kp = 0.3
ki = 0.01
kd = 0.3
mn = 0. # minimum throttle value
mx = 0.2 # maximum throttle value
self.throttle_controller = PID(kp, ki, kd, mn, mx)
# play around with lowpassfilter
tau = 0.5 # 1/(2pi*tau) is cutoff frequency
ts = 0.02 # Sample time
self.vel_lpf = LowPassFilter(tau, ts)
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.last_time = rospy.get_time()
def control(self, current_vel, dbw_enabled, linear_vel, angular_vel):
if not dbw_enabled:
self.throttle_controller.reset()
return 0., 0., 0.
# just to filter out noise from current velocity
current_vel = self.vel_lpf.filter(current_vel)
# rospy.logwarn("Angular vel: {0}".format(angular_vel))
# rospy.logwarn("Target vel: {0}".format(linear_vel))
# rospy.logwarn("Target angular vel: {0}".format(angular_vel))
# rospy.logwarn("Current vel: {0}".format(current_vel))
# rospy.logwarn("Filter vel: {0}".format(self.vel_lpf.get()))
steering = self.yaw_controller.get_steering(linear_vel, angular_vel,
current_vel)
vel_error = linear_vel - current_vel
self.last_vel = current_vel
current_time = rospy.get_time()
sample_time = current_time - self.last_time
self.last_time = current_time
throttle = self.throttle_controller.step(vel_error, sample_time)
brake = 0
if linear_vel == 0 and current_vel < 0.1:
throttle = 0
brake = 700 # torque N*m Acceleration - 1m/s^2
elif throttle < 0.1 and vel_error < 0:
throttle = 0
decel = max(vel_error, self.decel_limit)
brake = abs(
decel) * self.vehicle_mass * self.wheel_radius # torque N*m
return throttle, brake, steering
class Controller(object):
def __init__(self, **kwargs):
self.wheel_base = kwargs['wheel_base']
self.steer_ratio = kwargs['steer_ratio']
self.max_lateral_acceleration = kwargs['max_lateral_acceleration']
self.max_steer_angle = kwargs['max_steer_angle']
self.vehicle_mass = kwargs['vehicle_mass']
self.wheel_radius = kwargs['wheel_radius']
self.fuel_capacity = kwargs['fuel_capacity']
self.acceleration_limit = kwargs['acceleration_limit']
self.deceleration_limit = kwargs['deceleration_limit']
self.brake_deadband = kwargs['brake_deadband']
# add mass of fuel to vehicle mass
self.vehicle_mass = self.vehicle_mass + (self.fuel_capacity * GAS_DENSITY)
self.brake_torque = self.vehicle_mass * self.wheel_radius
# get max allowed velocity in kmph
self.MAX_VELOCITY = rospy.get_param('/waypoint_loader/velocity')
# convert velocity to m/s
self.MAX_VELOCITY = self.MAX_VELOCITY * 0.2778
self.throttle_PID = PID(THROTTLE_KP, THROTTLE_KI, THROTTLE_KD,
-1*self.acceleration_limit, self.acceleration_limit)
yaw_controller_init_kwargs = {
'wheel_base' : self.wheel_base,
'steer_ratio' : self.steer_ratio,
'max_lateral_acceleration' : self.max_lateral_acceleration,
'min_speed' : 0.1,
'max_steer_angle' : self.max_steer_angle
}
self.yaw_controller = YawController(**yaw_controller_init_kwargs)
self.throttle_lowpass = LowPassFilter(THROTTLE_TAU,THROTTLE_TS)
self.last_time = rospy.get_time()
def control(self, **kwargs):
'''
controls the vehicle taking into consideration the twist_cmd
Params:
- `target_linear_velocity` is the desired linear velocity the car should reach
- `target_angular_velocity` is the desired angular velocity the car should reach
- `current_velocity` is the current velocity of the car
- `dbw_enabled` - Boolean, whether auto-drive is enabled or not
- `dt` - time in seconds, between each twist_cmd
Returns throttle, brake and steer values
'''
target_linear_velocity = kwargs['target_linear_velocity']
target_angular_velocity = kwargs['target_angular_velocity']
current_velocity = kwargs['current_velocity']
dbw_enabled = kwargs['dbw_enabled']
dt = kwargs['dt']
rospy.loginfo('Current velocity: {}'.format(current_velocity))
rospy.loginfo('target_linear_velocity : {}'.format(target_linear_velocity))
# pass current_velocity through low pass filter to reduce high freq variations
current_velocity = self.throttle_lowpass.filter(current_velocity)
# if driver has taken control, reset throttle PID to avoid accumulation
# of error
if not dbw_enabled:
self.throttle_PID.reset()
self.throttle_lowpass.reset()
error = target_linear_velocity - current_velocity
throttle = self.throttle_PID.step(error, dt)
steer = self.yaw_controller.get_steering(target_linear_velocity, target_angular_velocity,
current_velocity)
#steer = self.steer_PID.step(steer,dt)
#steer = self.steer_lowpass.filter(steer)
# if target linear velocity is very less, apply hard brake
brake = 0.0
if target_linear_velocity == 0.0 and current_velocity < 0.1:
brake = 400
throttle = 0.0
elif throttle < 0.1 and error < 0:
throttle = 0.0
decel = min(abs(error/dt), abs(self.deceleration_limit))
brake = decel * self.brake_torque
return throttle, brake, steer
class Controller(object):
def __init__(self):
max_lat_accel = rospy.get_param('~max_lat_accel', 3.0)
max_steer_angle = rospy.get_param('~max_steer_angle', 8.0)
steer_ratio = rospy.get_param('~steer_ratio', 2.67)
wheel_base = rospy.get_param('~wheel_base', 3.0)
self.brake_deadband = rospy.get_param('~brake_deadband', 0.2)
self.last_time = None
self.pid_control = PID(5.0, 0.1, 0.02)
self.pid_steering = PID(0.6, 0.7, 0.4)
rospy.Subscriber('/kp', Float32, self.kp_cb)
rospy.Subscriber('/ki', Float32, self.ki_cb)
rospy.Subscriber('/kd', Float32, self.kd_cb)
self.lpf_pre = LowPassFilter(0.2, 0.1)
self.lpf_post = LowPassFilter(0.7, 0.1)
self.yaw_control = YawController(wheel_base=wheel_base,
steer_ratio=steer_ratio,
min_speed=0.0,
max_lat_accel=max_lat_accel,
max_steer_angle=max_steer_angle)
def control(self, **kwargs):
dbw_enabled = kwargs['dbw_enabled']
tc_l = kwargs['twist_cmd'].twist.linear
tc_a = kwargs['twist_cmd'].twist.angular
cv_l = kwargs['current_velocity'].twist.linear
cv_a = kwargs['current_velocity'].twist.angular
desired_linear_velocity = tc_l.x
desired_angular_velocity = tc_a.z
current_linear_velocity = cv_l.x
current_angular_velocity = cv_a.z
if dbw_enabled is False:
self.pid_control.reset()
self.pid_steering.reset()
if self.last_time is not None:
time = rospy.get_time()
delta_t = time - self.last_time
self.last_time = time
velocity_error = desired_linear_velocity - current_linear_velocity
control = self.pid_control.update(velocity_error, delta_t)
throttle = max(0.0, control)
brake = max(0.0, -control) + self.brake_deadband
desired_steering = self.yaw_control.get_steering(
desired_linear_velocity, desired_angular_velocity,
desired_linear_velocity)
current_steering = self.yaw_control.get_steering(
current_linear_velocity, current_angular_velocity,
current_linear_velocity)
steering_error = desired_steering - current_steering
steering_error = self.lpf_pre.filter(steering_error)
steering = self.pid_steering.update(steering_error, delta_t)
steering = self.lpf_post.filter(steering)
return throttle, brake, steering
else:
self.last_time = rospy.get_time()
return 0.0, 0.0, 0.0
def kp_cb(self, msg):
self.pid_steering.kp = msg.data
def ki_cb(self, msg):
self.pid_steering.ki = msg.data
def kd_cb(self, msg):
self.pid_steering.kd = msg.data
class DBWNode(object):
def __init__(self):
rospy.init_node('dbw_node', log_level=rospy.INFO)
self.target_linear_velocity = None
self.target_angular_velocity = None
self.current_linear_velocity = None
self.current_angular_velocity = None
self.current_acceleration = None
self.current_cte = None
self.dbw_enabled = None
self.accel_tau = 0.5
self.sample_rate_in_hertz = 50.0 # 50Hz
self.min_speed = 1.0
self.wheel_base = rospy.get_param('~wheel_base', 2.8498)
self.steer_ratio = rospy.get_param('~steer_ratio', 14.8)
self.max_lat_accel = rospy.get_param('~max_lat_accel', 3.)
self.max_steer_angle = rospy.get_param('~max_steer_angle', 8.)
self.vehicle_mass = rospy.get_param('~vehicle_mass', 1736.35)
self.wheel_radius = rospy.get_param('~wheel_radius', 0.2413)
self.fuel_capacity = rospy.get_param('~fuel_capacity', 13.5) * 3.785
self.fuel_density = 780
self.total_vehicle_mass = self.vehicle_mass + self.fuel_capacity * self.fuel_density
self.controller = Controller(
sample_rate_in_hertz=self.sample_rate_in_hertz,
vehicle_mass=self.vehicle_mass,
fuel_capacity=self.fuel_capacity,
brake_deadband=rospy.get_param('~brake_deadband', .1),
wheel_radius=self.wheel_radius,
decel_limit=rospy.get_param('~decel_limit', -5),
accel_limit=rospy.get_param('~accel_limit', 1.),
max_steer_angle=self.max_steer_angle)
self.lpf_steer = LowPassFilter(0.05)
self.lpf_brake = LowPassFilter(0.05)
self.lpf_throttle = LowPassFilter(0.1)
self.yaw_controller = YawController(self.wheel_base, self.steer_ratio,
self.min_speed, self.max_lat_accel,
self.max_steer_angle)
# Publishers
self.steer_pub = rospy.Publisher('/vehicle/steering_cmd',
SteeringCmd,
queue_size=1)
self.throttle_pub = rospy.Publisher('/vehicle/throttle_cmd',
ThrottleCmd,
queue_size=1)
self.brake_pub = rospy.Publisher('/vehicle/brake_cmd',
BrakeCmd,
queue_size=1)
# Subscribe to required topics:
rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_cb)
rospy.Subscriber('/current_velocity', TwistStamped, self.velocity_cb)
rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_enabled_cb)
rospy.Subscriber('/current_cte', Float64, self.cte_cb)
rospy.Subscriber('/steering_controller_p', Float64,
self.steering_controller_p_cb)
rospy.Subscriber('/steering_controller_i', Float64,
self.steering_controller_i_cb)
rospy.Subscriber('/steering_controller_d', Float64,
self.steering_controller_d_cb)
rospy.Subscriber('/speed_controller_p', Float64,
self.speed_controller_p_cb)
rospy.Subscriber('/speed_controller_i', Float64,
self.speed_controller_i_cb)
rospy.Subscriber('/speed_controller_d', Float64,
self.speed_controller_d_cb)
self.loop()
def loop(self):
rate = rospy.Rate(self.sample_rate_in_hertz) # 50Hz
while not rospy.is_shutdown():
if self.current_linear_velocity is None \
or self.target_linear_velocity is None \
or self.current_cte is None \
or not self.dbw_enabled:
continue
throttle, brake, steer_a = self.controller.control(
self.target_linear_velocity, self.target_angular_velocity,
self.current_linear_velocity, self.current_angular_velocity,
self.current_acceleration, self.current_cte)
steer_b = self.yaw_controller.get_steering(
self.target_linear_velocity, self.target_angular_velocity,
self.current_linear_velocity)
steer = self.lpf_steer.filter(steer_a + steer_b)
throttle = self.lpf_throttle.filter(throttle)
brake = self.lpf_brake.filter(brake)
if brake < 0.01:
brake = 0.0
self.publish(throttle, brake, steer)
rate.sleep()
def publish(self, throttle, brake, steer):
tcmd = ThrottleCmd()
tcmd.enable = True
tcmd.pedal_cmd_type = ThrottleCmd.CMD_PERCENT
tcmd.pedal_cmd = throttle
self.throttle_pub.publish(tcmd)
scmd = SteeringCmd()
scmd.enable = True
scmd.steering_wheel_angle_cmd = steer
self.steer_pub.publish(scmd)
bcmd = BrakeCmd()
bcmd.enable = True
bcmd.pedal_cmd_type = BrakeCmd.CMD_TORQUE
bcmd.pedal_cmd = brake * 400
self.brake_pub.publish(bcmd)
def twist_cb(self, msg):
self.target_linear_velocity = msg.twist.linear.x
self.target_angular_velocity = msg.twist.angular.z
def velocity_cb(self, msg):
self.current_linear_velocity = msg.twist.linear.x
self.current_angular_velocity = msg.twist.angular.z
def dbw_enabled_cb(self, msg):
self.dbw_enabled = msg.data
if (self.dbw_enabled):
rospy.loginfo('DBW enabled.')
else:
rospy.loginfo('DBW disabled. Resetting Twist Controller.')
self.controller.reset()
def cte_cb(self, msg):
self.current_cte = msg.data
def steering_controller_p_cb(self, msg):
rospy.logdebug("Setting P to %.2f", msg.data)
self.controller.steering_pid.kp = msg.data
def steering_controller_i_cb(self, msg):
rospy.logdebug("Setting I to %.2f", msg.data)
self.controller.steering_pid.ki = msg.data
def steering_controller_d_cb(self, msg):
rospy.logdebug("Setting D to %.2f", msg.data)
self.controller.steering_pid.kd = msg.data
def speed_controller_p_cb(self, msg):
rospy.logdebug("Setting P to %.2f", msg.data)
self.controller.speed_pid.kp = msg.data
def speed_controller_i_cb(self, msg):
rospy.logdebug("Setting I to %.2f", msg.data)
self.controller.speed_pid.ki = msg.data
def speed_controller_d_cb(self, msg):
rospy.logdebug("Setting D to %.2f", msg.data)
self.controller.speed_pid.kd = msg.data
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, max_throttle):
# Yaw controller to controller the turning
self.vehicle_min_velocity = .1
self.yaw_controller = YawController(
wheel_base=wheel_base,
steer_ratio=steer_ratio,
min_speed=self.vehicle_min_velocity,
max_lat_accel=max_lat_accel,
max_steer_angle=max_steer_angle)
self.steering_controller = PID(kp=.0, ki=.001, kd=.1, min=-.2, max=.2)
# Throttle controller
self.throttle_controller = PID(kp=.8,
ki=.001,
kd=.1,
min=0,
max=max_throttle)
# Remove high frequency noise on the velocity
self.low_pass = LowPassFilter(tau=.5, ts=.02)
# Assume the tank is full
fuel_mass = fuel_capacity * GAS_DENSITY
self.total_mass = vehicle_mass + fuel_mass
self.decel_limit = decel_limit
self.wheel_radius = wheel_radius
self.last_time = rospy.get_time()
def control(self, current_velocity, linear_velocity, angular_velocity,
cte):
current_velocity = self.low_pass.filter(current_velocity)
# Calculate the velocity error
dv = linear_velocity - current_velocity
# Update time
current_time = rospy.get_time()
dt = current_time - self.last_time
self.last_time = current_time
# Calculate the predictive path and correct for the cross track error
steering = self.yaw_controller.get_steering(linear_velocity,
angular_velocity,
current_velocity)
steering -= self.steering_controller.step(cte, dt)
# Calculate the throttle and brake
throttle = self.throttle_controller.step(dv, dt)
brake = 0.
if linear_velocity < 0. and current_velocity < self.vehicle_min_velocity:
# Keep car in place when it is stopped
throttle = 0.
brake = 700.
elif throttle < .1 and dv < 0.:
throttle = 0.
decel = max(dv, self.decel_limit)
brake = abs(decel) * self.total_mass * self.wheel_radius
return throttle, brake, steering
class Controller(object):
def __init__(self):
self.vehicle_mass = rospy.get_param('~vehicle_mass')
self.fuel_capacity = rospy.get_param('~fuel_capacity')
self.decel_limit = rospy.get_param('~decel_limit')
self.accel_limit = rospy.get_param('~accel_limit')
self.wheel_radius = rospy.get_param('~wheel_radius')
self.wheel_base = rospy.get_param('~wheel_base')
self.steer_ratio = rospy.get_param('~steer_ratio')
self.max_lat_accel = rospy.get_param('~max_lat_accel')
self.max_steer_angle = rospy.get_param('~max_steer_angle')
self.total_vehicle_mass = self.vehicle_mass + self.fuel_capacity * GAS_DENSITY
self.max_brake_torque = BRAKE_MAX * self.total_vehicle_mass * abs(self.decel_limit) * self.wheel_radius
self.last_time = None
self.last_throttle = 0.0
self.pid_control = PID(5.0, 0.05, 0.0)
self.brake_lpf = LowPassFilter(BRAKE_TAU, 0.02)
self.yaw_control = YawController(wheel_base = self.wheel_base,
steer_ratio = self.steer_ratio,
min_speed = 0.0,
max_lat_accel = self.max_lat_accel,
max_steer_angle = self.max_steer_angle)
def control(self, twist_cmd, current_velocity, dbw_enabled):
throttle = 0.0
brake = 0.0
steering = 0.0
if not all((twist_cmd, current_velocity)):
return throttle, brake, steering
if self.last_time is None:
self.last_time = rospy.get_time()
return throttle, brake, steering
time = rospy.get_time()
delta_t = time - self.last_time
self.last_time = time
desired_linear_velocity = twist_cmd.twist.linear.x
desired_angular_velocity = twist_cmd.twist.angular.z
current_linear_velocity = current_velocity.twist.linear.x
current_angular_velocity = current_velocity.twist.angular.z
if abs(desired_linear_velocity) < RESET_SPEED:
self.pid_control.reset()
if dbw_enabled:
velocity_error = desired_linear_velocity - current_linear_velocity
control = self.pid_control.update(velocity_error, delta_t)
if control >= 0.0:
throttle = self.soft_scale(control, THROTTLE_MAX, THROTTLE_CONST)
if throttle - self.last_throttle > MAX_THROTTLE_CHANGE:
throttle = self.last_throttle + MAX_THROTTLE_CHANGE
self.last_throttle = throttle
brake = 0.0
elif control >= BRAKE_SKIP:
throttle = 0.0
self.last_throttle = 0.0
brake = 0.0
else:
throttle = 0.0
self.last_throttle = 0.0
brake = self.soft_scale(-control, self.max_brake_torque, BRAKE_CONST)
brake = self.brake_lpf.filter(brake)
if brake < MIN_BRAKING:
brake = 0.0
#rospy.logwarn('Error: {: 04.2f}'.format(velocity_error))
#rospy.logwarn('Control: {: 04.2f}'.format(control))
#rospy.logwarn('Throttle: {: 04.2f}'.format(throttle))
#rospy.logwarn('Brake: {: 06.2f}'.format(brake))
#rospy.logwarn('Speed: {: 04.2f}'.format(current_linear_velocity))
#rospy.logwarn('')
steering = self.yaw_control.get_steering(desired_linear_velocity,
desired_angular_velocity,
current_linear_velocity)
return throttle, brake, steering
else:
self.pid_control.reset()
return throttle, brake, steering
def soft_scale(self, value, scale, stretch):
if value <= 0.0:
return 0.0
else:
return scale * math.tanh(value * stretch)