def step(self, action):
distance_to_next = self.dist_to(self.waypoints[0])
startx, starty = self.x, self.y
if action == None:
self.raw_step(0, 0)
self.last_action = action
elif type(action) == SteeringAccAction:
self.raw_step(*action.get_action())
self.last_action = action
elif type(action) == SteeringAction:
fluids_assert(False, "Cars cannot receive a raw steering action")
elif type(action) == VelocityAction:
steer, acc = self.PIDController(action).get_action()
# steer += np.random.randn() * 0.5 * steer
# acc += np.random.randn() * 0.5 * acc / 5
self.raw_step(steer, acc)
self.last_action = action
elif type(action) == SteeringVelAction:
steer, vel = action.get_action()
_, acc = self.PIDController(VelocityAction(vel)).get_action()
self.raw_step(steer, acc)
self.last_action = action
elif type(action) == LastValidAction:
self.step(self.last_action)
return
else:
fluids_assert(False, "Car received an illegal action")
while (len(self.waypoints) < self.planning_depth
and len(self.waypoints) and len(self.waypoints[-1].nxt)):
next_edge = random.choice(self.waypoints[-1].nxt)
next_waypoint = next_edge.out_p
line = next_edge.shapely_obj
# line = shapely.geometry.LineString([(self.waypoints[-1].x, self.waypoints[-1].y),
# (next_waypoint.x, next_waypoint.y)]).buffer(self.ydim*0.5)
self.trajectory.append((
(self.waypoints[-1].x, self.waypoints[-1].y),
(next_waypoint.x, next_waypoint.y),
line,
))
self.waypoints.append(next_waypoint)
self.last_to_goal = distance_to_next - self.dist_to(self.waypoints[0])
self.last_distance = np.linalg.norm([self.x - startx, self.y - starty])
if self.last_distance == 0:
self.stopped_time += 1
else:
self.stopped_time = 0
if len(self.waypoints) and self.intersects(self.waypoints[0]):
self.waypoints.pop(0)
if len(self.trajectory):
self.trajectory.pop(0)
return
def make_observation(self, obs_space=OBS_NONE, **kwargs):
if obs_space == OBS_NONE:
self.last_obs = None
elif obs_space == OBS_GRID:
self.last_obs = GridObservation(self, **kwargs)
elif obs_space == OBS_BIRDSEYE:
self.last_obs = BirdsEyeObservation(self, **kwargs)
elif obs_space == OBS_QLIDAR:
self.last_obs = QLidarObservation(self, **kwargs)
elif obs_space:
fluids_assert(False, "Observation space not legal")
return self.last_obs
def step(self, action):
if len(self.waypoints) <= 0:
return True
distance_to_next = self.dist_to(self.waypoints[0])
startx, starty = self.x, self.y
if action is None:
self.raw_step(0, 0)
self.last_action = action
elif type(action) == SteeringAccAction:
self.raw_step(*action.get_action())
self.last_action = action
elif type(action) == SteeringAction:
fluids_assert(False, "Cars cannot receive a raw steering action")
elif type(action) == VelocityAction:
steer, acc = self.PIDController(action).get_action()
# steer += np.random.randn() * 0.5 * steer
# acc += np.random.randn() * 0.5 * acc / 5
self.raw_step(steer, acc)
self.last_action = action
elif type(action) == SteeringVelAction:
steer, vel = action.get_action()
_, acc = self.PIDController(VelocityAction(vel)).get_action()
self.raw_step(steer, acc)
self.last_action = action
elif type(action) == LastValidAction:
self.step(self.last_action)
return
else:
fluids_assert(False, "Car received an illegal action")
while len(self.waypoints) < self.planning_depth \
and len(self.waypoints) \
and len(self.waypoints[-1].nxt)\
and not (self.generated_first_traj and self.color == (0x0b, 0x04, 0xf4)):
if len(self.waypoints[-1].nxt) > 1:
if self.path == -1 or self.keep_random:
next_edge = random.choice(self.waypoints[-1].nxt)
else:
next_edge = self.waypoints[-1].nxt[self.path]
self.keep_random = True
else:
next_edge = self.waypoints[-1].nxt[0]
next_waypoint = next_edge.out_p
line = next_edge.shapely_obj
self.trajectory.append(((self.waypoints[-1].x, self.waypoints[-1].y),
(next_waypoint.x, next_waypoint.y), line))
self.waypoints.append(next_waypoint)
self.generated_first_traj = True
self.last_to_goal = distance_to_next - self.dist_to(self.waypoints[0])
self.last_distance = np.linalg.norm([self.x - startx, self.y - starty])
if self.last_distance == 0:
self.stopped_time += 1
else:
self.stopped_time = 0
if len(self.waypoints) and self.intersects(self.waypoints[0]):
self.waypoints.pop(0)
if len(self.trajectory):
self.trajectory.pop(0)
# Update positions
self.last_four_positions.pop(0)
self.last_four_positions.append(np.asarray((self.x, self.y)))
# Update jerk for jerk reward metric
self.jerk = calc_jerk(self.last_four_positions)
# Update total reward
# Collisions/infractions are handled by the rewards function
self.total_time += 1
self.total_jerk += self.jerk
self.total_traj_follow += self.last_to_goal
self.total_liveliness += self.vel / self.max_vel
# This might cause issues because current reward is only updated when the reward function is called
self.total_reward += self.current_reward
return