class DBWNode(object):
def __init__(self):
rospy.init_node('dbw_node')
self.current_linear_velocity = 0.0
self.target_linear_velocity = 0.0
self.current_angular_velocity = 0.0
self.target_angular_velocity = 0.0
self.dbw_enabled = False
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.) # meters/s^2
max_steer_angle = rospy.get_param('~max_steer_angle',
8.) #=458 degrees
# steer:wheel = 14.8:1,
# max_steer_angle = 8 rads = 458 degrees
# 458/14.8 = ~30 degrees (max steering range: -15 to 15)
# steering_sensitivity is in radians^(-1)
# this number is used to get the noramlized steering
self.steering_sensitivity = steer_ratio / max_steer_angle * 2.0
#rospy.logerr('st_sen:{} {} {}'.format( steer_ratio, max_steer_angle, self.steering_sensitivity))
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 = TwistController() #(<Arguments you wish to provide>)
#max_steer_angle is half as in yaw_controller, we have range of (-1* max_steer_angle to max_steer_angle)
self.controller.yaw_controller = YawController(wheel_base, steer_ratio,
min_speed,
max_lat_accel,
max_steer_angle)
rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_cb)
rospy.Subscriber('/current_velocity', TwistStamped, self.velocity_cb)
rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_cb)
rospy.spin() # ===> use spin, publishing cmd right away in twist_cb
def dbw_cb(self, msg):
if (self.dbw_enabled != msg.data):
self.dbw_enabled = msg.data
pass
def velocity_cb(self, msg):
self.current_linear_velocity = msg.twist.linear.x
self.current_angular_velocity = msg.twist.angular.z
pass
def twist_cb(self, msg):
self.target_linear_velocity = msg.twist.linear.x
self.target_angular_velocity = msg.twist.angular.z
target_linear_velocity = self.target_linear_velocity * MPH_2_mps
if self.dbw_enabled: #<dbw is enabled>:
throttle, brake, steer = self.controller.control(
self.current_linear_velocity, target_linear_velocity,
self.steering_sensitivity, self.target_angular_velocity)
self.publish(throttle, brake, steer)
#if brake > 10:
# rospy.logerr('dbw_node:{: f}\t{: f}\t{: f}'.format(steer, throttle, brake))
pass
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)
''' brake command will set throttle value to 0, even if brake is zero'''
''' therefore the brake command is only sent if we are braking '''
if brake > 0:
bcmd = BrakeCmd()
bcmd.enable = True
bcmd.pedal_cmd_type = BrakeCmd.CMD_TORQUE
bcmd.pedal_cmd = brake
self.brake_pub.publish(bcmd)
class DBWNode(object):
def __init__(self):
rospy.init_node('dbw_node')
# Fetch value from parameter server
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.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)
max_throttle_percentage = rospy.get_param('~max_throttle_percentage')
max_braking_percentage = rospy.get_param('~max_braking_percentage')
params = {
'vehicle_mass': vehicle_mass,
'fuel_capacity': fuel_capacity,
'brake_deadband': brake_deadband,
'decel_limit': decel_limit,
'accel_limit': accel_limit,
'wheel_radius': wheel_radius,
'wheel_base': wheel_base,
'steer_ratio': steer_ratio,
'max_lat_accel': max_lat_accel,
'max_steer_angle': max_steer_angle,
'max_throttle_percentage': max_throttle_percentage,
'max_braking_percentage': max_braking_percentage
}
self.current_velocity = None
self.twist_cmd = None
self.dbw_enabled = False
self.controller = TwistController(**params)
rospy.Subscriber('/current_velocity', TwistStamped,
self.current_velocity_cb)
rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_cmd_cb)
rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_enabled_cb)
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 and angular velocity>,
# <current linear and angular velocity>,
# <dbw status>,
# <any other argument you need>)
# if <dbw is enabled>:
# self.publish(throttle, brake, steer)
if self.current_velocity and self.twist_cmd:
# Adjust control commands according to target twist(linear/angular speed)
throttle, brake, steer = self.controller.control(
self.twist_cmd.twist, self.current_velocity.twist,
self.dbw_enabled)
if self.dbw_enabled:
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
self.brake_pub.publish(bcmd)
def current_velocity_cb(self, msg):
self.current_velocity = msg
def twist_cmd_cb(self, msg):
self.twist_cmd = msg
rospy.loginfo(msg)
def dbw_enabled_cb(self, msg):
self.dbw_enabled = msg
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.)
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 = False
self.tl_detector_ready = False
self.target_velocity = 0.
self.target_yaw_dot = 0.
self.current_velocity = 0.
self.current_yaw_dot = 0.
self.last_update_time = None
# Initialize TwistController
self.twist_controller = TwistController(accel_limit, -1.,
max_steer_angle,
BrakeCmd.TORQUE_MAX,
wheel_base, steer_ratio,
max_lat_accel, max_steer_angle)
# Subscribe to topics
rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_cmd_callback)
rospy.Subscriber('/current_velocity', TwistStamped,
self.current_velocity_callback)
rospy.Subscriber('/vehicle/dbw_enabled', Bool,
self.dbw_enabled_callback)
rospy.Subscriber('/tl_detector_ready', Bool,
self.tl_detector_ready_callback)
self.loop()
def loop(self):
rate = rospy.Rate(50) # 50Hz
while not rospy.is_shutdown():
# hold brake if tl_detector not ready
if not self.tl_detector_ready or self.last_update_time is None:
self.last_update_time = rospy.Time.now()
self.publish(0, BrakeCmd.TORQUE_MAX, 0)
rate.sleep()
continue
current_time = rospy.Time.now()
dt = current_time.to_sec() - self.last_update_time.to_sec()
self.last_update_time = current_time
if (dt > 0.075):
rospy.logwarn('slow DBW update, dt:%.3fs freq:%.1fhz', dt,
1. / dt)
# reset controller PIDs and skip calculations if DBW is off
if not self.dbw_enabled:
self.twist_controller.reset()
rate.sleep()
continue
accel, steer = self.twist_controller.control(
self.target_velocity, self.target_yaw_dot,
self.current_velocity, dt)
throttle = 0
brake = 0
if (accel < 0):
brake = -accel
else:
throttle = accel
# record data for debugging
# self.data_recorder(self.target_velocity, self.target_yaw_dot, throttle, brake, steer)
# rospy.loginfo('DBW a:%.3f y:%.3f', self.target_velocity, self.target_yaw_dot)
# rospy.loginfo('DBW t:%.3f b:%.3f s:%.3f', throttle, brake, steer)
self.publish(throttle, brake, steer)
rate.sleep()
def twist_cmd_callback(self, msg):
self.target_velocity = msg.twist.linear.x
self.target_yaw_dot = msg.twist.angular.z
# rospy.loginfo('twist_cmd: v:%.3f yd:%.3f', self.target_velocity, self.target_yaw_dot)
# log_twist_msg(msg, 'twist_cmd:')
def current_velocity_callback(self, msg):
self.current_velocity = msg.twist.linear.x
self.current_yaw_dot = msg.twist.angular.z
# rospy.loginfo('current_velocity: v:%.3f yd:%.3f', self.current_velocity, self.current_yaw_dot)
# log_twist_msg(msg, 'current_velocity:')
def dbw_enabled_callback(self, msg):
self.dbw_enabled = msg.data
# rospy.loginfo('dbw_enabled: %d', self.dbw_enabled)
def tl_detector_ready_callback(self, msg):
self.tl_detector_ready = msg.data
#rospy.loginfo('tl_detector_ready: %s', self.tl_detector_ready)
def publish(self, throttle, brake, steer):
#rospy.logwarn("In publish: %d; %d; %d;", 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 data_recorder(self, target_velocity, target_yaw_dot, throttle, brake,
steer):
dbw_data_filename = '/tmp/dbw_data.csv'
try:
foo = self.dbw_data
except:
self.dbw_data = []
with open(dbw_data_filename, 'wb') as csvfile:
rospy.loginfo('DBW data file: %s', csvfile.name)
csv_writer = csv.writer(csvfile, delimiter=',')
csv_writer.writerow([
'time', 't_speed', 't_yd', 'throttle', 'brake', 'steer',
'c_speed', 'c_yd'
])
time = rospy.Time.now().to_sec()
self.dbw_data.append([
time, target_velocity, target_yaw_dot, throttle, brake, steer,
self.current_velocity, self.current_yaw_dot
])
if len(self.dbw_data) == 1000:
with open(dbw_data_filename, 'ab') as csvfile:
csv_writer = csv.writer(csvfile, delimiter=',')
for row in self.dbw_data:
csv_writer.writerow(row)
rospy.loginfo('DBW data saved')
self.dbw_data = []
class DBWNode(object):
def __init__(self):
rospy.init_node('dbw_node')
#rospy.loginfo('dbw node init')
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.steer_pub = rospy.Publisher('/vehicle/steering_cmd',
SteeringCmd,
queue_size=30)
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(wheel_base, wheel_radius,
vehicle_mass, steer_ratio,
max_lat_accel, max_steer_angle)
# TODO: Subscribe to all the topics you need to
sub1 = rospy.Subscriber("/twist_cmd", TwistStamped,
self.twistCmdCallback)
sub2 = rospy.Subscriber("/current_velocity", TwistStamped,
self.currentVelCallback)
sub3 = rospy.Subscriber("/vehicle/dbw_enabled", Bool,
self.enabledCallback)
self.target_linear_vel = 0
self.current_vel = 0
self.target_angular_vel = 0
self.dbw_enabled = False
self.loop()
def loop(self):
#rospy.loginfo('dbw node loop called')
#rate = rospy.Rate(10) # 10Hz
rate = rospy.Rate(50) # 50Hz
while not rospy.is_shutdown():
#rospy.logdebug('dbw node while loop start')
# 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>)
throttle, brake, steering = self.controller.control(
self.target_linear_vel, self.target_angular_vel,
self.current_vel)
rospy.loginfo('dbw node in loop before enable')
if self.dbw_enabled:
rospy.loginfo('dbw node enabled publish')
self.publish(throttle, brake, steering)
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 twistCmdCallback(self, msg):
self.target_linear_vel = msg.twist.linear.x
self.target_angular_vel = msg.twist.angular.z
def currentVelCallback(self, msg):
self.current_vel = msg.twist.linear.x
def enabledCallback(self, msg):
rospy.loginfo('enable callback')
self.dbw_enabled = msg.data
class DBWNode(object):
def __init__(self):
rospy.init_node('dbw_node')
rospy.loginfo('abhishek - Starting 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.)
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
# specify the controller gain parameters
#1) Velocity controller
#self.velocity_control_params = [0.3, 0.01, 0.02]
self.velocity_control_params = [0.4, 0.0, 0.02]
#2 Yaw controller
yaw_controller = YawController(wheel_base, steer_ratio, ONE_MPH,
max_lat_accel, max_steer_angle)
total_vehicle_mass = vehicle_mass + fuel_capacity * GAS_DENSITY
# Initalize the TwistController object
self.controller = TwistController(self.velocity_control_params,
total_vehicle_mass, accel_limit,
decel_limit, wheel_radius,
yaw_controller)
# TODO: Subscribe to all the topics you need to
rospy.Subscriber('/current_velocity',
TwistStamped,
self.cv_callback,
queue_size=1)
rospy.Subscriber('/twist_cmd',
TwistStamped,
self.twist_callback,
queue_size=1)
rospy.Subscriber('/vehicle/dbw_enabled',
Bool,
self.dbw_callback,
queue_size=1)
self.dbw_enabled = False
# Init Twist values
self.twist_cmd_twist = TwistStamped().twist
self.current_velocity_twist = TwistStamped().twist
# self.desired_linear_vel = 0.0 # only x direction required
# self.desired_ang_vel = 0.0 # only z direction required
# self.actual_linear_vel = 0.0
# self.actual_ang_vel = 0.0
# Timers for the logitudinal controller. Not sure if we can rely on 50Hz rate.
self.prev_vel_msg_time = 0.0
# Specify the loop rate for the node
self.loop_rate = 50.0
self.loop()
def cv_callback(self, msg):
# Maybe it is to keep it ad twist object as we need to pass it to twist_controller for yaw_controller
self.current_velocity_twist = msg.twist
# self.actual_linear_vel = msg.twist.linear.x
# self.actual_ang_vel = msg.twist.angular.z
# rospy.loginfo('abhishek - cv_callback: act_vel: %s' % str(self.actual_linear_vel))
def twist_callback(self, msg):
# Maybe it is to keep it ad twist object as we need to pass it to twist_controller for yaw_controller
self.twist_cmd_twist = msg.twist
# self.desired_linear_vel = msg.twist.linear.x
# self.desired_ang_vel = msg.twist.angular.z
# rospy.loginfo('abhishek - twist_callback: desired_linear_vel: %s' % str(self.desired_linear_vel))
def dbw_callback(self, msg):
self.dbw_enabled = msg.data # data is a boolean value
rospy.loginfo('abhishek - dbw_received: %s' % self.dbw_enabled)
def loop(self):
rate = rospy.Rate(int(self.loop_rate)) # 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
current_time = rospy.get_time()
delta_time = current_time - self.prev_vel_msg_time
self.prev_vel_msg_time = current_time
# calculate the velocity error . in meter / sec
# velocity_error_mps = self.desired_linear_vel - self.actual_linear_vel
# Use dbw value to reset the controller states if needed.
throttle, brake, steering = self.controller.control(
self.dbw_enabled, self.twist_cmd_twist,
self.current_velocity_twist, delta_time)
# rospy.loginfo('dbw_enable: %s, vel_error: %s , throttle: %s \n '% (self.dbw_enabled, velocity_error_mps, throttle))
rospy.loginfo('throttle: %s, brake: %s, steering: %s \n ' %
(throttle, brake, steering))
if self.dbw_enabled:
self.publish(throttle, brake, steering)
# self.publish(1.0, 0.0, 0.0)
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)
class DBWNode(object):
def __init__(self):
rospy.init_node('dbw_node', log_level=LOG_LEVEL)
# Fetch value from parameter server
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_throttle_percentage = rospy.get_param('~max_throttle_percentage',
0.8)
max_brake_percentage = rospy.get_param('~max_brake_percentage', -0.8)
params = {
'vehicle_mass': vehicle_mass,
'fuel_capacity': fuel_capacity,
'brake_deadband': brake_deadband,
'decel_limit': decel_limit,
'accel_limit': accel_limit,
'wheel_radius': wheel_radius,
'wheel_base': wheel_base,
'steer_ratio': steer_ratio,
'max_lat_accel': max_lat_accel,
'max_steer_angle': max_steer_angle,
'max_throttle_percentage': max_throttle_percentage,
'max_brake_percentage': max_brake_percentage
}
self.current_velocity = None
self.twist_cmd = None
self.dbw_enabled = False
self.twist_controller = TwistController(**params)
rospy.Subscriber('/current_velocity', TwistStamped,
self.current_velocity_cb)
rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_cmd_cb)
rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_enabled_cb)
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.loop()
def loop(self):
rate = rospy.Rate(DBW_CONTROLLER_RATE)
while not rospy.is_shutdown():
if self.current_velocity and self.twist_cmd:
# Run twist controller to get throttle brake and steer
throttle, brake, steer = self.twist_controller.control(
proposed_twist=self.twist_cmd.twist,
current_twist=self.current_velocity.twist,
dbw_enabled=self.dbw_enabled)
rospy.loginfo("throttle=%.3f, brake=%.3f, steer=%.3f",
throttle, brake, steer)
# Publish only if dbw_enabled
if self.dbw_enabled:
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
self.brake_pub.publish(bcmd)
def current_velocity_cb(self, msg):
self.current_velocity = msg
def twist_cmd_cb(self, msg):
self.twist_cmd = msg
# rospy.loginfo(msg)
def dbw_enabled_cb(self, msg):
self.dbw_enabled = msg
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.)
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(vehicle_mass, brake_deadband, decel_limit, accel_limit, wheel_radius, wheel_base, steer_ratio, max_lat_accel, max_steer_angle)
# TODO: Subscribe to all the topics you need to
self.current_velocity_sub = rospy.Subscriber("/current_velocity", TwistStamped, self.current_velocity_callback)
self.twist_cmd_sub = rospy.Subscriber("/twist_cmd", TwistStamped, self.twist_cmd_callback)
self.dbw_enabled_sub = rospy.Subscriber("/vehicle/dbw_enabled", Bool, self.dbw_enabled_callback)
self.pose_sub = rospy.Subscriber('/current_pose', PoseStamped, self.pose_callback)
self.waypoint_sub = rospy.Subscriber('final_waypoints', Lane, self.waypoint_callback)
#set up class variables to store data from subscribers
self.current_velocity = 0.0
self.velocity_cmd = 0.0
self.angular_velocity_cmd = 0.0
self.dbw_enabled = False
self.car_position = [0, 0, 0]
self.waypoint_position = [0, 0, 0]
#set up timestamp for measuring actual cycle time
self.time = rospy.get_time()
self.loop()
def current_velocity_callback(self, data):
self.current_velocity = data.twist.linear.x
def twist_cmd_callback(self, data):
self.velocity_cmd = data.twist.linear.x
self.angular_velocity_cmd = data.twist.angular.z
def dbw_enabled_callback(self, data):
rospy.logwarn("dbw_enabled:{}".format(data))
self.dbw_enabled = data
def pose_callback(self, data):
self.car_position[0] = data.pose.position.x
self.car_position[1] = data.pose.position.y
self.car_position[2] = data.pose.position.z
def waypoint_callback(self, data):
#get position of first waypoint ahead of car
self.waypoint_position[0] = data.waypoints[0].pose.pose.position.x
self.waypoint_position[1] = data.waypoints[0].pose.pose.position.y
self.waypoint_position[2] = data.waypoints[0].pose.pose.position.z
def loop(self):
rate = rospy.Rate(50) # 50Hz
while not rospy.is_shutdown():
new_time = rospy.get_time()
dt = new_time - self.time
self.time = new_time
# TODO: Get predicted throttle, brake, and steering using `twist_controller`
# You should only publish the control commands if dbw is enabled
#calculate distance between car and next waypoint
distance = math.sqrt( (self.waypoint_position[0] - self.car_position[0])**2 + (self.waypoint_position[1] - self.car_position[1])**2 + (self.waypoint_position[2] - self.car_position[2])**2)
#calculate desired acceleration using equation vf^2 = vi^2 + 2*a*d
if distance == 0:
acceleration = 0.0
else:
acceleration = (self.velocity_cmd**2 - self.current_velocity**2)/(2*distance)
throttle, brake, steering = self.controller.control(self.velocity_cmd, self.current_velocity, acceleration, self.angular_velocity_cmd, dt, self.dbw_enabled)
if self.dbw_enabled:
self.publish(throttle, brake, steering)
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)
class DBWNode(object):
def __init__(self):
rospy.init_node('dbw_node')
vehicle_capabilites = {
'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.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)
# Set the initial stage prior to recieving any messages.
self.dbw_enabled = False
self.latest_current_velocity_msg = None
self.latest_twist_msg = None
self.controller = TwistController(**vehicle_capabilites)
# Messages arrive async, each callback updates this
# class with private copy of last update seen.
#Topic to receive target linear and angular velocities
rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_cmd_callback)
# Subscribe to current velocity topic.
rospy.Subscriber('/current_velocity', TwistStamped,
self.current_vel_callback)
#Messge that indicates if dbw is enabled or not.
rospy.Subscriber('/vehicle/dbw_enabled', Bool,
self.dbw_enabled_callback)
self.loop()
def loop(self):
rate = rospy.Rate(50) # 50Hz
while not rospy.is_shutdown():
# Get predicted throttle, brake, and steering using `twist_controller`
if (self.latest_twist_msg != None
and self.latest_current_velocity_msg != None):
proposed_linear_vel = self.latest_twist_msg.twist.linear.x
proposed_angular_vel = self.latest_twist_msg.twist.angular.z
current_linear_vel = self.latest_current_velocity_msg.twist.linear.x
#if current_linear_vel < 0.001 :
# current_linear_vel = 0.
throttle, brake, steer = self.controller.control(
proposed_linear_vel, proposed_angular_vel,
current_linear_vel, self.dbw_enabled)
else:
throttle = brake = steer = 0.
if (self.dbw_enabled is True):
self.publish(throttle, brake, steer)
rate.sleep()
def publish(self, throttle, brake, steer):
#rospy.logwarn(" Throttle:%f : brake:%f Steer:%f ",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 dbw_enabled_callback(self, cb_msg):
# Extract True/False message.
self.dbw_enabled = cb_msg.data
def current_vel_callback(self, cb_msg):
self.latest_current_velocity_msg = cb_msg
def twist_cmd_callback(self, cb_msg):
self.latest_twist_msg = cb_msg
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.)
self.rate = 50 # Hz
self.sample_time = 1.0 / self.rate
self.dbw_enabled = True
self.initialized = False
self.current_linear_velocity = None
self.target_linear_velocity = None
self.target_angular_velocity = None
self.final_waypoints = None
self.current_pose = None
self.logger = DBWLogger(self, rate=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)
yaw_controller = YawController(wheel_base=wheel_base,
steer_ratio=steer_ratio,
min_speed=0.,
max_lat_accel=max_lat_accel,
max_steer_angle=max_steer_angle)
self.controller = TwistController(yaw_controller, max_steer_angle,
self.sample_time)
rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_enabled_cb)
rospy.Subscriber('/current_velocity',
TwistStamped,
self.current_velocity_cb,
queue_size=1)
rospy.Subscriber('/twist_cmd',
TwistStamped,
self.twist_cmd_cb,
queue_size=1)
rospy.Subscriber('/final_waypoints',
Lane,
self.final_waypoints_cb,
queue_size=1)
rospy.Subscriber('/current_pose',
PoseStamped,
self.current_pose_cb,
queue_size=1)
self.loop()
def dbw_enabled_cb(self, msg):
self.dbw_enabled = msg.data
if self.dbw_enabled:
self.controller.reset()
@staticmethod
def compute_absolute_velocity(velocity_vector):
x = velocity_vector.x
y = velocity_vector.y
z = velocity_vector.z
return math.sqrt(x**2 + y**2 + z**2)
def current_velocity_cb(self, msg):
self.current_linear_velocity = self.compute_absolute_velocity(
msg.twist.linear)
def twist_cmd_cb(self, msg):
self.target_linear_velocity = self.compute_absolute_velocity(
msg.twist.linear)
self.target_angular_velocity = self.compute_absolute_velocity(
msg.twist.angular)
def final_waypoints_cb(self, msg):
self.final_waypoints = msg.waypoints
def current_pose_cb(self, msg):
self.current_pose = msg.pose
def loop(self):
rate = rospy.Rate(self.rate)
while not rospy.is_shutdown():
throttle, brake, steer = 1, None, None
if (self.final_waypoints is not None) \
& (self.current_pose is not None) \
& (self.current_linear_velocity is not None) \
& (self.target_linear_velocity is not None) \
& (self.target_angular_velocity is not None):
self.initialized = True
cte = CTE.compute_cte(self.final_waypoints, self.current_pose)
throttle, brake, steer = self.controller.control(
self.target_linear_velocity, self.target_angular_velocity,
self.current_linear_velocity, cte)
self.logger.log(throttle, brake, steer, 0, cte)
if self.initialized and self.dbw_enabled:
# sending None for break, ensures we're not throttling/breaking at the same time
self.publish(throttle, brake, steer)
rate.sleep()
def publish(self, throttle, brake, steer):
if throttle is not None:
tcmd = ThrottleCmd()
tcmd.enable = True
tcmd.pedal_cmd_type = ThrottleCmd.CMD_PERCENT
tcmd.pedal_cmd = throttle
self.throttle_pub.publish(tcmd)
if brake is not None:
bcmd = BrakeCmd()
bcmd.enable = True
bcmd.pedal_cmd_type = BrakeCmd.CMD_TORQUE
bcmd.pedal_cmd = brake
self.brake_pub.publish(bcmd)
if steer is not None:
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')
cp = CarParams()
cp.vehicle_mass = rospy.get_param('~vehicle_mass', 1736.35)
cp.fuel_capacity = rospy.get_param('~fuel_capacity', 13.5)
cp.brake_deadband = rospy.get_param('~brake_deadband', .1)
cp.decel_limit = rospy.get_param('~decel_limit', -5)
cp.accel_limit = rospy.get_param('~accel_limit', 1.)
cp.wheel_radius = rospy.get_param('~wheel_radius', 0.2413)
cp.wheel_base = rospy.get_param('~wheel_base', 2.8498)
cp.steer_ratio = rospy.get_param('~steer_ratio', 14.8)
cp.max_lat_accel = rospy.get_param('~max_lat_accel', 3.)
cp.max_steer_angle = rospy.get_param('~max_steer_angle', 8.)
cp.min_speed = 0.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.current_pose = None # needed?
self.dbw_enabled = True
self.reset_flag = True
self.current_velocity = None
self.latest_twist_cmd = None
# TIME
self.previous_timestamp = rospy.get_time()
# Create `TwistController` object
self.controller = TwistController(cp=cp)
# Subscribe to all the topics you need to
rospy.Subscriber('/twist_cmd',
TwistStamped,
self.twist_cmd_cb,
queue_size=5)
rospy.Subscriber('/current_velocity',
TwistStamped,
self.current_velocity_cb,
queue_size=5)
rospy.Subscriber('/vehicle/dbw_enabled',
Bool,
self.dbw_enabled_cb,
queue_size=1)
self.loop()
def dbw_enabled_cb(self, dbw_enabled):
try:
self.dbw_enabled = bool(dbw_enabled.data)
except Exception:
self.dbw_enabled = dbw_enabled
def current_velocity_cb(self, current_velocity):
self.current_velocity = current_velocity
def twist_cmd_cb(self, twist_cmd):
self.latest_twist_cmd = twist_cmd
def loop(self):
rate = rospy.Rate(10) #Hz
while not rospy.is_shutdown():
# TIME
current_timestamp = rospy.get_time()
del_time = current_timestamp - self.previous_timestamp
self.previous_timestamp = current_timestamp
if self.dbw_enabled and self.current_velocity is not None and self.latest_twist_cmd is not None:
if self.reset_flag:
self.controller.reset()
self.reset_flag = False
throttle, brake, steering = self.controller.control(
twist_cmd=self.latest_twist_cmd,
current_velocity=self.current_velocity,
del_time=del_time)
self.publish(throttle, brake, steering)
else:
self.reset_flag = True
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)
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)
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')
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.brake_deadband = brake_deadband
self.decel_limit = decel_limit
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 = False
self.twist_controller = TwistController(max_steer_angle, accel_limit,
decel_limit)
self.gain_controller = GainController(max_throttle=1.0,
max_brake=1.0,
max_steer_angle=max_steer_angle,
delay_seconds=1.0,
steer_ratio=steer_ratio)
self.goal_acceleration = 0
self.goal_yaw_rate = 0.
self.current_linear = [0, 0]
rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_cmd_callback)
rospy.Subscriber('/current_velocity', TwistStamped,
self.current_velocity_callback)
rospy.Subscriber('/vehicle/dbw_enabled', Bool,
self.dbw_enabled_callback)
srv = Server(PIDParamsConfig, self.config_callback)
self.loop()
def config_callback(self, config, level):
rospy.logwarn("Updating Steering PID %s, %s, %s", config["Steer_P"],
config["Steer_I"], config["Steer_D"])
rospy.logwarn("Updating Throttle PID %s, %s, %s", config["Throttle_P"],
config["Throttle_I"], config["Throttle_D"])
self.twist_controller.update_steer_pid(config["Steer_P"],
config["Steer_I"],
config["Steer_D"])
self.twist_controller.update_throttle_pid(config["Throttle_P"],
config["Throttle_I"],
config["Throttle_D"])
return config
def twist_cmd_callback(self, msg):
new_goal_acceleration = msg.twist.linear.x
if new_goal_acceleration < 0 and self.goal_acceleration > 0:
self.twist_controller.reset_throttle_pid()
#rospy.logwarn("Updating intended acceleration: %s", new_goal_acceleration)
self.goal_acceleration = new_goal_acceleration
self.goal_yaw_rate = msg.twist.angular.z
def current_velocity_callback(self, msg):
self.current_linear = [msg.twist.linear.x, msg.twist.linear.y]
def dbw_enabled_callback(self, msg):
self.dbw_enabled = msg.data
def loop(self):
rate = rospy.Rate(50) # 50Hz
while not rospy.is_shutdown():
linear_speed = 0.0
angular_velocity = 0.0
linear_acceleration = 0.0
angular_acceleration = 0.0
deltat = 0.02
goal_linear_acceleration, goal_angular_velocity = self.twist_controller.control(
self.goal_acceleration, self.goal_yaw_rate,
self.current_linear, deltat, self.dbw_enabled)
# rospy.logwarn("c:%.2f, g:%.2f, o:%.2f", self.current_linear[0],
# self.goal_linear[0], goal_linear_acceleration)
#if(self.goal_linear[0] != 0 and goal_linear_acceleration < 0 and goal_linear_acceleration > -self.brake_deadband):
# goal_linear_acceleration = 0
throttle, brake, steering = self.gain_controller.control(
goal_linear_acceleration, goal_angular_velocity, linear_speed,
angular_velocity, linear_acceleration, angular_acceleration,
deltat, self.dbw_enabled)
if brake > 0:
brake = brake * BrakeCmd.TORQUE_MAX / -self.decel_limit
# rospy.logwarn("c:%.2f, g:%.2f, o:%.2f, b:%.2f", self.current_linear[0],
# self.goal_linear[0], goal_linear_acceleration, brake)
if self.dbw_enabled:
self.publish(throttle, brake, steering)
rate.sleep()
def publish(self, throttle, brake, steer):
tcmd = ThrottleCmd()
tcmd.enable = True
tcmd.pedal_cmd_type = ThrottleCmd.CMD_PERCENT # range [0,1]
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)
class DBWNode(object):
def __init__(self):
rospy.init_node('dbw_node')
rospy.loginfo('DBW Node Initialized')
vehicle_params = {
'vehicle_mass': rospy.get_param('~vehicle_mass', 1736.35), # kg
'fuel_capacity': rospy.get_param('~fuel_capacity', 13.5), # kg/gal
'brake_deadband': rospy.get_param('~brake_deadband', .1),
'decel_limit': rospy.get_param('~decel_limit', -5),
'accel_limit': rospy.get_param('~accel_limit', 1.), # 1 g?
'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_speed': 0.0,
}
self.dbw_enabled = False
self.actual_velocity = None
self.proposed_command = None
self.previous_timestamp = rospy.get_time()
self.controller = TwistController(vehicle_params)
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)
rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_enabled_cb, queue_size=1)
rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_cmd_cb, queue_size=1)
rospy.Subscriber('/current_velocity', TwistStamped, self.current_velocity_cb, queue_size=1)
self.loop()
def is_ready(self):
return all((self.dbw_enabled, self.actual_velocity, self.proposed_command,))
def loop(self):
rate = rospy.Rate(50)
while not rospy.is_shutdown():
rate.sleep()
current_timestamp = rospy.get_time()
time_diff = current_timestamp - self.previous_timestamp
self.previous_timestamp = current_timestamp
if self.is_ready() and self.dbw_enabled:
throttle, brake, steering = self.controller.control(
self.proposed_command,
self.actual_velocity,
time_diff)
self.publish(throttle, brake, steering)
else:
self.controller.reset_pids()
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 dbw_enabled_cb(self, msg):
self.dbw_enabled = msg.data
rospy.loginfo('DBW Enabled')
def twist_cmd_cb(self, msg):
self.proposed_command = msg.twist
def current_velocity_cb(self, msg):
self.actual_velocity = msg.twist
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.)
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)
# Create `TwistController` object
self.controller = TwistController(vehicle_mass, fuel_capacity, brake_deadband, decel_limit, accel_limit, wheel_radius, wheel_base, \
steer_ratio, max_lat_accel, max_steer_angle, min_speed)
self.current_velocity = None
self.twist_cmd = None
self.dbw_enabled = False
self.previous_time = None
# You will need to write ROS subscribers for the /current_velocity, /twist_cmd, and /vehicle/dbw_enabled topics
rospy.Subscriber('/current_velocity', TwistStamped, self.current_velocity_cb)
rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_cmd_cb)
rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_enabled_cb)
self.loop()
def loop(self):
rate = rospy.Rate(50) # 50Hz
#rate = rospy.Rate(10)
while not rospy.is_shutdown():
if (self.current_velocity is not None) and (self.twist_cmd is not None):
current_time = rospy.get_time()
dt = (current_time - self.previous_time) if (self.previous_time is not None) else 0
self.previous_time = current_time
proposed_linear_velocity = self.twist_cmd.twist.linear
proposed_angular_velocity = self.twist_cmd.twist.angular
self.controller.enable(self.dbw_enabled)
# Get predicted throttle, brake, and steering using `twist_controller`
throttle, brake, steering = self.controller.control(proposed_linear_velocity,
proposed_angular_velocity,
self.current_velocity,
self.dbw_enabled, dt)
# You should only publish the control commands if dbw is enabled
if self.dbw_enabled:
self.publish(throttle, brake, steering)
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 current_velocity_cb(self, msg):
self.current_velocity = msg
def twist_cmd_cb(self, msg):
self.twist_cmd = msg
def dbw_enabled_cb(self, msg):
self.dbw_enabled = msg.data
class DBWNode(object):
def __init__(self):
rospy.init_node('dbw_node')
parameters = dict()
parameters['vehicle_mass'] = rospy.get_param('~vehicle_mass', 1736.35)
parameters['fuel_capacity'] = rospy.get_param('~fuel_capacity', 13.5)
parameters['dbrake_deadband'] = rospy.get_param('~brake_deadband', .1)
parameters['decel_limit'] = rospy.get_param('~decel_limit', -5)
parameters['accel_limit'] = rospy.get_param('~accel_limit', 1.)
parameters['wheel_radius'] = rospy.get_param('~wheel_radius', 0.2413)
parameters['wheel_base'] = rospy.get_param('~wheel_base', 2.8498)
parameters['steer_ratio'] = rospy.get_param('~steer_ratio', 14.8)
parameters['max_lat_accel'] = rospy.get_param('~max_lat_accel', 3.)
parameters['max_steer_angle'] = rospy.get_param('~max_steer_angle', 8.)
parameters['min_speed'] = 0.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.current_twist = None
self.twist_cmd = None
self.t_0 = rospy.get_time()
# TODO: Create `TwistController` object
self.controller = TwistController(parameters=parameters)
# Subscribe to all the needed topics
rospy.Subscriber('/twist_cmd',
TwistStamped,
self.twist_cmd_cb,
queue_size=5)
rospy.Subscriber('/current_velocity',
TwistStamped,
self.current_velocity_cb,
queue_size=5)
rospy.Subscriber('/vehicle/dbw_enabled',
Bool,
self.dbw_enabled_cb,
queue_size=1)
self.loop()
def dbw_enabled_cb(self, dbw_enabled):
self.dbw_enabled = dbw_enabled
print dbw_enabled
def current_velocity_cb(self, current_twist):
self.current_twist = current_twist
def twist_cmd_cb(self, twist_cmd):
self.twist_cmd = twist_cmd
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
if self.dbw_enabled and self.current_twist is not None and self.twist_cmd is not None:
throttle = 1.0
brake = 0.0
steering = 0.0
t_1 = rospy.get_time()
dt = t_1 - self.t_0
self.t_0 = t_1
throttle, brake, steering = self.controller.control(
twist_cmd=self.twist_cmd,
current_twist=self.current_twist,
dt=dt)
self.publish(throttle, brake, steering)
else:
self.controller.reset()
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)
class DBWNode(object):
def __init__(self):
rospy.init_node('dbw_node')
self.vehicle_mass = rospy.get_param('~vehicle_mass', 1736.35)
self.fuel_capacity = rospy.get_param('~fuel_capacity', 13.5)
self.brake_deadband = rospy.get_param('~brake_deadband', .1)
self.decel_limit = rospy.get_param('~decel_limit', -5)
self.accel_limit = rospy.get_param('~accel_limit', 1.)
self.wheel_radius = rospy.get_param('~wheel_radius', 0.2413)
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.min_speed = rospy.get_param('~min_speed', 4. * 0.44704)
self.max_throttle_percentage = rospy.get_param(
'~max_throttle_percentage', 0.1)
self.max_braking_percentage = rospy.get_param(
'~max_braking_percentage', -0.1)
self.max_vel_mps = rospy.get_param(
'waypoint_loader/velocity') * MPH_to_MPS
self.loop_freq = rospy.get_param('~loop_freq', 2)
# the frequency to process vehicle messages
self.desired_velocity, self.current_velocity = None, None
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)
# DONE: Create `TwistController` object
self.controller = TwistController(vehicle_mass=self.vehicle_mass,
fuel_capacity=self.fuel_capacity,
brake_deadband=self.brake_deadband,
decel_limit=self.decel_limit,
accel_limit=self.accel_limit,
wheel_radius=self.wheel_radius,
wheel_base=self.wheel_base,
steer_ratio=self.steer_ratio,
max_lat_accel=self.max_lat_accel,
max_steer_angle=self.max_steer_angle)
# min_speed=self.min_speed,
# ,
# max_braking_percentage=self.max_braking_percentage,
# max_throttle_percentage=self.max_throttle_percentage,
# max_vel_mps=self.max_vel_mps
# DONE: Subscribe to all the topics you need to
self.dbw_enabled = False
self.current_velocity = None
self.desired_velocity = None
rospy.Subscriber('/current_velocity',
TwistStamped,
self.current_velocity_cb,
queue_size=1)
rospy.Subscriber('/twist_cmd',
TwistStamped,
self.twist_cmd_cb,
queue_size=1)
rospy.Subscriber('/vehicle/dbw_enabled',
Bool,
self.dbw_enabled_cb,
queue_size=1)
self.loop()
def loop(self):
rate = rospy.Rate(self.loop_freq) # from 50Hz to 2Hz
dt = 1. / self.loop_freq
while not rospy.is_shutdown():
if self.desired_velocity and self.current_velocity:
# DONE: Get predicted throttle, brake, and steering using `twist_controller`
# You should only publish the control commands if dbw is enabled
if self.dbw_enabled:
throttle, brake, steering = self.controller.control(
self.desired_velocity, self.current_velocity, dt)
self.publish(throttle, brake, steering)
# end of self.dbw_enabled
self.desired_velocity, self.current_velocity = None, None
# end of if self.desired_velocity and self.current_velocity
rate.sleep()
def current_velocity_cb(self, msg):
if self.current_velocity is None:
self.current_velocity = msg # .twist
# end of if self.current_velocity is None:
# self.current_velocity = msg.twist
def twist_cmd_cb(self, msg):
self.desired_velocity = msg # .twist
def dbw_enabled_cb(self, msg):
self.dbw_enabled = msg.data
if (self.dbw_enabled):
self.controller.velocity_pid.reset()
# end of def dbw_enabled_cb(self, msg)
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)
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.)
min_speed = 0.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)
# TODO: Create `TwistController` object
# self.controller = TwistController(<Arguments you wish to provide>)
self.controller = TwistController(vehicle_mass, fuel_capacity,
brake_deadband, decel_limit,
accel_limit, wheel_radius,
wheel_base, steer_ratio,
max_lat_accel, max_steer_angle,
min_speed)
self.dbw_enabled = True
self.reset_controller = True
self.current_velocity_msg = None
self.twist_cmd = None
self.current_velocity = 0.0
self.current_angualr_velocity = 0.0
self.twist_velocity = 0.0
self.twist_angualr_velocity = 0.0
self.prev_time = rospy.get_time()
# TODO: Subscribe to all the topics you need to
rospy.Subscriber('/twist_cmd',
TwistStamped,
self.twist_cmd_cb,
queue_size=5)
rospy.Subscriber('/current_velocity',
TwistStamped,
self.current_velocity_cb,
queue_size=5)
rospy.Subscriber('/vehicle/dbw_enabled',
Bool,
self.dbw_enabled_cb,
queue_size=1)
self.loop()
def twist_cmd_cb(self, msg):
self.twist_cmd = msg
self.twist_velocity = msg.twist.linear.x
self.twist_angualr_velocity = msg.twist.angular.z
def current_velocity_cb(self, msg):
self.current_velocity_msg = msg
self.current_velocity = msg.twist.linear.x
self.current_angualr_velocity = msg.twist.angular.z
def dbw_enabled_cb(self, msg):
try:
self.dbw_enabled = bool(msg.data)
except Exception:
self.dbw_enabled = msg
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)
# TIME
now = rospy.get_time()
dt = now - self.prev_time
self.prev_time = now
if self.dbw_enabled and self.current_velocity_msg is not None and self.twist_cmd is not None:
if self.reset_controller:
self.controller.reset()
self.reset_controller = False
throttle, brake, steering = self.controller.control(
twist_velocity=self.twist_velocity,
twist_angualr_velocity=self.twist_angualr_velocity,
current_velocity=self.current_velocity,
delta_time=dt)
self.publish(throttle, brake, steering)
else:
self.reset_controller = True
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)
class DBWNode(object):
def __init__(self):
rospy.init_node('dbw_node')
self.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.)
self.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.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)
# create subscribers
self.sub_current_velocity = rospy.Subscriber(
"/current_velocity", TwistStamped, self.current_velocity_callback)
self.sub_twist_cmd = rospy.Subscriber("/twist_cmd", TwistStamped,
self.twist_cmd_callback)
self.sub_dbw_enabled = rospy.Subscriber("/vehicle/dbw_enabled", Bool,
self.dbw_enabled_callback)
self.twist_controller = TwistController(wheel_base, steer_ratio,
max_lat_accel, max_steer_angle,
decel_limit)
self.dbw_enabled = False
self.current_velocity = 0
self.desired_angular_velocity = 0
self.desired_long_velocity = 0
self.loop()
def deceleration_to_Nm(self, desired_deceleration):
brake_torque = self.vehicle_mass * self.wheel_radius * desired_deceleration
return brake_torque
def current_velocity_callback(self, twist_msg):
# current longitudinal velocity
self.current_velocity = twist_msg.twist.linear.x
def twist_cmd_callback(self, twist_msg):
# desired angular_velocity
self.desired_angular_velocity = twist_msg.twist.angular.z
# desired longitudinal velocity
self.desired_long_velocity = twist_msg.twist.linear.x
def dbw_enabled_callback(self, bool_msg):
self.dbw_enabled = bool_msg.data
rospy.logwarn('self.dbw_enabled = %d', self.dbw_enabled)
return
def loop(self):
rate = rospy.Rate(50) # 50Hz
while not rospy.is_shutdown():
if self.dbw_enabled:
throttle, deceleration, steering = self.twist_controller.control(
self.current_velocity, self.desired_long_velocity,
self.desired_angular_velocity)
#rospy.logwarn('current_velocity: %f, desired_velocity: %f, throttle: %f, brake: %f',
# self.current_velocity, self.desired_long_velocity, throttle, deceleration)
# throttle message
throttle_msg = ThrottleCmd()
throttle_msg.enable = True
throttle_msg.pedal_cmd_type = ThrottleCmd.CMD_PERCENT
throttle_msg.pedal_cmd = throttle # 0 ... 1
self.throttle_pub.publish(throttle_msg)
# brake message
brake_msg = BrakeCmd()
brake_msg.enable = True
brake_msg.pedal_cmd_type = BrakeCmd.CMD_TORQUE # Nm, range 0 to 3250
if deceleration > 0:
brake_msg.boo_cmd = True
brake_msg.pedal_cmd = self.deceleration_to_Nm(deceleration)
else:
brake_msg.boo_cmd = False
brake_msg.pedal_cmd = 0
self.brake_pub.publish(brake_msg)
# steering message
steering_msg = SteeringCmd()
steering_msg.enable = True
steering_msg.steering_wheel_angle_cmd = steering # in rad, range -8.2 to 8.2
self.steer_pub.publish(steering_msg)
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)
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.)
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>)
min_speed = .1 # TODO made up value, what is needed?
self.controller = TwistController(wheel_base, steer_ratio, min_speed,
max_lat_accel, max_steer_angle,
decel_limit, accel_limit)
# TODO: Subscribe to all the topics you need to
rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_cmd_cb)
rospy.Subscriber('/current_velocity', TwistStamped,
self.current_velocity_cb)
rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_enabled_cb)
# Maybe subscribe to /final_waypoints for now, until following is complete
#rospy.Subscriber('/final_waypoints', Lane, self.final_waypoints_cb)
self.twist_cmd = None
self.current_velocity = None
self.dbw_enabled = None
self.loop()
def loop(self):
rate = rospy.Rate(50) # 50Hz
while not rospy.is_shutdown():
if (self.twist_cmd != None and self.current_velocity != None):
# 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>)
#rospy.logwarn("twist_cmd {}".format(self.twist_cmd))
#print("twist_cmd: xxx{}".format(self.twist_cmd))
throttle, brake, steering = self.controller.control(
self.twist_cmd.twist.linear.x,
self.twist_cmd.twist.angular.z,
self.current_velocity.twist.linear.x, self.dbw_enabled)
#self.wheel_base,
#self.steer_ratio,
#min_speed,
#self.max_lat_accel,
#self.max_steer_angle)
if self.dbw_enabled:
self.publish(throttle, brake, steering)
rate.sleep()
def twist_cmd_cb(self, msg):
self.twist_cmd = msg
def current_velocity_cb(self, msg):
#rospy.logwarn("current_velocity {}".format(self.current_velocity))
self.current_velocity = msg
def dbw_enabled_cb(self, msg):
self.dbw_enabled = bool(msg.data)
rospy.logwarn("dbw_enabled {}".format(self.dbw_enabled))
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)
