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(<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)
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 <dbw is enabled>:
# self.publish(throttle, brake, steer)
if not None in (self.current_vel, self.linear_vel,
self.angular_vel):
self.throttle, self.brake, self.steering = self.controller.control(
self.dbw_enabled, self.current_vel, 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.)
max_throttle = rospy.get_param('~max_throttle', 1.)
self.steer_pub = rospy.Publisher('/vehicle/steering_cmd',
SteeringCmd,
queue_size=1)
self.throttle_pub = rospy.Publisher('/vehicle/throttle_cmd',
ThrottleCmd,
queue_size=1)
self.brake_pub = rospy.Publisher('/vehicle/brake_cmd',
BrakeCmd,
queue_size=1)
# Create `TwistController` object
self.controller = Controller(accel_limit=accel_limit,
decel_limit=decel_limit,
wheel_base=wheel_base,
steer_ratio=steer_ratio,
max_lat_accel=max_lat_accel,
max_steer_angle=max_steer_angle,
vehicle_mass=vehicle_mass,
fuel_capacity=fuel_capacity,
wheel_radius=wheel_radius,
brake_deadband=brake_deadband,
max_throttle=max_throttle)
self.dbw_enabled = False
self.twist = None
self.current_velocity = None
self.last_dbw_status = False
self.last_time = None
self.last_action = ''
# Subscribe to all the topics you need to
rospy.Subscriber('/current_velocity', TwistStamped, self.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():
# Get predicted throttle, brake, and steering using `twist_controller`
# You should only publish the control commands if dbw is enabled
now = rospy.get_rostime()
if self.dbw_enabled and self.current_velocity is not None \
and self.twist is not None \
and self.last_time:
self.reset_controller_if_needed()
diff = now - self.last_time
throttle, brake, steer = self.controller.control(
self.current_velocity, self.twist, diff.to_sec())
self.publish(throttle, brake, steer)
# Update variables
self.last_time = now
self.last_dbw_status = self.dbw_enabled
rate.sleep()
def reset_controller_if_needed(self):
if self.dbw_enabled != self.last_dbw_status and self.dbw_enabled:
self.controller.reset()
def publish(self, throttle, brake, steer):
tcmd = ThrottleCmd()
tcmd.enable = True
tcmd.pedal_cmd_type = ThrottleCmd.CMD_PERCENT
tcmd.pedal_cmd = throttle
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
# do not publish throttle and brake at the same time,
# unless the action is different
action = 'brake' if brake > 0.0 else 'throttle'
if action != self.last_action:
self.brake_pub.publish(bcmd)
self.throttle_pub.publish(tcmd)
elif action == 'brake':
self.brake_pub.publish(bcmd)
elif action == 'throttle':
self.throttle_pub.publish(tcmd)
self.last_action = action
def velocity_cb(self, msg):
self.current_velocity = msg.twist.linear.x
def twist_cmd_cb(self, msg):
self.twist = msg.twist
def dbw_enabled_cb(self, msg):
self.dbw_enabled = msg.data
rospy.logwarn('dbw_enabled: %s', self.dbw_enabled)
class DBWNode(object):
def __init__(self):
rospy.init_node('dbw_node')
rospy.loginfo('Start initialization of DBWNode')
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.linear_velocity_future = None
self.angular_velocity_future = None
self.linear_velocity_current = None
self.angular_velocity_current = None
self.acceleration_current = None
self.dbw_enabled = None
self.time_help = None
self.rate = 50 # Rate in Hz
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(vehicle_mass=vehicle_mass,
fuel_capacity=fuel_capacity,
brake_deadband=brake_deadband,
decel_limit=decel_limit,
accel_limit=accel_limit,
wheel_radius=wheel_radius,
rate=self.rate)
# TODO: Subscribe to all the topics you need to
rospy.Subscriber('/twist_cmd',
TwistStamped,
self.twistcmd_cb,
queue_size=1)
rospy.Subscriber('/current_velocity',
TwistStamped,
self.cur_vel_cb,
queue_size=1)
rospy.Subscriber('/vehicle/dbw_enabled',
Bool,
self.dbw_enabled_cb,
queue_size=1)
rospy.loginfo('DBWNode: Subscribed to relevant topics')
self.loop()
def loop(self):
rospy.loginfo('DBWNode: Started looping')
rate = rospy.Rate(self.rate)
while not rospy.is_shutdown():
now = rospy.get_rostime()
if self.has_valid_data():
diff = now - self.time_help
throttle, brake, steer = self.controller.control(
linear_velocity_future=self.linear_velocity_future,
angular_velocity_future=self.angular_velocity_future,
linear_velocity_current=self.linear_velocity_current,
angular_velocity_current=self.angular_velocity_current,
acceleration_current=self.acceleration_current,
time_step=diff.to_sec())
#rospy.loginfo('DBWNode: Controller output: throttle -> %.2f brake -> %.2f steer -> %.2f', throttle, brake, steer)
#rospy.loginfo('_____________________')
#rospy.loginfo(self.linear_velocity_future)
#rospy.loginfo(self.linear_velocity_current)
self.publish(throttle, brake, steer)
self.time_help = now
rate.sleep()
def publish(self, throttle, brake, steer):
if brake > 0.0:
bcmd = BrakeCmd()
bcmd.enable = True
bcmd.pedal_cmd_type = BrakeCmd.CMD_TORQUE
bcmd.pedal_cmd = brake
self.brake_pub.publish(bcmd)
else:
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)
# TODO: Implement
def twistcmd_cb(self, msg):
self.linear_velocity_future = msg.twist.linear.x
self.angular_velocity_future = msg.twist.angular.z
#rospy.loginfo('DBWNode: Updated twist')
# TODO: Implement
def cur_vel_cb(self, msg):
if (self.linear_velocity_current is not None):
self.acceleration_current = self.rate * (
self.linear_velocity_current - msg.twist.linear.x)
self.linear_velocity_current = msg.twist.linear.x
self.angular_velocity_current = msg.twist.angular.z
#rospy.loginfo('DBWNode: Updated velocity')
# TODO: Implement
def dbw_enabled_cb(self, msg):
self.dbw_enabled = msg.data
rospy.loginfo('DBWNode: Updated dbw_enabled with %d', self.dbw_enabled)
if (not self.dbw_enabled):
self.controller.reset()
def has_valid_data(self):
return (self.time_help
is not None) & (self.linear_velocity_future is not None) & (
self.linear_velocity_current is not None)
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.)
loop_freq = rospy.get_param('~loop_freq', 2)
self.dt = 1. / loop_freq
self.dbw_enabled = False
self.current_velocity = None
self.desired_velocity = None
self.steer_pub = rospy.Publisher('/vehicle/steering_cmd',
SteeringCmd,
queue_size=1)
self.throttle_pub = rospy.Publisher('/vehicle/throttle_cmd',
ThrottleCmd,
queue_size=1)
self.brake_pub = rospy.Publisher('/vehicle/brake_cmd',
BrakeCmd,
queue_size=1)
# 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,
sample_period=self.dt)
# 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,
queue_size=3)
rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_enabled_cb)
self.loop()
def twist_cmd_cb(self, msg):
self.desired_velocity = msg.twist
def current_velocity_cb(self, msg):
self.current_velocity = msg.twist
def dbw_enabled_cb(self, msg):
self.dbw_enabled = msg.data
self.controller.velocity_controller.reset()
def loop(self):
rate = rospy.Rate(50) # 50Hz
while not rospy.is_shutdown():
# TODO: Get predicted throttle, brake, and steering using `twist_controller`
# You should only publish the control commands if dbw is enabled
if self.dbw_enabled:
if self.desired_velocity and self.current_velocity:
throttle, brake, steering = self.controller.control(
self.desired_velocity, self.current_velocity, self.dt)
self.publish(throttle, brake, steering)
self.desired_velocity = None
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.)
parameters = {
'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.steer_pub = rospy.Publisher('/vehicle/steering_cmd',
SteeringCmd, queue_size=1)
self.throttle_pub = rospy.Publisher('/vehicle/throttle_cmd',
ThrottleCmd, queue_size=1)
self.brake_pub = rospy.Publisher('/vehicle/brake_cmd',
BrakeCmd, queue_size=1)
self.dbw_enabled = False
self.current_velocity = None
self.current_setpoint = None
# TODO: Create `Controller` object
# self.controller = Controller(<Arguments you wish to provide>)
self.controller = Controller(**parameters)
# TODO: Subscribe to all the topics you need to
self.current_velocity_sub = rospy.Subscriber('/current_velocity', TwistStamped, self.current_speed_cb)
self.twist_cmd_sub = rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_cmd_cb)
self.dbw_enabled_sub = rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_enabled_cb)
self.loop()
def current_speed_cb(self, current_velocity):
self.current_velocity = current_velocity.twist
pass
def twist_cmd_cb(self, twist_cmd):
self.current_setpoint = twist_cmd.twist
pass
def dbw_enabled_cb(self, msg):
rospy.logwarn('dbw_enabled %s', msg.data)
self.dbw_enabled = bool(msg.data)
pass
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_enabled:
throttle, brake, steer = self.controller.control(self.current_setpoint.linear.x, self.current_setpoint.angular.z, self.current_velocity.linear.x, 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)
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
class DBWNode(object):
def __init__(self):
rospy.init_node('dbw_node',log_level=rospy.DEBUG)
vehicle_mass = rospy.get_param('~vehicle_mass', 1736.35)
fuel_capacity = rospy.get_param('~fuel_capacity', 13.5)
brake_deadband = rospy.get_param('~brake_deadband', .1)
decel_limit = rospy.get_param('~decel_limit', -5)
accel_limit = rospy.get_param('~accel_limit', 1.)
wheel_radius = rospy.get_param('~wheel_radius', 0.2413)
wheel_base = rospy.get_param('~wheel_base', 2.8498)
steer_ratio = rospy.get_param('~steer_ratio', 14.8)
max_lat_accel = rospy.get_param('~max_lat_accel', 3.)
max_steer_angle = rospy.get_param('~max_steer_angle', 8.)
self.steer_pub = rospy.Publisher('/vehicle/steering_cmd',
SteeringCmd, queue_size=1)
self.throttle_pub = rospy.Publisher('/vehicle/throttle_cmd',
ThrottleCmd, queue_size=1)
self.brake_pub = rospy.Publisher('/vehicle/brake_cmd',
BrakeCmd, queue_size=1)
# TODO: Create `TwistController` object
# self.controller = TwistController(<Arguments you wish to provide>)
min_speed = 0
throttle_pid = PID(1, 1, 1, mn=0, mx=1)
brake_pid = PID(600, 10, 0, mn=0, mx=3000)
steering_pid = PID(4, 1, 2, mn=-math.pi/3, mx=math.pi/3)
yaw_controller = YawController(wheel_base,
steer_ratio,
min_speed,
max_lat_accel,
max_steer_angle)
low_pass_filter = LowPassFilter(1, 1)
self.controller = Controller(throttle_controller = throttle_pid,
brake_controller = brake_pid,
steering_controller = yaw_controller,
steering_adjustment_controller = steering_pid,
smoothing_filter = low_pass_filter)
# 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)
# init valuables
self.dbw_enabled = False
self.proposed_velocity = (0, 0)
self.current_velocity = (0, 0)
self.current_position = (0, 0)
# Kick off the loop operation
self.loop()
def loop(self):
rate = rospy.Rate(50) # 50Hz
while not rospy.is_shutdown():
# 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==True):
throttle, brake, steering = self.controller.control(
sample_time = 0.02,
proposed_velocity = self.proposed_velocity,
current_velocity = self.current_velocity)
rospy.loginfo("=== lazzzy === : throttle: %f, brake: %f, steering: %f",throttle, brake, steering)
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_cb(self, msg):
self.proposed_velocity = (msg.twist.linear.x, msg.twist.angular.z)
self.controller.reset()
def current_velocity_cb(self, msg):
self.current_velocity = (msg.twist.linear.x, msg.twist.angular.z)
def dbw_enabled_cb(self, msg):
self.dbw_enabled = msg.data
if self.dbw_enabled == False:
self.controller.reset_on_dbw_enabled()
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
min_speed = 0 #?!
self.controller = Controller(wheel_base, steer_ratio, min_speed,
max_lat_accel, max_steer_angle,
vehicle_mass, fuel_capacity,
brake_deadband, decel_limit, accel_limit,
wheel_radius)
# TODO: Subscribe to all the topics you need to
rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_cb)
rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_enabled_cb)
rospy.Subscriber('/current_velocity', TwistStamped,
self.current_velocity_cb)
self.proposed_angular_velocity = self.proposed_linear_velocity = None
self.current_angular_velocity = self.current_linear_velocity = None
self.dbw_enabled = False
self.loop()
def twist_cb(self, msg):
self.proposed_angular_velocity = msg.twist.angular
self.proposed_linear_velocity = msg.twist.linear
return
def dbw_enabled_cb(self, msg):
self.dbw_enabled = msg
rospy.logwarn('dbw_enabled: ' + str(self.dbw_enabled))
return
def current_velocity_cb(self, msg):
self.current_angular_velocity = msg.twist.angular
self.current_linear_velocity = msg.twist.linear
return
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_enabled:
# self.publish(throttle, brake, steer)
if not self.proposed_linear_velocity or not self.proposed_angular_velocity or not self.current_linear_velocity:
rospy.logwarn('velocity info not available')
else:
throttle, brake, steering = self.controller.control(
self.proposed_linear_velocity,
self.proposed_angular_velocity,
self.current_linear_velocity,
(1. / 50), #sample time (1/50Hz) sec
self.dbw_enabled)
if self.dbw_enabled:
self.publish(throttle, brake, steering)
rate.sleep()
def publish(self, throttle, brake, steer):
tcmd = ThrottleCmd()
tcmd.enable = True
tcmd.pedal_cmd_type = ThrottleCmd.CMD_PERCENT
tcmd.pedal_cmd = throttle
self.throttle_pub.publish(tcmd)
scmd = SteeringCmd()
scmd.enable = True
scmd.steering_wheel_angle_cmd = steer
self.steer_pub.publish(scmd)
bcmd = BrakeCmd()
bcmd.enable = True
bcmd.pedal_cmd_type = BrakeCmd.CMD_TORQUE
bcmd.pedal_cmd = brake
self.brake_pub.publish(bcmd)
class DBWNode(object):
def __init__(self):
rospy.init_node('dbw_node')
vehicle_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) # 'brake' is a torque
accel_limit = rospy.get_param('~accel_limit',
1.) # 'accel' is a percentage
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', 0.1)
Kp = rospy.get_param('~pid_kp', 1.1)
Ki = rospy.get_param('~pid_ki', 0.010)
Kd = rospy.get_param('~pid_kd', 0.005)
pid_cmd_range = rospy.get_param('~pid_cmd_range', 4)
filter_tau = rospy.get_param('~filter_tau', 0.0)
self.throttle_pub = rospy.Publisher('/vehicle/throttle_cmd',
ThrottleCmd,
queue_size=1)
self.brake_pub = rospy.Publisher('/vehicle/brake_cmd',
BrakeCmd,
queue_size=1)
self.steer_pub = rospy.Publisher('/vehicle/steering_cmd',
SteeringCmd,
queue_size=1)
self.vx_pub = rospy.Publisher('/debug_vx', Float64, queue_size=1)
self.sz_pub = rospy.Publisher('/debug_sz', Float64, queue_size=1)
self.vxd_pub = rospy.Publisher('/debug_vxd', Float64, queue_size=1)
self.szd_pub = rospy.Publisher('/debug_szd', Float64, queue_size=1)
# def __init__(self, wheel_base, steer_ratio, min_speed, max_lat_accel, max_steer_angle):
self.yaw_controller = YawController(wheel_base, steer_ratio, min_speed,
max_lat_accel, max_steer_angle)
# def __init__(self, tau, ts):
self.filter = LowPassFilter(filter_tau, 0.8) # hm why 0.8?
# Pid controller for the target velocity (decel_limit is already negative)
self.pid_vel = PID(Kp, Ki, Kd, decel_limit, accel_limit)
# write controller
self.controller = Controller(self.yaw_controller, self.pid_vel,
self.filter)
# set controller parameters
vehicle_mass_offset = 25.0 + 70.0 + 30.0 # additional weight (gas, passenger, load)
self.controller.set_vehicle_parameters(vehicle_mass,
vehicle_mass_offset,
brake_deadband, wheel_radius)
self.current_velocity = None
self.target_velocity = None
self.dbw_enabled = True # maybe change to False by default
self.pose = None
rospy.Subscriber("/current_velocity", TwistStamped, self.velocity_cb)
rospy.Subscriber("/twist_cmd", TwistStamped, self.twist_cb)
rospy.Subscriber("/vehicle/dbw_enabled", Bool, self.dbw_cb)
rospy.Subscriber('/current_pose', PoseStamped, self.pose_cb)
self.loop()
def velocity_cb(self, msg):
self.current_velocity = msg.twist
self.vx_pub.publish(self.current_velocity.linear.x)
self.sz_pub.publish(self.current_velocity.angular.z)
def twist_cb(self, msg):
self.target_velocity = msg.twist
self.vxd_pub.publish(self.target_velocity.linear.x)
self.szd_pub.publish(self.target_velocity.angular.z)
def dbw_cb(self, msg):
self.dbw_enabled = msg.data
rospy.loginfo('received dbw_enabled: %s', str(msg.data))
def pose_cb(self, msg):
self.pose = msg.pose
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 self.current_velocity:
rospy.logwarn('no current velocity has been set')
rate.sleep()
continue
if not self.target_velocity:
rospy.logwarn('no target velocity has been set')
rate.sleep()
continue
if not self.pose:
rospy.logwarn('no pose has been set')
rate.sleep()
continue
if not self.dbw_enabled:
rospy.logdebug('no driving by wire')
#Reset the PID controller
self.controller.reset()
rate.sleep()
continue
throttle, brake, steer = self.controller.control(
current_velocity=self.current_velocity.linear.x,
linear_velocity=self.target_velocity.linear.x,
angular_velocity=self.target_velocity.angular.z)
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)
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.)
#I added min_speed here extra to video but hardcoded else where so I put it here as a variable to be reset if needed
min_speed = 0.1 # 10 #0.1
self.steer_pub = rospy.Publisher('/vehicle/steering_cmd',
SteeringCmd,
queue_size=1)
self.throttle_pub = rospy.Publisher('/vehicle/throttle_cmd',
ThrottleCmd,
queue_size=1)
self.brake_pub = rospy.Publisher('/vehicle/brake_cmd',
BrakeCmd,
queue_size=1)
# TODO: Create `Controller` object
# self.controller = Controller(<Arguments you wish to provide>)
# i missed this got error evenutally about controller
# all atribute from above
'''
self.controller = Controller( vehicle_mass = vehicle_mass , # i thkn i am not using kwargs correctly https://pythontips.com/2013/08/04/args-and-kwargs-in-python-explained/
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,
min_speed = min_speed)# I added min_speed here was hardcoced in video as 0.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',
min_speed = 'min_speed')
'''
#another attempt around **kwargs##belwo seems to work
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, min_speed)
# TODO: Subscribe to all the topics you need to
#from Q+A
rospy.Subscriber('/vehicle/dbw_enabled', Bool,
self.dbw_enabled_cb) #turning off for a test
rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_cb)
rospy.Subscriber('/current_velocity', TwistStamped, self.velocity_cb)
self.current_vel = None
self.current_ang_vel = None
self.dbw_enabled = False # False # None # should this be false?
self.linear_vel = None
self.angular_vel = None
self.throttle = self.steering = self.brake = 0
self.loop()
def loop(self):
rate = rospy.Rate(
50
) # maybe small difference at 10# 20 made no apparent difference #original value 50 I am trying to fix the passing the green dots problem by adjusting this rate# 50Hz
while not rospy.is_shutdown():
# TODO: Get predicted throttle, brake, and steering using `twist_controller`
# You should only publish the control commands if dbw is enabled
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)
#self.throttle = 1# testing to see it is executing in here and it is
#self.brake = 0
#self.steering = 0.5#this much worked..test
#throttle, brake, steering = self.controller.control(<proposed linear velocity>,
# <proposed angular velocity>,
# <current linear velocity>,
# <dbw status>,
# <any other argument you need>)
#self.dbw_enabled = False #True# a test It does seem to go in here and execute
if self.dbw_enabled: # changed== True : #was this in video <dbw is enabled>:
self.publish(self.throttle, self.brake, self.steering)
#self.publish(1,0,0)#this much worked..test
rate.sleep()
def dbw_enabled_cb(self, msg): #from Q+A
self.dbw_enabled = msg
def twist_cb(self, msg): # from Q+A
self.linear_vel = msg.twist.linear.x
self.angular_vel = msg.twist.angular.z # had a typo here
def velocity_cb(self, msg): #from Q+A
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.)
#Minimum speed to steer
steering_min_speed = 2.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)
#initialize freq, dbw, target & current twist values
self.sample_freq = 50
self.is_dbw_enabled = False
self.target_twist = None
self.current_twist = None
# TODO: Create `Controller` object
# self.controller = Controller(<Arguments you wish to provide>)
self.controller = Controller(wheel_base, steer_ratio,
steering_min_speed, max_lat_accel,
max_steer_angle, brake_deadband,
accel_limit, decel_limit, vehicle_mass,
wheel_radius, fuel_capacity,
self.sample_freq)
# TODO: Subscribe to all the topics you need to
rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_enabled_cb)
rospy.Subscriber('/current_velocity', TwistStamped,
self.current_twist_cb)
rospy.Subscriber('/twist_cmd', TwistStamped, self.target_twist_cb)
'''
#Debug comment out later
rospy.Subscriber('/vehicle/steering_report', SteeringReport, self.steer_cb)
rospy.Subscriber('/vehicle/throttle_report', ThrottleReport, self.throttle_cb)
rospy.Subscriber('/vehicle/brake_report', BrakeReport, self.brake_cb)
rospy.Subscriber('/vehicle/steering_cmd', SteeringCmd, self.steer_cb)
rospy.Subscriber('/vehicle/throttle_cmd', ThrottleCmd, self.throttle_cb)
rospy.Subscriber('/vehicle/brake_cmd', BrakeCmd, self.brake_cb)
'''
self.loop()
def loop(self):
rate = rospy.Rate(self.sample_freq) # 50Hz
while not rospy.is_shutdown():
if not self.target_twist or not self.current_twist:
continue
# TODO: Get predicted throttle, brake, and steering using `twist_controller`
# You should only publish the control commands if dbw is enabled
if self.is_dbw_enabled:
self.throttle, self.brake, self.steering = self.controller.control(
self.is_dbw_enabled, self.target_twist.linear.x,
self.target_twist.angular.z, self.current_twist.linear.x)
self.publish(self.throttle, self.brake, self.steering)
rate.sleep()
def target_twist_cb(self, msg):
rospy.loginfo("target twist callback")
self.target_twist = msg.twist
def current_twist_cb(self, msg):
rospy.loginfo("current twist callback")
self.current_twist = msg.twist
def dbw_enabled_cb(self, msg):
self.is_dbw_enabled = msg.data
rospy.loginfo("dbw_enabled callback invoked with value %s",
self.is_dbw_enabled)
#Debug log invocation
'''
def steer_cb(self, msg):
rospy.logwarn("ROSBAG throttle is %s", msg.pedal_cmd)
def throttle_cb(self, msg):
rospy.logwarn("ROSBAG throttle is %s", msg.pedal_cmd)
def brake_cb(self, msg):
rospy.logwarn("ROSBAG brake is %s", msg.pedal_cmd)
'''
def publish(self, throttle, brake, steer):
tcmd = ThrottleCmd()
tcmd.enable = True
tcmd.pedal_cmd_type = ThrottleCmd.CMD_PERCENT
tcmd.pedal_cmd = throttle
self.throttle_pub.publish(tcmd)
scmd = SteeringCmd()
scmd.enable = True
scmd.steering_wheel_angle_cmd = steer
self.steer_pub.publish(scmd)
bcmd = BrakeCmd()
bcmd.enable = True
bcmd.pedal_cmd_type = BrakeCmd.CMD_TORQUE
bcmd.pedal_cmd = brake
self.brake_pub.publish(bcmd)
class DBWNode(object):
def __init__(self):
rospy.init_node('dbw_node')
self.vehicle_mass = rospy.get_param('~vehicle_mass', 1736.35)
self.fuel_capacity = rospy.get_param('~fuel_capacity', 13.5)
self.brake_deadband = rospy.get_param('~brake_deadband', .1)
self.decel_limit = rospy.get_param('~decel_limit', -5)
self.accel_limit = rospy.get_param('~accel_limit', 1.)
self.wheel_radius = rospy.get_param('~wheel_radius', 0.2413)
self.wheel_base = rospy.get_param('~wheel_base', 2.8498)
self.steer_ratio = rospy.get_param('~steer_ratio', 14.8)
self.max_lat_accel = rospy.get_param('~max_lat_accel', 3.)
self.max_steer_angle = rospy.get_param('~max_steer_angle', 8.)
self.min_speed = 0.1
self.steer_pub = rospy.Publisher('/vehicle/steering_cmd',
SteeringCmd,
queue_size=1)
self.throttle_pub = rospy.Publisher('/vehicle/throttle_cmd',
ThrottleCmd,
queue_size=1)
self.brake_pub = rospy.Publisher('/vehicle/brake_cmd',
BrakeCmd,
queue_size=1)
# self.current_pose = None # needed?
self.dbw_enabled = True
self.reset_flag = True
self.current_velocity = None
self.twist_cmd = None
# TIME
self.previous_timestamp = rospy.get_time()
# Create `Controller` object
self.controller = Controller(self.wheel_base, self.steer_ratio,
self.min_speed, self.max_lat_accel,
self.max_steer_angle, self.decel_limit,
self.accel_limit, self.brake_deadband,
self.vehicle_mass, self.fuel_capacity,
self.wheel_radius)
# Subscribe to all the topics you need to
rospy.Subscriber('/current_velocity',
TwistStamped,
self.current_velocity_cb,
queue_size=1)
rospy.Subscriber('/twist_cmd',
TwistStamped,
self.twist_cmd_cb,
queue_size=1)
rospy.Subscriber('/vehicle/dbw_enabled',
Bool,
self.dbw_enabled_cb,
queue_size=1)
self.loop()
def loop(self):
rate = rospy.Rate(50) #Hz
while not rospy.is_shutdown():
# TIME
current_timestamp = rospy.get_time()
del_time = current_timestamp - self.previous_timestamp
self.previous_timestamp = current_timestamp
if self.dbw_enabled and self.current_velocity is not None and self.twist_cmd is not None:
if self.reset_flag:
self.controller.reset()
self.reset_flag = False
throttle, brake, steering = self.controller.control(
self.twist_cmd, self.current_velocity, del_time)
self.publish(throttle, brake, steering)
else:
self.reset_flag = True
rate.sleep()
def dbw_enabled_cb(self, dbw_enabled):
try:
self.dbw_enabled = bool(dbw_enabled.data)
except Exception:
self.dbw_enabled = dbw_enabled
def current_velocity_cb(self, current_velocity):
self.current_velocity = current_velocity
def twist_cmd_cb(self, twist_cmd):
self.twist_cmd = twist_cmd
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):
self.dbw_enabled = False
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, decel_limit,
vehicle_mass, wheel_radius,
brake_deadband)
# TODO: Subscr8ibe to all the topics you need to
rospy.Subscriber("/vehicle/dbw_enabled", Bool, self.toggle_dbw)
rospy.Subscriber("/twist_cmd", TwistStamped, self.update_twist_cmd)
rospy.Subscriber("/current_velocity", TwistStamped,
self.update_current_velocity)
self.current_twist = None
self.current_velocity = None
self.loop()
def update_current_velocity(self, value):
self.current_velocity = value.twist
def toggle_dbw(self, value):
self.controller.is_enabled = value.data
def update_twist_cmd(self, value):
self.current_twist = value.twist
def loop(self):
rate = rospy.Rate(50) # 50Hz
while not rospy.is_shutdown():
if self.current_velocity and self.current_twist:
throttle, brake, steering = self.controller.control(
self.current_twist.linear.x, self.current_twist.angular.z,
self.current_velocity.linear.x)
if self.controller.is_enabled:
self.publish(throttle, brake, steering)
rate.sleep()
def publish(self, throttle, brake, steer):
tcmd = ThrottleCmd()
tcmd.enable = True
tcmd.pedal_cmd_type = ThrottleCmd.CMD_PERCENT
tcmd.pedal_cmd = throttle
self.throttle_pub.publish(tcmd)
scmd = SteeringCmd()
scmd.enable = True
scmd.steering_wheel_angle_cmd = steer
self.steer_pub.publish(scmd)
bcmd = BrakeCmd()
bcmd.enable = True
bcmd.pedal_cmd_type = BrakeCmd.CMD_TORQUE
bcmd.pedal_cmd = brake
self.brake_pub.publish(bcmd)
class DBWNode(object):
def __init__(self):
rospy.init_node('dbw_node')
self.vehicle_mass = rospy.get_param('~vehicle_mass', 1736.35)
self.fuel_capacity = rospy.get_param('~fuel_capacity', 13.5)
self.brake_deadband = rospy.get_param('~brake_deadband', .1)
self.decel_limit = rospy.get_param('~decel_limit', -5)
self.accel_limit = rospy.get_param('~accel_limit', 1.)
self.wheel_radius = rospy.get_param('~wheel_radius', 0.2413)
self.wheel_base = rospy.get_param('~wheel_base', 2.8498)
self.steer_ratio = rospy.get_param('~steer_ratio', 14.8)
self.max_lat_accel = rospy.get_param('~max_lat_accel', 3.)
self.max_steer_angle = rospy.get_param('~max_steer_angle', 8.)
# steering
self.steer_pub = rospy.Publisher('/vehicle/steering_cmd',
SteeringCmd,
queue_size=1)
# throttle
self.throttle_pub = rospy.Publisher('/vehicle/throttle_cmd',
ThrottleCmd,
queue_size=1)
# brake
self.brake_pub = rospy.Publisher('/vehicle/brake_cmd',
BrakeCmd,
queue_size=1)
self.controller = Controller(self.vehicle_mass, self.fuel_capacity,
self.brake_deadband, self.decel_limit,
self.accel_limit, self.wheel_radius,
self.wheel_base, self.steer_ratio,
self.max_lat_accel, self.max_steer_angle)
rospy.Subscriber('/final_waypoints', Lane, self.waypoints_cb)
self.cte = 0.
rospy.Subscriber('/cte', CTE, self.cte_cb)
self.velocity = 0.
rospy.Subscriber('/current_velocity', TwistStamped,
self.vehicle_velocity_cb)
# steering
rospy.Subscriber('/actual/steering_cmd', SteeringCmd,
self.actual_steer_cb)
# throttle
rospy.Subscriber('/actual/throttle_cmd', ThrottleCmd,
self.actual_throttle_cb)
# brake
rospy.Subscriber('/actual/brake_cmd', BrakeCmd, self.actual_brake_cb)
# Drive-by-Wire enabled notification
self.dbw_enabled = False
rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_enabled_cb)
self.target_speed = 0.
self.loop()
def waypoints_cb(self, msg):
if len(msg.waypoints):
self.target_speed = msg.waypoints[0].twist.twist.linear.x
def vehicle_velocity_cb(self, msg):
self.velocity = msg.twist.linear.x
def cte_cb(self, msg):
self.cte = msg.cte
def loop(self):
"""
Main loop that periodically controls the vehicle
"""
rate = rospy.Rate(50) # 50Hz
while not rospy.is_shutdown():
if self.dbw_enabled:
throttle, brake, steering = self.controller.control(\
rospy.get_time(), self.target_speed, self.velocity, self.cte)
rospy.logdebug("tgt v {:.2f} velocity {:.2f} cte {:.2f} thr {:.2f} brk {:.2f} str {:.2f}".format(\
self.target_speed, \
self.velocity, self.cte, throttle, brake, steering))
self.publish(throttle, brake, -steering)
rate.sleep()
def publish(self, throttle, brake, steer):
"""
Publishes throttle, brake and steering values to car
:param throttle: throttle value
:param brake: brake value
:param steer: steering value
"""
# publish throttle value to /vehicle/throttle_cmd
tcmd = ThrottleCmd()
tcmd.enable = True
tcmd.pedal_cmd_type = ThrottleCmd.CMD_PERCENT
tcmd.pedal_cmd = throttle
self.throttle_pub.publish(tcmd)
# publish steering value to /vehicle/steering_cmd
scmd = SteeringCmd()
scmd.enable = True
scmd.steering_wheel_angle_cmd = steer
self.steer_pub.publish(scmd)
# publish brake value to /vehicle/brake_cmd
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):
"""
Callback for /vehicle/dbw_enabled topic subscriber
:param msg: message from dbw_enabled topic
"""
self.dbw_enabled = msg.data
if self.dbw_enabled:
self.controller.reset(rospy.get_time(), self.cte)
def actual_steer_cb(self, msg):
"""
Callback for /actual/steering topic subscriber
:param msg: message from dbw_enabled topic
"""
if self.dbw_enabled and self.steer is not None:
self.steer_data.append({
'actual': msg.steering_wheel_angle_cmd,
'proposed': self.steer
})
self.steer = None
def actual_throttle_cb(self, msg):
"""
Callback for /actual/ topic subscriber
:param msg: message from dbw_enabled topic
"""
if self.dbw_enabled and self.throttle is not None:
self.throttle_data.append({
'actual': msg.pedal_cmd,
'proposed': self.throttle
})
self.throttle = None
def actual_brake_cb(self, msg):
"""
Callback for /actual/ topic subscriber
:param msg: message from dbw_enabled topic
"""
if self.dbw_enabled and self.brake is not None:
self.brake_data.append({
'actual': msg.pedal_cmd,
'proposed': self.brake
})
self.brake = None
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):
"""
DBWNode is in charge of publishing all control values for the car.
Using information about current state of the car ( velocity and position )
plus the desired trajectory ( velocity and path to follow ) it publishes
the control values for the car: throttle, brake, steer.
"""
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_pct = rospy.get_param('~max_throttle_percentage', 1.)
rospy.logwarn("dbw_node: max throttle pct: %f", max_throttle_pct)
max_braking_pct = rospy.get_param('~max_braking_percentage', -1.)
rospy.logwarn("dbw_node: max braking pct: %f", max_braking_pct)
self.lock = threading.Lock()
self.dbw_enabled = False
self.last_throttle = 2 * THROTTLE_EPSILON
self.last_brake = 2 * BRAKE_EPSILON
self.last_steer = 2 * STEERING_EPSILON
self.activated = False
self.current_velocity = None
self.proposed_velocities = None
self.current_pose = None
self.waypoints = None
self.steer_pub = rospy.Publisher('/vehicle/steering_cmd',
SteeringCmd,
queue_size=1)
self.throttle_pub = rospy.Publisher('/vehicle/throttle_cmd',
ThrottleCmd,
queue_size=1)
self.brake_pub = rospy.Publisher('/vehicle/brake_cmd',
BrakeCmd,
queue_size=1)
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,
max_throttle_pct, max_braking_pct)
rospy.Subscriber('/current_velocity',
TwistStamped,
self.current_velocity_cb,
queue_size=SUBSCRIBER_QUEUE_SIZE)
rospy.Subscriber('/twist_cmd',
TwistStamped,
self.twist_cmd_cb,
queue_size=SUBSCRIBER_QUEUE_SIZE)
rospy.Subscriber('/vehicle/dbw_enabled',
Bool,
self.dbw_enabled_cb,
queue_size=SUBSCRIBER_QUEUE_SIZE)
rospy.Subscriber('/current_pose',
PoseStamped,
self.current_pose_cb,
queue_size=SUBSCRIBER_QUEUE_SIZE)
rospy.Subscriber('/final_waypoints',
Lane,
self.waypoints_cb,
queue_size=SUBSCRIBER_QUEUE_SIZE)
self.loop()
def loop(self):
"""Loop that computes throttle, brake and steer to publish."""
rate = rospy.Rate(50)
while not rospy.is_shutdown():
if self._valid_state():
with self.lock:
is_activated = self.activated
cte = compute_cte(self.waypoints, self.current_pose)
throttle, brake, steer = self.controller.control(
is_activated, cte, self.proposed_velocities.twist.linear.x,
self.proposed_velocities.twist.angular.z,
self.current_velocity.twist.linear.x)
if is_activated:
rospy.logdebug("%f, %f, %f", throttle, brake, steer)
self.publish(throttle, brake, steer)
rate.sleep()
def publish(self, throttle, brake, steer):
"""Publish throttle, brake and steer."""
if abs(throttle - self.last_throttle) < THROTTLE_EPSILON:
tcmd = ThrottleCmd()
tcmd.enable = True
tcmd.pedal_cmd_type = ThrottleCmd.CMD_PERCENT
tcmd.pedal_cmd = throttle
self.throttle_pub.publish(tcmd)
else:
rospy.logdebug("not publish throttle: %f",
abs(throttle - self.last_throttle))
if abs(steer - self.last_steer) < STEERING_EPSILON:
scmd = SteeringCmd()
scmd.enable = True
scmd.steering_wheel_angle_cmd = steer
self.steer_pub.publish(scmd)
else:
rospy.logdebug("not publish steer: %f",
abs(steer - self.last_steer))
if abs(brake - self.last_brake) < BRAKE_EPSILON:
bcmd = BrakeCmd()
bcmd.enable = True
bcmd.pedal_cmd_type = BrakeCmd.CMD_TORQUE
bcmd.pedal_cmd = brake
self.brake_pub.publish(bcmd)
else:
rospy.logdebug("not publish brake: %f",
abs(brake - self.last_brake))
self.last_throttle = throttle
self.last_brake = brake
self.last_steer = steer
def current_velocity_cb(self, msg):
self.current_velocity = msg
def twist_cmd_cb(self, msg):
self.proposed_velocities = msg
def dbw_enabled_cb(self, msg):
if (self.activated != msg.data):
rospy.logwarn("%s has been %s", rospy.get_name(),
"activated" if msg.data else "deactivated")
self.activated = msg.data
def current_pose_cb(self, msg):
with self.lock:
self.current_pose = msg.pose
def waypoints_cb(self, msg):
with self.lock:
self.waypoints = msg.waypoints
def _valid_state(self):
""" Checks whether node has all information needed to operate correctly.
This node needs: Proposed and current velocity, current position and waypoints to follow.
"""
return self.proposed_velocities is not None and self.current_velocity is not None and \
self.current_pose is not None and self.waypoints is not None
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')
# Control scheme
# Angle control is done by pure pursuit through Autoware
# Linear control will be done through linear PID in twister_controller
self.controller = Controller(
vehicle_mass=rospy.get_param('~vehicle_mass', 1736.35),
fuel_capacity=rospy.get_param('~fuel_capacity', 13.5),
decel_limit=rospy.get_param('~decel_limit', -5.0),
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.),
max_steer_angle=rospy.get_param('~max_steer_angle', 8.),
min_speed=rospy.get_param('~min_speed', 0.0),
linear_p_term=rospy.get_param('~linear_p_term', 0.3),
linear_i_term=rospy.get_param('~linear_i_term', 0),
linear_d_term=rospy.get_param('~linear_d_term', 0))
# Current command values
self.steering = 0.0
self.throttle = 0.0
# Target velocities
self.angular_velocity = 0.0
self.linear_velocity = 0.0
# DBW activation state
self.dbw_enabled = 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)
self.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.dbw_twist_cb)
]
def dbw_enabled_cb(self, msg):
self.dbw_enabled = msg.data
def current_velocity_cb(self, msg):
if not self.dbw_enabled:
self.controller.reset()
return
command = self.controller.control(
angular_velocity_setpoint=self.angular_velocity,
linear_velocity_setpoint=self.linear_velocity,
current_linear_velocity=msg.twist.linear.x)
self.publish(*command)
def dbw_twist_cb(self, msg):
self.angular_velocity = msg.twist.angular.z
self.linear_velocity = msg.twist.linear.x
def spin(self):
if rospy.get_param('~use_dbw', True):
rospy.spin()
def publish(self, throttle, brake, steering):
if throttle > 0.0:
self.publish_throttle(throttle)
else:
self.publish_brake(brake)
self.publish_steering(steering)
def publish_brake(self, brake):
if abs(self.throttle + brake) < 1.0:
return
self.throttle = -brake
bcmd = BrakeCmd()
bcmd.enable = True
bcmd.pedal_cmd_type = BrakeCmd.CMD_TORQUE
bcmd.pedal_cmd = brake
self.brake_pub.publish(bcmd)
def publish_steering(self, steering):
if abs(self.steering - steering) < 0.01:
return
self.steering = steering
scmd = SteeringCmd()
scmd.enable = True
scmd.steering_wheel_angle_cmd = steering
self.steer_pub.publish(scmd)
def publish_throttle(self, throttle):
if abs(self.throttle - throttle) < 0.01:
return
self.throttle = throttle
tcmd = ThrottleCmd()
tcmd.enable = True
tcmd.pedal_cmd_type = ThrottleCmd.CMD_PERCENT
tcmd.pedal_cmd = throttle
self.throttle_pub.publish(tcmd)
class DBWNode(object):
def __init__(self):
rospy.init_node('dbw_node')
# start with manual drive
self._is_dbw_enabled = None
self._current_vel_lin = None
self._current_vel_ang = None
self._des_lin_vel = None
self._des_ang_vel = None
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.contoller = Controller(vehicle_mass, fuel_capacity, wheel_radius,
decel_limit, accel_limit, max_steer_angle,
wheel_base, steer_ratio, max_lat_accel)
# Subscribe to all the topics you need to
rospy.Subscriber('/twist_cmd', TwistStamped, self._get_twist_cmd)
rospy.Subscriber('/vehicle/dbw_enabled', Bool, self._set_bdw)
rospy.Subscriber('/current_velocity', TwistStamped,
self._get_current_vel)
self.loop()
def _set_bdw(self, is_dbw_enabled):
self._is_dbw_enabled = is_dbw_enabled
def _get_current_vel(self, curr_vel_twisted_stamped):
self._current_vel_lin = curr_vel_twisted_stamped.twist.linear.x
self._current_vel_ang = curr_vel_twisted_stamped.twist.angular.z
def _get_twist_cmd(self, desired_speeds):
self._des_lin_vel = desired_speeds.twist.linear.x
self._des_ang_vel = desired_speeds.twist.angular.z
def loop(self):
rate = rospy.Rate(50) # 50Hz
while not rospy.is_shutdown():
# Get predicted throttle, brake, and steering using `twist_controller`
# You should only publish the control commands if dbw is enabled
# todo remove
rospy.loginfo("c_vel: %s, is_dbw: %s, des_lv: %s, des_av: %s",
self._current_vel_lin, self._is_dbw_enabled,
self._des_lin_vel, self._des_ang_vel)
if self._current_vel_lin is not None and \
self._des_lin_vel is not None and \
self._des_ang_vel is not None and \
(self._is_dbw_enabled is not None):
throttle, brake, steering = self.contoller.control(
self._des_lin_vel, # proposedlinearvelocity
self._des_ang_vel, # proposed angular velocity
self._current_vel_lin # current linear velocity
)
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)
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.)
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.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(**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()
# 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
if (self.current_velocity is not None) and (self.proposed_velocity is not None) and (self.final_waypoints is not None):
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
current_linear_velocity = self.current_velocity.twist.linear.x
target_linear_velocity = self.proposed_velocity.twist.linear.x
target_angular_velocity = self.proposed_velocity.twist.angular.z
throttle, brake, steering = self.controller.control(target_linear_velocity,
target_angular_velocity,
current_linear_velocity, duration_in_seconds)
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:
# rospy.logwarn('reset controller (DBW {}, current veolocity {}, proposed velocity {})'.format(
# self.is_dbw_enabled, self.current_velocity.twist.linear.x, self.proposed_velocity.twist.linear.x))
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, message):
"""
Message format:
std_msgs/Header header
uint32 seq
time stamp
string frame_id
geometry_msgs/Twist twist
geometry_msgs/Vector3 linear
float64 x
float64 y
float64 z
geometry_msgs/Vector3 angular
float64 x
float64 y
float64 z
"""
self.proposed_velocity = message
def current_velocity_callback(self, message):
"""
Message format:
std_msgs/Header header
uint32 seq
time stamp
string frame_id
geometry_msgs/Twist twist
geometry_msgs/Vector3 linear
float64 x
float64 y
float64 z
geometry_msgs/Vector3 angular
float64 x
float64 y
float64 z
"""
self.current_velocity = message
def dbw_enabled_callback(self, message):
"""
message: bool
"""
rospy.logwarn("DBW_ENABLED %s" % message)
self.is_dbw_enabled = message.data
def final_waypoints_cb(self, message):
self.final_waypoints = message.waypoints
def current_pose_cb(self, message):
self.current_pose = message
class DBWNode(object):
def __init__(self):
rospy.init_node('dbw_node')
self.vehicle_mass = rospy.get_param('~vehicle_mass', 1736.35)
self.fuel_capacity = rospy.get_param('~fuel_capacity', 13.5)
self.brake_deadband = rospy.get_param('~brake_deadband', .1)
self.decel_limit = rospy.get_param('~decel_limit', -5)
self.accel_limit = rospy.get_param('~accel_limit', 1.)
self.wheel_radius = rospy.get_param('~wheel_radius', 0.2413)
self.wheel_base = rospy.get_param('~wheel_base', 2.8498)
self.steer_ratio = rospy.get_param('~steer_ratio', 14.8)
self.max_lat_accel = rospy.get_param('~max_lat_accel', 3.)
self.max_steer_angle = rospy.get_param('~max_steer_angle', 8.)
self.min_speed = rospy.get_param('~min_speed', 4.*0.44704)
self.max_throttle_percentage = rospy.get_param('~max_throttle_percentage', 0.1)
self.max_braking_percentage = rospy.get_param('~max_braking_percentage', -0.1)
self.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.shutdown = False
self.dbw_enabled = False
self.current_velocity = 0.
self.current_linear_velocity = 0.
self.current_angular_velocity = 0.
self.linear_velocity = 0.
self.angular_velocity = 0.
# TODO: Create `TwistController` object
self.controller = Controller(
self.wheel_base, self.steer_ratio, self.min_speed,
self.accel_limit, self.max_steer_angle,
self.vehicle_mass, self.wheel_radius, self.brake_deadband,
self.max_throttle_percentage, self.max_braking_percentage)
# TODO: Subscribe to all the topics you need to
rospy.Subscriber('/current_velocity', TwistStamped, self.velocity_cb)
rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_enabled_cb)
rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_cb)
# go into event loop
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(self.linear_velocity,
self.angular_velocity,
self.current_linear_velocity,
self.dbw_enabled)
if self.dbw_enabled:
self.publish(throttle, brake, steering)
rate.sleep()
def dbw_enabled_cb(self, msg):
self.dbw_enabled = msg
def velocity_cb(self, msg):
self.current_linear_velocity = msg.twist.linear.x
self.current_angular_velocity = msg.twist.angular.z
print "velocity: ", msg
print ""
def twist_cb(self, msg):
self.twist_cmd = msg
self.linear_velocity = self.twist_cmd.twist.linear.x
self.angular_velocity = self.twist_cmd.twist.angular.z
print "twist_cmd:", msg
print ""
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_PERCENT
bcmd.pedal_cmd = brake
self.brake_pub.publish(bcmd)
class DBWNode(object):
def __init__(self):
rospy.init_node('dbw_node')
self.dbw_enabled = False
self.linear_velocity = 0
self.angular_velocity = 0
self.current_velocity = 0
self.drive_by_wire_first_input_time = 0
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.max_braking_percent = rospy.get_param('~max_braking_percentage',
-0.1)
self.steer_pub = rospy.Publisher('/vehicle/steering_cmd',
SteeringCmd,
queue_size=1)
self.throttle_pub = rospy.Publisher('/vehicle/throttle_cmd',
ThrottleCmd,
queue_size=1)
self.brake_pub = rospy.Publisher('/vehicle/brake_cmd',
BrakeCmd,
queue_size=1)
self.dbw_enable_sub = rospy.Subscriber('/vehicle/dbw_enabled', Bool,
self.dbw_enabled_callback)
self.current_velocity_sub = rospy.Subscriber(
'/current_velocity', TwistStamped, self.current_velocity_callback)
self.twist_cmd_sub = rospy.Subscriber('/twist_cmd', TwistStamped,
self.twist_cmd_callback)
# TODO: Create `TwistController` object
self.controller = Controller(wheel_base, steer_ratio, 0, max_lat_accel,
max_steer_angle, accel_limit, decel_limit,
vehicle_mass)
# TODO: Subscribe to all the topics you need to
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>:
if self.drive_by_wire_first_input_time == 0:
self.drive_by_wire_first_input_time = time.time() * 1000
if self.dbw_enabled:
throttle, brake, steering = self.controller.control(
self.linear_velocity, self.angular_velocity,
self.current_velocity)
#giving sometime for tf and warm up first
if time.time(
) * 1000 - self.drive_by_wire_first_input_time < 3000:
throttle = 0.04
self.publish(throttle, brake, steering)
else:
self.controller.reset()
rate.sleep()
def publish(self, throttle, brake, steer):
tcmd = ThrottleCmd()
tcmd.enable = True
tcmd.pedal_cmd_type = ThrottleCmd.CMD_PERCENT
tcmd.pedal_cmd = throttle
self.throttle_pub.publish(tcmd)
scmd = SteeringCmd()
scmd.enable = True
scmd.steering_wheel_angle_cmd = steer
self.steer_pub.publish(scmd)
bcmd = BrakeCmd()
bcmd.enable = True
bcmd.pedal_cmd_type = BrakeCmd.CMD_TORQUE
bcmd.pedal_cmd = brake
self.brake_pub.publish(bcmd)
def dbw_enabled_callback(self, data):
self.dbw_enabled = data.data
def current_velocity_callback(self, data):
self.current_velocity = data.twist.linear.x
def twist_cmd_callback(self, data):
self.linear_velocity = data.twist.linear.x
self.angular_velocity = data.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', 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')
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)
'''config_string = rospy.get_param("/traffic_light_config")
self.config = yaml.load(config_string)
if self.config['type'] =='sim':
throttle_limit = 0.7
tor_limit = 700
else:
throttle_limit = 0.025
tor_limit = 0.25'''
throttle_limit = 0.7
tor_limit = 700
self.controller = Controller(vehicle_mass, fuel_capacity, wheel_base, wheel_radius ,
steer_ratio,0, max_lat_accel, max_steer_angle,
decel_limit, accel_limit, throttle_limit , tor_limit)
# TODO: Subscribe to all the topics you need to
rospy.Subscriber('/twist_cmd' , TwistStamped , self.twist_cb)
rospy.Subscriber('/vehicle/dbw_enabled', Bool , self.dbw_enabled_cb)
rospy.Subscriber('/current_velocity' , TwistStamped, self.vel_cb)
self.dbw_enabled = None
self.proposed_vel =0.0
self.proposed_angular_vel = 0.0
self.current_vel = 0.0
self.throttle = self.steering = self.brake =0
self.loop()
def loop(self):
rate = rospy.Rate(50) # 50Hz
while not rospy.is_shutdown():
# You should only publish the control commands if dbw is enabled
# TODO: Get predicted throttle, brake, and steering using `twist_controller`
self.throttle, self.brake, self.steering = self.controller.control(self.current_vel,
self.dbw_enabled,
self.proposed_vel,
self.proposed_angular_vel)
if self.dbw_enabled:
self.publish(self.throttle, self.brake, self.steering)
rate.sleep()
def publish(self, throttle, brake, steer):
rospy.loginfo("Publishing Throttle : {0} , Brake : {1} , Steering : {2}".format(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.proposed_vel = msg.twist.linear.x
self.proposed_angular_vel = msg.twist.angular.z
def dbw_enabled_cb(self, msg):
self.dbw_enabled = msg
def vel_cb(self , msg):
self.current_vel = msg.twist.linear.x
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, decel_limit, accel_limit, wheel_radius,
wheel_base, steer_ratio, max_lat_accel, max_steer_angle)
# Subscribe to all the needed topics
rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_enabled_callback)
rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_cmd_callback)
rospy.Subscriber('/current_velocity', TwistStamped, self.current_velocity_callback)
self.dbw_enabled = None
self.cmd_linear_velocity = None
self.cmd_angular_velocity = None
self.current_velocity = None
self.current_angular_velocity = None
# For handling cmd being too old
self.cmd_time = None
self.wheel_radius = wheel_radius
self.vehicle_mass = vehicle_mass
self.decel_limit = decel_limit
self.loop()
def loop(self):
rate = rospy.Rate(50) # 50Hz
while not rospy.is_shutdown():
# Get predicted throttle, brake, and steering using `twist_controller`
# Only publish the control commands if dbw is enabled
# Starting value
throttle = steering = 0.0
brake = 700 # To prevent Carla from moving requires about 700 Nm of torque.
# After getting at least one cmd
if not None in (self.dbw_enabled, self.cmd_linear_velocity, self.cmd_angular_velocity):
throttle, brake, steering = self.controller.control(self.dbw_enabled,
self.cmd_linear_velocity,
self.cmd_angular_velocity,
self.current_velocity)
# # cmd is too old, something may be wrong, stop the car
# if (not self.cmd_time == None) and rospy.get_time() - self.cmd_time > 1:
# throttle = steering = 0.0
# # torque = radius * force = radius * mass * acceleration
# brake = self.wheel_radius * self.vehicle_mass * self.decel_limit
#
# # How to handle dbw_enabled being too old?
if self.dbw_enabled:
self.publish(throttle, brake, steering)
rate.sleep()
def dbw_enabled_callback(self, msg):
self.dbw_enabled = msg
def twist_cmd_callback(self, msg):
self.cmd_linear_velocity = msg.twist.linear.x
self.cmd_angular_velocity = msg.twist.angular.z
self.cmd_time = msg.header.stamp.secs
def current_velocity_callback(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.)
max_acceleration = rospy.get_param('~max_acceleration', 1.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)
# Create `TwistController` object
# self.controller = TwistController(<Arguments you wish to provide>)
kwargs = {
"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_acceleration": max_acceleration
}
#rospy.logwarn(kwargs)
self.controller = Controller(**kwargs)
# Subscribe to all the topics you need to
self.dbw_enabled = True
self.waypoints = None
self.twist = None
self.velocity = None
self.w = None
self.current_pose = None
rospy.Subscriber('/final_waypoints', Lane, self.waypoints_cb)
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)
rospy.Subscriber('/current_pose',
PoseStamped,
self.current_pose_cb,
queue_size=1)
self.loop()
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)
#rospy.logwarn(self.dbw_enabled)
# If node isnt able to receive waypoints
if self.waypoints is None:
#rate.sleep()
continue
# If node isnt able to receive twist
if self.twist is None:
#rate.sleep()
continue
if self.current_pose is None:
continue
if self.velocity is None:
continue
#num_of_waypoints = len(self.waypoints)
#rospy.logwarn(self.current_pose)
# Calculate cte (to find steer)
cte_args = {
"waypoints": self.waypoints,
"curpose": self.current_pose,
"maxpoints": 10
}
#cte = calculate_cte(**cte_args)
cte = self.twist.angular.z
#rospy.logwarn(cte)
# Find: velocity, target velocity, ...
v_target = self.twist.linear.x
v = self.velocity
w_target = self.twist.angular.z
w = self.w
# Control
controller_args = {
"cte": cte,
"dbw_enabled": self.dbw_enabled,
"v": v,
"v_target": v_target,
"w_target": w_target,
"w": w
}
throttle, brake, steer = self.controller.control(**controller_args)
# Publish control data to ROS topics
# Is Manual Driving
#rospy.logwarn(v)
#rospy.logwarn(v_target)
#rospy.logwarn(throttle)
#rospy.logwarn(brake)
if self.dbw_enabled:
#rospy.logwarn("OK")
self.publish(throttle, brake, steer)
#else:
#rospy.logwarn("Manual")
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 waypoints_cb(self, msg):
# TODO: Change it later, Simple implementation only to test the partial waypoint node.
#first_w = msg.waypoints[0]
#rospy.loginfo('Received waypoints of size {}'.format(len(msg.waypoints)))
#self.publish(1, 0, 0)
self.waypoints = msg.waypoints
def dbw_enabled_cb(self, msg):
self.dbw_enabled = bool(msg.data)
def twist_cb(self, msg):
self.twist = msg.twist
def velocity_cb(self, msg):
self.velocity = msg.twist.linear.x
self.w = msg.twist.angular.z
def current_pose_cb(self, msg):
self.current_pose = msg.pose
class DBWNode(object):
def __init__(self):
rospy.init_node('dbw_node')
# Extract all parameters from ros server
vehicle_mass = rospy.get_param('~vehicle_mass', 1736.35)
fuel_capacity = rospy.get_param('~fuel_capacity', 13.5)
brake_deadband = rospy.get_param('~brake_deadband', .1)
decel_limit = rospy.get_param('~decel_limit', -5)
accel_limit = rospy.get_param('~accel_limit', 1.)
wheel_radius = rospy.get_param('~wheel_radius', 0.2413)
wheel_base = rospy.get_param('~wheel_base', 2.8498)
steer_ratio = rospy.get_param('~steer_ratio', 14.8)
max_lat_accel = rospy.get_param('~max_lat_accel', 3.)
max_steer_angle = rospy.get_param('~max_steer_angle', 8.)
# Make a dictionary to pass the parameters on to the controller
Parameters = {
'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
}
# 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)
# Create controller object
self.controller = Controller(**Parameters)
# Class variables
self.dbw_enabled = False
self.current_velocity = None
self.twist_cmd = None
self.previous_time = rospy.get_time()
# Subscribers
rospy.Subscriber('/current_velocity',
TwistStamped,
self.current_velocity_cb,
queue_size=1)
rospy.Subscriber('/vehicle/dbw_enabled',
Bool,
self.dbw_enabled_cb,
queue_size=1)
rospy.Subscriber('/twist_cmd',
TwistStamped,
self.twist_cmd_cb,
queue_size=5)
self.loop()
def loop(self):
"""loop"""
rate = rospy.Rate(50) # 50Hz
while not rospy.is_shutdown():
current_time = rospy.get_time()
time_interval = current_time - self.previous_time
if self.twist_cmd is not None and self.current_velocity is not None:
throttle, brake, steering = self.controller.control(
self.twist_cmd, self.current_velocity, time_interval)
if self.dbw_enabled:
self.publish(throttle, brake, steering)
else:
self.controller.reset()
self.previous_time = current_time
rate.sleep()
def current_velocity_cb(self, msg):
"""current_velocity_cb
Callback function for current_velocity
:param msg:
[geometry_msgs/TwistStamped]:
std_msgs/Header header
uint32 seq
time stamp
string frame_id
geometry_msgs/Twist twist
geometry_msgs/Vector3 linear
float64 x
float64 y
float64 z
geometry_msgs/Vector3 angular
float64 x
float64 y
float64 z
"""
self.current_velocity = msg
def dbw_enabled_cb(self, msg):
"""dbw_enabled_cb
Callback function for dbw_enabled
:param msg:
[std_msgs/Bool]:
bool data
"""
self.dbw_enabled = msg
def twist_cmd_cb(self, msg):
"""twist_cmd_cb
Callback function for twist_cmd
:param msg:
[geometry_msgs/TwistStamped]:
std_msgs/Header header
uint32 seq
time stamp
string frame_id
geometry_msgs/Twist twist
geometry_msgs/Vector3 linear
float64 x
float64 y
float64 z
geometry_msgs/Vector3 angular
float64 x
float64 y
float64 z
"""
self.twist_cmd = msg
def publish(self, throttle, brake, steer):
"""publish
Publish the control values to the cmd nodes
:param throttle:
:param brake:
:param 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.current_vel = None
self.current_ang_vel = None
self.dbw_enabled = None
self.linear_vel = None
self.angular_vel = None
self.throttle = self.brake = self.steering = 0
# Creating the `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)
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)
# Subscribing to the needed topics
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.loop()
def loop(self):
rate = rospy.Rate(FREQUENCY)
while not rospy.is_shutdown():
# Getting the predicted throttle, brake, and steering using `twist_controller`
if not None in (self.current_vel, self.linear_vel,
self.angular_vel):
self.throttle, self.brake, self.steering = self.controller.control(
self.dbw_enabled, self.current_vel, self.linear_vel,
self.angular_vel)
# Only publishing the control commands if dbw is enabled
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.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('/current_velocity', TwistStamped,
self.get_velocity_cb)
rospy.Subscriber('/twist_cmd', TwistStamped, self.get_twist_cb)
rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.get_dbw_enable_cb)
self.velocity, self.twist, self.dbw_enable = None, None, None
self.loop()
def get_velocity_cb(self, v):
self.velocity = v
def get_twist_cb(self, twist):
self.twist = twist
def get_dbw_enable_cb(self, dbw_enable):
self.dbw_enable = dbw_enable
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) <any other argument you need>)
#if not initilized, have to wait
if self.twist == None or self.velocity == None or self.dbw_enable == None:
rate.sleep()
continue
# Need restart PID controller when switch to manual
if self.dbw_enable.data == False:
self.controller.resetpid()
else:
# Use rate = 50 to calculate accleration.
throttle, brake, steer = self.controller.control(
target_linear=self.twist.twist.linear,
target_angular=self.twist.twist.angular,
current_velocity=self.velocity,
dbw_enable=self.dbw_enable.data,
rate=50)
if self.dbw_enable.data == True:
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)
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.current_vel = 0.0
self.target_angular_vel = 0.0
self.target_linear_vel = 0.0
# TODO: Create `TwistController` object
min_speed = 0.0
self.controller = Controller(wheel_base, steer_ratio, min_speed,
max_lat_accel, max_steer_angle)
# TODO: Subscribe to all the topics you need to
rospy.Subscriber('/current_velocity', TwistStamped, self.velocity_cb)
rospy.Subscriber('/twist_cmd', TwistStamped, self.angular_vel_cb)
rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_enabled_cb)
rospy.Subscriber('/final_waypoints', Lane, self.final_waypoints_cb)
self.dbw_enabled = False
self.loop()
rospy.spin()
def velocity_cb(self, msg):
self.current_vel = msg.twist.linear.x # m/s
# print " [-] current velocity in m/s = %.3f" %(self.current_vel)
def angular_vel_cb(self, msg):
self.target_angular_vel = msg.twist.angular.z # rad/s
def dbw_enabled_cb(self, msg):
self.dbw_enabled = msg
def final_waypoints_cb(self, msg):
self.target_linear_vel = msg.waypoints[0].twist.twist.linear.x
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)
# get current linear speed
if self.target_angular_vel != None and self.current_vel != None:
is_data = True
else:
is_data = False
if is_data:
throttle, brake, steering = self.controller.control(
self.current_vel, self.target_linear_vel,
self.target_angular_vel, self.dbw_enabled)
if self.dbw_enabled:
# print('Throttle: ' + str(throttle))
# print('Brake: ' + str(brake))
# print " [-] target steering wheel angle = %.2f (deg)" % (steering * 3.141592 / 180)
self.publish(throttle, brake, steering)
rate.sleep()
def publish(self, throttle, brake, steer):
tcmd = ThrottleCmd()
tcmd.enable = True
tcmd.pedal_cmd_type = ThrottleCmd.CMD_PERCENT
tcmd.pedal_cmd = throttle
self.throttle_pub.publish(tcmd)
scmd = SteeringCmd()
scmd.enable = True
scmd.steering_wheel_angle_cmd = steer
self.steer_pub.publish(scmd)
bcmd = BrakeCmd()
bcmd.enable = True
bcmd.pedal_cmd_type = BrakeCmd.CMD_TORQUE
bcmd.pedal_cmd = brake
self.brake_pub.publish(bcmd)
class DBWNode(object):
def __init__(self):
rospy.init_node('dbw_node')
vehicle_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)
self.dbw_enabled = False
self.velocity = None
self.cmd = None
rospy.Subscriber('/twist_cmd', TwistStamped, self.cmd_cb)
rospy.Subscriber('/current_velocity', TwistStamped, self.velocity_cb)
rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_cb)
self.loop()
def loop(self):
rate = rospy.Rate(50) # 50Hz
while not rospy.is_shutdown():
if self.velocity and self.cmd:
throttle, brake, steering = self.controller.control(
self.dbw_enabled, self.velocity.linear.x,
self.cmd.linear.x, self.cmd.angular.z)
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 cmd_cb(self, msg):
self.cmd = msg.twist
def velocity_cb(self, msg):
self.velocity = msg.twist
def dbw_cb(self, msg):
self.dbw_enabled = msg.data
class DBWNode(object):
""" generate drive-by-wire(DBW) command for autonomous driving
@subscribed /vehicle/dbw_enabled: the indicator for whether the car is under dbw or driver control
@subscribed /current_velocity: the vehicle's target linear velocities
@subscribed /twist_cmd: the vehicle's target angular velocities
@published /vehicle/brake_cmd: the final brake for electronic control
@published /vehicle/throttle_cmd: the final throttle for electronic control
@published /vehicle/steering_cmd: the final steering for electronic control
"""
DBW_UPDATE_FREQ = 50 # Waypoint update frequency
def __init__(self):
rospy.init_node('dbw_node')
# load ego 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.)
# state variables:
self.is_dbw_enabled = None
self.actual_longitudinal_velocity = None
self.target_longitudinal_velocity = None
self.target_angular_velocity = None
# 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:
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)
# publish:
self.steer_pub = rospy.Publisher('/vehicle/steering_cmd',
SteeringCmd,
queue_size=1)
self.throttle_pub = rospy.Publisher('/vehicle/throttle_cmd',
ThrottleCmd,
queue_size=1)
self.brake_pub = rospy.Publisher('/vehicle/brake_cmd',
BrakeCmd,
queue_size=1)
self.loop()
def dbw_enabled_cb(self, is_dbw_enabled):
""" update DBW activation status
"""
self.is_dbw_enabled = is_dbw_enabled
def current_velocity_cb(self, current_velocity):
""" update waypoint velocities
"""
self.actual_longitudinal_velocity = current_velocity.twist.linear.x
def twist_cmd_cb(self, target_velocity):
""" update twist command
"""
self.target_longitudinal_velocity = target_velocity.twist.linear.x
self.target_angular_velocity = target_velocity.twist.angular.z
def loop(self):
"""
The DBW system on Carla expects messages at 50Hz
It will disengage (reverting control back to the driver) if control messages are published at less than 10hz
"""
rate = rospy.Rate(DBWNode.DBW_UPDATE_FREQ) # at least 50Hz
while not rospy.is_shutdown():
# Get predicted throttle, brake, and steering using `twist_controller`
throttle, brake, steer = self.controller.control(
is_dbw_enabled=self.is_dbw_enabled,
actual_longitudinal_velocity=self.actual_longitudinal_velocity,
target_longitudinal_velocity=self.target_longitudinal_velocity,
target_angular_velocity=self.target_angular_velocity)
# Only publish the control commands if dbw is enabled
if self.is_dbw_enabled:
self.publish(throttle, brake, steer)
rate.sleep()
def publish(self, throttle, brake, steer):
""" publish drive-by-wire(DBW) control command for autonomous driving
"""
# throttle:
tcmd = ThrottleCmd()
tcmd.enable = True
tcmd.pedal_cmd_type = ThrottleCmd.CMD_PERCENT
tcmd.pedal_cmd = throttle
self.throttle_pub.publish(tcmd)
# steering:
scmd = SteeringCmd()
scmd.enable = True
scmd.steering_wheel_angle_cmd = steer
self.steer_pub.publish(scmd)
# brake:
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)
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)
fuel_capacity = rospy.get_param('~fuel_capacity', 13.5)
brake_deadband = rospy.get_param('~brake_deadband', .1)
decel_limit = rospy.get_param('~decel_limit', -5)
accel_limit = rospy.get_param('~accel_limit', 1)
wheel_radius = rospy.get_param('~wheel_radius', 0.2413)
wheel_base = rospy.get_param('~wheel_base', 2.8498)
steer_ratio = rospy.get_param('~steer_ratio', 14.8)
max_lat_accel = rospy.get_param('~max_lat_accel', 3.)
max_steer_angle = rospy.get_param('~max_steer_angle', 8.)
min_speed = 0.1
self.process_rate = 5
self.send_rate = 50 # must be a multiple of the processing rate, change to 10 for the simulator
self.process_itr = 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(<Arguments you wish to provide>)
self.dbw_controller = Controller(vehicle_mass, wheel_base,
wheel_radius, steer_ratio, min_speed,
max_lat_accel, max_steer_angle,
accel_limit, decel_limit,
self.process_rate)
# TODO: Subscribe to all the topics you need to
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.linear_velocity = None
self.angular_velocity = None
self.dbw_enabled = None
self.throttle = self.steering = 0
self.brake = 700
self.loop()
def loop(self):
rate = rospy.Rate(self.send_rate) # 50Hz
while not rospy.is_shutdown():
# TODO: Get predicted throttle, brake, and steering using `twist_controller`
# You should only publish the control commands if dbw is enabled
if (self.process_itr % (self.send_rate / self.process_rate) == 0
) and not None in (self.current_velocity, self.linear_velocity,
self.angular_velocity):
self.throttle, self.brake, self.steering = self.dbw_controller.control(
self.current_velocity, self.dbw_enabled,
self.linear_velocity, self.angular_velocity)
self.process_itr = 0
if self.dbw_enabled:
self.publish(self.throttle, self.brake, self.steering)
self.process_itr = self.process_itr + 1
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_cb(self, msg):
self.linear_velocity = msg.twist.linear.x
self.angular_velocity = msg.twist.angular.z
def current_vel_cb(self, msg):
self.current_velocity = msg.twist.linear.x
def dbw_enabled_cb(self, msg):
self.dbw_enabled = msg
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.)
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, fuel_capacity,
brake_deadband, decel_limit, accel_limit,
wheel_radius, wheel_base, steer_ratio,
max_lat_accel, max_steer_angle)
# TODO: Subscribe to all the topics you need to
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.current_angular_vel = None
self.linear_vel = None
self.angular_vel = None
self.dbw_enabled = None
self.throttle = 0
self.steering = 0
self.brake = 0
self.loop()
def loop(self):
rate = rospy.Rate(50)
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.angular_vel, self.linear_vel,
self.dbw_enabled)
# Publish only if dbw is enabled or else it is manual control
if self.dbw_enabled:
self.publish(self.throttle, self.brake, self.steering)
rate.sleep()
def dbw_enabled_cb(self, msg):
self.dbw_enabled = msg.data
if not self.dbw_enabled:
rospy.logwarn('Manual Mode Enabled')
else:
rospy.logwarn('Autonomous Mode Enabled')
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)
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
