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