def _extrapolate_to_now(vs: VehicleState, staleness: float, lin_acc_slope: float, ang_acc_slope: float): """Here we just linearly extrapolate the acceleration to "now" from the previous state for each vehicle and then use standard kinematics to project the velocity and position from that.""" # The following ~10 lines are a hack b/c I'm too stupid to figure out # how to do calculus on quaternions... heading = vs.pose.heading heading_delta_vec = staleness * ( vs.angular_velocity + 0.5 * vs.angular_acceleration * staleness + ang_acc_slope * staleness**2 / 6.0) heading += vec_to_radians(heading_delta_vec[:2]) + (0.5 * math.pi) heading %= 2 * math.pi vs.pose.orientation = fast_quaternion_from_angle(heading) # XXX: also need to remove the cached heading_ since we've changed orientation vs.pose.heading_ = None # I assume the following should be updated based on changing # heading from above, but I'll leave that for now... vs.pose.position += staleness * ( vs.linear_velocity + 0.5 * vs.linear_acceleration * staleness + lin_acc_slope * staleness**2 / 6.0) vs.linear_velocity += staleness * (vs.linear_acceleration + 0.5 * lin_acc_slope * staleness) vs.speed = np.linalg.norm(vs.linear_velocity) vs.angular_velocity += staleness * (vs.angular_acceleration + 0.5 * ang_acc_slope * staleness) vs.linear_acceleration += staleness * lin_acc_slope vs.angular_acceleration += staleness * ang_acc_slope
def scan_for_vehicles( target_prefix, angle_a, angle_b, activation_dist_squared, self_vehicle_state, other_vehicle_states, short_circuit: bool = False, ): if target_prefix: other_vehicle_states = filter( lambda v: v.id.startswith(target_prefix), other_vehicle_states ) min_angle, max_angle = min(angle_a, angle_b), max(angle_a, angle_b) vehicles = [] for vehicle_state in other_vehicle_states: sqd = squared_dist(self_vehicle_state.position, vehicle_state.position) # check for activation distance if sqd < activation_dist_squared: direction = np.sum( [vehicle_state.position, -self_vehicle_state.position], axis=0 ) angle = Heading(vec_to_radians(direction[:2])) rel_angle = angle.relative_to(self_vehicle_state.heading) if min_angle <= rel_angle <= max_angle: vehicles.append(vehicle_state) if short_circuit: break return vehicles
def to_position_and_heading(road_id, lane_index, offset, road_map): road = road_map.road_by_id(road_id) lane = road.lane_at_index(lane_index) offset = resolve_offset(offset, lane.length) position = lane.from_lane_coord(RefLinePoint(s=offset)) lane_vector = lane.vector_at_offset(offset) heading = vec_to_radians(lane_vector[:2]) return position, Heading(heading)
def to_position_and_heading(edge_id, lane_index, offset, road_network): edge = road_network.edge_by_id(edge_id) lane = edge.getLanes()[lane_index] offset = resolve_offset(offset, lane.getLength()) position = road_network.world_coord_from_offset(lane, offset) lane_vector = road_network.lane_vector_at_offset(lane, offset) heading = vec_to_radians(lane_vector) return tuple(position), Heading(heading)
def scan_for_vehicles( target_prefix, angle_a, angle_b, activation_dist_squared, self_vehicle_state, other_vehicle_states, short_circuit: bool = False, ): """Sense test for vehicles within a semi-circle radius of a vehicle. Args: target_prefix: The whitelist of vehicles with vehicle_ids starting with this prefix for quick elimination. angle_a: The minimum sweep angle between -pi and pi. angle_b: The maximum sweep angle between -pi and pi. activation_dist_squared: The distance to check for the target. self_vehicle_state: The vehicle state of the vehicle that is scanning. other_vehicle_states: The set of vehicles to test for. Returns: If the tested for vehicle is within the semi-circle range of the base vehicle. """ if target_prefix: other_vehicle_states = filter(lambda v: v.id.startswith(target_prefix), other_vehicle_states) min_angle, max_angle = min(angle_a, angle_b), max(angle_a, angle_b) vehicles = [] for vehicle_state in other_vehicle_states: sqd = squared_dist(self_vehicle_state.position, vehicle_state.position) # check for activation distance if sqd < activation_dist_squared: direction = np.sum( [vehicle_state.position, -self_vehicle_state.position], axis=0) angle = Heading(vec_to_radians(direction[:2])) rel_angle = angle.relative_to(self_vehicle_state.heading) if min_angle <= rel_angle <= max_angle: vehicles.append(vehicle_state) if short_circuit: break return vehicles
def _cal_heading(self, window_param) -> float: window = window_param[0] new_heading = 0 prev_heading = None den = 0 for w in range(self._heading_window - 1): c = window[w, :2] n = window[w + 1, :2] if any(np.isnan(c)) or any(np.isnan(n)): if prev_heading is not None: new_heading += prev_heading den += 1 continue s = np.linalg.norm(n - c) ispeed = window[w, 2] if s == 0.0 or ( self._heading_min_speed is not None and (s < self._heading_min_speed or ispeed < self._heading_min_speed) ): if prev_heading is not None: new_heading += prev_heading den += 1 continue vhat = (n - c) / s inst_heading = vec_to_radians(vhat) if prev_heading is not None: if inst_heading == 0 and prev_heading > math.pi: inst_heading = 2 * math.pi if self._max_angular_velocity: # XXX: could try to divide by sim_time delta here instead of assuming .1s angular_velocity = (inst_heading - prev_heading) / 0.1 if abs(angular_velocity) > self._max_angular_velocity: inst_heading = ( prev_heading + np.sign(angular_velocity) * self._max_angular_velocity * 0.1 ) den += 1 new_heading += inst_heading prev_heading = inst_heading if den > 0: new_heading /= den else: new_heading = self._prev_heading self._prev_heading = new_heading return new_heading % (2 * math.pi)
def scan_for_vehicle( target_prefix: str, angle_a: float, angle_b: float, activation_dist_squared: float, self_vehicle_state, other_vehicle_state, ): """Sense test for another vehicle within a semi-circle range of a vehicle. Args: target_prefix: The whitelist of vehicles with vehicle_ids starting with this prefix for quick elimination. angle_a: The minimum sweep angle between -pi and pi. angle_b: The maximum sweep angle between -pi and pi. activation_dist_squared: The distance to check for the target. self_vehicle_state: The vehicle state of the vehicle that is scanning. other_vehicle_state: The vehicle to test for. Returns: If the tested for vehicle is within the semi-circle range of the base vehicle. """ if target_prefix and not other_vehicle_state.id.startswith(target_prefix): return False min_angle, max_angle = min(angle_a, angle_b), max(angle_a, angle_b) sqd = squared_dist(self_vehicle_state.position, other_vehicle_state.position) # check for activation distance if sqd < activation_dist_squared: direction = np.sum( [other_vehicle_state.position, -self_vehicle_state.position], axis=0) angle = Heading(vec_to_radians(direction[:2])) rel_angle = angle.relative_to(self_vehicle_state.heading) return min_angle <= rel_angle <= max_angle return False
def column_val_in_row(self, row, col_name: str) -> Any: row_name = self._col_map.get(col_name) if row_name: return row[row_name] if col_name == "length": return row.get("length", 0.0) if col_name == "width": return row.get("width", 0.0) if col_name == "type": return self._lookup_agent_type(row["agent_type"]) if col_name == "speed": if self._next_row: # XXX: could try to divide by sim_time delta here instead of assuming .1s dx = (float(self._next_row["x"]) - float(row["x"])) / 0.1 dy = (float(self._next_row["y"]) - float(row["y"])) / 0.1 else: dx, dy = float(row["vx"]), float(row["vy"]) return np.linalg.norm((dx, dy)) if col_name == "heading_rad": if self._next_row: dx = float(self._next_row["x"]) - float(row["x"]) dy = float(self._next_row["y"]) - float(row["y"]) dm = np.linalg.norm((dx, dy)) if dm != 0.0 and dm > self._heading_min_speed: new_heading = vec_to_radians((dx, dy)) if self._max_angular_velocity and self._prev_heading is not None: # XXX: could try to divide by sim_time delta here instead of assuming .1s angular_velocity = (new_heading - self._prev_heading) / 0.1 if abs(angular_velocity) > self._max_angular_velocity: new_heading = ( self._prev_heading + np.sign(angular_velocity) * self._max_angular_velocity * 0.1 ) self._prev_heading = new_heading return new_heading # Note: pedestrian track files won't have this self._prev_heading = float(row.get("psi_rad", 0.0)) - math.pi / 2 return self._prev_heading # XXX: should probably check for and handle x_offset_px here too like in NGSIM return None
def scan_for_vehicle( target_prefix, angle_a, angle_b, activation_dist_squared, self_vehicle_state, other_vehicle_state, ): if target_prefix and not other_vehicle_state.id.startswith(target_prefix): return False min_angle, max_angle = min(angle_a, angle_b), max(angle_a, angle_b) sqd = squared_dist(self_vehicle_state.position, other_vehicle_state.position) # check for activation distance if sqd < activation_dist_squared: direction = np.sum( [other_vehicle_state.position, -self_vehicle_state.position], axis=0 ) angle = Heading(vec_to_radians(direction[:2])) rel_angle = angle.relative_to(self_vehicle_state.heading) return min_angle <= rel_angle <= max_angle return False
def yaw_rate(self) -> float: """Returns 2-D rotational speed in rad/sec.""" _, velocity_rotational = np.array( self._client.getBaseVelocity(self._bullet_id)) return vec_to_radians(velocity_rotational[:2])
def _shape_lanepoints_along_lane( self, lane: Lane, lanepoint_by_lane_memo: dict ) -> Tuple[LinkedLanePoint, List[LinkedLanePoint]]: lane_queue = queue.Queue() lane_queue.put((lane, None)) shape_lanepoints = [] initial_lanepoint = None while not lane_queue.empty(): lane, previous_lp = lane_queue.get() first_lanepoint = lanepoint_by_lane_memo.get(lane.getID()) if first_lanepoint: if previous_lp: previous_lp.nexts.append(first_lanepoint) continue lane_data = self._road_network.lane_data_for_lane(lane) lane_shape = [np.array(p) for p in lane.getShape(False)] assert len(lane_shape) >= 2, repr(lane_shape) heading = vec_to_radians(lane_shape[1] - lane_shape[0]) heading = Heading(heading) first_lanepoint = LinkedLanePoint( lp=LanePoint( pos=lane_shape[0], heading=heading, lane_width=lane.getWidth(), speed_limit=lane_data.lane_speed, lane_id=lane.getID(), lane_index=lane.getIndex(), ), nexts=[], is_shape_lp=True, ) if previous_lp is not None: previous_lp.nexts.append(first_lanepoint) if initial_lanepoint is None: initial_lanepoint = first_lanepoint lanepoint_by_lane_memo[lane.getID()] = first_lanepoint shape_lanepoints.append(first_lanepoint) curr_lanepoint = first_lanepoint for p1, p2 in zip(lane_shape[1:], lane_shape[2:]): heading_ = vec_to_radians(p2 - p1) heading_ = Heading(heading_) linked_lanepoint = LinkedLanePoint( lp=LanePoint( pos=p1, heading=heading_, lane_width=lane.getWidth(), speed_limit=lane_data.lane_speed, lane_id=lane.getID(), lane_index=lane.getIndex(), ), nexts=[], is_shape_lp=True, ) shape_lanepoints.append(linked_lanepoint) curr_lanepoint.nexts.append(linked_lanepoint) curr_lanepoint = linked_lanepoint # Add a lanepoint for the last point of the current lane last_linked_lanepoint = LinkedLanePoint( lp=LanePoint( pos=lane_shape[-1], heading=curr_lanepoint.lp.heading, lane_width=curr_lanepoint.lp.lane_width, speed_limit=curr_lanepoint.lp.speed_limit, lane_id=curr_lanepoint.lp.lane_id, lane_index=curr_lanepoint.lp.lane_index, ), nexts=[], is_shape_lp=True, ) shape_lanepoints.append(last_linked_lanepoint) curr_lanepoint.nexts.append(last_linked_lanepoint) curr_lanepoint = last_linked_lanepoint for out_connection in lane.getOutgoing(): out_lane = out_connection.getToLane() # Use internal lanes of junctions (if we're at a junction) via_lane_id = out_connection.getViaLaneID() if via_lane_id: out_lane = self._road_network.lane_by_id(via_lane_id) lane_queue.put((out_lane, curr_lanepoint)) return initial_lanepoint, shape_lanepoints
def _process_interp_for_lane_lp( shape_lp: LinkedLanePoint, first_linked_lanepoint: LinkedLanePoint, next_shape_lp: LinkedLanePoint, spacing: float, newly_created_lanepoints: List[LinkedLanePoint], ) -> LinkedLanePoint: lane_id = shape_lp.lp.lane_id curr_lanepoint = first_linked_lanepoint lane_seg_vec = next_shape_lp.lp.pos - shape_lp.lp.pos lane_seg_len = np.linalg.norm(lane_seg_vec) # We set the initial distance into the lane at `spacing` because # we already have a lanepoint along this segment (curr_lanepoint) dist_into_lane_seg = spacing while dist_into_lane_seg < lane_seg_len: p = dist_into_lane_seg / lane_seg_len pos = shape_lp.lp.pos + lane_seg_vec * p # The thresholds for calculating last lanepoint. If the # midpoint between the current lanepoint and the next shape # lanepoint is less than the minimum distance then the last # lanepoint position will be that midpoint. If the midpoint # is closer than last spacing threshold to the next shape # lanepoint, then the last lanepoint will be the current # lanepoint. # XXX: the map scale should be taken into account here. last_spacing_threshold_dist = 0.8 * spacing minimum_dist_next_shape_lp = 1.4 half_distant_current_next_shape_lp = np.linalg.norm( 0.5 * (curr_lanepoint.lp.pos - next_shape_lp.lp.pos)) mid_point_current_next_shape_lp = 0.5 * (next_shape_lp.lp.pos + curr_lanepoint.lp.pos) if half_distant_current_next_shape_lp < minimum_dist_next_shape_lp: pos = mid_point_current_next_shape_lp dist_pos_next_shape_lp = np.linalg.norm(next_shape_lp.lp.pos - pos) if dist_pos_next_shape_lp < last_spacing_threshold_dist: break heading = vec_to_radians(lane_seg_vec) lane_width = lerp(shape_lp.lp.lane_width, next_shape_lp.lp.lane_width, p) speed_limit = lerp(shape_lp.lp.speed_limit, next_shape_lp.lp.speed_limit, p) linked_lanepoint = LinkedLanePoint( lp=LanePoint( pos=pos, heading=Heading(heading), lane_width=lane_width, speed_limit=speed_limit, lane_id=lane_id, lane_index=shape_lp.lp.lane_index, ), nexts=[], is_shape_lp=False, ) curr_lanepoint.nexts.append(linked_lanepoint) curr_lanepoint = linked_lanepoint newly_created_lanepoints.append(linked_lanepoint) dist_into_lane_seg += spacing return curr_lanepoint
def _process_interp_for_lane_lp( shape_lp: LinkedLanePoint, first_linked_lanepoint: LinkedLanePoint, next_shape_lp: LinkedLanePoint, spacing: float, newly_created_lanepoints: List[LinkedLanePoint], ) -> LinkedLanePoint: rmlane = shape_lp.lp.lane curr_lanepoint = first_linked_lanepoint lane_seg_vec = next_shape_lp.lp.pose.position - shape_lp.lp.pose.position lane_seg_len = np.linalg.norm(lane_seg_vec) # We set the initial distance into the lane at `spacing` because # we already have a lanepoint along this segment (curr_lanepoint) dist_into_lane_seg = spacing while dist_into_lane_seg < lane_seg_len: p = dist_into_lane_seg / lane_seg_len pos = shape_lp.lp.pose.position + lane_seg_vec * p # The thresholds for calculating last lanepoint. If the # midpoint between the current lanepoint and the next shape # lanepoint is less than the minimum distance then the last # lanepoint position will be that midpoint. If the midpoint # is closer than last spacing threshold to the next shape # lanepoint, then the last lanepoint will be the current # lanepoint. # XXX: the map scale should be taken into account here. last_spacing_threshold_dist = 0.8 * spacing minimum_dist_next_shape_lp = 1.4 half_distant_current_next_shape_lp = np.linalg.norm( 0.5 * (curr_lanepoint.lp.pose.position - next_shape_lp.lp.pose.position)) mid_point_current_next_shape_lp = 0.5 * ( next_shape_lp.lp.pose.position + curr_lanepoint.lp.pose.position) if half_distant_current_next_shape_lp < minimum_dist_next_shape_lp: pos = mid_point_current_next_shape_lp dist_pos_next_shape_lp = np.linalg.norm( next_shape_lp.lp.pose.position - pos) if dist_pos_next_shape_lp < last_spacing_threshold_dist: break heading = vec_to_radians(lane_seg_vec) orientation = fast_quaternion_from_angle(heading) rmlane_coord = rmlane.to_lane_coord( Point(x=pos[0], y=pos[1], z=0.0)) linked_lanepoint = LinkedLanePoint( lp=LanePoint( lane=rmlane, pose=Pose(position=pos, orientation=orientation), lane_width=rmlane.width_at_offset(rmlane_coord.s), ), nexts=[], is_inferred=True, ) curr_lanepoint.nexts.append(linked_lanepoint) curr_lanepoint = linked_lanepoint newly_created_lanepoints.append(linked_lanepoint) dist_into_lane_seg += spacing return curr_lanepoint
def _shape_lanepoints_along_lane( road_map: OpenDriveRoadNetwork, lane: RoadMap.Lane, lanepoint_by_lane_memo: dict, ) -> Tuple[LinkedLanePoint, List[LinkedLanePoint]]: lane_queue = queue.Queue() lane_queue.put((lane, None)) shape_lanepoints = [] initial_lanepoint = None while not lane_queue.empty(): curr_lane, previous_lp = lane_queue.get() first_lanepoint = lanepoint_by_lane_memo.get(curr_lane.lane_id) if first_lanepoint: if previous_lp: previous_lp.nexts.append(first_lanepoint) continue lane_shape = [np.array(p) for p in curr_lane.centerline_points] assert len(lane_shape) >= 2, repr(lane_shape) heading = vec_to_radians(lane_shape[1] - lane_shape[0]) heading = Heading(heading) orientation = fast_quaternion_from_angle(heading) first_lane_coord = curr_lane.to_lane_coord( Point(x=lane_shape[0][0], y=lane_shape[0][1], z=0.0)) first_lanepoint = LinkedLanePoint( lp=LanePoint( lane=curr_lane, pose=Pose(position=lane_shape[0], orientation=orientation), lane_width=curr_lane.width_at_offset( first_lane_coord.s), ), nexts=[], is_inferred=False, ) if previous_lp is not None: previous_lp.nexts.append(first_lanepoint) if initial_lanepoint is None: initial_lanepoint = first_lanepoint lanepoint_by_lane_memo[curr_lane.lane_id] = first_lanepoint shape_lanepoints.append(first_lanepoint) curr_lanepoint = first_lanepoint for p1, p2 in zip(lane_shape[1:], lane_shape[2:]): heading_ = vec_to_radians(p2 - p1) heading_ = Heading(heading_) orientation_ = fast_quaternion_from_angle(heading_) lp_lane_coord = curr_lane.to_lane_coord( Point(x=p1[0], y=p1[1], z=0.0)) linked_lanepoint = LinkedLanePoint( lp=LanePoint( lane=curr_lane, pose=Pose(position=p1, orientation=orientation_), lane_width=curr_lane.width_at_offset( lp_lane_coord.s), ), nexts=[], is_inferred=False, ) shape_lanepoints.append(linked_lanepoint) curr_lanepoint.nexts.append(linked_lanepoint) curr_lanepoint = linked_lanepoint # Add a lanepoint for the last point of the current lane last_lane_coord = curr_lanepoint.lp.lane.to_lane_coord( Point(x=lane_shape[-1][0], y=lane_shape[-1][1], z=0.0)) last_linked_lanepoint = LinkedLanePoint( lp=LanePoint( lane=curr_lanepoint.lp.lane, pose=Pose( position=lane_shape[-1], orientation=curr_lanepoint.lp.pose.orientation, ), lane_width=curr_lanepoint.lp.lane.width_at_offset( last_lane_coord.s), ), nexts=[], is_inferred=False, ) shape_lanepoints.append(last_linked_lanepoint) curr_lanepoint.nexts.append(last_linked_lanepoint) curr_lanepoint = last_linked_lanepoint outgoing_roads_added = [] for out_lane in curr_lane.outgoing_lanes: if out_lane.is_drivable: lane_queue.put((out_lane, curr_lanepoint)) outgoing_road = out_lane.road if out_lane.road not in outgoing_roads_added: outgoing_roads_added.append(outgoing_road) for outgoing_road in outgoing_roads_added: for next_lane in outgoing_road.lanes: if (next_lane.is_drivable and next_lane not in curr_lane.outgoing_lanes): lane_queue.put((next_lane, curr_lanepoint)) return initial_lanepoint, shape_lanepoints
def _shape_lanepoints_along_lane( road_map: SumoRoadNetwork, lane: RoadMap.Lane, lanepoint_by_lane_memo: dict ) -> Tuple[LinkedLanePoint, List[LinkedLanePoint]]: lane_queue = queue.Queue() lane_queue.put((lane, None)) shape_lanepoints = [] initial_lanepoint = None while not lane_queue.empty(): lane, previous_lp = lane_queue.get() first_lanepoint = lanepoint_by_lane_memo.get(lane.getID()) if first_lanepoint: if previous_lp: previous_lp.nexts.append(first_lanepoint) continue lane_shape = [np.array(p) for p in lane.getShape(False)] assert len(lane_shape) >= 2, repr(lane_shape) heading = vec_to_radians(lane_shape[1] - lane_shape[0]) heading = Heading(heading) orientation = fast_quaternion_from_angle(heading) first_lanepoint = LinkedLanePoint( lp=LanePoint( lane=road_map.lane_by_id(lane.getID()), pose=Pose(position=lane_shape[0], orientation=orientation), lane_width=road_map.lane_by_id( lane.getID()).width_at_offset(0), ), nexts=[], is_inferred=False, ) if previous_lp is not None: previous_lp.nexts.append(first_lanepoint) if initial_lanepoint is None: initial_lanepoint = first_lanepoint lanepoint_by_lane_memo[lane.getID()] = first_lanepoint shape_lanepoints.append(first_lanepoint) curr_lanepoint = first_lanepoint for p1, p2 in zip(lane_shape[1:], lane_shape[2:]): heading_ = vec_to_radians(p2 - p1) heading_ = Heading(heading_) orientation_ = fast_quaternion_from_angle(heading_) linked_lanepoint = LinkedLanePoint( lp=LanePoint( lane=road_map.lane_by_id(lane.getID()), pose=Pose(position=p1, orientation=orientation_), lane_width=road_map.lane_by_id( lane.getID()).width_at_offset(0), ), nexts=[], is_inferred=False, ) shape_lanepoints.append(linked_lanepoint) curr_lanepoint.nexts.append(linked_lanepoint) curr_lanepoint = linked_lanepoint # Add a lanepoint for the last point of the current lane last_linked_lanepoint = LinkedLanePoint( lp=LanePoint( lane=curr_lanepoint.lp.lane, pose=Pose( position=lane_shape[-1], orientation=curr_lanepoint.lp.pose.orientation, ), lane_width=curr_lanepoint.lp.lane.width_at_offset(0), ), nexts=[], is_inferred=False, ) shape_lanepoints.append(last_linked_lanepoint) curr_lanepoint.nexts.append(last_linked_lanepoint) curr_lanepoint = last_linked_lanepoint for out_connection in lane.getOutgoing(): out_lane = out_connection.getToLane() # Use internal lanes of junctions (if we're at a junction) via_lane_id = out_connection.getViaLaneID() if via_lane_id: out_lane = road_map._graph.getLane(via_lane_id) lane_queue.put((out_lane, curr_lanepoint)) return initial_lanepoint, shape_lanepoints