def test_waypoints_sensor_with_cut_in_task(cut_in_scenarios):
scenario = next(cut_in_scenarios)
sim = mock.Mock()
nei_vehicle = mock.Mock()
nei_vehicle.speed = 10
sim.elapsed_sim_time = 4
nei_vehicle.pose = Pose(
position=np.array([25, -68, 0]),
orientation=[0, 0, 0, 0],
heading_=Heading(0),
)
sim.neighborhood_vehicles_around_vehicle = mock.MagicMock(
return_value=[nei_vehicle])
vehicle = mock.Mock()
vehicle.pose = Pose(
position=np.array([35, -65, 0]),
orientation=[0, 0, 0, 0],
heading_=Heading(0),
)
mission_planner = MissionPlanner(scenario.waypoints, scenario.road_network,
AgentBehavior(aggressiveness=3))
mission = scenario.missions[AGENT_ID]
mission_planner.plan(mission)
sensor = WaypointsSensor(sim, vehicle, mission_planner)
waypoints = sensor()
assert len(waypoints) == 1
def test_waypoints_sensor_with_uturn_task(uturn_scenarios):
scenario = next(uturn_scenarios)
sim = mock.Mock()
vehicle = mock.Mock()
sim.elapsed_sim_time = 1
sim.timestep_sec = 0.1
nei_vehicle = mock.Mock()
nei_vehicle.pose = Pose(
position=np.array([93, -59, 0]),
orientation=[0, 0, 0, 0],
heading_=Heading(0),
)
nei_vehicle.speed = 13.8
sim.neighborhood_vehicles_around_vehicle = mock.MagicMock(
return_value=[nei_vehicle])
vehicle.pose = Pose(
position=np.array([25, -61, 0]),
orientation=[0, 0, 0, 0],
heading_=Heading(0),
)
mission_planner = MissionPlanner(scenario.waypoints, scenario.road_network,
AgentBehavior(aggressiveness=3))
mission = scenario.missions[AGENT_ID]
mission_planner.plan(mission)
mission_planner._task_is_triggered = True
sensor = WaypointsSensor(sim, vehicle, mission_planner)
waypoints = sensor()
assert len(waypoints) == 1
def perform_action(cls, dt, vehicle, action):
chassis = vehicle.chassis
if isinstance(action, (int, float)):
# special case: setting the initial speed
if isinstance(chassis, BoxChassis):
vehicle.control(vehicle.pose, action, dt)
elif isinstance(chassis, AckermannChassis):
chassis.speed = action # hack that calls control internally
return
assert isinstance(action, (list, tuple)) and len(action) == 2
# acceleration is scalar in m/s^2, angular_velocity is scalar in rad/s
# acceleration is in the direction of the heading only.
acceleration, angular_velocity = action
# Note: we only use angular_velocity (not angular_acceleration) to determine
# the new heading and position in this action space. This sacrifices
# some realism/control, but helps simplify the imitation learning model.
target_heading = (vehicle.heading + angular_velocity * dt) % (2 *
math.pi)
if isinstance(chassis, BoxChassis):
# Since BoxChassis does not use pybullet for force-to-motion computations (only collision detection),
# we have to update the position and other state here (instead of pybullet.stepSimulation()).
heading_vec = radians_to_vec(vehicle.heading)
dpos = heading_vec * vehicle.speed * dt
new_pose = Pose(
position=vehicle.position + np.append(dpos, 0.0),
orientation=fast_quaternion_from_angle(target_heading),
)
target_speed = vehicle.speed + acceleration * dt
vehicle.control(new_pose, target_speed, dt)
elif isinstance(chassis, AckermannChassis):
mass = chassis.mass_and_inertia[0] # in kg
wheel_radius = chassis.wheel_radius
# XXX: should also take wheel inertia into account here
# XXX: ... or else we should apply this force directly to the main link point of the chassis.
if acceleration >= 0:
# necessary torque is N*m = kg*m*acceleration
torque_ratio = mass / (4 * wheel_radius * chassis.max_torque)
throttle = np.clip(acceleration * torque_ratio, 0, 1)
brake = 0
else:
throttle = 0
# necessary torque is N*m = kg*m*acceleration
torque_ratio = mass / (4 * wheel_radius * chassis.max_btorque)
brake = np.clip(acceleration * torque_ratio, 0, 1)
steering = np.clip(dt * -angular_velocity * chassis.steering_ratio,
-1, 1)
vehicle.control(throttle=throttle, brake=brake, steering=steering)
else:
raise Exception("unsupported chassis type")
def _pose_from_ros(ros_pose) -> Pose:
return Pose(
position=(ros_pose.position.x, ros_pose.position.y,
ros_pose.position.z),
orientation=(
ros_pose.orientation.x,
ros_pose.orientation.y,
ros_pose.orientation.z,
ros_pose.orientation.w,
),
)
def _pose_from_ros(ros_pose) -> Pose:
return Pose(
position=np.array([
ros_pose.position.x, ros_pose.position.y, ros_pose.position.z
]),
orientation=np.array([
ros_pose.orientation.x,
ros_pose.orientation.y,
ros_pose.orientation.z,
ros_pose.orientation.w,
]),
)
def test_renderer(self):
self._rdr.setup(self._scenario)
pose = Pose(
position=np.array([71.65, 53.78, 0]),
orientation=np.array([0, 0, 0, 0]),
heading_=Heading(math.pi * 0.91),
)
self._rdr.create_vehicle_node("simple_car.glb", self._vid,
SceneColors.SocialVehicle.value, pose)
self._rdr.begin_rendering_vehicle(self._vid, is_agent=False)
for s in range(self._num_steps):
self._rdr.render()
pose.position[0] = pose.position[0] + s
pose.position[1] = pose.position[1] + s
self._rdr.update_vehicle_node(self._vid, pose)
self._rdr.remove_vehicle_node(self._vid)
def test_waypoints_sensor(scenarios):
scenario = next(scenarios)
sim = mock.Mock()
vehicle = mock.Mock()
vehicle.pose = Pose(
position=np.array([33, -65, 0]),
orientation=np.array([0, 0, 0, 0]),
heading_=Heading(0),
)
mission = scenario.missions[AGENT_ID]
plan = Plan(scenario.road_map, mission)
sensor = WaypointsSensor(vehicle, plan)
waypoints = sensor()
assert len(waypoints) == 3
def test_waypoints_sensor(scenarios):
scenario = next(scenarios)
sim = mock.Mock()
vehicle = mock.Mock()
vehicle.pose = Pose(
position=np.array([33, -65, 0]),
orientation=[0, 0, 0, 0],
heading_=Heading(0),
)
mission_planner = MissionPlanner(scenario.waypoints, scenario.road_network)
mission = scenario.missions[AGENT_ID]
mission_planner.plan(mission)
sensor = WaypointsSensor(sim, vehicle, mission_planner)
waypoints = sensor()
assert len(waypoints) == 3
def _entity_to_vs(entity: EntityState) -> VehicleState:
veh_id = entity.entity_id
veh_type = ROSDriver._decode_entity_type(entity.entity_type)
veh_dims = Dimensions(entity.length, entity.width, entity.height)
vs = VehicleState(
source="EXTERNAL",
vehicle_id=veh_id,
vehicle_config_type=veh_type,
pose=Pose(
ROSDriver._xyz_to_vect(entity.pose.position),
ROSDriver._xyzw_to_vect(entity.pose.orientation),
),
dimensions=veh_dims,
linear_velocity=ROSDriver._xyz_to_vect(entity.velocity.linear),
angular_velocity=ROSDriver._xyz_to_vect(entity.velocity.angular),
linear_acceleration=ROSDriver._xyz_to_vect(
entity.acceleration.linear),
angular_acceleration=ROSDriver._xyz_to_vect(
entity.acceleration.angular),
)
vs.set_privileged()
vs.speed = np.linalg.norm(vs.linear_velocity)
return vs
def original_pose():
return Pose(position=np.array([0, 1, 0]),
orientation=np.array([0, 0, 0, 1]))
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
