def test_update_from_traffic_sim(social_vehicle, provider_vehicle):
social_vehicle.control(
pose=provider_vehicle.pose, speed=provider_vehicle.speed,
)
sv_position, sv_heading = social_vehicle.pose.as_sumo(
social_vehicle.length, Heading(0)
)
provider_position, provider_heading = provider_vehicle.pose.as_sumo(
social_vehicle.length, Heading(0)
)
assert np.isclose(sv_position, provider_position, rtol=1e-02).all()
assert math.isclose(sv_heading, provider_heading, rel_tol=1e-05,)
assert social_vehicle.speed == provider_vehicle.speed
def discover_missions_of_traffic_histories(self) -> Dict[str, Mission]:
vehicle_missions = {}
map_offset = self._road_network.net_offset
for row in self._traffic_history.first_seen_times():
start_time = float(row[1])
pphs = self._traffic_history.vehicle_pose_at_time(row[0], start_time)
assert pphs
pos_x, pos_y, heading, speed = pphs
entry_tactic = default_entry_tactic(speed)
v_id = str(row[0])
veh_type = self._traffic_history.vehicle_type(v_id)
veh_length, veh_width, veh_height = self._traffic_history.vehicle_size(v_id)
# missions start from front bumper, but pos is center of vehicle
hhx, hhy = radians_to_vec(heading) * (0.5 * veh_length)
vehicle_missions[v_id] = Mission(
start=Start(
(pos_x + map_offset[0] + hhx, pos_y + map_offset[1] + hhy),
Heading(heading),
),
entry_tactic=entry_tactic,
goal=TraverseGoal(self.road_network),
start_time=start_time,
vehicle_spec=VehicleSpec(
veh_id=v_id,
veh_type=veh_type,
dimensions=BoundingBox(veh_length, veh_width, veh_height),
),
)
return vehicle_missions
def test_we_reach_target_pose_at_given_time(motion_planner_provider, loop_scenario):
motion_planner_provider.setup(loop_scenario)
# we sync with the empty provider state since we don't have any other active providers
motion_planner_provider.sync(ProviderState())
motion_planner_provider.create_vehicle(
VehicleState(
vehicle_id="EGO",
vehicle_type="passenger",
pose=Pose.from_center([0, 0, 0.5], heading=Heading(0)),
dimensions=VEHICLE_CONFIGS["passenger"].dimensions,
speed=0,
source="TESTS",
)
)
target_position = [32, -12]
target_heading = math.pi * 0.5
dt = 1.0
elapsed_sim_time = 0
for i in range(10):
t = 10 - i
provider_state = motion_planner_provider.step(
dt=dt,
target_poses_at_t={"EGO": np.array([*target_position, target_heading, t])},
elapsed_sim_time=elapsed_sim_time,
)
elapsed_sim_time += dt
assert len(provider_state.vehicles) == 1
ego_vehicle = provider_state.vehicles[0]
position, heading = ego_vehicle.pose.position, ego_vehicle.pose.heading
assert np.linalg.norm(position[:2] - np.array(target_position)) < 1e-16
assert np.isclose(heading, target_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 _shape_of_vehicle(self, sumo_vehicle_state, vehicle_id):
p = sumo_vehicle_state[vehicle_id][tc.VAR_POSITION]
length = sumo_vehicle_state[vehicle_id][tc.VAR_LENGTH]
width = sumo_vehicle_state[vehicle_id][tc.VAR_WIDTH]
heading = Heading.from_sumo(sumo_vehicle_state[vehicle_id][tc.VAR_ANGLE])
poly = shapely_box(p[0] - width * 0.5, p[1] - length, p[0] + width * 0.5, p[1],)
return shapely_rotate(poly, heading, use_radians=True)
def perform_trajectory_interpolation(
timestep_sec,
trajectory: np.ndarray,
):
"""Move vehicle by trajectory interpolation.
Trajectory mentioned here has 5 dimensions, which are TIME, X, Y, THETA and VEL.
TIME indicate
If you want vehicle stop at a specific pose,
trajectory[TrajectoryWithTime.TIME_INDEX][0] should be set as numpy.inf
Args:
sim : reference of smarts instance
vehicle : vehicle to be controlled
trajectory (np.ndarray): trajectory with time
"""
TrajectoryInterpolationProvider.is_legal_trajectory(trajectory)
ms0, ms1 = TrajectoryInterpolationProvider.locate_motion_state(
trajectory, timestep_sec
)
speed = 0.0
pose = []
if math.isinf(ms0[TrajectoryWithTime.TIME_INDEX]) or math.isinf(
ms1[TrajectoryWithTime.TIME_INDEX]
):
center_position = ms0[
TrajectoryWithTime.X_INDEX : TrajectoryWithTime.Y_INDEX + 1
]
center_heading = Heading(ms0[TrajectoryWithTime.THETA_INDEX])
pose = Pose.from_center(center_position, center_heading)
speed = 0.0
else:
ms = TrajectoryInterpolationProvider.interpolate(ms0, ms1, timestep_sec)
center_position = ms[
TrajectoryWithTime.X_INDEX : TrajectoryWithTime.Y_INDEX + 1
]
center_heading = Heading(ms[TrajectoryWithTime.THETA_INDEX])
pose = Pose.from_center(center_position, center_heading)
speed = ms[TrajectoryWithTime.VEL_INDEX]
return pose, speed
def update_vehicle_state(self):
"""Update this vehicle state."""
assert len(self.vector) == 20
self.vs.pose = Pose.from_center(
self.vector[1:4], Heading(self.vector[4] % (2 * math.pi)))
self.vs.dimensions = Dimensions(*self.vector[5:8])
self.linear_velocity = self.vector[8:11]
self.angular_velocity = self.vector[11:14]
self.linear_acceleration = self.vector[14:17]
self.angular_acceleration = self.vector[17:]
self.vs.speed = np.linalg.norm(self.linear_velocity)
