def act(self, action: Union[dict, str] = None): """ Execute an action. For now, no action is supported because the vehicle takes all decisions of acceleration and lane changes on its own, based on the IDM and MOBIL models. :param action: the action """ if self.crashed: return action = {} front_vehicle, rear_vehicle = self.traffic_mgr.neighbour_vehicles(self) # Lateral: MOBIL self.follow_road() if self.enable_lane_change: self.change_lane_policy() action['steering'] = self.steering_control(self.target_lane_index) action['steering'] = clip(action['steering'], -self.MAX_STEERING_ANGLE, self.MAX_STEERING_ANGLE) # Longitudinal: IDM action['acceleration'] = self.acceleration(ego_vehicle=self, front_vehicle=front_vehicle, rear_vehicle=rear_vehicle) # action['acceleration'] = self.recover_from_stop(action['acceleration']) action['acceleration'] = clip(action['acceleration'], -self.ACC_MAX, self.ACC_MAX) Vehicle.act( self, action ) # Skip ControlledVehicle.act(), or the command will be override.
def vehicle_state(self, vehicle): """ Wrap vehicle states to list """ # update out of road info = [] if hasattr(vehicle, "side_detector") and vehicle.side_detector.available: info += vehicle.side_detector.perceive( vehicle, vehicle.engine.physics_world.static_world).cloud_points else: lateral_to_left, lateral_to_right, = vehicle.dist_to_left_side, vehicle.dist_to_right_side total_width = float((vehicle.navigation.map.MAX_LANE_NUM + 1) * vehicle.navigation.map.MAX_LANE_WIDTH) lateral_to_left /= total_width lateral_to_right /= total_width info += [ clip(lateral_to_left, 0.0, 1.0), clip(lateral_to_right, 0.0, 1.0) ] # print("Heading Diff: ", [ # vehicle.heading_diff(current_reference_lane) # for current_reference_lane in vehicle.navigation.current_ref_lanes # ]) current_reference_lane = vehicle.navigation.current_ref_lanes[-1] info += [ vehicle.heading_diff(current_reference_lane), # Note: speed can be negative denoting free fall. This happen when emergency brake. clip((vehicle.speed + 1) / (vehicle.max_speed + 1), 0.0, 1.0), clip((vehicle.steering / vehicle.MAX_STEERING + 1) / 2, 0.0, 1.0), clip((vehicle.last_current_action[0][0] + 1) / 2, 0.0, 1.0), clip((vehicle.last_current_action[0][1] + 1) / 2, 0.0, 1.0) ] heading_dir_last = vehicle.last_heading_dir heading_dir_now = vehicle.heading cos_beta = heading_dir_now.dot(heading_dir_last) / ( norm(*heading_dir_now) * norm(*heading_dir_last)) beta_diff = np.arccos(clip(cos_beta, 0.0, 1.0)) # print(beta) yaw_rate = beta_diff / 0.1 # print(yaw_rate) info.append(clip(yaw_rate, 0.0, 1.0)) if vehicle.lane_line_detector.available: info += vehicle.lane_line_detector.perceive( vehicle, vehicle.engine.physics_world.static_world).cloud_points # else: # _, lateral = vehicle.lane.local_coordinates(vehicle.position) # info.append(clip((lateral * 2 / vehicle.navigation.map.MAX_LANE_WIDTH + 1.0) / 2.0, 0.0, 1.0)) # add vehicle length/width if self.config["random_agent_model"]: info.append(clip(vehicle.LENGTH / vehicle.MAX_LENGTH, 0.0, 1.0)) info.append(clip(vehicle.WIDTH / vehicle.MAX_WIDTH, 0.0, 1.0)) return info
def vehicle_state(vehicle): """ Wrap vehicle states to list """ # update out of road info = [] if hasattr(vehicle, "side_detector") and vehicle.side_detector is not None: info += vehicle.side_detector.get_cloud_points() else: lateral_to_left, lateral_to_right, = vehicle.dist_to_left_side, vehicle.dist_to_right_side total_width = float( (vehicle.routing_localization.get_current_lane_num() + 1) * vehicle.routing_localization.get_current_lane_width()) lateral_to_left /= total_width lateral_to_right /= total_width info += [ clip(lateral_to_left, 0.0, 1.0), clip(lateral_to_right, 0.0, 1.0) ] # print("Heading Diff: ", [ # vehicle.heading_diff(current_reference_lane) # for current_reference_lane in vehicle.routing_localization.current_ref_lanes # ]) current_reference_lane = vehicle.routing_localization.current_ref_lanes[ -1] info += [ vehicle.heading_diff(current_reference_lane), # Note: speed can be negative denoting free fall. This happen when emergency brake. clip((vehicle.speed + 1) / (vehicle.max_speed + 1), 0.0, 1.0), clip((vehicle.steering / vehicle.max_steering + 1) / 2, 0.0, 1.0), clip((vehicle.last_current_action[0][0] + 1) / 2, 0.0, 1.0), clip((vehicle.last_current_action[0][1] + 1) / 2, 0.0, 1.0) ] heading_dir_last = vehicle.last_heading_dir heading_dir_now = vehicle.heading cos_beta = heading_dir_now.dot(heading_dir_last) / ( norm(*heading_dir_now) * norm(*heading_dir_last)) beta_diff = np.arccos(clip(cos_beta, 0.0, 1.0)) # print(beta) yaw_rate = beta_diff / 0.1 # print(yaw_rate) info.append(clip(yaw_rate, 0.0, 1.0)) if vehicle.lane_line_detector is not None: info += vehicle.lane_line_detector.get_cloud_points() else: _, lateral = vehicle.lane.local_coordinates(vehicle.position) info.append( clip((lateral * 2 / vehicle.routing_localization.get_current_lane_width() + 1.0) / 2.0, 0.0, 1.0)) return info
def heading_diff(self, target_lane): lateral = None if isinstance(target_lane, StraightLane): lateral = np.asarray( get_vertical_vector(target_lane.end - target_lane.start)[1]) elif isinstance(target_lane, CircularLane): if target_lane.direction == -1: lateral = self.position - target_lane.center else: lateral = target_lane.center - self.position elif isinstance(target_lane, WayPointLane): lane_segment = target_lane.segment( target_lane.local_coordinates(self.position)[0]) lateral = lane_segment["lateral_direction"] lateral_norm = norm(lateral[0], lateral[1]) forward_direction = self.heading # print(f"Old forward direction: {self.forward_direction}, new heading {self.heading}") forward_direction_norm = norm(forward_direction[0], forward_direction[1]) if not lateral_norm * forward_direction_norm: return 0 cos = ((forward_direction[0] * lateral[0] + forward_direction[1] * lateral[1]) / (lateral_norm * forward_direction_norm)) # return cos # Normalize to 0, 1 return clip(cos, -1.0, 1.0) / 2 + 0.5
def speed(self): """ km/h """ velocity = self.chassis_np.node().get_linear_velocity() speed = norm(velocity[0], velocity[1]) * 3.6 return clip(speed, 0.0, 100000.0)
def speed(self): """ km/h """ velocity = self.body.get_linear_velocity() speed = norm(velocity[0], velocity[1]) * 3.6 return clip(speed, 0.0, 100000.0)
def heading_diff(self, target_lane): lateral = None if isinstance(target_lane, StraightLane): lateral = np.asarray( get_vertical_vector(target_lane.end - target_lane.start)[1]) elif isinstance(target_lane, CircularLane): if target_lane.direction == -1: lateral = self.position - target_lane.center else: lateral = target_lane.center - self.position else: raise ValueError("Unknown target lane type: {}".format( type(target_lane))) lateral_norm = norm(lateral[0], lateral[1]) forward_direction = self.heading # print(f"Old forward direction: {self.forward_direction}, new heading {self.heading}") forward_direction_norm = norm(forward_direction[0], forward_direction[1]) if not lateral_norm * forward_direction_norm: return 0 cos = ((forward_direction[0] * lateral[0] + forward_direction[1] * lateral[1]) / (lateral_norm * forward_direction_norm)) # return cos # Normalize to 0, 1 return clip(cos, -1.0, 1.0) / 2 + 0.5
def _set_incremental_action(self, action: np.ndarray): self.throttle_brake = action[1] self.steering += action[0] * self.STEERING_INCREMENT self.steering = clip(self.steering, -1, 1) steering = self.steering * self.max_steering self.system.setSteeringValue(steering, 0) self.system.setSteeringValue(steering, 1) self._apply_throttle_brake(action[1])
def act(self, agent_id): # clip to -1, 1 action = [ clip(self.engine.external_actions[agent_id][i], -1.0, 1.0) for i in range(len(self.engine.external_actions[agent_id])) ] if not self.discrete_action: return action else: return self.convert_to_continuous_action(action)
def act(self, action: Union[dict, str] = None) -> None: """ Perform a high-level action to change the desired lane or speed. - If a high-level action is provided, update the target speed and lane; - then, perform longitudinal and lateral control. :param action: a high-level action """ self.follow_road() if action == "FASTER": self.target_speed += self.DELTA_SPEED elif action == "SLOWER": self.target_speed -= self.DELTA_SPEED elif action == "LANE_RIGHT": _from, _to, _id = self.target_lane_index target_lane_index = _from, _to, clip( _id + 1, 0, len(self.traffic_mgr.map.road_network.graph[_from][_to]) - 1) if self.traffic_mgr.map.road_network.get_lane( target_lane_index).is_reachable_from(self.position): self.target_lane_index = target_lane_index elif action == "LANE_LEFT": _from, _to, _id = self.target_lane_index target_lane_index = _from, _to, clip( _id - 1, 0, len(self.traffic_mgr.map.road_network.graph[_from][_to]) - 1) if self.traffic_mgr.map.road_network.get_lane( target_lane_index).is_reachable_from(self.position): self.target_lane_index = target_lane_index action = { "steering": self.steering_control(self.target_lane_index), "acceleration": self.speed_control(self.target_speed) } action['steering'] = clip(action['steering'], -self.MAX_STEERING_ANGLE, self.MAX_STEERING_ANGLE) super().act(action)
def get_surrounding_vehicles_info(self, ego_vehicle, num_others: int = 4): from pgdrive.utils.math_utils import norm, clip surrounding_vehicles = list(self.get_surrounding_vehicles()) surrounding_vehicles.sort( key=lambda v: norm(ego_vehicle.position[0] - v.position[0], ego_vehicle.position[1] - v.position[1]) ) surrounding_vehicles += [None] * num_others res = [] for vehicle in surrounding_vehicles[:num_others]: if vehicle is not None: # assert isinstance(vehicle, IDMVehicle or Base), "Now PGDrive Doesn't support other vehicle type" relative_position = ego_vehicle.projection(vehicle.position - ego_vehicle.position) # It is possible that the centroid of other vehicle is too far away from ego but lidar shed on it. # So the distance may greater than perceive distance. res.append(clip((relative_position[0] / self.perceive_distance + 1) / 2, 0.0, 1.0)) res.append(clip((relative_position[1] / self.perceive_distance + 1) / 2, 0.0, 1.0)) relative_velocity = ego_vehicle.projection(vehicle.velocity - ego_vehicle.velocity) res.append(clip((relative_velocity[0] / ego_vehicle.max_speed + 1) / 2, 0.0, 1.0)) res.append(clip((relative_velocity[1] / ego_vehicle.max_speed + 1) / 2, 0.0, 1.0)) else: res += [0.0] * 4 return res
def vehicle_state(vehicle): """ Wrap vehicle states to list """ # update out of road current_reference_lane = vehicle.routing_localization.current_ref_lanes[ -1] lateral_to_left, lateral_to_right = ObservationType.vehicle_to_left_right( vehicle) if lateral_to_left < 0 or lateral_to_right < 0: vehicle.out_of_road = True total_width = float((vehicle.routing_localization.map.lane_num + 1) * vehicle.routing_localization.map.lane_width) info = [ clip(lateral_to_left / total_width, 0.0, 1.0), # lateral_to_left = distance to yellow line clip(lateral_to_right / total_width, 0.0, 1.0), # lateral_to_right = distance to pavement vehicle.heading_diff(current_reference_lane), # Note: speed can be negative denoting free fall. This happen when emergency brake. clip((vehicle.speed + 1) / (vehicle.max_speed + 1), 0.0, 1.0), clip((vehicle.steering / vehicle.max_steering + 1) / 2, 0.0, 1.0), clip((vehicle.last_current_action[0][0] + 1) / 2, 0.0, 1.0), clip((vehicle.last_current_action[0][1] + 1) / 2, 0.0, 1.0) ] heading_dir_last = vehicle.last_heading_dir heading_dir_now = vehicle.heading cos_beta = heading_dir_now.dot(heading_dir_last) / ( np.linalg.norm(heading_dir_now) * np.linalg.norm(heading_dir_last)) beta_diff = np.arccos(clip(cos_beta, 0.0, 1.0)) # print(beta) yaw_rate = beta_diff / 0.1 # print(yaw_rate) info.append(clip(yaw_rate, 0.0, 1.0)) _, lateral = vehicle.lane.local_coordinates(vehicle.position) info.append( clip((lateral * 2 / vehicle.routing_localization.map.lane_width + 1.0) / 2.0, 0.0, 1.0)) # breakpoint() return info
def steering_control(self, target_lane_index: LaneIndex) -> float: """ Steer the vehicle to follow the center of an given lane. 1. Lateral position is controlled by a proportional controller yielding a lateral speed command 2. Lateral speed command is converted to a heading reference 3. Heading is controlled by a proportional controller yielding a heading rate command 4. Heading rate command is converted to a steering angle :param target_lane_index: index of the lane to follow :return: a steering wheel angle command [rad] """ target_lane = self.traffic_mgr.map.road_network.get_lane( target_lane_index) lane_coords = target_lane.local_coordinates(self.position) lane_next_coords = lane_coords[0] + self.speed * self.PURSUIT_TAU lane_future_heading = target_lane.heading_at(lane_next_coords) # Lateral position control lateral_speed_command = -self.KP_LATERAL * lane_coords[1] # Lateral speed to heading heading_command = math.asin( clip(lateral_speed_command / utils.not_zero(self.speed), -1, 1)) heading_ref = lane_future_heading + clip(heading_command, -np.pi / 4, np.pi / 4) # Heading control heading_rate_command = self.KP_HEADING * utils.wrap_to_pi(heading_ref - self.heading) # Heading rate to steering angle steering_angle = math.asin( clip( self.LENGTH / 2 / utils.not_zero(self.speed) * heading_rate_command, -1, 1)) steering_angle = clip(steering_angle, -self.MAX_STEERING_ANGLE, self.MAX_STEERING_ANGLE) return float(steering_angle)
def reward_function(self, vehicle_id: str): """ Override this func to get a new reward function :param vehicle_id: id of BaseVehicle :return: reward """ vehicle = self.vehicles[vehicle_id] step_info = dict() # Reward for moving forward in current lane current_lane = vehicle.lane if vehicle.lane in vehicle.routing_localization.current_ref_lanes else \ vehicle.routing_localization.current_ref_lanes[0] long_last, _ = current_lane.local_coordinates(vehicle.last_position) long_now, lateral_now = current_lane.local_coordinates(vehicle.position) # reward for lane keeping, without it vehicle can learn to overtake but fail to keep in lane reward = 0.0 if self.config["use_lateral"]: lateral_factor = clip( 1 - 2 * abs(lateral_now) / vehicle.routing_localization.get_current_lane_width(), 0.0, 1.0 ) else: lateral_factor = 1.0 reward += self.config["driving_reward"] * (long_now - long_last) * lateral_factor # Penalty for waiting low_speed_penalty = 0 if vehicle.speed < 1: low_speed_penalty = self.config["low_speed_penalty"] # encourage car reward -= low_speed_penalty reward -= self.config["general_penalty"] reward += self.config["speed_reward"] * (vehicle.speed / vehicle.max_speed) step_info["step_reward"] = reward # for done if vehicle.crash_vehicle: reward = self.config["crash_vehicle_penalty"] elif vehicle.crash_object: reward = self.config["crash_object_penalty"] elif vehicle.out_of_route: reward = self.config["out_of_road_penalty"] elif vehicle.crash_sidewalk: reward = self.config["out_of_road_penalty"] elif vehicle.arrive_destination: reward += self.config["success_reward"] return reward, step_info
def reward_function(self, vehicle_id: str): """ Override this func to get a new reward function :param vehicle_id: id of BaseVehicle :return: reward """ vehicle = self.vehicles[vehicle_id] step_info = dict() # Reward for moving forward in current lane if vehicle.lane in vehicle.navigation.current_ref_lanes: current_lane = vehicle.lane else: current_lane = vehicle.navigation.current_ref_lanes[0] current_road = vehicle.current_road long_last, _ = current_lane.local_coordinates(vehicle.last_position) long_now, lateral_now = current_lane.local_coordinates( vehicle.position) # reward for lane keeping, without it vehicle can learn to overtake but fail to keep in lane if self.config["use_lateral"]: lateral_factor = clip( 1 - 2 * abs(lateral_now) / vehicle.navigation.get_current_lane_width(), 0.0, 1.0) else: lateral_factor = 1.0 reward = 0.0 reward += self.config["driving_reward"] * (long_now - long_last) * lateral_factor reward += self.config["speed_reward"] * (vehicle.speed / vehicle.max_speed) step_info["step_reward"] = reward if vehicle.arrive_destination: reward = +self.config["success_reward"] elif self._is_out_of_road(vehicle): reward = -self.config["out_of_road_penalty"] elif vehicle.crash_vehicle: reward = -self.config["crash_vehicle_penalty"] elif vehicle.crash_object: reward = -self.config["crash_object_penalty"] return reward, step_info
def speed(self): """ km/h """ speed = self.chassis_np.node().get_linear_velocity().length() * 3.6 return clip(speed, 0.0, 100000.0)
def vehicle_state(self, vehicle): # update out of road info = [] lateral_to_left, lateral_to_right, = vehicle.dist_to_left_side, vehicle.dist_to_right_side total_width = float( (vehicle.routing_localization.get_current_lane_num() + 1) * vehicle.routing_localization.get_current_lane_width()) lateral_to_left /= total_width lateral_to_right /= total_width info += [ clip(lateral_to_left, 0.0, 1.0), clip(lateral_to_right, 0.0, 1.0) ] current_reference_lane = vehicle.routing_localization.current_ref_lanes[ -1] info += [ vehicle.heading_diff(current_reference_lane), # Note: speed can be negative denoting free fall. This happen when emergency brake. clip((vehicle.speed + 1) / (vehicle.max_speed + 1), 0.0, 1.0), clip((vehicle.steering / vehicle.max_steering + 1) / 2, 0.0, 1.0), clip((vehicle.last_current_action[0][0] + 1) / 2, 0.0, 1.0), clip((vehicle.last_current_action[0][1] + 1) / 2, 0.0, 1.0) ] heading_dir_last = vehicle.last_heading_dir heading_dir_now = vehicle.heading # Add more information about the road if hasattr(current_reference_lane, "heading"): info.append(clip(current_reference_lane.heading, 0.0, 1.0)) else: info.append(0.0) info.append(clip(current_reference_lane.length / DISTANCE, 0.0, 1.0)) if hasattr(current_reference_lane, "direction"): dir = current_reference_lane.direction if isinstance(dir, int): info.append(self._to_zero_and_one(dir)) info.append(0.0) elif len(dir) == 2: info.append(self._to_zero_and_one(dir[0])) info.append(self._to_zero_and_one(dir[1])) else: info.extend([0.0, 0.0]) else: info.extend([0.0, 0.0]) cos_beta = heading_dir_now.dot(heading_dir_last) / ( norm(*heading_dir_now) * norm(*heading_dir_last)) beta_diff = np.arccos(clip(cos_beta, 0.0, 1.0)) yaw_rate = beta_diff / 0.1 info.append(clip(yaw_rate, 0.0, 1.0)) long, lateral = vehicle.lane.local_coordinates(vehicle.position) info.append( clip( (lateral * 2 / vehicle.routing_localization.get_current_lane_width() + 1.0) / 2.0, 0.0, 1.0)) info.append(clip(long / DISTANCE, 0.0, 1.0)) return info
def _to_zero_and_one(v): return (clip(v, -1, +1) + 1) / 2
def overwritten_get_surrounding_vehicles_info(self, lidar, ego_vehicle, num_others: int = 4): # surrounding_vehicles = list(lidar.get_surrounding_vehicles()) surrounding_vehicles = list( self._env.scene_manager.traffic_manager.vehicles)[1:] surrounding_vehicles.sort( key=lambda v: norm(ego_vehicle.position[0] - v.position[0], ego_vehicle.position[1] - v.position[1])) # dis = [(str(v), norm(ego_vehicle.position[0] - v.position[0], ego_vehicle.position[1] - v.position[1])) # for v in surrounding_vehicles] surrounding_vehicles += [None] * num_others res = [] for vehicle in surrounding_vehicles[:num_others]: if vehicle is not None: relative_position = ego_vehicle.projection( vehicle.position - ego_vehicle.position) dist = norm(relative_position[0], relative_position[1]) if dist > DISTANCE: if self.config["use_extra_state"]: res += [0.0] * self._traffic_vehicle_state_dim else: res += [0.0] * self._traffic_vehicle_state_dim_wo_extra continue relative_velocity = ego_vehicle.projection( vehicle.velocity - ego_vehicle.velocity) res.append(clip(dist / DISTANCE, 0.0, 1.0)) res.append( clip( norm(relative_velocity[0], relative_velocity[1]) / ego_vehicle.max_speed, 0.0, 1.0)) res.append( clip((relative_position[0] / DISTANCE + 1) / 2, 0.0, 1.0)) res.append( clip((relative_position[1] / DISTANCE + 1) / 2, 0.0, 1.0)) res.append( clip( (relative_velocity[0] / ego_vehicle.max_speed + 1) / 2, 0.0, 1.0)) res.append( clip( (relative_velocity[1] / ego_vehicle.max_speed + 1) / 2, 0.0, 1.0)) if self.config["use_extra_state"]: res.extend(self.traffic_vehicle_state(vehicle)) else: if self.config["use_extra_state"]: res += [0.0] * self._traffic_vehicle_state_dim else: res += [0.0] * self._traffic_vehicle_state_dim_wo_extra # p1 = [] # p2 = [] # for vehicle in surrounding_vehicles[:num_others]: # if vehicle is not None: # relative_position = ego_vehicle.projection(vehicle.position - ego_vehicle.position) # relative_velocity = ego_vehicle.projection(vehicle.velocity - ego_vehicle.velocity) # p1.append(relative_position) # p2.append(relative_velocity) # print("Detected Others Position: {}, Velocity: {}".format(p1, p2)) return res
def update_navigation_localization(self, ego_vehicle): position = ego_vehicle.position lane, lane_index = ray_localization(position, ego_vehicle.pg_world) if lane is None: lane, lane_index = ego_vehicle.lane, ego_vehicle.lane_index if self.FORCE_CALCULATE: lane_index, _ = self.map.road_network.get_closest_lane_index( position) lane = self.map.road_network.get_lane(lane_index) self._update_target_checkpoints(lane_index) target_road_1_start = self.checkpoints[ self.target_checkpoints_index[0]] target_road_1_end = self.checkpoints[self.target_checkpoints_index[0] + 1] target_road_2_start = self.checkpoints[ self.target_checkpoints_index[1]] target_road_2_end = self.checkpoints[self.target_checkpoints_index[1] + 1] target_lanes_1 = self.map.road_network.graph[target_road_1_start][ target_road_1_end] target_lanes_2 = self.map.road_network.graph[target_road_2_start][ target_road_2_end] res = [] self.current_ref_lanes = target_lanes_1 ckpts = [] lanes_heading = [] for lanes_id, lanes in enumerate([target_lanes_1, target_lanes_2]): ref_lane = lanes[0] later_middle = (float(self.map.lane_num) / 2 - 0.5) * self.map.lane_width if isinstance(ref_lane, CircularLane) and ref_lane.direction == 1: ref_lane = lanes[-1] later_middle *= -1 check_point = ref_lane.position(ref_lane.length, later_middle) if lanes_id == 0: # calculate ego v lane heading lanes_heading.append( ref_lane.heading_at( ref_lane.local_coordinates(ego_vehicle.position)[0])) else: lanes_heading.append( ref_lane.heading_at( min(self.PRE_NOTIFY_DIST, ref_lane.length))) ckpts.append(check_point) dir_vec = check_point - ego_vehicle.position dir_norm = norm(dir_vec[0], dir_vec[1]) if dir_norm > self.NAVI_POINT_DIST: dir_vec = dir_vec / dir_norm * self.NAVI_POINT_DIST proj_heading, proj_side = ego_vehicle.projection(dir_vec) bendradius = 0.0 dir = 0.0 angle = 0.0 if isinstance(ref_lane, CircularLane): bendradius = ref_lane.radius / ( BlockParameterSpace.CURVE[Parameter.radius].max + self.map.lane_num * self.map.lane_width) dir = ref_lane.direction if dir == 1: angle = ref_lane.end_phase - ref_lane.start_phase elif dir == -1: angle = ref_lane.start_phase - ref_lane.end_phase res += [ clip((proj_heading / self.NAVI_POINT_DIST + 1) / 2, 0.0, 1.0), clip((proj_side / self.NAVI_POINT_DIST + 1) / 2, 0.0, 1.0), clip(bendradius, 0.0, 1.0), clip((dir + 1) / 2, 0.0, 1.0), clip((np.rad2deg(angle) / BlockParameterSpace.CURVE[Parameter.angle].max + 1) / 2, 0.0, 1.0) ] if self.show_navi_point: pos_of_goal = ckpts[0] self.goal_node_path.setPos(pos_of_goal[0], -pos_of_goal[1], 1.8) self.goal_node_path.setH(self.goal_node_path.getH() + 3) self.update_navi_arrow(lanes_heading) self.navi_info = res return lane, lane_index