class DBWNode(object):
def __init__(self):
rospy.init_node('dbw_node')
vehicle_mass = rospy.get_param('~vehicle_mass', 1736.35)
fuel_capacity = rospy.get_param('~fuel_capacity', 13.5)
brake_deadband = rospy.get_param('~brake_deadband', .1)
decel_limit = rospy.get_param('~decel_limit', -5)
accel_limit = rospy.get_param('~accel_limit', 1.)
wheel_radius = rospy.get_param('~wheel_radius', 0.2413)
wheel_base = rospy.get_param('~wheel_base', 2.8498)
steer_ratio = rospy.get_param('~steer_ratio', 14.8)
max_lat_accel = rospy.get_param('~max_lat_accel', 3.)
max_steer_angle = rospy.get_param('~max_steer_angle', 8.)
self.steer_pub = rospy.Publisher('/vehicle/steering_cmd',
SteeringCmd,
queue_size=1)
self.throttle_pub = rospy.Publisher('/vehicle/throttle_cmd',
ThrottleCmd,
queue_size=1)
self.brake_pub = rospy.Publisher('/vehicle/brake_cmd',
BrakeCmd,
queue_size=1)
self.dbw_enabled = False
self.tl_detector_ready = False
self.target_velocity = 0.
self.target_yaw_dot = 0.
self.current_velocity = 0.
self.current_yaw_dot = 0.
self.last_update_time = None
# Initialize TwistController
self.twist_controller = TwistController(accel_limit, -1.,
max_steer_angle,
BrakeCmd.TORQUE_MAX,
wheel_base, steer_ratio,
max_lat_accel, max_steer_angle)
# Subscribe to topics
rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_cmd_callback)
rospy.Subscriber('/current_velocity', TwistStamped,
self.current_velocity_callback)
rospy.Subscriber('/vehicle/dbw_enabled', Bool,
self.dbw_enabled_callback)
rospy.Subscriber('/tl_detector_ready', Bool,
self.tl_detector_ready_callback)
self.loop()
def loop(self):
rate = rospy.Rate(50) # 50Hz
while not rospy.is_shutdown():
# hold brake if tl_detector not ready
if not self.tl_detector_ready or self.last_update_time is None:
self.last_update_time = rospy.Time.now()
self.publish(0, BrakeCmd.TORQUE_MAX, 0)
rate.sleep()
continue
current_time = rospy.Time.now()
dt = current_time.to_sec() - self.last_update_time.to_sec()
self.last_update_time = current_time
if (dt > 0.075):
rospy.logwarn('slow DBW update, dt:%.3fs freq:%.1fhz', dt,
1. / dt)
# reset controller PIDs and skip calculations if DBW is off
if not self.dbw_enabled:
self.twist_controller.reset()
rate.sleep()
continue
accel, steer = self.twist_controller.control(
self.target_velocity, self.target_yaw_dot,
self.current_velocity, dt)
throttle = 0
brake = 0
if (accel < 0):
brake = -accel
else:
throttle = accel
# record data for debugging
# self.data_recorder(self.target_velocity, self.target_yaw_dot, throttle, brake, steer)
# rospy.loginfo('DBW a:%.3f y:%.3f', self.target_velocity, self.target_yaw_dot)
# rospy.loginfo('DBW t:%.3f b:%.3f s:%.3f', throttle, brake, steer)
self.publish(throttle, brake, steer)
rate.sleep()
def twist_cmd_callback(self, msg):
self.target_velocity = msg.twist.linear.x
self.target_yaw_dot = msg.twist.angular.z
# rospy.loginfo('twist_cmd: v:%.3f yd:%.3f', self.target_velocity, self.target_yaw_dot)
# log_twist_msg(msg, 'twist_cmd:')
def current_velocity_callback(self, msg):
self.current_velocity = msg.twist.linear.x
self.current_yaw_dot = msg.twist.angular.z
# rospy.loginfo('current_velocity: v:%.3f yd:%.3f', self.current_velocity, self.current_yaw_dot)
# log_twist_msg(msg, 'current_velocity:')
def dbw_enabled_callback(self, msg):
self.dbw_enabled = msg.data
# rospy.loginfo('dbw_enabled: %d', self.dbw_enabled)
def tl_detector_ready_callback(self, msg):
self.tl_detector_ready = msg.data
#rospy.loginfo('tl_detector_ready: %s', self.tl_detector_ready)
def publish(self, throttle, brake, steer):
#rospy.logwarn("In publish: %d; %d; %d;", throttle, brake, steer)
tcmd = ThrottleCmd()
tcmd.enable = True
tcmd.pedal_cmd_type = ThrottleCmd.CMD_PERCENT
tcmd.pedal_cmd = throttle
self.throttle_pub.publish(tcmd)
scmd = SteeringCmd()
scmd.enable = True
scmd.steering_wheel_angle_cmd = steer
self.steer_pub.publish(scmd)
bcmd = BrakeCmd()
bcmd.enable = True
bcmd.pedal_cmd_type = BrakeCmd.CMD_TORQUE
bcmd.pedal_cmd = brake
self.brake_pub.publish(bcmd)
def data_recorder(self, target_velocity, target_yaw_dot, throttle, brake,
steer):
dbw_data_filename = '/tmp/dbw_data.csv'
try:
foo = self.dbw_data
except:
self.dbw_data = []
with open(dbw_data_filename, 'wb') as csvfile:
rospy.loginfo('DBW data file: %s', csvfile.name)
csv_writer = csv.writer(csvfile, delimiter=',')
csv_writer.writerow([
'time', 't_speed', 't_yd', 'throttle', 'brake', 'steer',
'c_speed', 'c_yd'
])
time = rospy.Time.now().to_sec()
self.dbw_data.append([
time, target_velocity, target_yaw_dot, throttle, brake, steer,
self.current_velocity, self.current_yaw_dot
])
if len(self.dbw_data) == 1000:
with open(dbw_data_filename, 'ab') as csvfile:
csv_writer = csv.writer(csvfile, delimiter=',')
for row in self.dbw_data:
csv_writer.writerow(row)
rospy.loginfo('DBW data saved')
self.dbw_data = []
class DBWNode(object):
def __init__(self):
rospy.init_node('dbw_node')
vehicle_mass = rospy.get_param('~vehicle_mass', 1736.35)
fuel_capacity = rospy.get_param('~fuel_capacity', 13.5)
brake_deadband = rospy.get_param('~brake_deadband', .1)
decel_limit = rospy.get_param('~decel_limit', -5)
accel_limit = rospy.get_param('~accel_limit', 1.)
wheel_radius = rospy.get_param('~wheel_radius', 0.2413)
wheel_base = rospy.get_param('~wheel_base', 2.8498)
steer_ratio = rospy.get_param('~steer_ratio', 14.8)
max_lat_accel = rospy.get_param('~max_lat_accel', 3.)
max_steer_angle = rospy.get_param('~max_steer_angle', 8.)
min_speed = 0.1
self.steer_pub = rospy.Publisher('/vehicle/steering_cmd',
SteeringCmd,
queue_size=1)
self.throttle_pub = rospy.Publisher('/vehicle/throttle_cmd',
ThrottleCmd,
queue_size=1)
self.brake_pub = rospy.Publisher('/vehicle/brake_cmd',
BrakeCmd,
queue_size=1)
# TODO: Create `TwistController` object
# self.controller = TwistController(<Arguments you wish to provide>)
self.controller = TwistController(vehicle_mass, fuel_capacity,
brake_deadband, decel_limit,
accel_limit, wheel_radius,
wheel_base, steer_ratio,
max_lat_accel, max_steer_angle,
min_speed)
self.dbw_enabled = True
self.reset_controller = True
self.current_velocity_msg = None
self.twist_cmd = None
self.current_velocity = 0.0
self.current_angualr_velocity = 0.0
self.twist_velocity = 0.0
self.twist_angualr_velocity = 0.0
self.prev_time = rospy.get_time()
# TODO: Subscribe to all the topics you need to
rospy.Subscriber('/twist_cmd',
TwistStamped,
self.twist_cmd_cb,
queue_size=5)
rospy.Subscriber('/current_velocity',
TwistStamped,
self.current_velocity_cb,
queue_size=5)
rospy.Subscriber('/vehicle/dbw_enabled',
Bool,
self.dbw_enabled_cb,
queue_size=1)
self.loop()
def twist_cmd_cb(self, msg):
self.twist_cmd = msg
self.twist_velocity = msg.twist.linear.x
self.twist_angualr_velocity = msg.twist.angular.z
def current_velocity_cb(self, msg):
self.current_velocity_msg = msg
self.current_velocity = msg.twist.linear.x
self.current_angualr_velocity = msg.twist.angular.z
def dbw_enabled_cb(self, msg):
try:
self.dbw_enabled = bool(msg.data)
except Exception:
self.dbw_enabled = msg
def loop(self):
rate = rospy.Rate(50) # 50Hz
while not rospy.is_shutdown():
# TODO: Get predicted throttle, brake, and steering using `twist_controller`
# You should only publish the control commands if dbw is enabled
# throttle, brake, steering = self.controller.control(<proposed linear velocity>,
# <proposed angular velocity>,
# <current linear velocity>,
# <dbw status>,
# <any other argument you need>)
# if <dbw is enabled>:
# self.publish(throttle, brake, steer)
# TIME
now = rospy.get_time()
dt = now - self.prev_time
self.prev_time = now
if self.dbw_enabled and self.current_velocity_msg is not None and self.twist_cmd is not None:
if self.reset_controller:
self.controller.reset()
self.reset_controller = False
throttle, brake, steering = self.controller.control(
twist_velocity=self.twist_velocity,
twist_angualr_velocity=self.twist_angualr_velocity,
current_velocity=self.current_velocity,
delta_time=dt)
self.publish(throttle, brake, steering)
else:
self.reset_controller = True
rate.sleep()
def publish(self, throttle, brake, steer):
tcmd = ThrottleCmd()
tcmd.enable = True
tcmd.pedal_cmd_type = ThrottleCmd.CMD_PERCENT
tcmd.pedal_cmd = throttle
self.throttle_pub.publish(tcmd)
scmd = SteeringCmd()
scmd.enable = True
scmd.steering_wheel_angle_cmd = steer
self.steer_pub.publish(scmd)
bcmd = BrakeCmd()
bcmd.enable = True
bcmd.pedal_cmd_type = BrakeCmd.CMD_TORQUE
bcmd.pedal_cmd = brake
self.brake_pub.publish(bcmd)
class DBWNode(object):
def __init__(self):
rospy.init_node('dbw_node')
parameters = dict()
parameters['vehicle_mass'] = rospy.get_param('~vehicle_mass', 1736.35)
parameters['fuel_capacity'] = rospy.get_param('~fuel_capacity', 13.5)
parameters['dbrake_deadband'] = rospy.get_param('~brake_deadband', .1)
parameters['decel_limit'] = rospy.get_param('~decel_limit', -5)
parameters['accel_limit'] = rospy.get_param('~accel_limit', 1.)
parameters['wheel_radius'] = rospy.get_param('~wheel_radius', 0.2413)
parameters['wheel_base'] = rospy.get_param('~wheel_base', 2.8498)
parameters['steer_ratio'] = rospy.get_param('~steer_ratio', 14.8)
parameters['max_lat_accel'] = rospy.get_param('~max_lat_accel', 3.)
parameters['max_steer_angle'] = rospy.get_param('~max_steer_angle', 8.)
parameters['min_speed'] = 0.1
self.steer_pub = rospy.Publisher('/vehicle/steering_cmd',
SteeringCmd,
queue_size=1)
self.throttle_pub = rospy.Publisher('/vehicle/throttle_cmd',
ThrottleCmd,
queue_size=1)
self.brake_pub = rospy.Publisher('/vehicle/brake_cmd',
BrakeCmd,
queue_size=1)
self.dbw_enabled = True
self.current_twist = None
self.twist_cmd = None
self.t_0 = rospy.get_time()
# TODO: Create `TwistController` object
self.controller = TwistController(parameters=parameters)
# Subscribe to all the needed topics
rospy.Subscriber('/twist_cmd',
TwistStamped,
self.twist_cmd_cb,
queue_size=5)
rospy.Subscriber('/current_velocity',
TwistStamped,
self.current_velocity_cb,
queue_size=5)
rospy.Subscriber('/vehicle/dbw_enabled',
Bool,
self.dbw_enabled_cb,
queue_size=1)
self.loop()
def dbw_enabled_cb(self, dbw_enabled):
self.dbw_enabled = dbw_enabled
print dbw_enabled
def current_velocity_cb(self, current_twist):
self.current_twist = current_twist
def twist_cmd_cb(self, twist_cmd):
self.twist_cmd = twist_cmd
def loop(self):
rate = rospy.Rate(50) # 50Hz
while not rospy.is_shutdown():
# TODO: Get predicted throttle, brake, and steering using `twist_controller`
# You should only publish the control commands if dbw is enabled
if self.dbw_enabled and self.current_twist is not None and self.twist_cmd is not None:
throttle = 1.0
brake = 0.0
steering = 0.0
t_1 = rospy.get_time()
dt = t_1 - self.t_0
self.t_0 = t_1
throttle, brake, steering = self.controller.control(
twist_cmd=self.twist_cmd,
current_twist=self.current_twist,
dt=dt)
self.publish(throttle, brake, steering)
else:
self.controller.reset()
rate.sleep()
def publish(self, throttle, brake, steer):
tcmd = ThrottleCmd()
tcmd.enable = True
tcmd.pedal_cmd_type = ThrottleCmd.CMD_PERCENT
tcmd.pedal_cmd = throttle
self.throttle_pub.publish(tcmd)
scmd = SteeringCmd()
scmd.enable = True
scmd.steering_wheel_angle_cmd = steer
self.steer_pub.publish(scmd)
bcmd = BrakeCmd()
bcmd.enable = True
bcmd.pedal_cmd_type = BrakeCmd.CMD_TORQUE
bcmd.pedal_cmd = brake
self.brake_pub.publish(bcmd)
class DBWNode(object):
def __init__(self):
rospy.init_node('dbw_node')
vehicle_mass = rospy.get_param('~vehicle_mass', 1736.35)
fuel_capacity = rospy.get_param('~fuel_capacity', 13.5)
brake_deadband = rospy.get_param('~brake_deadband', .1)
decel_limit = rospy.get_param('~decel_limit', -5)
accel_limit = rospy.get_param('~accel_limit', 1.)
wheel_radius = rospy.get_param('~wheel_radius', 0.2413)
wheel_base = rospy.get_param('~wheel_base', 2.8498)
steer_ratio = rospy.get_param('~steer_ratio', 14.8)
max_lat_accel = rospy.get_param('~max_lat_accel', 3.)
max_steer_angle = rospy.get_param('~max_steer_angle', 8.)
self.rate = 50 # Hz
self.sample_time = 1.0 / self.rate
self.dbw_enabled = True
self.initialized = False
self.current_linear_velocity = None
self.target_linear_velocity = None
self.target_angular_velocity = None
self.final_waypoints = None
self.current_pose = None
self.logger = DBWLogger(self, rate=1)
self.steer_pub = rospy.Publisher('/vehicle/steering_cmd',
SteeringCmd,
queue_size=1)
self.throttle_pub = rospy.Publisher('/vehicle/throttle_cmd',
ThrottleCmd,
queue_size=1)
self.brake_pub = rospy.Publisher('/vehicle/brake_cmd',
BrakeCmd,
queue_size=1)
yaw_controller = YawController(wheel_base=wheel_base,
steer_ratio=steer_ratio,
min_speed=0.,
max_lat_accel=max_lat_accel,
max_steer_angle=max_steer_angle)
self.controller = TwistController(yaw_controller, max_steer_angle,
self.sample_time)
rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_enabled_cb)
rospy.Subscriber('/current_velocity',
TwistStamped,
self.current_velocity_cb,
queue_size=1)
rospy.Subscriber('/twist_cmd',
TwistStamped,
self.twist_cmd_cb,
queue_size=1)
rospy.Subscriber('/final_waypoints',
Lane,
self.final_waypoints_cb,
queue_size=1)
rospy.Subscriber('/current_pose',
PoseStamped,
self.current_pose_cb,
queue_size=1)
self.loop()
def dbw_enabled_cb(self, msg):
self.dbw_enabled = msg.data
if self.dbw_enabled:
self.controller.reset()
@staticmethod
def compute_absolute_velocity(velocity_vector):
x = velocity_vector.x
y = velocity_vector.y
z = velocity_vector.z
return math.sqrt(x**2 + y**2 + z**2)
def current_velocity_cb(self, msg):
self.current_linear_velocity = self.compute_absolute_velocity(
msg.twist.linear)
def twist_cmd_cb(self, msg):
self.target_linear_velocity = self.compute_absolute_velocity(
msg.twist.linear)
self.target_angular_velocity = self.compute_absolute_velocity(
msg.twist.angular)
def final_waypoints_cb(self, msg):
self.final_waypoints = msg.waypoints
def current_pose_cb(self, msg):
self.current_pose = msg.pose
def loop(self):
rate = rospy.Rate(self.rate)
while not rospy.is_shutdown():
throttle, brake, steer = 1, None, None
if (self.final_waypoints is not None) \
& (self.current_pose is not None) \
& (self.current_linear_velocity is not None) \
& (self.target_linear_velocity is not None) \
& (self.target_angular_velocity is not None):
self.initialized = True
cte = CTE.compute_cte(self.final_waypoints, self.current_pose)
throttle, brake, steer = self.controller.control(
self.target_linear_velocity, self.target_angular_velocity,
self.current_linear_velocity, cte)
self.logger.log(throttle, brake, steer, 0, cte)
if self.initialized and self.dbw_enabled:
# sending None for break, ensures we're not throttling/breaking at the same time
self.publish(throttle, brake, steer)
rate.sleep()
def publish(self, throttle, brake, steer):
if throttle is not None:
tcmd = ThrottleCmd()
tcmd.enable = True
tcmd.pedal_cmd_type = ThrottleCmd.CMD_PERCENT
tcmd.pedal_cmd = throttle
self.throttle_pub.publish(tcmd)
if brake is not None:
bcmd = BrakeCmd()
bcmd.enable = True
bcmd.pedal_cmd_type = BrakeCmd.CMD_TORQUE
bcmd.pedal_cmd = brake
self.brake_pub.publish(bcmd)
if steer is not None:
scmd = SteeringCmd()
scmd.enable = True
scmd.steering_wheel_angle_cmd = steer
self.steer_pub.publish(scmd)
class DBWNode(object):
def __init__(self):
rospy.init_node('dbw_node')
cp = CarParams()
cp.vehicle_mass = rospy.get_param('~vehicle_mass', 1736.35)
cp.fuel_capacity = rospy.get_param('~fuel_capacity', 13.5)
cp.brake_deadband = rospy.get_param('~brake_deadband', .1)
cp.decel_limit = rospy.get_param('~decel_limit', -5)
cp.accel_limit = rospy.get_param('~accel_limit', 1.)
cp.wheel_radius = rospy.get_param('~wheel_radius', 0.2413)
cp.wheel_base = rospy.get_param('~wheel_base', 2.8498)
cp.steer_ratio = rospy.get_param('~steer_ratio', 14.8)
cp.max_lat_accel = rospy.get_param('~max_lat_accel', 3.)
cp.max_steer_angle = rospy.get_param('~max_steer_angle', 8.)
cp.min_speed = 0.1
self.steer_pub = rospy.Publisher('/vehicle/steering_cmd',
SteeringCmd,
queue_size=1)
self.throttle_pub = rospy.Publisher('/vehicle/throttle_cmd',
ThrottleCmd,
queue_size=1)
self.brake_pub = rospy.Publisher('/vehicle/brake_cmd',
BrakeCmd,
queue_size=1)
# self.current_pose = None # needed?
self.dbw_enabled = True
self.reset_flag = True
self.current_velocity = None
self.latest_twist_cmd = None
# TIME
self.previous_timestamp = rospy.get_time()
# Create `TwistController` object
self.controller = TwistController(cp=cp)
# Subscribe to all the topics you need to
rospy.Subscriber('/twist_cmd',
TwistStamped,
self.twist_cmd_cb,
queue_size=5)
rospy.Subscriber('/current_velocity',
TwistStamped,
self.current_velocity_cb,
queue_size=5)
rospy.Subscriber('/vehicle/dbw_enabled',
Bool,
self.dbw_enabled_cb,
queue_size=1)
self.loop()
def dbw_enabled_cb(self, dbw_enabled):
try:
self.dbw_enabled = bool(dbw_enabled.data)
except Exception:
self.dbw_enabled = dbw_enabled
def current_velocity_cb(self, current_velocity):
self.current_velocity = current_velocity
def twist_cmd_cb(self, twist_cmd):
self.latest_twist_cmd = twist_cmd
def loop(self):
rate = rospy.Rate(10) #Hz
while not rospy.is_shutdown():
# TIME
current_timestamp = rospy.get_time()
del_time = current_timestamp - self.previous_timestamp
self.previous_timestamp = current_timestamp
if self.dbw_enabled and self.current_velocity is not None and self.latest_twist_cmd is not None:
if self.reset_flag:
self.controller.reset()
self.reset_flag = False
throttle, brake, steering = self.controller.control(
twist_cmd=self.latest_twist_cmd,
current_velocity=self.current_velocity,
del_time=del_time)
self.publish(throttle, brake, steering)
else:
self.reset_flag = True
rate.sleep()
def publish(self, throttle, brake, steer):
tcmd = ThrottleCmd()
tcmd.enable = True
tcmd.pedal_cmd_type = ThrottleCmd.CMD_PERCENT
tcmd.pedal_cmd = throttle
self.throttle_pub.publish(tcmd)
scmd = SteeringCmd()
scmd.enable = True
scmd.steering_wheel_angle_cmd = steer
self.steer_pub.publish(scmd)
bcmd = BrakeCmd()
bcmd.enable = True
bcmd.pedal_cmd_type = BrakeCmd.CMD_TORQUE
bcmd.pedal_cmd = brake
self.brake_pub.publish(bcmd)
class DBWNode(object):
def __init__(self):
rospy.init_node('dbw_node')
# Params:
vehicle_mass = rospy.get_param('~vehicle_mass', 1736.35)
fuel_capacity = rospy.get_param('~fuel_capacity', 13.5)
brake_deadband = rospy.get_param('~brake_deadband', .1)
decel_limit = rospy.get_param('~decel_limit', -5)
accel_limit = rospy.get_param('~accel_limit', 1.)
wheel_radius = rospy.get_param('~wheel_radius', 0.2413)
wheel_base = rospy.get_param('~wheel_base', 2.8498)
steer_ratio = rospy.get_param('~steer_ratio', 14.8)
max_lat_accel = rospy.get_param('~max_lat_accel', 3.)
max_steer_angle = rospy.get_param('~max_steer_angle', 8.)
min_lon_speed = rospy.get_param('~min_lon_speed', 0.)
controller_rate = rospy.get_param('~controller_rate', 20.)
tau_acceleration = rospy.get_param('~tau_acceleration', 0.3)
self.feed_forward_gain = 1
self.controller_rate = controller_rate
# Subscriber:
self.dbw_enabled = False
self.current_linear_velocity = None
self.current_linear_acceleration = None
self.current_pose = None
self.target_linear_velocity = None
self.target_angular_velocity = None
self.final_waypoints = []
rospy.Subscriber('/twist_cmd',
TwistStamped,
self.twist_cmd_cb,
queue_size=1)
rospy.Subscriber('/current_velocity',
TwistStamped,
self.current_velocity_cb,
queue_size=1)
rospy.Subscriber('/current_pose',
PoseStamped,
self.current_pose_cb,
queue_size=1)
rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_enabled_cb)
rospy.Subscriber('/final_waypoints',
Lane,
self.final_waypoints_cb,
queue_size=1)
# Controller:
self.speed_controller = SpeedController(
controller_rate,
accel_limit,
decel_limit,
brake_deadband,
vehicle_mass,
wheel_radius,
)
self.twist_controller = TwistController(controller_rate,
max_steer_angle)
self.yaw_controller = YawController(wheel_base, steer_ratio,
min_lon_speed, max_lat_accel,
max_steer_angle)
# Filter
self.lowpass_acceleration = LowPassFilter(tau=tau_acceleration,
ts=1.0 /
self.controller_rate)
# Publisher:
self.throttle_pub = rospy.Publisher('/vehicle/throttle_cmd',
ThrottleCmd,
queue_size=1)
self.brake_pub = rospy.Publisher('/vehicle/brake_cmd',
BrakeCmd,
queue_size=1)
self.steer_pub = rospy.Publisher('/vehicle/steering_cmd',
SteeringCmd,
queue_size=1)
# Execute loop for each message (this depends on the defined rate):
self.loop()
def twist_cmd_cb(self, msg):
self.target_linear_velocity = msg.twist.linear.x
self.target_angular_velocity = msg.twist.angular.z
def current_velocity_cb(self, msg):
if self.current_linear_velocity is not None:
accel = (self.current_linear_velocity -
msg.twist.linear.x) * self.controller_rate
self.current_linear_acceleration = self.lowpass_acceleration.filt(
accel)
self.current_linear_velocity = msg.twist.linear.x
def final_waypoints_cb(self, msg):
self.final_waypoints = msg.waypoints
def current_pose_cb(self, msg):
self.current_pose = msg.pose
def dbw_enabled_cb(self, msg):
if msg.data:
self.dbw_enabled = True
else:
self.dbw_enabled = False
self.speed_controller.reset()
self.twist_controller.reset()
def loop(self):
rate = rospy.Rate(self.controller_rate)
while not rospy.is_shutdown():
rospy.loginfo(self.final_waypoints)
if self.target_linear_velocity is None \
or self.current_linear_velocity is None \
or self.current_linear_acceleration is None \
or self.current_pose is None \
or self.final_waypoints is None:
continue
if (len(self.final_waypoints) > 2):
# Calculate errors
cross_track_error = get_cross_track_error_from_frenet(
self.final_waypoints, self.current_pose)
steer_twist = self.twist_controller.control(cross_track_error)
target_linear_velocity = float(
np.sqrt(self.final_waypoints[1].twist.twist.linear.x**2 +
self.final_waypoints[1].twist.twist.linear.y**2))
steer_yaw = self.yaw_controller.get_steering(
linear_velocity=target_linear_velocity,
angular_velocity=self.target_angular_velocity,
current_velocity=self.current_linear_velocity)
throttle, brake = self.speed_controller.control(
target_linear_velocity=target_linear_velocity,
current_linear_velocity=self.current_linear_velocity,
current_linear_acceleration=self.
current_linear_acceleration)
steer = steer_twist + steer_yaw * self.feed_forward_gain
#rospy.logwarn('cte %0.2f, ang_vel %0.2f, steer(twist/yaw) %0.2f %0.2f', \
# cross_track_error, self.target_angular_velocity, steer_twist, steer_yaw)
#rospy.logwarn('Target WP Velocity %0.2f, throttle %0.2f, brake %0.2f', target_linear_velocity, throttle, brake)
else:
rospy.logwarn('[dbw_node] No more final_waypoints')
throttle = 0
brake = 10000
steer = 0
if self.dbw_enabled:
self.publish(throttle, brake, steer)
rate.sleep(
) # wiki.ros.org/rospy/Overview/Time#Sleeping_and_Rates --> wait until next rate
def publish(self, throttle, brake, steer):
tcmd = ThrottleCmd()
tcmd.enable = True
tcmd.pedal_cmd_type = ThrottleCmd.CMD_PERCENT
tcmd.pedal_cmd = throttle
self.throttle_pub.publish(tcmd)
bcmd = BrakeCmd()
bcmd.enable = True
bcmd.pedal_cmd_type = BrakeCmd.CMD_TORQUE
bcmd.pedal_cmd = brake
self.brake_pub.publish(bcmd)
scmd = SteeringCmd()
scmd.enable = True
scmd.steering_wheel_angle_cmd = steer
self.steer_pub.publish(scmd)