def __init__(self):
"""
1. Reads basics parameters.
2. Subscibes to the required channels to get realtime data.
3. Creates required channels to publish contoller actions.
4. Creates the required controllers
5. Starts the internal infinite loop.
"""
rospy.init_node('dbw_node')
# class variables
self.dbw_enabled = True # dbw_enabled = false => manual driving
self.waypoints = None
self.pose = None
self.velocity = None
self.twist = None
# read ros parameters
vehicle_mass = rospy.get_param('~vehicle_mass', 1736.35)
fuel_capacity = rospy.get_param('~fuel_capacity', 13.5)
brake_deadband = rospy.get_param('~brake_deadband', .1)
wheel_radius = rospy.get_param('~wheel_radius', 0.2413)
decel_limit = rospy.get_param('~decel_limit', -5)
accel_limit = rospy.get_param('~accel_limit', 1.)
wheel_base = rospy.get_param('~wheel_base', 2.8498)
steer_ratio = rospy.get_param('~steer_ratio', 14.8)
max_lat_accel = rospy.get_param('~max_lat_accel', 3.)
max_steer_angle = rospy.get_param('~max_steer_angle', 8.)
max_acceleration = rospy.get_param('~max_acceleration', 1.5)
min_speed = 0.0
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)
self.controller = Controller(max_steer_angle)
# pylint disable=E1121
self.speed_controller = SpeedController(vehicle_mass,
wheel_radius,
accel_limit,
decel_limit,
brake_deadband,
fuel_capacity,
max_acceleration)
self.yaw_controller = YawController(wheel_base,
steer_ratio,
min_speed,
max_lat_accel,
max_steer_angle)
# Subscribe to all the topics you need to
rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_cb, queue_size=1)
rospy.Subscriber('/final_waypoints', Lane, self.waypoints_cb, queue_size=1)
rospy.Subscriber('/current_pose', PoseStamped, self.pose_cb, queue_size=1)
rospy.Subscriber('/current_velocity', TwistStamped, self.velocity_cb, queue_size=1)
rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_cb, queue_size=1)
self.loop()
def __init__(self):
rospy.init_node('dbw_node', log_level=rospy.DEBUG)
rospy.logdebug('DBW Initialization')
vehicle_mass = rospy.get_param('~vehicle_mass', 1736.35)
fuel_capacity = rospy.get_param('~fuel_capacity', 13.5)
brake_deadband = rospy.get_param('~brake_deadband', .1)
decel_limit = rospy.get_param('~decel_limit', -5)
accel_limit = rospy.get_param('~accel_limit', 1.)
wheel_radius = rospy.get_param('~wheel_radius', 0.2413)
wheel_base = rospy.get_param('~wheel_base', 2.8498)
steer_ratio = rospy.get_param('~steer_ratio', 14.8)
max_lat_accel = rospy.get_param('~max_lat_accel', 3.)
max_steer_angle = rospy.get_param('~max_steer_angle', 8.)
self.speed = 0
self.speed_demand = 0
self.yaw_rate = 0
self.yaw_rate_demand = 0
self.rate = 50 # DBW node rate (Hz)
self.dbw_enabled = False
# Publications
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)
self.speed_control_diag_pub = rospy.Publisher('/speed_control_diag',
ControlDiag,
queue_size=1)
# Subscriptions
self.dbw_enabled_sub = rospy.Subscriber('/vehicle/dbw_enabled', Bool,
self.dbw_enabled_cb)
self.speed_sub = rospy.Subscriber('/current_velocity', TwistStamped,
self.speed_cb)
self.twist_cmd_sub = rospy.Subscriber('/twist_cmd', TwistStamped,
self.twist_cmd_cb)
self.speed_controller = SpeedController()
self.yaw_rate_controller = YawRateController(max_steer_angle,
steer_ratio, wheel_base)
self.loop()
def __init__(self):
rospy.init_node('dbw_node')
vehicle_mass = rospy.get_param('~vehicle_mass', 1736.35)
fuel_capacity = rospy.get_param('~fuel_capacity', 13.5)
brake_deadband = rospy.get_param('~brake_deadband', .1)
decel_limit = rospy.get_param('~decel_limit', -5)
accel_limit = rospy.get_param('~accel_limit', 1.)
wheel_radius = rospy.get_param('~wheel_radius', 0.2413)
wheel_base = rospy.get_param('~wheel_base', 2.8498)
steer_ratio = rospy.get_param('~steer_ratio', 14.8)
max_lat_accel = rospy.get_param('~max_lat_accel', 3.)
max_steer_angle = rospy.get_param('~max_steer_angle', 8.)
carla_low_speed_test = rospy.get_param('~carla_low_speed_test', 0.)
min_speed = 0 # obviously ... or not ?
self.steer_ratio = steer_ratio # DBW_TEST
self.carla_low_speed_test = carla_low_speed_test # DBW_TEST
self.yaw_controller = YawController(wheel_base, steer_ratio, min_speed,
max_lat_accel, max_steer_angle)
self.speed_controller = SpeedController(wheel_radius, vehicle_mass,
fuel_capacity, accel_limit,
decel_limit, brake_deadband,
carla_low_speed_test)
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)
# TODO: Create `TwistController` object
# self.controller = TwistController(<Arguments you wish to provide>)
# TODO: Subscribe to all the topics you need to
self.current_linear_vel = None
self.current_angular_vel = None
self.proposed_linear_vel = None
self.proposed_angular_vel = None
self.dbw_enabled = False
self.gpu_ready = False
self.sub1 = rospy.Subscriber("/twist_cmd",
TwistStamped,
self.twist_cmd_callback,
queue_size=1)
self.sub2 = rospy.Subscriber("/current_velocity",
TwistStamped,
self.current_velocity_callback,
queue_size=1)
self.sub3 = rospy.Subscriber("/vehicle/dbw_enabled",
Bool,
self.dbw_enabled_callback,
queue_size=1)
self.sub4 = rospy.Subscriber("/current_pose",
PoseStamped,
self.current_pose_callback,
queue_size=1)
self.sub5 = rospy.Subscriber("/base_waypoints",
Lane,
self.base_waypoints_callback,
queue_size=1)
self.sub6 = rospy.Subscriber("/gpu_ready",
Int32,
self.gpu_ready_callback,
queue_size=1)
self.loop()
class DBWNode(object):
def __init__(self):
rospy.init_node('dbw_node')
vehicle_mass = rospy.get_param('~vehicle_mass', 1736.35)
fuel_capacity = rospy.get_param('~fuel_capacity', 13.5)
brake_deadband = rospy.get_param('~brake_deadband', .1)
decel_limit = rospy.get_param('~decel_limit', -5)
accel_limit = rospy.get_param('~accel_limit', 1.)
wheel_radius = rospy.get_param('~wheel_radius', 0.2413)
wheel_base = rospy.get_param('~wheel_base', 2.8498)
steer_ratio = rospy.get_param('~steer_ratio', 14.8)
max_lat_accel = rospy.get_param('~max_lat_accel', 3.)
max_steer_angle = rospy.get_param('~max_steer_angle', 8.)
carla_low_speed_test = rospy.get_param('~carla_low_speed_test', 0.)
min_speed = 0 # obviously ... or not ?
self.steer_ratio = steer_ratio # DBW_TEST
self.carla_low_speed_test = carla_low_speed_test # DBW_TEST
self.yaw_controller = YawController(wheel_base, steer_ratio, min_speed,
max_lat_accel, max_steer_angle)
self.speed_controller = SpeedController(wheel_radius, vehicle_mass,
fuel_capacity, accel_limit,
decel_limit, brake_deadband,
carla_low_speed_test)
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)
# TODO: Create `TwistController` object
# self.controller = TwistController(<Arguments you wish to provide>)
# TODO: Subscribe to all the topics you need to
self.current_linear_vel = None
self.current_angular_vel = None
self.proposed_linear_vel = None
self.proposed_angular_vel = None
self.dbw_enabled = False
self.gpu_ready = False
self.sub1 = rospy.Subscriber("/twist_cmd",
TwistStamped,
self.twist_cmd_callback,
queue_size=1)
self.sub2 = rospy.Subscriber("/current_velocity",
TwistStamped,
self.current_velocity_callback,
queue_size=1)
self.sub3 = rospy.Subscriber("/vehicle/dbw_enabled",
Bool,
self.dbw_enabled_callback,
queue_size=1)
self.sub4 = rospy.Subscriber("/current_pose",
PoseStamped,
self.current_pose_callback,
queue_size=1)
self.sub5 = rospy.Subscriber("/base_waypoints",
Lane,
self.base_waypoints_callback,
queue_size=1)
self.sub6 = rospy.Subscriber("/gpu_ready",
Int32,
self.gpu_ready_callback,
queue_size=1)
self.loop()
def get_yaw(self, orientation_q):
orientation_list = [
orientation_q.x, orientation_q.y, orientation_q.z, orientation_q.w
]
roll, pitch, yaw = euler_from_quaternion(orientation_list)
# roll and pitch are always 0 anyways ...
return yaw
def base_waypoints_callback(self, msg):
num_waypoints = len(msg.waypoints)
for i in range(0, 20):
wp = msg.waypoints[i]
x = wp.pose.pose.position.x
y = wp.pose.pose.position.y
z = wp.pose.pose.position.z
yaw = self.get_yaw(wp.pose.pose.orientation)
theta_z = wp.twist.twist.angular.z
# Note that the coordinates for linear velocity are vehicle-centered
# So only the x-direction linear velocity should be nonzero.
vx = wp.twist.twist.linear.x
# all zero
#vy = wp.twist.twist.linear.y
#vz = wp.twist.twist.linear.z
#theta_x = wp.twist.twist.angular.x
#theta_y = wp.twist.twist.angular.y
#theta_z = wp.twist.twist.angular.z
#rospy.logwarn("wp[%d] x=%f y=%f z=%f yaw=%f theta_z=%f vx=%f", i, x, y, z, yaw, theta_z, vx)
def current_pose_callback(self, msg):
self.ego_x = msg.pose.position.x
self.ego_y = msg.pose.position.y
self.ego_z = msg.pose.position.z
yaw = self.get_yaw(msg.pose.orientation)
#rospy.logwarn("ego x=%f y=%f z=%f yaw=%f", self.ego_x, self.ego_y, self.ego_z, yaw)
def twist_cmd_callback(self, msg):
# in [x, y] ego coord
self.proposed_linear_vel = msg.twist.linear.x
self.proposed_angular_vel = msg.twist.angular.z
def current_velocity_callback(self, msg):
# in [x, y] ego coord
self.current_linear_vel = msg.twist.linear.x
self.current_angular_vel = msg.twist.angular.z
def gpu_ready_callback(self, msg):
self.gpu_ready = True
def dbw_enabled_callback(self, msg):
self.dbw_enabled = msg.data
rospy.logwarn("dbw_enabled=%d", self.dbw_enabled)
def loop(self):
rate = rospy.Rate(DBW_FREQUENCY) # 50Hz
while not rospy.is_shutdown():
# TODO: Get predicted throttle, brake, and steering using `twist_controller`
# You should only publish the control commands if dbw is enabled
# throttle, brake, steering = self.controller.control(<proposed linear velocity>,
# <proposed angular velocity>,
# <current linear velocity>,
# <dbw status>,
# <any other argument you need>)
# if <dbw is enabled>:
# self.publish(throttle, brake, steer)
if self.proposed_angular_vel is not None and self.current_linear_vel is not None:
if self.carla_low_speed_test:
steer = self.proposed_angular_vel * self.steer_ratio # DBW_TEST
else:
steer = self.yaw_controller.get_steering(
self.proposed_linear_vel, self.proposed_angular_vel,
self.current_linear_vel)
throttle, brake = self.speed_controller.get_throttle_brake(
self.proposed_linear_vel, self.current_linear_vel,
1.0 / DBW_FREQUENCY)
else:
throttle, brake, steer = 0., 0., 0.
if not self.gpu_ready:
# fake non 0 commands while GPU is not ready but with braking
throttle, brake, steer = 0.01, 0.25, steer
self.publish(throttle, brake, steer)
rospy.logwarn("throttle=%f brake=%f steer=%f", 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
self.brake_pub.publish(bcmd)
def __init__(self):
rospy.init_node('dbw_node')
vehicle_mass = rospy.get_param('~vehicle_mass', 1736.35)
fuel_capacity = rospy.get_param('~fuel_capacity', 13.5)
brake_deadband = rospy.get_param('~brake_deadband', .1)
decel_limit = rospy.get_param('~decel_limit', -5)
accel_limit = rospy.get_param('~accel_limit', 1.)
wheel_radius = rospy.get_param('~wheel_radius', 0.2413)
wheel_base = rospy.get_param('~wheel_base', 2.8498)
steer_ratio = rospy.get_param('~steer_ratio', 14.8)
max_lat_accel = rospy.get_param('~max_lat_accel', 3.)
max_steer_angle = rospy.get_param('~max_steer_angle', 8.)
max_acceleration = rospy.get_param('~max_acceleration', 1)
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)
self.dbw_enabled = True
self.waypoints = None
self.pose = None
self.velocity = None
self.twist = None
# TODO: Create `TwistController` object
# self.controller = TwistController(<Arguments you wish to provide>)
self.twist_controller = TwistController(max_steer_angle)
# TODO: Subscribe to all the topics you need to
rospy.Subscriber('/vehicle/dbw_enabled',
Bool,
self.dbw_cb,
queue_size=1)
rospy.Subscriber('/final_waypoints',
Lane,
self.waypoints_cb,
queue_size=1)
rospy.Subscriber('/current_pose',
PoseStamped,
self.pose_cb,
queue_size=1)
rospy.Subscriber('/current_velocity',
TwistStamped,
self.current_velocity_cb,
queue_size=1)
rospy.Subscriber('/twist_cmd',
TwistStamped,
self.twist_cb,
queue_size=1)
self.speed_controller = SpeedController(vehicle_mass, wheel_radius,
accel_limit, decel_limit,
brake_deadband, fuel_capacity,
max_acceleration)
self.yaw_controller = YawController(wheel_base, steer_ratio, 0,
max_lat_accel, max_steer_angle)
self.loop()
class DBWNode(object):
def __init__(self):
rospy.init_node('dbw_node')
vehicle_mass = rospy.get_param('~vehicle_mass', 1736.35)
fuel_capacity = rospy.get_param('~fuel_capacity', 13.5)
brake_deadband = rospy.get_param('~brake_deadband', .1)
decel_limit = rospy.get_param('~decel_limit', -5)
accel_limit = rospy.get_param('~accel_limit', 1.)
wheel_radius = rospy.get_param('~wheel_radius', 0.2413)
wheel_base = rospy.get_param('~wheel_base', 2.8498)
steer_ratio = rospy.get_param('~steer_ratio', 14.8)
max_lat_accel = rospy.get_param('~max_lat_accel', 3.)
max_steer_angle = rospy.get_param('~max_steer_angle', 8.)
max_acceleration = rospy.get_param('~max_acceleration', 1)
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)
self.dbw_enabled = True
self.waypoints = None
self.pose = None
self.velocity = None
self.twist = None
# TODO: Create `TwistController` object
# self.controller = TwistController(<Arguments you wish to provide>)
self.twist_controller = TwistController(max_steer_angle)
# TODO: Subscribe to all the topics you need to
rospy.Subscriber('/vehicle/dbw_enabled',
Bool,
self.dbw_cb,
queue_size=1)
rospy.Subscriber('/final_waypoints',
Lane,
self.waypoints_cb,
queue_size=1)
rospy.Subscriber('/current_pose',
PoseStamped,
self.pose_cb,
queue_size=1)
rospy.Subscriber('/current_velocity',
TwistStamped,
self.current_velocity_cb,
queue_size=1)
rospy.Subscriber('/twist_cmd',
TwistStamped,
self.twist_cb,
queue_size=1)
self.speed_controller = SpeedController(vehicle_mass, wheel_radius,
accel_limit, decel_limit,
brake_deadband, fuel_capacity,
max_acceleration)
self.yaw_controller = YawController(wheel_base, steer_ratio, 0,
max_lat_accel, max_steer_angle)
self.loop()
def loop(self):
rate = rospy.Rate(50) # 50Hz
while not rospy.is_shutdown():
# TODO: Get predicted throttle, brake, and steering using `twist_controller`
# You should only publish the control commands if dbw is enabled
# throttle, brake, steering = self.controller.control(<proposed linear velocity>,
# <proposed angular velocity>,
# <current linear velocity>,
# <dbw status>,
# <any other argument you need>)
# if <dbw is enabled>:
# self.publish(throttle, brake, steer)
if self.twist is None or self.velocity is None:
continue
try:
# Read target and current velocities
cte = dbw_helperfunctions.cte(self.pose, self.waypoints)
target_velocity = self.waypoints[0].twist.twist.linear.x
current_velocity = self.velocity.linear.x
# Get corrected steering using `twist_controller`
yaw_steering = self.yaw_controller.get_steering(
self.twist.linear.x, self.twist.angular.z,
current_velocity)
steer = self.twist_controller.control(cte, self.dbw_enabled)
throttle, brake = self.speed_controller.control(
target_velocity, current_velocity, 0.5)
except:
yaw_steering = 0
throttle, brake, steer = 0, 0, 0
if self.dbw_enabled:
self.publish(throttle, brake, steer + yaw_steering)
rate.sleep()
def dbw_cb(self, message):
"""From the incoming message extract the dbw_enabled variable """
self.dbw_enabled = bool(message.data)
def pose_cb(self, message):
self.pose = message.pose
def twist_cb(self, message):
"""From the incoming message extract the pose message """
self.twist = message.twist
def current_velocity_cb(self, message):
"""From the incoming message extract the velocity message """
self.velocity = message.twist
def waypoints_cb(self, message):
self.waypoints = message.waypoints
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
self.brake_pub.publish(bcmd)
def __init__(self):
rospy.init_node('dbw_node')
# Params:
vehicle_mass = rospy.get_param('~vehicle_mass', 1736.35)
fuel_capacity = rospy.get_param('~fuel_capacity', 13.5)
brake_deadband = rospy.get_param('~brake_deadband', .1)
decel_limit = rospy.get_param('~decel_limit', -5)
accel_limit = rospy.get_param('~accel_limit', 1.)
wheel_radius = rospy.get_param('~wheel_radius', 0.2413)
wheel_base = rospy.get_param('~wheel_base', 2.8498)
steer_ratio = rospy.get_param('~steer_ratio', 14.8)
max_lat_accel = rospy.get_param('~max_lat_accel', 3.)
max_steer_angle = rospy.get_param('~max_steer_angle', 8.)
min_lon_speed = rospy.get_param('~min_lon_speed', 0.)
controller_rate = rospy.get_param('~controller_rate', 20.)
tau_acceleration = rospy.get_param('~tau_acceleration', 0.3)
self.feed_forward_gain = 1
self.controller_rate = controller_rate
# Subscriber:
self.dbw_enabled = False
self.current_linear_velocity = None
self.current_linear_acceleration = None
self.current_pose = None
self.target_linear_velocity = None
self.target_angular_velocity = None
self.final_waypoints = []
rospy.Subscriber('/twist_cmd',
TwistStamped,
self.twist_cmd_cb,
queue_size=1)
rospy.Subscriber('/current_velocity',
TwistStamped,
self.current_velocity_cb,
queue_size=1)
rospy.Subscriber('/current_pose',
PoseStamped,
self.current_pose_cb,
queue_size=1)
rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_enabled_cb)
rospy.Subscriber('/final_waypoints',
Lane,
self.final_waypoints_cb,
queue_size=1)
# Controller:
self.speed_controller = SpeedController(
controller_rate,
accel_limit,
decel_limit,
brake_deadband,
vehicle_mass,
wheel_radius,
)
self.twist_controller = TwistController(controller_rate,
max_steer_angle)
self.yaw_controller = YawController(wheel_base, steer_ratio,
min_lon_speed, max_lat_accel,
max_steer_angle)
# Filter
self.lowpass_acceleration = LowPassFilter(tau=tau_acceleration,
ts=1.0 /
self.controller_rate)
# Publisher:
self.throttle_pub = rospy.Publisher('/vehicle/throttle_cmd',
ThrottleCmd,
queue_size=1)
self.brake_pub = rospy.Publisher('/vehicle/brake_cmd',
BrakeCmd,
queue_size=1)
self.steer_pub = rospy.Publisher('/vehicle/steering_cmd',
SteeringCmd,
queue_size=1)
# Execute loop for each message (this depends on the defined rate):
self.loop()
class DBWNode(object):
def __init__(self):
rospy.init_node('dbw_node')
# Params:
vehicle_mass = rospy.get_param('~vehicle_mass', 1736.35)
fuel_capacity = rospy.get_param('~fuel_capacity', 13.5)
brake_deadband = rospy.get_param('~brake_deadband', .1)
decel_limit = rospy.get_param('~decel_limit', -5)
accel_limit = rospy.get_param('~accel_limit', 1.)
wheel_radius = rospy.get_param('~wheel_radius', 0.2413)
wheel_base = rospy.get_param('~wheel_base', 2.8498)
steer_ratio = rospy.get_param('~steer_ratio', 14.8)
max_lat_accel = rospy.get_param('~max_lat_accel', 3.)
max_steer_angle = rospy.get_param('~max_steer_angle', 8.)
min_lon_speed = rospy.get_param('~min_lon_speed', 0.)
controller_rate = rospy.get_param('~controller_rate', 20.)
tau_acceleration = rospy.get_param('~tau_acceleration', 0.3)
self.feed_forward_gain = 1
self.controller_rate = controller_rate
# Subscriber:
self.dbw_enabled = False
self.current_linear_velocity = None
self.current_linear_acceleration = None
self.current_pose = None
self.target_linear_velocity = None
self.target_angular_velocity = None
self.final_waypoints = []
rospy.Subscriber('/twist_cmd',
TwistStamped,
self.twist_cmd_cb,
queue_size=1)
rospy.Subscriber('/current_velocity',
TwistStamped,
self.current_velocity_cb,
queue_size=1)
rospy.Subscriber('/current_pose',
PoseStamped,
self.current_pose_cb,
queue_size=1)
rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_enabled_cb)
rospy.Subscriber('/final_waypoints',
Lane,
self.final_waypoints_cb,
queue_size=1)
# Controller:
self.speed_controller = SpeedController(
controller_rate,
accel_limit,
decel_limit,
brake_deadband,
vehicle_mass,
wheel_radius,
)
self.twist_controller = TwistController(controller_rate,
max_steer_angle)
self.yaw_controller = YawController(wheel_base, steer_ratio,
min_lon_speed, max_lat_accel,
max_steer_angle)
# Filter
self.lowpass_acceleration = LowPassFilter(tau=tau_acceleration,
ts=1.0 /
self.controller_rate)
# Publisher:
self.throttle_pub = rospy.Publisher('/vehicle/throttle_cmd',
ThrottleCmd,
queue_size=1)
self.brake_pub = rospy.Publisher('/vehicle/brake_cmd',
BrakeCmd,
queue_size=1)
self.steer_pub = rospy.Publisher('/vehicle/steering_cmd',
SteeringCmd,
queue_size=1)
# Execute loop for each message (this depends on the defined rate):
self.loop()
def twist_cmd_cb(self, msg):
self.target_linear_velocity = msg.twist.linear.x
self.target_angular_velocity = msg.twist.angular.z
def current_velocity_cb(self, msg):
if self.current_linear_velocity is not None:
accel = (self.current_linear_velocity -
msg.twist.linear.x) * self.controller_rate
self.current_linear_acceleration = self.lowpass_acceleration.filt(
accel)
self.current_linear_velocity = msg.twist.linear.x
def final_waypoints_cb(self, msg):
self.final_waypoints = msg.waypoints
def current_pose_cb(self, msg):
self.current_pose = msg.pose
def dbw_enabled_cb(self, msg):
if msg.data:
self.dbw_enabled = True
else:
self.dbw_enabled = False
self.speed_controller.reset()
self.twist_controller.reset()
def loop(self):
rate = rospy.Rate(self.controller_rate)
while not rospy.is_shutdown():
rospy.loginfo(self.final_waypoints)
if self.target_linear_velocity is None \
or self.current_linear_velocity is None \
or self.current_linear_acceleration is None \
or self.current_pose is None \
or self.final_waypoints is None:
continue
if (len(self.final_waypoints) > 2):
# Calculate errors
cross_track_error = get_cross_track_error_from_frenet(
self.final_waypoints, self.current_pose)
steer_twist = self.twist_controller.control(cross_track_error)
target_linear_velocity = float(
np.sqrt(self.final_waypoints[1].twist.twist.linear.x**2 +
self.final_waypoints[1].twist.twist.linear.y**2))
steer_yaw = self.yaw_controller.get_steering(
linear_velocity=target_linear_velocity,
angular_velocity=self.target_angular_velocity,
current_velocity=self.current_linear_velocity)
throttle, brake = self.speed_controller.control(
target_linear_velocity=target_linear_velocity,
current_linear_velocity=self.current_linear_velocity,
current_linear_acceleration=self.
current_linear_acceleration)
steer = steer_twist + steer_yaw * self.feed_forward_gain
#rospy.logwarn('cte %0.2f, ang_vel %0.2f, steer(twist/yaw) %0.2f %0.2f', \
# cross_track_error, self.target_angular_velocity, steer_twist, steer_yaw)
#rospy.logwarn('Target WP Velocity %0.2f, throttle %0.2f, brake %0.2f', target_linear_velocity, throttle, brake)
else:
rospy.logwarn('[dbw_node] No more final_waypoints')
throttle = 0
brake = 10000
steer = 0
if self.dbw_enabled:
self.publish(throttle, brake, steer)
rate.sleep(
) # wiki.ros.org/rospy/Overview/Time#Sleeping_and_Rates --> wait until next rate
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)
bcmd = BrakeCmd()
bcmd.enable = True
bcmd.pedal_cmd_type = BrakeCmd.CMD_TORQUE
bcmd.pedal_cmd = brake
self.brake_pub.publish(bcmd)
scmd = SteeringCmd()
scmd.enable = True
scmd.steering_wheel_angle_cmd = steer
self.steer_pub.publish(scmd)
class DBWNode(object):
def __init__(self):
rospy.init_node('dbw_node', log_level=rospy.DEBUG)
rospy.logdebug('DBW Initialization')
vehicle_mass = rospy.get_param('~vehicle_mass', 1736.35)
fuel_capacity = rospy.get_param('~fuel_capacity', 13.5)
brake_deadband = rospy.get_param('~brake_deadband', .1)
decel_limit = rospy.get_param('~decel_limit', -5)
accel_limit = rospy.get_param('~accel_limit', 1.)
wheel_radius = rospy.get_param('~wheel_radius', 0.2413)
wheel_base = rospy.get_param('~wheel_base', 2.8498)
steer_ratio = rospy.get_param('~steer_ratio', 14.8)
max_lat_accel = rospy.get_param('~max_lat_accel', 3.)
max_steer_angle = rospy.get_param('~max_steer_angle', 8.)
self.speed = 0
self.speed_demand = 0
self.yaw_rate = 0
self.yaw_rate_demand = 0
self.rate = 50 # DBW node rate (Hz)
self.dbw_enabled = False
# Publications
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)
self.speed_control_diag_pub = rospy.Publisher('/speed_control_diag',
ControlDiag,
queue_size=1)
# Subscriptions
self.dbw_enabled_sub = rospy.Subscriber('/vehicle/dbw_enabled', Bool,
self.dbw_enabled_cb)
self.speed_sub = rospy.Subscriber('/current_velocity', TwistStamped,
self.speed_cb)
self.twist_cmd_sub = rospy.Subscriber('/twist_cmd', TwistStamped,
self.twist_cmd_cb)
self.speed_controller = SpeedController()
self.yaw_rate_controller = YawRateController(max_steer_angle,
steer_ratio, wheel_base)
self.loop()
def loop(self):
rate = rospy.Rate(self.rate) # 50Hz
rospy.logdebug('DBW Rate set')
while not rospy.is_shutdown():
# TODO Can we assume the sampling time is constant? Or do we need to calculated the time elapsed since last exec?
dt = 1. / self.rate
throttle, brake = self.speed_controller.control(
self.speed_demand, self.speed, dt)
steer = self.yaw_rate_controller.control(self.speed,
self.yaw_rate_demand)
if self.dbw_enabled:
rospy.loginfo('Speed Demand: %f, Speed Actual: %f',
self.speed_demand, self.speed)
rospy.loginfo('Yaw Rate Demand: %f, Yaw Rate Actual: %f',
self.yaw_rate_demand, self.yaw_rate)
self.publish(throttle, brake, steer)
else:
self.speed_controller.reset()
# Diagnotics
speed_diag = ControlDiag()
speed_diag.reference = self.speed_demand
speed_diag.actuator = self.speed_controller.pid.actuator
speed_diag.error = self.speed_controller.pid.error
speed_diag.i_error = self.speed_controller.pid.i_error
speed_diag.d_error = self.speed_controller.pid.d_error
self.speed_control_diag_pub.publish(speed_diag)
rate.sleep()
def publish(self, throttle, brake, steer):
# Take care not to publish both throttle and brake
if throttle > 0:
tcmd = ThrottleCmd()
tcmd.enable = True
tcmd.pedal_cmd_type = ThrottleCmd.CMD_PERCENT
tcmd.pedal_cmd = throttle
self.throttle_pub.publish(tcmd)
if self.speed > 0.1:
scmd = SteeringCmd()
scmd.enable = True
scmd.steering_wheel_angle_cmd = steer
self.steer_pub.publish(scmd)
rospy.loginfo('steer cmd: %s', steer)
if brake > 0:
bcmd = BrakeCmd()
bcmd.enable = True
bcmd.pedal_cmd_type = BrakeCmd.CMD_PERCENT
# It's uncertain what these units should be. It looks like
# CMD_PERCENT is 0-1 for ThrottleCmd but 0-100 for BrakeCmd
# Further it looks like 100% is not a lot of braking at all
# So I've doubled that amount here to have a moderate amount
# of braking as observed in the simulator
bcmd.pedal_cmd = brake * 200
bcmd.boo_cmd = True
self.brake_pub.publish(bcmd)
def speed_cb(self, msg):
self.speed = msg.twist.linear.x
self.yaw_rate = msg.twist.angular.z
def dbw_enabled_cb(self, msg):
self.dbw_enabled = msg.data
def twist_cmd_cb(self, msg):
self.speed_demand = msg.twist.linear.x # m/s
self.yaw_rate_demand = msg.twist.angular.z # rad/s
class DBWNode(object):
#pylint: disable=too-many-instance-attributes
"""
Drive by Wire Node:
The goal of this node is to get waypoint information and transform it
to controller commands (throttle, brake, steer).
The commands are published to their related channels.
"""
def __init__(self):
"""
1. Reads basics parameters.
2. Subscibes to the required channels to get realtime data.
3. Creates required channels to publish contoller actions.
4. Creates the required controllers
5. Starts the internal infinite loop.
"""
rospy.init_node('dbw_node')
# class variables
self.dbw_enabled = True # dbw_enabled = false => manual driving
self.waypoints = None
self.pose = None
self.velocity = None
self.twist = None
# read ros parameters
vehicle_mass = rospy.get_param('~vehicle_mass', 1736.35)
fuel_capacity = rospy.get_param('~fuel_capacity', 13.5)
brake_deadband = rospy.get_param('~brake_deadband', .1)
wheel_radius = rospy.get_param('~wheel_radius', 0.2413)
decel_limit = rospy.get_param('~decel_limit', -5)
accel_limit = rospy.get_param('~accel_limit', 1.)
wheel_base = rospy.get_param('~wheel_base', 2.8498)
steer_ratio = rospy.get_param('~steer_ratio', 14.8)
max_lat_accel = rospy.get_param('~max_lat_accel', 3.)
max_steer_angle = rospy.get_param('~max_steer_angle', 8.)
max_acceleration = rospy.get_param('~max_acceleration', 1.5)
min_speed = 0.0
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)
self.controller = Controller(max_steer_angle)
# pylint disable=E1121
self.speed_controller = SpeedController(vehicle_mass,
wheel_radius,
accel_limit,
decel_limit,
brake_deadband,
fuel_capacity,
max_acceleration)
self.yaw_controller = YawController(wheel_base,
steer_ratio,
min_speed,
max_lat_accel,
max_steer_angle)
# Subscribe to all the topics you need to
rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_cb, queue_size=1)
rospy.Subscriber('/final_waypoints', Lane, self.waypoints_cb, queue_size=1)
rospy.Subscriber('/current_pose', PoseStamped, self.pose_cb, queue_size=1)
rospy.Subscriber('/current_velocity', TwistStamped, self.velocity_cb, queue_size=1)
rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_cb, queue_size=1)
self.loop()
def loop(self):
"""The main functionallity of the DBW Node."""
rate = rospy.Rate(10) # 10Hz
while not rospy.is_shutdown():
data = [self.velocity, self.waypoints, self.pose]
all_available = all([x is not None for x in data])
if not all_available:
continue
if len(self.waypoints) >= dbw_helper.POINTS_TO_FIT:
# Read target and current velocities
cte = dbw_helper.cte(self.pose, self.waypoints)
target_velocity = self.waypoints[0].twist.twist.linear.x
current_velocity = self.velocity.linear.x
# Get corrected steering using `twist_controller`
steer = self.controller.control(cte, self.dbw_enabled)
# Get predicted steering angle from road curvature
yaw_steer = self.yaw_controller.get_steering(self.twist.linear.x,
self.twist.angular.z,
current_velocity)
throttle, brake = self.speed_controller.control(target_velocity,
current_velocity,
0.5)
else:
# if too few waypoints and publish a hard break
rospy.logwarn("Number of waypoint received: %s", len(self.waypoints))
throttle, brake, steer = 0, 2000, 0
if self.dbw_enabled:
self.publish(throttle, brake, steer + PREDICTIVE_STEERING * yaw_steer)
rate.sleep()
def publish(self, throttle, brake, steer):
"""
Publishes the contoller commands to the corresponding topic channels
Args:
throttle (float): Throttle to apply
brake (float): Brake to apply
steer (float): Steering angle in radians
"""
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
self.brake_pub.publish(bcmd)
def dbw_cb(self, message):
"""From the incoming message extract the dbw_enabled variable """
self.dbw_enabled = bool(message.data)
def velocity_cb(self, message):
"""From the incoming message extract the velocity message """
self.velocity = message.twist
def pose_cb(self, message):
"""From the incoming message extract the pose message """
self.pose = message.pose
def twist_cb(self, message):
"""From the incoming message extract the pose message """
self.twist = message.twist
def waypoints_cb(self, message):
"""Update final waypoints array when a new message arrives
on the corresponding channel
"""
self.waypoints = message.waypoints