def calculate_score_for_leaderboard():
"""
Evaluate the performance of the network. This is the function to be used for
the final ranking on the course-wide leader-board, only with a different set
of seeds. Better not change it.
"""
# action variables
a = np.array([0.0, 0.0, 0.0])
# init environement
env = CarRacing()
env.render()
env.reset()
seeds = [
22597174, 68545857, 75568192, 91140053, 86018367, 49636746, 66759182,
91294619, 84274995, 31531469
]
total_reward = 0
for episode in range(10):
env.seed(seeds[episode])
observation = env.reset()
# init modules of the pipeline
LD_module = LaneDetection(gradient_threshold=25, spline_smoothness=20)
LatC_module = LateralController(gain_constant=1.8,
damping_constant=0.05)
LongC_module = LongitudinalController(KD=0.001)
reward_per_episode = 0
for t in range(600):
# perform step
s, r, done, speed, info = env.step(a)
# lane detection
lane1, lane2 = LD_module.lane_detection(s)
# waypoint and target_speed prediction
waypoints = waypoint_prediction(lane1, lane2)
target_speed = target_speed_prediction(waypoints,
max_speed=60,
exp_constant=6)
# control
a[0] = LatC_module.stanley(waypoints, speed)
a[1], a[2] = LongC_module.control(speed, target_speed)
# reward
reward_per_episode += r
env.render()
print('episode %d \t reward %f' % (episode, reward_per_episode))
total_reward += np.clip(reward_per_episode, 0, np.infty)
print('---------------------------')
print(' total score: %f' % (total_reward / 10))
print('---------------------------')
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.) # for steering wheel
self.steer_pub = rospy.Publisher('/vehicle/steering_cmd',
SteeringCmd,
queue_size=1)
self.throttle_pub = rospy.Publisher('/vehicle/throttle_cmd',
ThrottleCmd,
queue_size=1)
self.brake_pub = rospy.Publisher('/vehicle/brake_cmd',
BrakeCmd,
queue_size=1)
rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_cmd_cb)
rospy.Subscriber('/final_waypoints', Lane, self.waypoint_cb)
rospy.Subscriber('/current_velocity', TwistStamped, self.velocity_cb)
rospy.Subscriber('/current_pose', PoseStamped, self.pose_cb)
rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_enabled_cb)
# output file for debug purpose
# base_path = os.path.dirname(os.path.abspath(__file__))
# self.steerfile = os.path.join(base_path, 'steers.csv')
# self.speedfile = os.path.join(base_path, 'speed.csv')
#
# self.steer_data = []
# self.speed_data = []
self.longitudinal_control = LongitudinalController(
vehicle_mass, wheel_radius, accel_limit, decel_limit, DELAY)
self.lateral_control = LateralController(vehicle_mass, wheel_base,
steer_ratio, max_lat_accel,
max_steer_angle, 0, 5, DELAY)
self.dbw_enabled = False
self.waypoints = None
self.pose = None
self.velocity = None
self.yawrate = None
self.current_command = None
self.frame_id = None
self.loop()
def evaluate():
"""
"""
# action variables
a = np.array([0.0, 0.0, 0.0])
# init environement
env = CarRacing()
env.render()
env.reset()
for episode in range(5):
observation = env.reset()
# init modules of the pipeline
LD_module = LaneDetection()
LatC_module = LateralController()
LongC_module = LongitudinalController()
reward_per_episode = 0
for t in range(500):
# perform step
s, r, done, speed, info = env.step(a)
# lane detection
lane1, lane2 = LD_module.lane_detection(s)
# waypoint and target_speed prediction
waypoints = waypoint_prediction(lane1, lane2)
target_speed = target_speed_prediction(waypoints,
max_speed=60,
exp_constant=4.5)
# control
a[0] = LatC_module.stanley(waypoints, speed)
a[1], a[2] = LongC_module.control(speed, target_speed)
# reward
reward_per_episode += r
env.render()
print('episode %d \t reward %f' % (episode, reward_per_episode))
# action variables
a = np.array( [0.0, 0.1, 0.0] )
# init environement
env = CarRacing()
env.render()
env.reset()
# define variables
total_reward = 0.0
steps = 0
restart = False
# init modules of the pipeline
LD_module = LaneDetection()
LatC_module = LateralController()
Speed_module = SpeedPrediction(max_speed=58, exp_constant=8, offset_speed=40, num_waypoints_used=6)
# init extra plot
fig = plt.figure()
plt.ion()
plt.show()
while True:
# perform step
s, r, done, speed, info = env.step(a)
# lane detection
lane1, lane2 = LD_module.lane_detection(s)
# waypoint and target_speed prediction
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.) # for steering wheel
self.steer_pub = rospy.Publisher('/vehicle/steering_cmd',
SteeringCmd,
queue_size=1)
self.throttle_pub = rospy.Publisher('/vehicle/throttle_cmd',
ThrottleCmd,
queue_size=1)
self.brake_pub = rospy.Publisher('/vehicle/brake_cmd',
BrakeCmd,
queue_size=1)
rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_cmd_cb)
rospy.Subscriber('/final_waypoints', Lane, self.waypoint_cb)
rospy.Subscriber('/current_velocity', TwistStamped, self.velocity_cb)
rospy.Subscriber('/current_pose', PoseStamped, self.pose_cb)
rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_enabled_cb)
# output file for debug purpose
# base_path = os.path.dirname(os.path.abspath(__file__))
# self.steerfile = os.path.join(base_path, 'steers.csv')
# self.speedfile = os.path.join(base_path, 'speed.csv')
#
# self.steer_data = []
# self.speed_data = []
self.longitudinal_control = LongitudinalController(
vehicle_mass, wheel_radius, accel_limit, decel_limit, DELAY)
self.lateral_control = LateralController(vehicle_mass, wheel_base,
steer_ratio, max_lat_accel,
max_steer_angle, 0, 5, DELAY)
self.dbw_enabled = False
self.waypoints = None
self.pose = None
self.velocity = None
self.yawrate = None
self.current_command = None
self.frame_id = None
self.loop()
def loop(self):
rate = rospy.Rate(SAMPLE_RATE) # 50Hz
sample_time = 1.0 / SAMPLE_RATE
self.longitudinal_control.set_sample_time(sample_time)
self.lateral_control.set_sample_time(sample_time)
while not rospy.is_shutdown():
if self.waypoints is None \
or self.pose is None \
or self.velocity is None:
continue
close_way_point_id = dbw_helper.get_closest_waypoint_index(
self.pose, self.waypoints)
ref_spd_raw = self.waypoints[
close_way_point_id].twist.twist.linear.x
waypoint_coefficient = self.lateral_control.set_waypoint_coeff(
self.pose,
self.waypoints,
self.velocity,
polynomial_order=3,
points_to_fit=20)
sample_time_displacement = sample_time * ref_spd_raw
radius_evaluation_point = np.array([
0.4 * sample_time_displacement, 0.5 * sample_time_displacement,
0.6 * sample_time_displacement
])
current_radius = dbw_helper.calculateRCurve(
waypoint_coefficient, radius_evaluation_point)
max_ref_spd = self.lateral_control.get_max_ref_speed(
np.average(current_radius))
ref_spd = ref_spd_raw
if ref_spd > max_ref_spd:
ref_spd = max_ref_spd
throttle, brake = self.longitudinal_control.control_lqr(
ref_spd, self.velocity, self.dbw_enabled)
steer, cte_distance, cte_yaw = self.lateral_control.control_lqr(
self.dbw_enabled)
# output file for debug purpose
#
# fieldnames_steer = ['proposed', 'cte_distance', 'cte_yaw']
# fieldnames_speed = ['ref_speed_raw', 'max_ref_spd','ref_speed','current_speed']
#
# self.speed_data.append({'ref_speed_raw': ref_spd_raw,
# 'max_ref_spd': max_ref_spd,
# 'ref_speed': ref_spd,
# 'current_speed': self.velocity})
#
# with open(self.speedfile, 'w') as csvfile:
# writer = csv.DictWriter(csvfile, fieldnames=fieldnames_speed)
# writer.writeheader()
# writer.writerows(self.speed_data)
#
# self.steer_data.append({'proposed': steer,
# 'cte_distance': cte_distance,
# 'cte_yaw': cte_yaw})
#
# with open(self.steerfile, 'w') as csvfile:
# writer = csv.DictWriter(csvfile, fieldnames=fieldnames_steer)
# writer.writeheader()
# writer.writerows(self.steer_data)
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 twist_cmd_cb(self, msg):
self.current_command = msg.twist
def velocity_cb(self, msg):
self.velocity = msg.twist.linear.x
self.yawrate = msg.twist.angular.z
def dbw_enabled_cb(self, msg):
self.dbw_enabled = bool(msg.data)
def waypoint_cb(self, msg):
self.waypoints = msg.waypoints
def pose_cb(self, msg):
self.pose = msg.pose
self.frame_id = msg.header.frame_id