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)