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)
args = [vehicle_mass, fuel_capacity, brake_deadband, decel_limit,
accel_limit, wheel_radius, wheel_base,
steer_ratio, max_lat_accel, max_steer_angle]
self.controller = Controller(*args)
# Subscriptions
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('/dbw_enabled', Bool, self.dbw_enabled_cb, queue_size=1)
# local variables
self.dbw_enabled = True
self.current_velocity = None
self.twist_cmd = None
self.loop()
def __init__(self):
rospy.init_node('dbw_node')
vehicle_mass = rospy.get_param('~vehicle_mass', 1736.35) # need this
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) # need this
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 = .4
# brakes are in units of Torque
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(
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=max_throttle)
rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_enabled_cb)
rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_cb)
rospy.Subscriber('/current_velocity', TwistStamped, self.velocity_cb)
# To compute the CTE
rospy.Subscriber('/current_pose', PoseStamped, self.pose_cb)
rospy.Subscriber('/final_waypoints', Lane, self.waypoints_cb)
self.cte_pub = rospy.Publisher('/vehicle/cte',
Float32, queue_size=1)
self.current_velocity = None
self.current_ang_velocity = None
self.dbw_enabled = None
self.linear_velocity = None
self.angular_velocity = None
self.current_position = None
self.waypoints = None
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', 2.)
max_steer_angle = rospy.get_param('~max_steer_angle', 5.)
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 = Controller(
YawController(wheel_base, steer_ratio, 0.001, max_lat_accel, max_steer_angle),
wheel_radius * vehicle_mass)
# min_speed = 0.001: in YawController.get_angle, it is divided by a radius that is
# proportional to current speed. To avoid division by zero, we need a minimum speed.
# Some important variables:
self.dbw_enabled = False
self.curr_linear_velocity = 0.0
self.curr_angular_velocity = 0.0
self.des_linear_velocity = 0.0
self.des_angular_velocity = 0.0
self.previous_timestamp = rospy.get_rostime().secs
self.current_timestamp = 0.0
self.vel_cur = 0.0
self.delta_t = 0.0
# TODO: Subscribe to all the topics you need to
rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_enabled_cb, queue_size=1)
rospy.Subscriber('/current_velocity', TwistStamped, self.current_velocity_cb, queue_size=1)
rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_cmd_cb, queue_size=1)
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.)
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)
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
}
self.controller = Controller(**params)
self.dbw_enabled = False
self.current_setpoint = None
self.current_velocity = None
self.final_waypoints = None
self.current_pose = None
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)
rospy.Subscriber('/final_waypoints', Lane, self.final_waypoints_cb)
rospy.Subscriber('/current_pose', PoseStamped, self.current_pose_cb)
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.)
decel_limit = rospy.get_param('~max_lon_accel', 3.)
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)
# NOTE: To prevent Carla from moving requires about 700 Nm of torque
# (can be calculated from the `vehicle_mass`, `wheel_radius`, and desired
# acceleration)
# Create the `Controller` object
self.controller = Controller(wheel_base, wheel_radius, steer_ratio, max_lat_accel,
max_steer_angle, decel_limit, vehicle_mass)
# Subscribe to all the necessary topics
self.dbw_enabled_sub = rospy.Subscriber('/vehicle/dbw_enabled',
Bool, self.dbw_enabled_cb)
self.curr_velocity_sub = rospy.Subscriber('/current_velocity',
TwistStamped, self.curr_velocity_cb)
self.twist_cmd_sub = rospy.Subscriber('/twist_cmd',
TwistStamped, self.twist_cb)
self.dbw_enabled = None # From '/vehicle/dbw_enabled'
self.curr_velocity = None # From '/current_velocity'
self.linear_vel = None # From '/twist_cmd'
self.angular_vel = None # From '/twist_cmd'
self.loop()
def __init__(self):
rospy.init_node('dbw_node')
# Save param from launch file as dictionary
ros_param_dict = {
'vehicle_mass': rospy.get_param('~vehicle_mass', 1736.35), # must need for stoping the car
'fuel_capacity': rospy.get_param('~fuel_capacity', 13.5), # optional go precise with the weight
'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), # must
'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.)
}
# Subscribe to all the topics you need to
rospy.Subscriber('/current_velocity', TwistStamped, self.cur_velocity_cb)
rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_cmd_cb)
rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_enabled_cb)
# Publish topics.
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(**ros_param_dict)
# List of variables need to use
self.dbw_enabled = None
self.cur_linear_velocity = None # may need low pass filter to reduce noisy
self.cur_angular_velocity = None
self.target_linear_velocity = None
self.target_angular_velocity = None
# flag for checking msg received
self.twist_cmd = False
self.current_velocity = False
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.)
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>)
parms = {
'wheel_base': wheel_base,
'steer_ratio': steer_ratio,
'min_velocity': 0.,
'max_lat_accel': max_lat_accel,
'max_steer_angle': max_steer_angle,
'decel_limit': decel_limit,
'accel_limit': accel_limit,
'deadband': brake_deadband
}
self.controller = Controller(**parms)
self.current_command = None
self.current_velocity = None
self.dbw_enabled = False
# TODO: Subscribe to all the topics you need to
rospy.Subscriber('/twist_cmd', TwistStamped, self.callback_twist_cmd)
rospy.Subscriber('/current_velocity', TwistStamped,
self.callback_current_velocity)
rospy.Subscriber('/vehicle/dbw_enabled', Bool,
self.callback_dbw_enabled)
rospy.Subscriber('/final_waypoints', Lane, self.final_waypoints_cb)
self.loop()
def final_waypoints_cb(self, msg): #should be OK
self.final_waypoints = msg
#rospy.logwarn(self.final_waypoints)
#rospy.logwarn('dbw_node ' + str(self.final_waypoints.waypoints[1].twist.twist.linear.z))
#rospy.logwarn(self.final_waypoints.waypoints[1].twist.twist.linear.x)
def loop(self):
rate = rospy.Rate(20) # 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.current_command is not None) and (self.current_velocity
is not None):
#current velocity, target velocity, and target angle and pass into control
#rospy.logwarn('Setting Velocity')
linear_target = self.current_command.twist.linear.x
#rospy.logwarn(str(linear_target) )
angular_target = self.current_command.twist.angular.z
linear_current = self.current_velocity.twist.linear.x
angular_current = self.current_velocity.twist.angular.z
throttle, brake, steering = self.controller.control(
linear_target, angular_target, linear_current)
#rospy.logwarn(str(throttle))
brake = brake * 500
#rospy.logwarn(str(brake))
# publish the control commands if dbw is enabled
if self.dbw_enabled is True:
self.publish(throttle, brake, steering)
else:
rospy.logwarn('resetting controller')
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)
def callback_twist_cmd(self, msg):
self.current_command = msg
def callback_current_velocity(self, msg):
self.current_velocity = msg
def callback_dbw_enabled(self, msg):
self.dbw_enabled = msg.data
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 `Controller` object
self.controller = Controller(vehicle_mass, decel_limit, accel_limit,
wheel_radius, wheel_base, steer_ratio,
max_lat_accel, max_steer_angle,
acc_kp = 1.5,acc_ki = 0.003, acc_kd = 3.5)
# TODO: Subscribe to all the topics you need to
rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_enabled_cb)
rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_cb)
rospy.Subscriber('/current_velocity', TwistStamped, self.velocity_cb)
self.current_vel = None
self.cur_angle_vel = None
self.linear_vel = None
self.angular_vel = None
self.dbw_enabled = None
self.throttle = self.steering = self.brake = 0
# base_path = os.path.dirname(os.path.abspath(__file__))
# self.speedfile = os.path.join(base_path, 'speed.csv')
# self.speed_data = []
self.loop()
def dbw_enabled_cb(self, msg):
self.dbw_enabled = msg
def twist_cb(self, msg):
self.linear_vel = msg.twist.linear.x
self.angular_vel = msg.twist.angular.z
def velocity_cb(self, msg):
self.current_vel = msg.twist.linear.x
if self.dbw_enabled and self.current_vel is not None:
self.speed_data.append({'actual': self.current_vel,
'proposed': self.linear_vel})
def loop(self):
rate = rospy.Rate(50) # 50Hz
while not rospy.is_shutdown():
# TODO: Get predicted throttle, brake, and steering using `twist_controller`
if None not in (self.current_vel, self.linear_vel, self.angular_vel):
self.throttle, self.brake, self.steering = self.controller.controller(self.current_vel, self.dbw_enabled,
self.linear_vel, self.angular_vel)
if self.dbw_enabled:
self.publish(self.throttle, self.brake, self.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_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)
args = [
vehicle_mass, fuel_capacity, brake_deadband, decel_limit,
accel_limit, wheel_radius, wheel_base, steer_ratio, max_lat_accel,
max_steer_angle
]
self.controller = Controller(*args)
# Subscriptions
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('/dbw_enabled',
Bool,
self.dbw_enabled_cb,
queue_size=1)
# local variables
self.dbw_enabled = True
self.current_velocity = None
self.twist_cmd = None
self.loop()
def loop(self):
rate = rospy.Rate(COMMAND_FREQUENCY)
while not rospy.is_shutdown():
# Will ignore when we don't have info from `current_velocity`, `twist_cmd` and when dbw is not enabled
if (self.twist_cmd is None) or (self.current_velocity is
None) or (not self.dbw_enabled):
continue
throttle, brake, steering = self.controller.control(
self.twist_cmd.twist.linear, self.twist_cmd.twist.angular,
self.current_velocity.twist.linear)
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)
# Helper for subscriber below.
def twist_cmd_cb(self, msg):
"""Subscribe to twist_cmd."""
self.twist_cmd = msg
def current_velocity_cb(self, msg):
"""Subscribe to current_velocity_cb."""
self.current_velocity = msg
def dbw_enabled_cb(self, msg):
"""Subscribe to dbw_enabled."""
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", 0.1)
decel_limit = rospy.get_param("~decel_limit", -5)
accel_limit = rospy.get_param("~accel_limit", 1.0)
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.0)
max_steer_angle = rospy.get_param("~max_steer_angle", 8.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)
# TODO: Create `Controller` object
self.controller = Controller(
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,
)
# TODO: Subscribe to all the topics you need to
rospy.Subscriber("/vehicle/dbw_enabled",
Bool,
self.dbw_enabled_cb,
queue_size=1)
rospy.Subscriber("/twist_cmd",
TwistStamped,
self.twist_cb,
queue_size=1)
rospy.Subscriber("/current_velocity",
TwistStamped,
self.velocity_cb,
queue_size=5)
self.current_vel = None
self.curr_ang_vel = None
self.dbw_enabled = None
self.linear_vel = None
self.angular_vel = None
self.throttle = self.steering = self.brake = 0
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
if not None in (self.current_vel, self.linear_vel,
self.angular_vel):
self.throttle, self.brake, self.steering = self.controller.control(
self.current_vel,
self.dbw_enabled,
self.linear_vel,
self.angular_vel,
)
if self.dbw_enabled:
self.publish(self.throttle, self.brake, self.steering)
rate.sleep()
def dbw_enabled_cb(self, msg):
self.dbw_enabled = msg
def twist_cb(self, msg):
self.linear_vel = msg.twist.linear.x
self.angular_vel = msg.twist.angular.z
def velocity_cb(self, msg):
self.current_vel = msg.twist.linear.x
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>)
self.car_controller = Controller(wheel_base, steer_ratio, 1.,
max_lat_accel, max_steer_angle)
# TODO: Subscribe to all the topics you need to
rospy.Subscriber('/twist_cmd', TwistStamped, self.command_cb)
rospy.Subscriber('/current_velocity', TwistStamped, self.velocity_cb)
rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_state_cb)
# Add other member variables
self.current_velocity = 0
self.target_velocity = 0
self.current_angular_velocity = 0
self.target_angular_velocity = 0
self.dbw_state = False
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 self.dbw_state is True:
throttle, brake, steering = self.car_controller.control(
self.target_velocity, self.target_angular_velocity,
self.current_velocity)
self.publish(throttle, brake, steering)
else:
self.car_controller.speed_controller.reset()
rate.sleep()
def publish(self, throttle, brake, steer):
if throttle is not 0:
tcmd = ThrottleCmd()
tcmd.enable = True
tcmd.pedal_cmd_type = ThrottleCmd.CMD_PERCENT
tcmd.pedal_cmd = throttle
self.throttle_pub.publish(tcmd)
else:
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)
def command_cb(self, cmd):
vx = cmd.twist.linear.x
az = cmd.twist.angular.z
self.target_velocity = vx
self.target_angular_velocity = az
def velocity_cb(self, vel):
vx = vel.twist.linear.x
az = vel.twist.angular.z
self.current_velocity = vx
self.current_angular_velocity = az
def dbw_state_cb(self, dbw):
self.dbw_state = dbw.data
rospy.logwarn('dbw_state: %s', self.dbw_state)
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>)
config = {
'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
}
self.controller = Controller(**config)
self.is_dbw_enabled = False
self.current_velocity = None
self.proposed_velocity = None
self.final_waypoints = None
self.current_pose = None
self.previous_loop_time = rospy.get_rostime()
# TODO: Subscribe to all the topics you need to
self.twist_sub = rospy.Subscriber('/twist_cmd',
TwistStamped,
self.twist_message_callback,
queue_size=1)
self.velocity_sub = rospy.Subscriber('/current_velocity',
TwistStamped,
self.current_velocity_callback,
queue_size=1)
self.dbw_sub = rospy.Subscriber('/vehicle/dbw_enabled',
Bool,
self.dbw_enabled_callback,
queue_size=1)
self.final_wp_sub = rospy.Subscriber('final_waypoints',
Lane,
self.final_waypoints_cb,
queue_size=1)
self.pose_sub = rospy.Subscriber('/current_pose',
PoseStamped,
self.current_pose_cb,
queue_size=1)
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.current_velocity
is not None) and (self.proposed_velocity
is not None) and (self.final_waypoints
is not None):
ros_time = rospy.get_rostime()
ros_dura = ros_time - self.previous_loop_time
dura_secs = ros_dura.secs + (1e-9 * ros_dura.nsecs)
self.previous_loop_time = ros_time
curr_lin_vel = self.current_velocity.twist.linear.x
targ_lin_vel = self.proposed_velocity.twist.linear.x
targ_ang_vel = self.proposed_velocity.twist.angular.z
"""
The final waypoints are used to fit a polynomial which represents the trajectory that the car is expected to take
(1) The expected y value at the current x value of the car is evaluated by using the curve
(2) This is compared with the current y value obtained from the car pose
The difference between 1 and 2 is the CTE(Cross Track Error)
We will transform world coordinates to car coordinates so that all x values are in the direction of the car and
all y values represent lateral movement
"""
origin = self.final_waypoints[0].pose.pose.position
# Given a list of waypoints, returns a list of [x,y] coordinates associated with those wp.
wp_matrix = list(
map(
lambda waypoint: [
waypoint.pose.pose.position.x, waypoint.pose.pose.
position.y
], self.final_waypoints))
# Convert the coordinates [x,y] in the world view to the car's coordinate.
# Shift the points to the origin.
shifted_matrix = wp_matrix - np.array([origin.x, origin.y])
# Derive an angle by which to rotate the points.
offset = 11
angle = np.arctan2(shifted_matrix[offset, 1],
shifted_matrix[offset, 0])
rotation_matrix = np.array([[np.cos(angle), -np.sin(angle)],
[np.sin(angle),
np.cos(angle)]])
rotated_matrix = np.dot(shifted_matrix, rotation_matrix)
# Fit a 2 degree polynomial to the waypoints.
degree = 2
coefficients = np.polyfit(rotated_matrix[:, 0],
rotated_matrix[:, 1], degree)
# Transform the current pose of the car to be in the car's coordinate system.
shifted_pose = np.array([
self.current_pose.pose.position.x - origin.x,
self.current_pose.pose.position.y - origin.y
])
rotated_pose = np.dot(shifted_pose, rotation_matrix)
expected_y_value = np.polyval(coefficients, rotated_pose[0])
actual_y_value = rotated_pose[1]
cross_track_err = expected_y_value - actual_y_value
throttle, brake, steering = self.controller.control(
targ_lin_vel, targ_ang_vel, curr_lin_vel, cross_track_err,
dura_secs)
if not self.is_dbw_enabled or abs(
self.current_velocity.twist.linear.x) < 1e-5 and abs(
self.proposed_velocity.twist.linear.x) < 1e-5:
self.controller.reset()
if self.is_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 twist_message_callback(self, mesg):
self.proposed_velocity = mesg
def current_velocity_callback(self, mesg):
self.current_velocity = mesg
def dbw_enabled_callback(self, mesg):
rospy.logwarn("DBW_ENABLED %s" % mesg)
self.is_dbw_enabled = mesg.data
def final_waypoints_cb(self, mesg):
self.final_waypoints = mesg.waypoints
def current_pose_cb(self, mesg):
self.current_pose = mesg
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)
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
}
self.controller = Controller(**params)
self.dbw_enabled = False
self.current_setpoint = None
self.current_velocity = None
self.final_waypoints = None
self.current_pose = None
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)
rospy.Subscriber('/final_waypoints', Lane, self.final_waypoints_cb)
rospy.Subscriber('/current_pose', PoseStamped, self.current_pose_cb)
self.loop()
def loop(self):
rate = rospy.Rate(20) # 50Hz
while not rospy.is_shutdown():
# You should only publish the control commands if dbw is enabled
if (self.final_waypoints is not None) and (self.current_pose is not None) \
and (self.current_velocity is not None) and (self.current_setpoint is not None):
fwp_size = len(self.final_waypoints.waypoints)
final_waypoint1 = self.final_waypoints.waypoints[0] if fwp_size>0 else None
final_waypoint2 = self.final_waypoints.waypoints[1] if fwp_size>1 else None
current_location = self.current_pose.pose.position
linear_setpoint = self.current_setpoint.twist.linear.x
angular_setpoint = self.current_setpoint.twist.angular.z
linear_current = self.current_velocity.twist.linear.x
angular_current = self.current_velocity.twist.angular.z
throttle, brake, steering = self.controller.control(linear_setpoint, angular_setpoint, linear_current, angular_current, self.dbw_enabled, final_waypoint1, final_waypoint2, current_location)
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)
pass
def dbw_enabled_cb(self, msg):
self.dbw_enabled = msg.data
pass
def twist_cmd_cb(self, msg):
self.current_setpoint = msg
pass
def current_velocity_cb(self, msg):
self.current_velocity = msg
pass
def final_waypoints_cb(self, msg):
self.final_waypoints = msg
pass
def current_pose_cb(self, msg):
self.current_pose = msg
pass
class DBWNode(object):
def __init__(self):
rospy.init_node('dbw_node')
vehicle_mass = rospy.get_param('~vehicle_mass', 1736.35) # need this
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) # need this
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 = .4
# brakes are in units of Torque
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(
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=max_throttle)
rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_enabled_cb)
rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_cb)
rospy.Subscriber('/current_velocity', TwistStamped, self.velocity_cb)
# To compute the CTE
rospy.Subscriber('/current_pose', PoseStamped, self.pose_cb)
rospy.Subscriber('/final_waypoints', Lane, self.waypoints_cb)
self.cte_pub = rospy.Publisher('/vehicle/cte',
Float32, queue_size=1)
self.current_velocity = None
self.current_ang_velocity = None
self.dbw_enabled = None
self.linear_velocity = None
self.angular_velocity = None
self.current_position = None
self.waypoints = None
self.loop()
def loop(self):
rate = rospy.Rate(50) # 50Hz
while not rospy.is_shutdown():
if self.dbw_enabled and None not in (
self.current_velocity,
self.linear_velocity,
self.angular_velocity):
cte = 0.
if None not in (self.current_position, self.waypoints):
cte = self.calculate_cte()
self.cte_pub.publish(cte)
throttle, brake, steering = self.controller.control(
self.current_velocity,
self.linear_velocity,
self.angular_velocity,
cte)
self.publish(throttle, brake, steering)
rate.sleep()
def dbw_enabled_cb(self, msg):
self.dbw_enabled = msg
def twist_cb(self, msg):
self.linear_velocity = msg.twist.linear.x
self.angular_velocity = msg.twist.angular.z
def velocity_cb(self, msg):
self.current_velocity = msg.twist.linear.x
def pose_cb(self, msg):
self.current_position = [msg.pose.position.x, msg.pose.position.y]
def waypoints_cb(self, msg):
self.waypoints = msg.waypoints
def calculate_cte(self):
def position(waypoint):
return [waypoint.pose.pose.position.x,
waypoint.pose.pose.position.y]
# Get waypoint positions relative to the first
positions = np.array(
[position(waypoint) for waypoint in self.waypoints])
origin = positions[0]
positions = positions - origin
# Rotate the positions so that they are oriented in the direction of travel
offset = 10
angle = np.arctan2(positions[offset, 1], positions[offset, 0])
rotation = np.array([
[np.cos(angle), -np.sin(angle)],
[np.sin(angle), np.cos(angle)],
])
positions = np.dot(positions, rotation)
# Transform the current pose of the car to be in the car's coordinate system
translated = np.array(self.current_position) - origin
rotated = np.dot(translated, rotation)
# The CTE is simply the difference between the actual position and the expected position
coefficients = np.polyfit(positions[:, 0], positions[:, 1], deg=2)
expected = np.polyval(coefficients, rotated[0])
actual = rotated[1]
return actual - expected
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', 2.)
max_steer_angle = rospy.get_param('~max_steer_angle', 5.)
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 = Controller(
YawController(wheel_base, steer_ratio, 0.001, max_lat_accel, max_steer_angle),
wheel_radius * vehicle_mass)
# min_speed = 0.001: in YawController.get_angle, it is divided by a radius that is
# proportional to current speed. To avoid division by zero, we need a minimum speed.
# Some important variables:
self.dbw_enabled = False
self.curr_linear_velocity = 0.0
self.curr_angular_velocity = 0.0
self.des_linear_velocity = 0.0
self.des_angular_velocity = 0.0
self.previous_timestamp = rospy.get_rostime().secs
self.current_timestamp = 0.0
self.vel_cur = 0.0
self.delta_t = 0.0
# TODO: Subscribe to all the topics you need to
rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_enabled_cb, queue_size=1)
rospy.Subscriber('/current_velocity', TwistStamped, self.current_velocity_cb, queue_size=1)
rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_cmd_cb, queue_size=1)
self.loop()
def loop(self):
rate = rospy.Rate(20) # 20Hz
# counter = 0 # just for testing throttle _AND_ brake
# des_linear_velocity = self.des_linear_velocity
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_rostime()
current_secs = current_time.secs
current_nsecs = current_time.nsecs
self.current_timestamp = current_secs + current_nsecs/1000000000.0
self.delta_t = (self.current_timestamp - self.previous_timestamp)
self.previous_timestamp = self.current_timestamp
#self.dbw_enabled = True
#if counter>100: # just for testing throttle _AND_ brake
# counter = 0.
# if des_linear_velocity == 0.:
# des_linear_velocity = self.des_linear_velocity
# else:
# des_linear_velocity = 0.
# rospy.logwarn("speed = "+str(des_linear_velocity))
if self.dbw_enabled:
#self.controller.reload_params()
throttle, brake, steering = self.controller.control(
self.delta_t, self.des_linear_velocity, self.des_angular_velocity,
self.curr_linear_velocity)
self.publish(throttle, brake, steering)
else:
self.controller.init(self.curr_linear_velocity)
rate.sleep()
# counter = counter + 1 # just for testing throttle _AND_ brake
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):
if (msg.data == True):
self.dbw_enabled = True
rospy.logwarn("DBW_ENABLED")
else:
self.dbw_enabled = False
rospy.logwarn("DBW_DISABLED")
def current_velocity_cb(self, message):
# """From the incoming message extract the velocity message """
self.curr_linear_velocity = message.twist.linear.x
self.curr_angular_velocity = message.twist.angular.z
def twist_cmd_cb(self, message):
# """From the incoming message extract the desired velocity (twist) message """
self.des_linear_velocity = message.twist.linear.x
self.des_angular_velocity = message.twist.angular.z
class DBWNode(object):
def __init__(self):
rospy.init_node('dbw_node')
# rospy.logwarn("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 = Controller(wheel_base, steer_ratio, max_lat_accel,
max_steer_angle, vehicle_mass,
wheel_radius, fuel_capacity, accel_limit,
decel_limit)
self.dbw_enabled = False
self.cur_lin_vel = None
self.cur_ang_vel = None
self.des_lin_vel = None
self.des_ang_vel = None
# TODO: Subscribe to all the topics you need to
rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_enabled_cb)
rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_smd_cb)
rospy.Subscriber('/current_velocity', TwistStamped,
self.cur_velocity_cb)
rospy.loginfo("DBW initialized!")
self.loop()
def loop(self):
rate = rospy.Rate(50) # 50Hz
# rospy.loginfo("Looping... state is: ")
# rospy.loginfo(rospy.is_shutdown())
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>)
# self.cur_ang_vel = 0
# rospy.loginfo(self.cur_lin_vel)
# rospy.loginfo(self.cur_ang_vel)
# rospy.loginfo(self.des_lin_vel)
# rospy.loginfo(self.des_ang_vel)
if self.cur_lin_vel is None or self.cur_ang_vel is None or self.des_lin_vel is None or self.des_ang_vel is None:
continue
throttle, brake, steer = self.controller.control(
self.dbw_enabled, self.cur_lin_vel, self.cur_ang_vel,
self.des_lin_vel, self.des_ang_vel)
# throttle = 1.0
# brake = 0.0
# steer = 0.0
if self.dbw_enabled:
# rospy.logwarn("throttle= %s, brake= %s, steer= %s" , throttle, brake, steer)
# rospy.logwarn("thr= %s, bra= %s", throttle, brake)
self.publish(throttle, brake * 100, 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 dbw_enabled_cb(self, msg):
self.dbw_enabled = msg.data
pass
def twist_smd_cb(self, msg):
self.des_lin_vel = msg.twist.linear.x
self.des_ang_vel = msg.twist.angular.z
pass
def cur_velocity_cb(self, msg):
self.cur_lin_vel = msg.twist.linear.x
self.cur_ang_vel = msg.twist.angular.z
pass
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.is_dbw_enabled = False
self.last_twist_command = None
self.current_velocity = None
self.current_pose = None
self.final_waypoints = None
self.previous_loop_time = rospy.get_rostime()
self.previous_debug_time = rospy.get_rostime()
self.throttle_pid = pid.PID(kp=10.0,
ki=0.01,
kd=1.0,
mn=decel_limit,
mx=0.5 * accel_limit)
self.brake_pid = pid.PID(kp=120.0,
ki=0.0,
kd=1.0,
mn=brake_deadband,
mx=5000)
self.steering_pid = pid.PID(kp=1.0,
ki=0.1,
kd=0.6,
mn=-max_steer_angle,
mx=max_steer_angle)
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 = Controller(self.throttle_pid, self.brake_pid,
self.steering_pid)
# TODO: Subscribe to all the topics you need to
rospy.Subscriber('/twist_cmd',
TwistStamped,
self.twist_commands_cb,
queue_size=1)
rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_enabled_cb)
rospy.Subscriber('/current_velocity',
TwistStamped,
self.current_velocity_cb,
queue_size=1)
rospy.Subscriber('/current_pose',
geometry_msgs.msg.PoseStamped,
self.current_pose_cb,
queue_size=1)
rospy.Subscriber('/final_waypoints',
styx_msgs.msg.Lane,
self.final_wp_cb,
queue_size=1)
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.)
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>)
config = {
'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
}
self.controller = Controller(**config)
self.is_dbw_enabled = False
self.current_velocity = None
self.proposed_velocity = None
self.final_waypoints = None
self.current_pose = None
self.previous_loop_time = rospy.get_rostime()
# TODO: Subscribe to all the topics you need to
self.twist_sub = rospy.Subscriber('/twist_cmd',
TwistStamped,
self.twist_message_callback,
queue_size=1)
self.velocity_sub = rospy.Subscriber('/current_velocity',
TwistStamped,
self.current_velocity_callback,
queue_size=1)
self.dbw_sub = rospy.Subscriber('/vehicle/dbw_enabled',
Bool,
self.dbw_enabled_callback,
queue_size=1)
self.final_wp_sub = rospy.Subscriber('final_waypoints',
Lane,
self.final_waypoints_cb,
queue_size=1)
self.pose_sub = rospy.Subscriber('/current_pose',
PoseStamped,
self.current_pose_cb,
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.)
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 `Controller` object
self.controller = Controller(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)
# Subscribe to topics '/twist_cmd', '/current_velocity', '/vehicle/dbw_enabled'
rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_cb)
rospy.Subscriber('/current_velocity', TwistStamped,
self.current_vel_cb)
rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_enable_cb)
self.current_vel = None
# self.curr_ang_vel = None
self.dbw_enabled = None
self.linear_vel = None
self.angular_vel = None
self.throttle = self.steering = self.brake = 0
self.loop()
def loop(self):
# * Autoware safety feature, system auto shutdown when this is less than 10Hz
rate = rospy.Rate(50) # 50Hz
while not rospy.is_shutdown():
# Get predicted throttle, brake, and steering using `twist_controller`
# Only publish the control commands when dbw_enabled is true to avoid error accumulation in manual mode
if not None in (self.current_vel, self.linear_vel,
self.angular_vel):
self.throttle, self.brake, self.steering = self.controller.control(
self.current_vel, self.dbw_enabled, self.linear_vel,
self.angular_vel)
if self.dbw_enabled:
self.publish(self.throttle, self.brake, self.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 twist_cb(self, msg):
self.linear_vel = msg.twist.linear.x
self.angular_vel = msg.twist.angular.z
def current_vel_cb(self, msg):
self.current_vel = msg.twist.linear.x
def dbw_enable_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.proposed_linear_velocity = 0.0
self.proposed_angular_velocity = 0.0
self.current_linear_velocity = 0.0
self.current_angular_velocity = 0.0
self.dbw_enabled = False
# TODO: Create `TwistController` object
# self.controller = TwistController(<Arguments you wish to provide>)
self.min_speed = 5
self.controller = Controller(wheel_base, steer_ratio, self.min_speed,
max_lat_accel, max_steer_angle,
accel_limit, decel_limit, vehicle_mass,
wheel_radius, brake_deadband)
# TODO: Subscribe to all the topics you need to
rospy.Subscriber('/twist_cmd', TwistStamped,
self.twist_cmd_cb) # subscribing to /twist_cmd
rospy.Subscriber('/current_velocity', TwistStamped,
self.current_velocity_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 velocity>,
# <proposed angular velocity>,
# <current linear velocity>,
# <dbw status>,
# <any other argument you need>)
# if <dbw is enabled>:
# self.publish(throttle, brake, steer)
throttle, brake, steering = self.controller.control(
self.proposed_linear_velocity, self.proposed_angular_velocity,
self.current_linear_velocity, self.dbw_enabled)
if self.dbw_enabled:
self.publish(throttle, brake, steering)
rate.sleep()
def twist_cmd_cb(self, twist_stamp):
#rospy.logwarn('twist_cmd_handler: twist stamp = ' + str(twist_stamp))
self.proposed_linear_velocity = twist_stamp.twist.linear.x
# Assumption: Only the z component of angular velocity is non-zero
self.proposed_angular_velocity = twist_stamp.twist.angular.z
def current_velocity_cb(self, curr_v):
self.current_linear_velocity = curr_v.twist.linear.x
self.current_angular_velocity = curr_v.twist.angular.z
def dbw_enabled_cb(self, msg):
self.dbw_enabled = msg.data
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.)
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 `Controller` object
# self.controller = Controller(<Arguments you wish to provide>)
self.controller = Controller(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)
# TODO: Subscribe to all the topics you need to
rospy.Subscriber('/vehicle/dbw_enabled',
Bool,
self.dbw_enabled_cb,
queue_size=1)
rospy.Subscriber('/twist_cmd',
TwistStamped,
self.twist_cb,
queue_size=1)
rospy.Subscriber('/current_velocity',
TwistStamped,
self.velocity_cb,
queue_size=1)
self.dbw_enabled = False
self.current_vel = None
self.curr_ang_vel = None
self.linear_vel = None
self.angular_vel = None
self.throttle = 0
self.steering = 0
self.brake = 0
self.loop()
class DBWNode(object):
def __init__(self):
rospy.init_node('dbw_node', log_level=rospy.DEBUG)
# Parameter
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 = rospy.get_param('~min_speed', 1.)
# Publisher
self.steer_pub = rospy.Publisher('/vehicle/steering_cmd',
SteeringCmd,
queue_size=2)
self.throttle_pub = rospy.Publisher('/vehicle/throttle_cmd',
ThrottleCmd,
queue_size=2)
self.brake_pub = rospy.Publisher('/vehicle/brake_cmd',
BrakeCmd,
queue_size=2)
# Controller
controller_args = {
'wheel_base': wheel_base,
'steer_ratio': steer_ratio,
'min_speed': min_speed,
'max_lat_accel': max_lat_accel,
'max_steer_angle': max_steer_angle,
'decel_limit': decel_limit,
'accel_limit': accel_limit,
'vehicle_mass': vehicle_mass,
'wheel_radius': wheel_radius,
'brake_deadband': brake_deadband
}
self.controller = Controller(**controller_args)
# Variables
self.dbw_enabled = True # drive-by-wire is enabled by default and can be disable by a manual operator
self.curr_vel = 0.0 # Current velocity
self.curr_ang_vel = 0.0 # Current angular velocity
self.target_vel = 0.0 # Target velocity
self.target_ang_vel = 0.0 # Target angular velocity
self.current_pose = None # current pose of the car
self.final_waypoints = None # Lane object
self.last_timestamp = rospy.rostime.get_time()
self.curr_angle = 0.0
self.cte = 0.0
self.count = 0
# Subscriber
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,
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)
rospy.Subscriber('/vehicle/steering_report',
SteeringReport,
self.current_angle_cb,
queue_size=1)
rospy.Subscriber('/vehicle/cte', Float32, self.cte_cb, queue_size=1)
self.loop()
def loop(self):
rate = rospy.Rate(10) # 10Hz
while not rospy.is_shutdown():
now = rospy.rostime.get_time()
elapsed = now - self.last_timestamp
self.last_timestamp = now
# Saving Arguments for the Controller
control_args = {
'target_vel': self.target_vel, # target linear velocity
'curr_vel': self.curr_vel, # current linear velocity
'target_ang_vel':
self.target_ang_vel, # target angular velocity
'curr_ang_vel': self.curr_ang_vel, # current angular velocity
'dbw_enabled': self.dbw_enabled, # dbw status
'curr_angle': self.curr_angle,
'cte': self.cte,
'elapsed': elapsed
}
# Call Controller
throttle, brake, angle = self.controller.control(**control_args)
# We can uncomment the control calling above when they are done, first here some values for throttle, brake and angle
# throttle = 1.0
# brake = 0
# angle = 0.0
if self.dbw_enabled:
self.publish(throttle, brake, angle)
rate.sleep()
def publish(self, throttle, brake, steer):
#debug_msg = "DBW T:{} \t B:{} \t S:{}\t".format(throttle, brake, steer)
#rospy.logdebug(debug_msg)
self.count += 1
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 = bool(msg.data)
def twist_cmd_cb(self, msg):
self.target_vel = msg.twist.linear.x
self.target_ang_vel = msg.twist.angular.z
def current_velocity_cb(self, msg):
self.curr_vel = msg.twist.linear.x
self.curr_ang_vel = msg.twist.angular.z
def current_pose_cb(self, msg):
self.current_pose = msg.pose
def cte_cb(self, msg):
self.cte = msg.data
def final_waypoints_cb(self, msg):
self.final_waypoints_cb = msg.waypoints
def current_angle_cb(self, msg):
self.curr_angle = msg.steering_wheel_angle
class DBWNode(object):
def __init__(self):
rospy.init_node('dbw_node')
class Paramater_State():
vehicle_mass = None
fuel_capacity = None
brake_deadband = None
decel_limit = None
accel_limit = None
wheel_radius = None
wheel_base = None
steer_ratio = None
max_lat_accel = None
max_steer_angle = None
#Paramater States:
S = Paramater_State()
S.vehicle_mass = rospy.get_param('~vehicle_mass', 1736.35)
S.fuel_capacity = rospy.get_param('~fuel_capacity', 13.5)
S.brake_deadband = rospy.get_param('~brake_deadband', .1)
S.decel_limit = rospy.get_param('~decel_limit', -5)
S.accel_limit = rospy.get_param('~accel_limit', 1.)
S.wheel_radius = rospy.get_param('~wheel_radius', 0.2413)
S.wheel_base = rospy.get_param('~wheel_base', 2.8498)
S.steer_ratio = rospy.get_param('~steer_ratio', 14.8)
S.max_lat_accel = rospy.get_param('~max_lat_accel', 3.)
S.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.dbw_enabled = True
self.initial_time = rospy.get_time()
self.pose = None
self.twist_cmd = None
self.reset = True
self.waypoints = None
self.current_velocity = None
self.controller = Controller(S)
# TODO: Subscribe to all the topics you need to
rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_cmd_callback, queue_size=1)
rospy.Subscriber('/current_velocity', TwistStamped, self.current_velocity_callback, queue_size=1)
rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_enabled_callback, queue_size=1)
rospy.Subscriber('/current_pose', PoseStamped, self.pose_callback, queue_size=1)
rospy.Subscriber('/final_waypoints', Lane, self.waypoints_callback, queue_size=1)
self.loop()
def loop(self):
rate = rospy.Rate(100) # 50Hz
#rate = rospy.Rate(20) # 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
time = rospy.get_time()
delta_t = time - self.initial_time
# Check time stamps.
if time == self.initial_time:
continue
# Chck for divide by zero.
#FIXME: Commenting for now, check why this is erroing delta_t = (time.nsec - self.initial_time.nsec) * 1e-6
#delta_t = (time - self.initial_time) * 1e-9 # Isn't nsec 9 zeros
delta_t = (time - self.initial_time) * 1e-2 # Isn't nsec 9 zeros
# Make sure there are enough waypoints.
waypoints_in = self.waypoints is not None
if not waypoints_in:
continue
#continue driving if less than 3 waypoints.
#FIXME: -- commented the next 2 lines for now.
###if(len(self.waypoints) < 3):
### continue
#Enable manual override and manual stop.
#Reset first
if self.dbw_enabled and self.twist_cmd is not None and self.current_velocity is not None and self.pose is not None:
if self.reset:
self.controller.reset()
self.reset = False
#Commands from twist controlller
throttle, brake, steering = self.controller.control(self.twist_cmd, self.current_velocity, delta_t, self.pose, self.waypoints)
rospy.loginfo("dbw_node: throttle: %f brake: %f steering: %f", throttle, brake, steering)
else:
self.reset = True
throttle = 0.0
brake = 0.0
steering = 0.0
rospy.loginfo("dbw_node: reset")
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 twist_cmd_callback(self, msg):
#self.twist_cmd = msg.twist
self.twist_cmd = msg
#Current_velocity callback comes from twist.
def current_velocity_callback(self, msg):
#self.current_velocity = msg.velocity
self.current_velocity = msg
#Pose callback comes from messages in pose.
def pose_callback(self, msg):
#self.pose = msg.pose
self.pose = msg
#DBW info comes in from data.
def dbw_enabled_callback(self, msg):
#self.dbw_enabled = msg.data
self.dbw_enabled = msg
#Populate waypoints
def waypoints_callback(self, msg):
#self.waypoints = msg.waypoints
self.waypoints = msg
def __init__(self):
rospy.init_node('dbw_node', log_level=rospy.DEBUG)
# Parameter
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 = rospy.get_param('~min_speed', 1.)
# Publisher
self.steer_pub = rospy.Publisher('/vehicle/steering_cmd',
SteeringCmd,
queue_size=2)
self.throttle_pub = rospy.Publisher('/vehicle/throttle_cmd',
ThrottleCmd,
queue_size=2)
self.brake_pub = rospy.Publisher('/vehicle/brake_cmd',
BrakeCmd,
queue_size=2)
# Controller
controller_args = {
'wheel_base': wheel_base,
'steer_ratio': steer_ratio,
'min_speed': min_speed,
'max_lat_accel': max_lat_accel,
'max_steer_angle': max_steer_angle,
'decel_limit': decel_limit,
'accel_limit': accel_limit,
'vehicle_mass': vehicle_mass,
'wheel_radius': wheel_radius,
'brake_deadband': brake_deadband
}
self.controller = Controller(**controller_args)
# Variables
self.dbw_enabled = True # drive-by-wire is enabled by default and can be disable by a manual operator
self.curr_vel = 0.0 # Current velocity
self.curr_ang_vel = 0.0 # Current angular velocity
self.target_vel = 0.0 # Target velocity
self.target_ang_vel = 0.0 # Target angular velocity
self.current_pose = None # current pose of the car
self.final_waypoints = None # Lane object
self.last_timestamp = rospy.rostime.get_time()
self.curr_angle = 0.0
self.cte = 0.0
self.count = 0
# Subscriber
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,
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)
rospy.Subscriber('/vehicle/steering_report',
SteeringReport,
self.current_angle_cb,
queue_size=1)
rospy.Subscriber('/vehicle/cte', Float32, self.cte_cb, 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.)
self.cont = Controller(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,
speed_kp=2.,
accel_kp=0.4,
accel_ki=0.1,
accel_tau=0.5)
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
rospy.Subscriber('/vehicle/steering_report', SteeringReport,
self.vel_cb)
rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_cb)
rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_enabled_cb)
self.throttle = self.brake = self.steer = 0.
self.current_velocity = self.linear = self.angular = None
self.dbw_enabled = False
self.loop()
def loop(self):
rate = rospy.Rate(50)
while not rospy.is_shutdown():
if not None in (self.current_velocity, self.linear, self.angular):
t, b, s = self.cont.control(self.linear, self.angular,
self.current_velocity,
self.dbw_enabled)
self.throttle, self.brake, self.steer = t, b, s
print(t)
if self.dbw_enabled:
self.publish(self.throttle, self.brake, self.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 vel_cb(self, msg):
self.current_velocity = msg.speed
def twist_cb(self, msg):
self.linear = msg.twist.linear.x
self.angular = msg.twist.angular.z
def dbw_enabled_cb(self, msg):
self.dbw_enabled = msg.data
class DBWNode(object):
def __init__(self):
rospy.init_node('dbw_node')
# Receive configuration 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)
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.)
# Create publishers
self.steer_pub = rospy.Publisher('/vehicle/steering_cmd',
SteeringCmd,
queue_size=1)
self.throttle_pub = rospy.Publisher('/vehicle/throttle_cmd',
ThrottleCmd,
queue_size=1)
self.brake_pub = rospy.Publisher('/vehicle/brake_cmd',
BrakeCmd,
queue_size=1)
# TODO: Create `Controller` object
# self.controller = Controller(<Arguments you wish to provide>)
self.controller = Controller(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)
# TODO: Subscribe to all the topics you need to
rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_enabled_cb)
rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_cb)
rospy.Subscriber('/current_velocity', TwistStamped, self.velocity_cb)
# init variables
self.current_vel = None
self.curr_ang_vel = None
self.dbw_enabled = None
self.target_linear_vel = None
self.target_angular_vel = None
self.throttle = 0
self.steering = 0
self.brake = 0
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
if not None in (self.current_vel, self.target_linear_vel,
self.target_angular_vel):
self.throttle, self.brake, self.steering = self.controller.control(
self.current_vel, self.dbw_enabled, self.target_linear_vel,
self.target_angular_vel)
if self.dbw_enabled:
self.publish(self.throttle, self.brake, self.steering)
rate.sleep()
def dbw_enabled_cb(self, msg):
self.dbw_enabled = msg
def twist_cb(self, msg):
#Only x-direccion linear and z-direcction angular are nonzero. Linear is already m/s and angular in rad/s.
self.target_linear_vel = msg.twist.linear.x
self.target_angular_vel = msg.twist.angular.z
#rospy.loginfo("dbw_node: Received /twist_cmd (linear speed={0} m/s, angular speed={1} rad/s)".format(self.target_linear_vel, self.target_angular_vel))
def velocity_cb(self, msg):
# Coordinates are vehicle centered. Only the x-direction linear velocity is nonzero. The speed is already in m/s.
self.current_vel = msg.twist.linear.x
#rospy.loginfo("dbw_node: Received /current_velocity (speed={0} m/s)".format(self.current_vel))
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)
self.controller = Controller(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)
rospy.Subscriber('/current_velocity', TwistStamped,
self.current_vel_cb)
rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_cmd_cb)
rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_enabled_cb)
self.current_velocity = None
self.req_linear_velocity = None
self.req_angular_velocity = None
self.dbw_enabled = False
self.throttle = 0
self.brake = 0
self.steer = 0
self.loop()
def loop(self):
rate = rospy.Rate(50) # 50Hz
while not rospy.is_shutdown():
if not None in (self.current_velocity, self.req_linear_velocity,
self.req_angular_velocity):
self.throttle, self.brake, self.steer = self.controller.control(\
self.current_velocity,
self.req_linear_velocity,
self.req_angular_velocity,
self.dbw_enabled)
if self.dbw_enabled:
self.publish(self.throttle, self.brake, self.steer)
rate.sleep()
def dbw_enabled_cb(self, msg):
self.dbw_enabled = msg
def twist_cmd_cb(self, msg):
self.req_linear_velocity = msg.twist.linear.x
self.req_angular_velocity = msg.twist.angular.z
def current_vel_cb(self, msg):
self.current_velocity = msg.twist.linear.x
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.0
self.dbw_en = False
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)
params = {"wheel_base": wheel_base, "wheel_radius":wheel_radius, "steer_ratio": steer_ratio, "min_speed": min_speed, "max_lat_accel": max_lat_accel,
"max_steer_angle": max_steer_angle, "brake_deadband": brake_deadband, "accel_limit": accel_limit,
"decel_limit": decel_limit, "vehicle_mass":vehicle_mass, "fuel_capacity":fuel_capacity}
# TODO: Create `TwistController` object
self.controller = Controller(**params)
# TODO: Subscribe to all the topics you need to
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():
try:
params = {'twist_cmd': self.twist_cmd,
'current_vel': self.current_velocity}
throttle, brake, steer = self.controller.control(**params)
if(self.dbw_en):
self.publish(throttle, brake, steer)
else:
self.controller.reset()
rate.sleep()
except AttributeError:
pass
# Callbacks for subscribers
def dbw_enabled_cb(self, dbw_en):
self.dbw_en = dbw_en
def twist_cmd_cb(self, twist):
self.twist_cmd = twist
def current_velocity_cb(self, vel):
self.current_velocity = vel
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')
# Define some vehicle 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.)
# Initialize steering, throttle and brake publishers
self.steer_pub = rospy.Publisher('/vehicle/steering_cmd',
SteeringCmd,
queue_size=1)
self.throttle_pub = rospy.Publisher('/vehicle/throttle_cmd',
ThrottleCmd,
queue_size=1)
self.brake_pub = rospy.Publisher('/vehicle/brake_cmd',
BrakeCmd,
queue_size=1)
# TODO: Create `TwistController` object
acc2torque = vehicle_mass * vehicle_mass #trasformation from acceleration 2 torque
self.controller = Controller(wheel_base, steer_ratio, max_lat_accel,
max_steer_angle, acc2torque, decel_limit,
accel_limit, wheel_radius)
# 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)
"""
Service initialized:
- Publishers:
- steer_pub = publisher of the steering cmd
- throttle_pub = publisher of the throttle cmd
- brake_pub = publisher of the brake cmd
- Subscribers:
- twist_cmd_cb = subscribe to requested twist
- current_velocity_cb = subscribe to requested velocity
- dbw_enabled_cb = subscribe to driving mode
"""
# variables
self.dbw_enabled = False
self.current_velocity = None
self.twist_cmd = None
self.target_velocity = None
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
if self.current_velocity is not None and self.twist_cmd is not None:
v_limit = rospy.get_param(
'/waypoint_loader/velocity'
) / 3.6 * 0.9 # Speed limit (with margin)
throttle, brake, steering = self.controller.control(
self.twist_cmd.linear, self.twist_cmd.angular,
self.current_velocity.linear,
self.current_velocity.angular, self.dbw_enabled, v_limit)
# Immediatly before publish control dbw_enable state
if self.dbw_enabled:
print "publish"
self.publish(throttle, brake, steering)
# slleep until next period fire
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.pedal_cmd_type = BrakeCmd.CMD_TORQUE # We want to command the brake in torque Nm
# If the requested torque is over the threshold power on the breake lights
bcmd.enable = brake > BrakeCmd.TORQUE_BOO
bcmd.pedal_cmd = brake # Set the brake value
self.brake_pub.publish(bcmd)
def twist_cmd_cb(self, msg):
self.twist_cmd = msg.twist
def current_velocity_cb(self, msg):
self.current_velocity = msg.twist
#rospy.logerr(msg.twist)
def dbw_enabled_cb(self, msg):
self.dbw_enabled = msg.data
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)
args = {
'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
}
self.controller = Controller(**args)
# TODO: Subscribe to all the topics you need to
self.current_velocity = None
self.twist_cmd = None
self.dbw_enabled = None
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
if self.current_velocity and self.twist_cmd:
throttle, brake, steering = self.controller.control(self.twist_cmd,
self.current_velocity,
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)
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
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.controller = Controller(wheel_base, steer_ratio, max_lat_accel,
max_steer_angle, vehicle_mass,
wheel_radius, brake_deadband)
# Subscribe to all needed topics
rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_cb)
rospy.Subscriber('/current_velocity', TwistStamped,
self.current_velocity_cb)
self.dbw_enabled = False
rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_enabled_cb)
self.curr_lin_vel = None
self.curr_ang_vel = None
self.trgt_lin_vel = None
self.trgt_ang_vel = None
self.loop()
def loop(self):
rate = rospy.Rate(CMD_RATE)
while not rospy.is_shutdown():
if self.curr_lin_vel is None \
or self.curr_ang_vel is None \
or self.trgt_lin_vel is None \
or self.trgt_ang_vel is None:
continue
# Get predicted throttle, brake, and steering using `twist_controller`
throttle, brake, steer = self.controller.control(
self.trgt_lin_vel, self.trgt_ang_vel, self.curr_lin_vel,
self.dbw_enabled)
# You should only publish the control commands if dbw is 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)
# Callback function for twist_cmd
def twist_cb(self, twist_stamped_msg):
self.trgt_lin_vel = twist_stamped_msg.twist.linear.x
self.trgt_ang_vel = twist_stamped_msg.twist.angular.z
# Callback function for current_velocity
def current_velocity_cb(self, twist_stamped_msg):
self.curr_lin_vel = twist_stamped_msg.twist.linear.x
self.curr_ang_vel = twist_stamped_msg.twist.angular.z
# Callback function for current_velocity
def dbw_enabled_cb(self, bool_msg):
self.dbw_enabled = bool_msg.data
class DBWNode(object):
def __init__(self):
rospy.init_node('dbw_node', log_level=rospy.INFO)
vehicle_mass = rospy.get_param('~vehicle_mass', 1736.35)
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.)
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(
YawController(wheel_base, steer_ratio, MIN_SPEED, max_lat_accel,
max_steer_angle), accel_limit, decel_limit,
vehicle_mass)
rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.enabled_cb)
rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_cb)
rospy.Subscriber('/current_velocity', TwistStamped, self.velocity_cb)
self.dbw_enabled = False
self.velocity = None
self.proposed_linear = None
self.proposed_angular = None
self.loop()
def enabled_cb(self, msg):
self.dbw_enabled = msg.data
if DEBUG:
rospy.loginfo("DBW enabled = " + str(self.dbw_enabled))
def twist_cb(self, msg):
self.proposed_linear = msg.twist.linear.x
self.proposed_angular = msg.twist.angular.z
if DEBUG:
rospy.logdebug("Twist update: angular = %d; linear = %d" %
(self.proposed_angular, self.proposed_linear))
def velocity_cb(self, msg):
self.velocity = msg.twist.linear.x
if DEBUG:
rospy.logdebug("Velocity update: %d" % self.velocity)
def loop(self):
rate = rospy.Rate(50) # 50Hz
while not rospy.is_shutdown():
if self.velocity is not None \
and self.proposed_linear is not None \
and self.proposed_angular is not None:
throttle, brake, steer = self.controller.control(
self.proposed_linear, self.proposed_angular, self.velocity,
self.dbw_enabled)
if self.dbw_enabled:
self.publish(throttle, brake, steer)
rate.sleep()
def publish(self, throttle, brake, steer):
if DEBUG:
rospy.loginfo("Controls: %.15f:%.15f:%.15f" %
(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.is_dbw_enabled = False
self.last_twist_command = None
self.current_velocity = None
self.current_pose = None
self.final_waypoints = None
self.previous_loop_time = rospy.get_rostime()
self.previous_debug_time = rospy.get_rostime()
self.throttle_pid = pid.PID(kp=10.0,
ki=0.01,
kd=1.0,
mn=decel_limit,
mx=0.5 * accel_limit)
self.brake_pid = pid.PID(kp=120.0,
ki=0.0,
kd=1.0,
mn=brake_deadband,
mx=5000)
self.steering_pid = pid.PID(kp=1.0,
ki=0.1,
kd=0.6,
mn=-max_steer_angle,
mx=max_steer_angle)
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 = Controller(self.throttle_pid, self.brake_pid,
self.steering_pid)
# TODO: Subscribe to all the topics you need to
rospy.Subscriber('/twist_cmd',
TwistStamped,
self.twist_commands_cb,
queue_size=1)
rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_enabled_cb)
rospy.Subscriber('/current_velocity',
TwistStamped,
self.current_velocity_cb,
queue_size=1)
rospy.Subscriber('/current_pose',
geometry_msgs.msg.PoseStamped,
self.current_pose_cb,
queue_size=1)
rospy.Subscriber('/final_waypoints',
styx_msgs.msg.Lane,
self.final_wp_cb,
queue_size=1)
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)
data = [
self.last_twist_command, self.current_velocity,
self.current_pose, self.final_waypoints
]
data_ready = all([x is not None for x in data])
if self.is_dbw_enabled and data_ready:
current_time = rospy.get_rostime()
ros_duration = current_time - self.previous_loop_time
duration_in_seconds = ros_duration.secs + (1e-9 *
ros_duration.nsecs)
self.previous_loop_time = current_time
# Linear velocity and cross track error calculations based on difference between
# current Vs desired speed after x waypoints
linear_velocity_error = self.final_waypoints[
1].twist.twist.linear.x - self.current_velocity.linear.x
cte = dbw_support.get_cross_track_error(
self.final_waypoints, self.current_pose)
# Node output calculations
throttle, brake, steering = self.controller.control(
linear_velocity_error, cte, duration_in_seconds)
# 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 current_velocity_cb(self, msg):
self.current_velocity = msg.twist
def current_pose_cb(self, msg):
self.current_pose = msg.pose
def twist_commands_cb(self, msg):
self.last_twist_command = msg.twist
def dbw_enabled_cb(self, msg):
self.is_dbw_enabled = bool(msg.data)
if self.is_dbw_enabled is True:
self.throttle_pid.reset()
self.brake_pid.reset()
self.steering_pid.reset()
def final_wp_cb(self, msg):
self.final_waypoints = msg.waypoints
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.)
throttle_gains = Gains(rospy.get_param('~throttle_Kp', 0.0),
rospy.get_param('~throttle_Ki', 0.0),
rospy.get_param('~throttle_Kd', 0.0))
steering_gains = Gains(rospy.get_param('~steering_Kp', 0.0),
rospy.get_param('~steering_Ki', 0.0),
rospy.get_param('~steering_Kd', 0.0))
# Parameterize loop rate with 50 Hz as default if not found
self.rate_param = rospy.get_param('~rate_param', 50)
# Publishers
self.steer_pub = rospy.Publisher('/vehicle/steering_cmd',
SteeringCmd,
queue_size=1)
self.throttle_pub = rospy.Publisher('/vehicle/throttle_cmd',
ThrottleCmd,
queue_size=1)
self.brake_pub = rospy.Publisher('/vehicle/brake_cmd',
BrakeCmd,
queue_size=1)
# TODO: Pass params to `Controller` constructor
self.controller = Controller(wheel_base=wheel_base,
steer_ratio=steer_ratio,
min_speed=1.0 * 0.447,
max_lat_accel=max_lat_accel,
max_steer_angle=max_steer_angle,
throttle_gains=throttle_gains,
steering_gains=steering_gains,
accel_limit=accel_limit,
decel_limit=decel_limit)
# Subscriptions
rospy.Subscriber('/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)
# Member vars
self.dbw_enabled = True
self.current_velocity = None
self.twist_cmd = None
self.loop()
def loop(self):
rate = rospy.Rate(
self.rate_param
) # 50Hz means that it goes through the loop 50 times per sec
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.twist_cmd is None or self.current_velocity is None:
continue
throttle, brake, steering = self.controller.control(
self.twist_cmd.twist.linear, self.twist_cmd.twist.angular,
self.current_velocity.twist.linear, self.dbw_enabled)
if self.dbw_enabled:
self.publish(throttle, brake, steering)
rate.sleep()
def publish(self, throttle, brake, steer):
rospy.loginfo("publishing throttle %f, brake %f, steer %f", throttle,
brake, steer)
# We'll either publish the throttle or brake, not both
if throttle != 0:
tcmd = ThrottleCmd()
tcmd.enable = True
tcmd.pedal_cmd_type = ThrottleCmd.CMD_PERCENT
tcmd.pedal_cmd = throttle
self.throttle_pub.publish(tcmd)
else:
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)
def dbw_enabled_cb(self, msg):
rospy.loginfo("DBW status changed to: %s", msg)
self.dbw_enabled = msg
def current_velocity_cb(self, msg):
self.current_velocity = msg
def twist_cmd_cb(self, msg):
rospy.loginfo("Received twist command %s", msg)
self.twist_cmd = 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.)
# Controller
# numbers are invented
controller_params = {
'kp': 1.5,
'ki': 0.003,
'kd': 3.5,
'wheel_base': wheel_base,
'wheel_radius': wheel_radius,
'steer_ratio': steer_ratio,
'min_speed': 0.1,
'max_lat_accel': max_lat_accel,
'max_steer_angle': max_steer_angle,
'tau': 0.5,
'ts': 0.02,
'vehicle_mass': vehicle_mass
}
self.controller = Controller(**controller_params)
# properties
self.dbw_enabled = False
self.last_velocity = None
self.twist_cmd = None
# Publishers
self.steer_pub = rospy.Publisher('/vehicle/steering_cmd',
SteeringCmd,
queue_size=1)
self.throttle_pub = rospy.Publisher('/vehicle/throttle_cmd',
ThrottleCmd,
queue_size=1)
self.brake_pub = rospy.Publisher('/vehicle/brake_cmd',
BrakeCmd,
queue_size=1)
# Subscribers
rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_enabled_cb)
rospy.Subscriber('/current_velocity', TwistStamped,
self.current_velocity_cb)
rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_cmd_cb)
# run the main loop
self.loop()
def dbw_enabled_cb(self, msg):
self.dbw_enabled = msg.data
def current_velocity_cb(self, msg):
self.last_velocity = msg.twist
def twist_cmd_cb(self, msg):
self.twist_cmd = msg.twist
def loop(self):
rospy.wait_for_message('/vehicle/dbw_enabled', Bool)
rospy.wait_for_message('/current_velocity', TwistStamped)
rospy.wait_for_message('/twist_cmd', TwistStamped)
rate = rospy.Rate(50) # 50Hz
while not rospy.is_shutdown():
if self.dbw_enabled:
throttle, brake, steer = self.controller.control(
self.twist_cmd, self.last_velocity)
self.publish(throttle, brake, steer)
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)
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.)
throttle_gains = Gains(rospy.get_param('~throttle_Kp', 0.0),
rospy.get_param('~throttle_Ki', 0.0),
rospy.get_param('~throttle_Kd', 0.0))
steering_gains = Gains(rospy.get_param('~steering_Kp', 0.0),
rospy.get_param('~steering_Ki', 0.0),
rospy.get_param('~steering_Kd', 0.0))
# Parameterize loop rate with 50 Hz as default if not found
self.rate_param = rospy.get_param('~rate_param', 50)
# Publishers
self.steer_pub = rospy.Publisher('/vehicle/steering_cmd',
SteeringCmd,
queue_size=1)
self.throttle_pub = rospy.Publisher('/vehicle/throttle_cmd',
ThrottleCmd,
queue_size=1)
self.brake_pub = rospy.Publisher('/vehicle/brake_cmd',
BrakeCmd,
queue_size=1)
# TODO: Pass params to `Controller` constructor
self.controller = Controller(wheel_base=wheel_base,
steer_ratio=steer_ratio,
min_speed=1.0 * 0.447,
max_lat_accel=max_lat_accel,
max_steer_angle=max_steer_angle,
throttle_gains=throttle_gains,
steering_gains=steering_gains,
accel_limit=accel_limit,
decel_limit=decel_limit)
# Subscriptions
rospy.Subscriber('/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)
# Member vars
self.dbw_enabled = True
self.current_velocity = None
self.twist_cmd = None
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.)
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 `Controller` object
self.controller = Controller(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)
# TODO: Subscribe to all the topics you need to
rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_enabled_cb)
rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_cb)
rospy.Subscriber('/current_velocity', TwistStamped, self.velocity_cb)
self.current_vel = None
self.curr_ang_vel = None
self.dbw_enabled = None
self.linear_vel = None
self.angular_vel = None
self.throttle = self.steering = self.brake = 0
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 not None in (self.current_vel, self.linear_vel,
self.angular_vel):
self.throttle, self.brake, self.steering = self.controller.control(
self.current_vel, self.dbw_enabled, self.linear_vel,
self.angular_vel)
# if <dbw is enabled>:
# self.publish(throttle, brake, steer)
if self.dbw_enabled:
self.publish(self.throttle, self.brake, self.steering)
rate.sleep()
def dbw_enabled_cb(self, msg):
self.dbw_enabled = msg
def twist_cb(self, msg):
self.linear_vel = msg.twist.linear.x
self.angular_vel = msg.twist.angular.z
def velocity_cb(self, msg):
self.current_vel = msg.twist.linear.x
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')
info = CarInfo()
info.vehicle_mass = rospy.get_param('~vehicle_mass', 1736.35)
info.fuel_capacity = rospy.get_param('~fuel_capacity', 13.5)
info.brake_deadband = rospy.get_param('~brake_deadband', .1)
info.decel_limit = rospy.get_param('~decel_limit', -5)
info.accel_limit = rospy.get_param('~accel_limit', 1.)
info.wheel_radius = rospy.get_param('~wheel_radius', 0.2413)
info.wheel_base = rospy.get_param('~wheel_base', 2.8498)
info.steer_ratio = rospy.get_param('~steer_ratio', 14.8)
info.max_lat_accel = rospy.get_param('~max_lat_accel', 3.)
info.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.current_velocity = None
self.twist_cmd = None
self.dbw_enabled = True
self.previous_timestamp = rospy.Time.now().to_sec()
self.reset = True
# Create `TwistController` object
# self.controller = TwistController(<Arguments you wish to provide>)
self.controller = Controller(info)
# Subscribe to all the topics you need to
rospy.Subscriber('/current_velocity',
TwistStamped,
self.velocity_cb,
queue_size=1)
rospy.Subscriber('/twist_cmd',
TwistStamped,
self.twist_cb,
queue_size=1)
rospy.Subscriber('/vehicle/dbw_enabled',
Bool,
self.dbw_enabled_cb,
queue_size=1)
self.loop()
def velocity_cb(self, msg):
self.current_velocity = msg
def twist_cb(self, msg):
self.twist_cmd = msg
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(10) # 10Hz
while not rospy.is_shutdown():
# Get time delta
current_timestamp = rospy.get_time()
delta_time = current_timestamp - self.previous_timestamp
self.previous_timestamp = current_timestamp
# 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.dbw_enabled and self.current_velocity is not None and self.twist_cmd is not None:
if self.reset:
self.controller.reset()
self.reset = False
throttle, brake, steer = self.controller.control(
twist_cmd=self.twist_cmd,
current_velocity=self.current_velocity,
delta_time=delta_time)
rospy.loginfo("CURRENT: %.3f, TARGET: %.3f, BRAKE: %.3f",
self.current_velocity.twist.linear.x,
self.twist_cmd.twist.linear.x, brake)
# if brake > 0.0:
# rospy.loginfo("THROTTLE: %s, BRAKE: %s", throttle, brake)
self.publish(throttle, brake, steer)
else:
self.reset = 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')
# Save param from launch file as dictionary
ros_param_dict = {
'vehicle_mass': rospy.get_param('~vehicle_mass', 1736.35), # must need for stoping the car
'fuel_capacity': rospy.get_param('~fuel_capacity', 13.5), # optional go precise with the weight
'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), # must
'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.)
}
# Subscribe to all the topics you need to
rospy.Subscriber('/current_velocity', TwistStamped, self.cur_velocity_cb)
rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_cmd_cb)
rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_enabled_cb)
# Publish topics.
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(**ros_param_dict)
# List of variables need to use
self.dbw_enabled = None
self.cur_linear_velocity = None # may need low pass filter to reduce noisy
self.cur_angular_velocity = None
self.target_linear_velocity = None
self.target_angular_velocity = None
# flag for checking msg received
self.twist_cmd = False
self.current_velocity = False
self.loop()
def cur_velocity_cb(self, msg):
self.current_velocity = True
self.cur_linear_velocity = msg.twist.linear.x
self.cur_angular_velocity = msg.twist.angular.z
def twist_cmd_cb(self, msg):
self.twist_cmd = True
self.target_linear_velocity = msg.twist.linear.x
self.target_angular_velocity = msg.twist.angular.z
def dbw_enabled_cb(self, msg):
# painful lesson: uncheck the manual to activate the topic otherwise nothing in echo topic
self.dbw_enabled = msg.data
rospy.loginfo(rospy.get_caller_id() + " dbw status is : %s", self.dbw_enabled)
def loop(self):
"""
Get predicted throttle, brake, and steering using `twist_controller`
throttle, brake, steering = self.controller.control(<proposed linear velocity>,
<proposed angular velocity>,
<current linear velocity>,
<dbw status>,
<any other argument you need>)
"""
rate = rospy.Rate(50) # 50Hz Do not Change! Lower than 20Hz will shut down the vehicle
while not rospy.is_shutdown():
# When current_velocity or twist_cmd is not ready
if ((not self.current_velocity) or (not self.twist_cmd)):
continue
throttle, brake, steer = self.controller.control(self.target_linear_velocity,
self.target_angular_velocity,
self.cur_linear_velocity,
self.dbw_enabled)
# You should only publish the control commands if dbw is enabled
if self.dbw_enabled:
self.publish(throttle, brake, steer)
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')
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 = rospy.get_param('~max_throttle', 0.3)
max_brake = rospy.get_param('~max_brake', -0.3)
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)
# TODO: Create `TwistController` object <Arguments you wish to provide>
self.controller = Controller(decel_limit, accel_limit, max_steer_angle,
max_lat_accel, min_speed, wheel_base,
steer_ratio, vehicle_mass, wheel_radius,
max_throttle, max_brake)
# self.yaw_controller = YawController(wheel_base, steer_ratio, 0.0, max_lat_accel, max_steer_angle)
self.dbw_enabled = None
self.current_velocity = None
self.twist_cmd = None
self.pose = None
self.final_waypoints = None
self.prev_clk = rospy.get_rostime().nsecs
self.prev_clk_ready = False
# TODO: Subscribe to all the topics you need to
rospy.Subscriber('/current_velocity',
TwistStamped,
self.curr_vel_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)
rospy.Subscriber('/current_pose',
PoseStamped,
self.pose_cb,
queue_size=1)
rospy.Subscriber('/final_waypoints',
Lane,
self.final_waypoints_cb,
queue_size=1)
self.loop()
def required_all(self):
required = [
self.dbw_enabled, self.current_velocity, self.twist_cmd, self.pose,
self.final_waypoints
]
return all([p is not None for p in required])
def curr_vel_cb(self, curr_vel_msg):
# rospy.loginfo("current_velocity = {}".format(curr_vel_msg.twist))
self.current_velocity = curr_vel_msg.twist
def twist_cmd_cb(self, twist_cmd_msg):
# rospy.loginfo("twist_cmd = {}".format(twist_cmd_msg.twist))
self.twist_cmd = twist_cmd_msg.twist
def dbw_enabled_cb(self, dbw_enabled):
# rospy.loginfo("dbw_enabled = {}".format(dbw_enabled.data))
self.dbw_enabled = dbw_enabled.data
def final_waypoints_cb(self, final_waypoints):
self.final_waypoints = final_waypoints.waypoints
def pose_cb(self, pose):
# rospy.loginfo("pose = {}".format(pose.pose))
self.pose = pose
# rospy.loginfo("pose x, y, yaw = {}, {}, {}".format(self.pose.position.x,
# self.pose.position.y, helper.yaw_from_orientation(self.pose.orientation)))
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 not self.required_all():
rospy.loginfo('Waiting for all params ...')
continue
# TODO: Move clock to pid controllers
clk = rospy.get_rostime()
if not self.prev_clk_ready:
self.prev_clk_ready = True
continue
# Yes, I now it will be always 0.02 but in case someone will change rate ...
delta_t = (clk.nsecs - self.prev_clk) * 1e-9
# rospy.loginfo('delta_t = {}'.format(delta_t))
self.prev_clk = clk.nsecs
target_linear_velocity = abs(
self.twist_cmd.linear.x
) # abs - fixing strange bug in twist_cmd ...
target_angular_velocity = self.twist_cmd.angular.z
current_linear_velocity = self.current_velocity.linear.x
current_angular_velocity = self.current_velocity.angular.z
current_yaw = helper.yaw_from_orientation(
self.pose.pose.orientation)
rospy.loginfo(
"tlv = {}, clv = {}, tav = {}, cav = {}, cyaw = {}".format(
target_linear_velocity, current_linear_velocity,
target_angular_velocity, current_angular_velocity,
current_yaw))
# rospy.loginfo('current_yaw = {}'.format(current_yaw))
steer_cte = helper.calc_steer_cte(self.pose, self.final_waypoints)
rospy.loginfo('STEER_CTE = {}'.format(steer_cte))
throttle, brake, steering = self.controller.control(
target_linear_velocity, current_linear_velocity,
target_angular_velocity, current_angular_velocity, steer_cte,
self.dbw_enabled) # <proposed linear velocity>,
# <proposed angular velocity>,
# <current linear velocity>,
# <dbw status>,
# <any other argument you need>)
# steering1 = self.yaw_controller.get_steering(target_linear_velocity, target_angular_velocity, current_linear_velocity)
rospy.loginfo("throttle, brake, steering = {}, {}, {}".format(
throttle, brake, steering))
# rospy.loginfo("steering1 = {}".format(steering1))
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 # BrakeCmd.CMD_TORQUE, BrakeCmd.CMD_PERCENT
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.)
decel_limit = rospy.get_param('~max_lon_accel', 3.)
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)
# NOTE: To prevent Carla from moving requires about 700 Nm of torque
# (can be calculated from the `vehicle_mass`, `wheel_radius`, and desired
# acceleration)
# Create the `Controller` object
self.controller = Controller(wheel_base, wheel_radius, steer_ratio, max_lat_accel,
max_steer_angle, decel_limit, vehicle_mass)
# Subscribe to all the necessary topics
self.dbw_enabled_sub = rospy.Subscriber('/vehicle/dbw_enabled',
Bool, self.dbw_enabled_cb)
self.curr_velocity_sub = rospy.Subscriber('/current_velocity',
TwistStamped, self.curr_velocity_cb)
self.twist_cmd_sub = rospy.Subscriber('/twist_cmd',
TwistStamped, self.twist_cb)
self.dbw_enabled = None # From '/vehicle/dbw_enabled'
self.curr_velocity = None # From '/current_velocity'
self.linear_vel = None # From '/twist_cmd'
self.angular_vel = None # From '/twist_cmd'
self.loop()
def loop(self):
rate = rospy.Rate(50) # 50Hz
# NOTE: if the rate is below 10Hz, Carla will disengage (reverting control
# to the driver)
# Better ensure that the control commands are published at 50Hz
while not rospy.is_shutdown():
# You should only publish the control commands if dbw is enabled
throttle, brake, steer = self.controller.control(
self.curr_velocity,
self.dbw_enabled,
self.linear_vel,
self.angular_vel,
)
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 dbw_enabled_cb(self, msg):
self.dbw_enabled = msg
def curr_velocity_cb(self, msg):
self.curr_velocity = msg.twist.linear.x
def twist_cb(self, msg):
self.linear_vel = msg.twist.linear.x
self.angular_vel = msg.twist.angular.z
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_throttle = rospy.get_param('~max_throttle', 0.3)
max_brake = rospy.get_param('~max_brake', -0.3)
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)
# TODO: Create `TwistController` object <Arguments you wish to provide>
self.controller = Controller(decel_limit, accel_limit, max_steer_angle,
max_lat_accel, min_speed, wheel_base,
steer_ratio, vehicle_mass, wheel_radius,
max_throttle, max_brake)
# self.yaw_controller = YawController(wheel_base, steer_ratio, 0.0, max_lat_accel, max_steer_angle)
self.dbw_enabled = None
self.current_velocity = None
self.twist_cmd = None
self.pose = None
self.final_waypoints = None
self.prev_clk = rospy.get_rostime().nsecs
self.prev_clk_ready = False
# TODO: Subscribe to all the topics you need to
rospy.Subscriber('/current_velocity',
TwistStamped,
self.curr_vel_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)
rospy.Subscriber('/current_pose',
PoseStamped,
self.pose_cb,
queue_size=1)
rospy.Subscriber('/final_waypoints',
Lane,
self.final_waypoints_cb,
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.)
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(vehicle_mass, fuel_capacity, brake_deadband, decel_limit,
accel_limit, wheel_radius, wheel_base, steer_ratio, max_lat_accel, max_steer_angle)
rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_enabled_cb)
rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_cb)
rospy.Subscriber('/current_velocity', TwistStamped, self.velocity_cb)
self.current_vel = None
self.curr_ang_vel = None
self.dbw_enabled = None
self.linear_vel = None
self.angular_vel = None
self.throttle = 0
self.steering = 0
self.brake = 0
self.loop()
def loop(self):
rate = rospy.Rate(50) # 50Hz
while not rospy.is_shutdown():
if not None in (self.current_vel, self.linear_vel, self.angular_vel):
self.throttle, self.brake, self.steering = self.controller.control(
self.current_vel,
self.dbw_enabled,
self.linear_vel,
self.angular_vel
)
if self.dbw_enabled:
self.publish(self.throttle, self.brake, self.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 dbw_enabled_cb(self, dbw_enabled):
self.dbw_enabled = dbw_enabled
def velocity_cb(self, velocity):
self.current_vel = velocity.twist.linear.x
def twist_cb(self, twist):
self.linear_vel = twist.twist.linear.x
self.angular_vel = twist.twist.angular.z
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)
args = [vehicle_mass, fuel_capacity, brake_deadband, decel_limit,
accel_limit, wheel_radius, wheel_base,
steer_ratio, max_lat_accel, max_steer_angle]
self.controller = Controller(*args)
# Subscriptions
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('/dbw_enabled', Bool, self.dbw_enabled_cb, queue_size=1)
# local variables
self.dbw_enabled = True
self.current_velocity = None
self.twist_cmd = None
self.loop()
def loop(self):
rate = rospy.Rate(COMMAND_FREQUENCY)
while not rospy.is_shutdown():
# Will ignore when we don't have info from `current_velocity`, `twist_cmd` and when dbw is not enabled
if (self.twist_cmd is None) or (self.current_velocity is None) or (not self.dbw_enabled):
continue
throttle, brake, steering = self.controller.control(self.twist_cmd.twist.linear,
self.twist_cmd.twist.angular,
self.current_velocity.twist.linear)
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)
# Helper for subscriber below.
def twist_cmd_cb(self, msg):
"""Subscribe to twist_cmd."""
self.twist_cmd = msg
def current_velocity_cb(self, msg):
"""Subscribe to current_velocity_cb."""
self.current_velocity = msg
def dbw_enabled_cb(self, msg):
"""Subscribe to dbw_enabled."""
self.dbw_enabled = msg
