From 3e30a9c926cf374cdce6a7a44ef1a3da8364e99e Mon Sep 17 00:00:00 2001 From: Montgomery Alban Date: Thu, 18 Jan 2024 18:15:25 +0000 Subject: [PATCH] Add shader uniforms test. --- examples/occlusion/mask.py | 2 +- smarts/core/events.py | 9 +- .../glsl/test_custom_shader_pass_shader.frag | 127 ++++++++++++++++++ smarts/core/observations.py | 54 +++++++- smarts/core/plan.py | 13 ++ smarts/core/sensor.py | 6 +- smarts/core/sensors/__init__.py | 8 +- smarts/core/shader_buffer.py | 7 +- smarts/core/smarts.py | 68 ++++++---- smarts/core/tests/test_renderers.py | 80 +++++++++++ .../utils/adapters/ego_centric_adapters.py | 3 +- smarts/core/utils/dummy.py | 16 ++- smarts/env/utils/observation_conversion.py | 2 +- smarts/p3d/renderer.py | 53 +++++--- 14 files changed, 374 insertions(+), 74 deletions(-) create mode 100644 smarts/core/glsl/test_custom_shader_pass_shader.frag diff --git a/examples/occlusion/mask.py b/examples/occlusion/mask.py index 4b2097db82..32e6badf2f 100644 --- a/examples/occlusion/mask.py +++ b/examples/occlusion/mask.py @@ -540,7 +540,7 @@ def downgrade_waypoints( def downgrade_vehicles( center: Tuple[float, float], - neighborhood_vehicle_states: List[VehicleObservation], + neighborhood_vehicle_states: Tuple[VehicleObservation], mode=ObservationOptions.multi_agent, ): if mode: diff --git a/smarts/core/events.py b/smarts/core/events.py index 405f6d636a..e5d408393a 100644 --- a/smarts/core/events.py +++ b/smarts/core/events.py @@ -17,14 +17,19 @@ # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN # THE SOFTWARE. +from __future__ import annotations + import warnings -from typing import NamedTuple, Sequence +from typing import TYPE_CHECKING, NamedTuple, Tuple + +if TYPE_CHECKING: + from smarts.core.vehicle_state import Collision class Events(NamedTuple): """Classified observations that can trigger agent done status.""" - collisions: Sequence # Sequence[Collision] + collisions: Tuple[Collision] """Collisions with other vehicles (if any).""" off_road: bool """True if vehicle is off the road, else False.""" diff --git a/smarts/core/glsl/test_custom_shader_pass_shader.frag b/smarts/core/glsl/test_custom_shader_pass_shader.frag new file mode 100644 index 0000000000..7622d8403b --- /dev/null +++ b/smarts/core/glsl/test_custom_shader_pass_shader.frag @@ -0,0 +1,127 @@ +#version 330 core +// This script is intended to test that all of the observation buffers work in association with +// the test script `test_renderers.py`. + +uniform int step_count; +uniform int steps_completed; +uniform int events_off_road; +uniform int events_off_route; +uniform int events_on_shoulder; +uniform int events_wrong_way; +uniform int events_not_moving; +uniform int events_reached_goal; +uniform int events_reached_max_episode_steps; +uniform int events_agents_done_alive; +uniform int events_interest_done; +uniform int ego_vehicle_state_road_id; +uniform int ego_vehicle_state_lane_id; +uniform int ego_vehicle_state_lane_index; +uniform int under_this_vehicle_control; +uniform int vehicle_type; + +uniform float dt; +uniform float ego_vehicle_state_heading; +uniform float ego_vehicle_state_speed; +uniform float ego_vehicle_state_steering; +uniform float ego_vehicle_state_yaw_rate; +uniform float elapsed_sim_time; +uniform float distance_travelled; + +uniform vec3 ego_vehicle_state_position; +uniform vec3 ego_vehicle_state_bounding_box; +uniform vec3 ego_vehicle_state_lane_position; + +uniform int neighborhood_vehicle_states_road_id[10]; +uniform int neighborhood_vehicle_states_lane_id[10]; +uniform int neighborhood_vehicle_states_lane_index[10]; +uniform int neighborhood_vehicle_states_interest[10]; +uniform int waypoint_paths_lane_id[10]; +uniform int waypoint_paths_lane_index[10]; +uniform int road_waypoints_lanes_lane_id[10]; +uniform int road_waypoints_lanes_lane_index[10]; +uniform int via_data_near_via_points_lane_index[10]; +uniform int via_data_near_via_points_road_id[10]; +uniform int via_data_near_via_points_hit[10]; +uniform int lidar_point_cloud_hits[100]; +uniform int signals_light_state[10]; + +uniform float neighborhood_vehicle_states_heading[10]; +uniform float neighborhood_vehicle_states_speed[10]; +uniform float waypoint_paths_heading[10]; +uniform float waypoint_paths_lane_width[10]; +uniform float waypoint_paths_speed_limit[10]; +uniform float waypoint_paths_lane_offset[10]; +uniform float road_waypoints_lanes_heading[10]; +uniform float road_waypoints_lanes_width[10]; +uniform float road_waypoints_lanes_speed_limit[10]; +uniform float road_waypoints_lanes_lane_offset[10]; +uniform float via_data_near_via_points_required_speed[10]; +uniform float signals_last_changed[10]; + +uniform vec2 ego_vehicle_state_linear_velocity[10]; +uniform vec2 ego_vehicle_state_angular_velocity[10]; +uniform vec2 ego_vehicle_state_linear_acceleration[10]; +uniform vec2 ego_vehicle_state_angular_acceleration[10]; +uniform vec2 ego_vehicle_state_linear_jerk[10]; +uniform vec2 ego_vehicle_state_angular_jerk[10]; +uniform vec2 waypoint_paths_pos[10]; +uniform vec2 road_waypoints_lanes_pos[10]; +uniform vec2 via_data_near_via_points_position[10]; +uniform vec2 signals_stop_point[10]; + +uniform vec3 neighborhood_vehicle_states_position[10]; +uniform vec3 neighborhood_vehicle_states_bounding_box[10]; +uniform vec3 neighborhood_vehicle_states_lane_position[10]; +uniform vec3 lidar_point_cloud_points[100]; +uniform vec3 lidar_point_cloud_origin[100]; +uniform vec3 lidar_point_cloud_direction[100]; +// SIGNALS_CONTROLLED_LANES = "signals_controlled_lanes" + +// Output color +out vec4 p3d_Color; + +uniform vec2 iResolution; + + +void mainImage( out vec4 fragColor, in vec2 fragCoord ) +{ + vec2 rec_res = 1.0 / iResolution.xy; + vec2 p = fragCoord.xy * rec_res; + + fragColor = vec4(0.0, 0.0, 0.0, 0.0); +} + +#ifndef SHADERTOY +void main(){ + int a = step_count + steps_completed + events_off_road + events_off_route + + events_on_shoulder + events_wrong_way + events_not_moving + + events_reached_goal + events_reached_max_episode_steps + + events_agents_done_alive + events_interest_done + ego_vehicle_state_road_id + + ego_vehicle_state_lane_id + ego_vehicle_state_lane_index + under_this_vehicle_control + vehicle_type; + float b = dt + ego_vehicle_state_heading + ego_vehicle_state_speed + + ego_vehicle_state_steering + ego_vehicle_state_yaw_rate + elapsed_sim_time + + distance_travelled; + vec3 c = ego_vehicle_state_position + ego_vehicle_state_bounding_box + ego_vehicle_state_lane_position; + int d = neighborhood_vehicle_states_road_id[0] + neighborhood_vehicle_states_lane_id[0] + + neighborhood_vehicle_states_lane_index[0] + neighborhood_vehicle_states_interest[0] + + waypoint_paths_lane_id[0] + waypoint_paths_lane_index[0] + road_waypoints_lanes_lane_id[0] + + road_waypoints_lanes_lane_index[0] + via_data_near_via_points_lane_index[0] + + via_data_near_via_points_road_id[0] + via_data_near_via_points_hit[0] + + lidar_point_cloud_hits[0] + signals_light_state[0]; + float e = neighborhood_vehicle_states_heading[0] + neighborhood_vehicle_states_speed[0] + + waypoint_paths_heading[0] + waypoint_paths_lane_width[0] + waypoint_paths_speed_limit[0] + + waypoint_paths_lane_offset[0] + road_waypoints_lanes_heading[0] + road_waypoints_lanes_width[0] + + road_waypoints_lanes_speed_limit[0] + road_waypoints_lanes_lane_offset[0] + + via_data_near_via_points_required_speed[0] + signals_last_changed[0]; + vec2 f = ego_vehicle_state_linear_velocity[0] + ego_vehicle_state_angular_velocity[0] + + ego_vehicle_state_linear_acceleration[0] + ego_vehicle_state_angular_acceleration[0] + + ego_vehicle_state_linear_jerk[0] + ego_vehicle_state_angular_jerk[0] + + waypoint_paths_pos[0] + road_waypoints_lanes_pos[0] + via_data_near_via_points_position[0] + + signals_stop_point[0]; + vec3 g = neighborhood_vehicle_states_position[0] + neighborhood_vehicle_states_bounding_box[0] + + neighborhood_vehicle_states_lane_position[0] + lidar_point_cloud_points[0] + + lidar_point_cloud_origin[0] + lidar_point_cloud_direction[0]; + + mainImage( p3d_Color, gl_FragCoord.xy ); +} +#endif \ No newline at end of file diff --git a/smarts/core/observations.py b/smarts/core/observations.py index 0f7a1296d2..95b5886340 100644 --- a/smarts/core/observations.py +++ b/smarts/core/observations.py @@ -25,6 +25,8 @@ import numpy as np +from smarts.core.utils.cache import cache + if TYPE_CHECKING: from smarts.core import plan, signals from smarts.core.coordinates import Dimensions, Heading, Point, RefLinePoint @@ -106,6 +108,9 @@ class RoadWaypoints(NamedTuple): lanes: Dict[str, List[List[Waypoint]]] """Mapping of road ids to their lane waypoints.""" + def __hash__(self) -> int: + return hash(tuple((k, len(v)) for k, v in self.lanes.items())) + class GridMapMetadata(NamedTuple): """Map grid metadata.""" @@ -130,6 +135,9 @@ class TopDownRGB(NamedTuple): data: np.ndarray """A RGB image with the ego vehicle at the center.""" + def __hash__(self) -> int: + return self.metadata.__hash__() + class OccupancyGridMap(NamedTuple): """Occupancy map.""" @@ -141,6 +149,9 @@ class OccupancyGridMap(NamedTuple): See https://en.wikipedia.org/wiki/Occupancy_grid_mapping.""" + def __hash__(self) -> int: + return self.metadata.__hash__() + class ObfuscationGridMap(NamedTuple): """Obfuscation map.""" @@ -150,6 +161,9 @@ class ObfuscationGridMap(NamedTuple): data: np.ndarray """A map showing what is visible from the ego vehicle""" + def __hash__(self) -> int: + return self.metadata.__hash__() + class DrivableAreaGridMap(NamedTuple): """Drivable area map.""" @@ -159,6 +173,9 @@ class DrivableAreaGridMap(NamedTuple): data: np.ndarray """A grid map that shows the static drivable area around the ego vehicle.""" + def __hash__(self) -> int: + return self.metadata.__hash__() + class CustomRenderData(NamedTuple): """Describes information about a custom render.""" @@ -168,6 +185,9 @@ class CustomRenderData(NamedTuple): data: np.ndarray """The image data from the render.""" + def __hash__(self) -> int: + return self.metadata.__hash__() + class ViaPoint(NamedTuple): """'Collectibles' that can be placed within the simulation.""" @@ -187,13 +207,13 @@ class ViaPoint(NamedTuple): class Vias(NamedTuple): """Listing of nearby collectible ViaPoints and ViaPoints collected in the last step.""" - near_via_points: List[ViaPoint] + near_via_points: Tuple[ViaPoint] """Ordered list of nearby points that have not been hit.""" @property - def hit_via_points(self) -> List[ViaPoint]: + def hit_via_points(self) -> Tuple[ViaPoint]: """List of points that were hit in the previous step.""" - return [vp for vp in self.near_via_points if vp.hit] + return tuple(vp for vp in self.near_via_points if vp.hit) class SignalObservation(NamedTuple): @@ -204,7 +224,7 @@ class SignalObservation(NamedTuple): stop_point: Point """The stopping point for traffic controlled by the signal, i.e., the point where actors should stop when the signal is in a stop state.""" - controlled_lanes: List[str] + controlled_lanes: Tuple[str] """If known, the lane_ids of all lanes controlled-by this signal. May be empty if this is not easy to determine.""" last_changed: Optional[float] @@ -228,7 +248,7 @@ class Observation(NamedTuple): """Ego vehicle status.""" under_this_agent_control: bool """Whether this agent currently has control of the vehicle.""" - neighborhood_vehicle_states: Optional[List[VehicleObservation]] + neighborhood_vehicle_states: Optional[Tuple[VehicleObservation]] """List of neighborhood vehicle states.""" waypoint_paths: Optional[List[List[Waypoint]]] """Dynamic evenly-spaced points on the road ahead of the vehicle, showing potential routes ahead.""" @@ -250,9 +270,31 @@ class Observation(NamedTuple): """Occupancy map.""" top_down_rgb: Optional[TopDownRGB] = None """RGB camera observation.""" - signals: Optional[List[SignalObservation]] = None + signals: Optional[Tuple[SignalObservation]] = None """List of nearby traffic signal (light) states on this time-step.""" obfuscation_grid_map: Optional[ObfuscationGridMap] = None """Observable area map.""" custom_renders: Tuple[CustomRenderData, ...] = tuple() """Custom renders.""" + + def __hash__(self): + return hash( + ( + self.dt, + self.step_count, + self.elapsed_sim_time, + self.events, + self.ego_vehicle_state, + # self.waypoint_paths, # likely redundant + self.neighborhood_vehicle_states, + self.distance_travelled, + self.road_waypoints, + self.via_data, + self.drivable_area_grid_map, + self.occupancy_grid_map, + self.top_down_rgb, + self.signals, + self.obfuscation_grid_map, + self.custom_renders, + ) + ) diff --git a/smarts/core/plan.py b/smarts/core/plan.py index a9ac288df0..396459a0ab 100644 --- a/smarts/core/plan.py +++ b/smarts/core/plan.py @@ -67,6 +67,19 @@ def from_pose(cls, pose: Pose): from_front_bumper=False, ) + def __hash__(self): + hash_ = getattr(self, "hash", None) + if not hash_: + hash_ = hash( + ( + tuple(self.position), + self.heading, + self.from_front_bumper, + ) + ) + object.__setattr__(self, "hash", hash_) + return hash_ + @dataclass(frozen=True, unsafe_hash=True) class Goal: diff --git a/smarts/core/sensor.py b/smarts/core/sensor.py index 03de59fa80..3d7c14a502 100644 --- a/smarts/core/sensor.py +++ b/smarts/core/sensor.py @@ -1029,7 +1029,7 @@ def closest_point_on_lane(position, lane_id, road_map): self._consumed_via_points.add(via) near_points.sort(key=lambda dist, _: dist) - return [p for _, p in near_points] + return tuple(p for _, p in near_points) def teardown(self, **kwargs): pass @@ -1098,12 +1098,12 @@ def __call__( SignalObservation( state=signal_state.state, stop_point=signal_state.stopping_pos, - controlled_lanes=controlled_lanes, + controlled_lanes=tuple(controlled_lanes), last_changed=signal_state.last_changed, ) ) - return result + return tuple(result) def _find_signals_ahead( self, diff --git a/smarts/core/sensors/__init__.py b/smarts/core/sensors/__init__.py index 7bc78b07d2..168b302686 100644 --- a/smarts/core/sensors/__init__.py +++ b/smarts/core/sensors/__init__.py @@ -424,7 +424,7 @@ def process_serialization_safe_sensors( else None ) - near_via_points = [] + near_via_points = () via_sensor = vehicle_sensors.get("via_sensor") if via_sensor: @@ -498,7 +498,7 @@ def process_serialization_safe_sensors( events=events, ego_vehicle_state=ego_vehicle, under_this_agent_control=agent_controls, - neighborhood_vehicle_states=neighborhood_vehicle_states, + neighborhood_vehicle_states=neighborhood_vehicle_states or (), waypoint_paths=waypoint_paths, distance_travelled=distance_travelled, road_waypoints=road_waypoints, @@ -667,7 +667,9 @@ def _is_done_with_events( ) events = Events( - collisions=sim_frame.filtered_vehicle_collisions(vehicle_state.actor_id), + collisions=tuple( + sim_frame.filtered_vehicle_collisions(vehicle_state.actor_id) + ), off_road=is_off_road, reached_goal=reached_goal, reached_max_episode_steps=reached_max_episode_steps, diff --git a/smarts/core/shader_buffer.py b/smarts/core/shader_buffer.py index 471d1fbb72..7f81d96549 100644 --- a/smarts/core/shader_buffer.py +++ b/smarts/core/shader_buffer.py @@ -39,6 +39,7 @@ class BufferID(Enum): STEPS_COMPLETED = "steps_completed" ELAPSED_SIM_TIME = "elapsed_sim_time" + EVENTS_COLLISIONS = "events_collisions" EVENTS_OFF_ROAD = "events_off_road" EVENTS_OFF_ROUTE = "events_off_route" EVENTS_ON_SHOULDER = "events_on_shoulder" @@ -94,8 +95,8 @@ class BufferID(Enum): ROAD_WAYPOINTS_POSITION = "road_waypoints_lanes_pos" ROAD_WAYPOINTS_HEADING = "road_waypoints_lanes_heading" - ROAD_WAYPOINTS_LANE_ID = "road_waypoints_lane_id" - ROAD_WAYPOINTS_LANE_WIDTH = "road_waypoints_lane_width" + ROAD_WAYPOINTS_LANE_ID = "road_waypoints_lanes_lane_id" + ROAD_WAYPOINTS_LANE_WIDTH = "road_waypoints_lanes_width" ROAD_WAYPOINTS_SPEED_LIMIT = "road_waypoints_lanes_speed_limit" ROAD_WAYPOINTS_LANE_INDEX = "road_waypoints_lanes_lane_index" ROAD_WAYPOINTS_LANE_OFFSET = "road_waypoints_lanes_lane_offset" @@ -115,5 +116,5 @@ class BufferID(Enum): SIGNALS_LIGHT_STATE = "signals_light_state" SIGNALS_STOP_POINT = "signals_stop_point" - SIGNALS_CONTROLLED_LANES = "signals_controlled_lanes" + # SIGNALS_CONTROLLED_LANES = "signals_controlled_lanes" SIGNALS_LAST_CHANGED = "signals_last_changed" diff --git a/smarts/core/smarts.py b/smarts/core/smarts.py index 0f9e1abcc4..ed08e194d1 100644 --- a/smarts/core/smarts.py +++ b/smarts/core/smarts.py @@ -141,7 +141,7 @@ def __init__( ): conf = config() self._log = logging.getLogger(self.__class__.__name__) - self._log.setLevel(level=logging.ERROR) + # self._log.setLevel(level=logging.DEBUG) self._sim_id = Id.new("smarts") self._is_setup = False self._is_destroyed = False @@ -348,10 +348,12 @@ def _step(self, agent_actions, time_delta_since_last_step: Optional[float] = Non # want these during their observation/reward computations. # This is a hack to give us some short term perf wins. Longer term we # need to expose better support for batched computations - self._sync_smarts_and_provider_actor_states(provider_state) - self._sensor_manager.clean_up_sensors_for_actors( - set(v.actor_id for v in self._vehicle_states), renderer=self.renderer_ref - ) + with timeit("Syncing provider state", self._log.debug): + self._sync_smarts_and_provider_actor_states(provider_state) + self._sensor_manager.clean_up_sensors_for_actors( + set(v.actor_id for v in self._vehicle_states), + renderer=self.renderer_ref, + ) # Reset frame state try: @@ -1387,39 +1389,47 @@ def _provider_actions( def _step_providers(self, actions) -> ProviderState: provider_vehicle_actions = dict() for provider in self.providers: - agent_actions, vehicle_actions = self._provider_actions(provider, actions) - provider_vehicle_actions[provider] = vehicle_actions - if isinstance(provider, AgentsProvider): - provider.perform_agent_actions(agent_actions) + with timeit( + f"Performing actions on {provider.__class__.__name__}", self._log.debug + ): + agent_actions, vehicle_actions = self._provider_actions( + provider, actions + ) + provider_vehicle_actions[provider] = vehicle_actions + if isinstance(provider, AgentsProvider): + provider.perform_agent_actions(agent_actions) - self._check_ground_plane() - self._step_pybullet() - self._process_collisions() + with timeit("Stepping physics", self._log.debug): + self._check_ground_plane() + self._step_pybullet() + self._process_collisions() accumulated_provider_state = ProviderState() agent_vehicle_ids = self._vehicle_index.agent_vehicle_ids() for provider in self.providers: - try: - provider_state = provider.step( - provider_vehicle_actions[provider], - self._last_dt, - self._elapsed_sim_time, - ) - except Exception as provider_error: - provider_state = self._handle_provider(provider, provider_error) - raise + with timeit(f"Stepping {provider.__class__.__name__}", self._log.debug): + try: + provider_state = provider.step( + provider_vehicle_actions[provider], + self._last_dt, + self._elapsed_sim_time, + ) + except Exception as provider_error: + provider_state = self._handle_provider(provider, provider_error) + raise - # by this point, "stop_managing()" should have been called for the hijacked vehicle on all TrafficProviders - assert not isinstance( - provider, TrafficProvider - ) or not provider_state.intersects( - agent_vehicle_ids - ), f"{agent_vehicle_ids} in {provider_state.actors}" + # by this point, "stop_managing()" should have been called for the hijacked vehicle on all TrafficProviders + assert not isinstance( + provider, TrafficProvider + ) or not provider_state.intersects( + agent_vehicle_ids + ), f"{agent_vehicle_ids} in {provider_state.actors}" - accumulated_provider_state.merge(provider_state) + accumulated_provider_state.merge(provider_state) - self._harmonize_providers(accumulated_provider_state) + with timeit("Harmonizing provider state", self._log.debug): + self._harmonize_providers(accumulated_provider_state) return accumulated_provider_state def _sync_smarts_and_provider_actor_states( diff --git a/smarts/core/tests/test_renderers.py b/smarts/core/tests/test_renderers.py index 03c7a6eec1..e0ffc37984 100644 --- a/smarts/core/tests/test_renderers.py +++ b/smarts/core/tests/test_renderers.py @@ -19,12 +19,15 @@ # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN # THE SOFTWARE. +import importlib.resources as pkg_resources import math import threading +from typing import Optional, Set import numpy as np import pytest +import smarts.core.glsl from smarts.core.agent_interface import ( ActionSpaceType, AgentInterface, @@ -34,10 +37,17 @@ from smarts.core.colors import SceneColors from smarts.core.coordinates import Heading, Pose from smarts.core.plan import EndlessGoal, Mission, Start +from smarts.core.renderer_base import ( + RendererBase, + RendererNotSetUpWarning, + ShaderStepBufferDependency, +) from smarts.core.scenario import Scenario +from smarts.core.shader_buffer import BufferID from smarts.core.smarts import SMARTS from smarts.core.sumo_traffic_simulation import SumoTrafficSimulation from smarts.core.utils.custom_exceptions import RendererException +from smarts.core.utils.tests.fixtures import large_observation AGENT_ID = "Agent-007" @@ -94,6 +104,26 @@ def scenario(): return scenario +@pytest.fixture(params=["p3d"], scope="module") +def renderer(request: pytest.FixtureRequest): + renderer: Optional[RendererBase] = None + + if request.param == "p3d": + from smarts.p3d.renderer import BACKEND_LITERALS, DEBUG_MODE, Renderer + + renderer = Renderer("n/a", debug_mode=DEBUG_MODE.WARNING) + + yield renderer + + renderer.destroy() + + +@pytest.fixture +def observation_buffers() -> Set[BufferID]: + + return [v for v in BufferID] + + def test_optional_renderer(smarts_w_renderer: SMARTS, scenario: Scenario): assert not smarts_w_renderer.is_rendering renderer = smarts_w_renderer.renderer @@ -119,3 +149,53 @@ def test_no_renderer(smarts_wo_renderer: SMARTS, scenario): smarts_wo_renderer.step({AGENT_ID: "keep_lane"}) assert not smarts_wo_renderer.is_rendering + + +def test_custom_shader_pass_buffers( + renderer: Optional[RendererBase], + observation_buffers: Set[BufferID], + large_observation, +): + # Use a full dummy observation. + # Construct the renderer + # Construct the shader pass + # Use a shader that uses all buffers + # Each buffer in the shader is assigned to a pixel. + # Assign all shader buffers in the pass. + # Update the shader pass using the observation + # Render + # Get the render texture from the shader pass + # Test each pixel for the expected value. + + assert renderer + renderer.reset() + camera_id = "test_shader" + with pkg_resources.path( + smarts.core.glsl, "test_custom_shader_pass_shader.frag" + ) as fshader: + renderer.build_shader_step( + camera_id, + fshader_path=fshader, + dependencies=[ + ShaderStepBufferDependency( + buffer_id=b_id, script_uniform_name=b_id.value + ) + for b_id in observation_buffers + ], + priority=40, + height=100, + width=100, + ) + + camera = renderer.camera_for_id(camera_id=camera_id) + camera.update(Pose.from_center(np.array([0, 0, 0]), Heading(0)), 10) + camera.update(observation=large_observation) + + with pytest.warns(RendererNotSetUpWarning): + renderer.render() + + ram_image = camera.wait_for_ram_image("RGB") + mem_view = memoryview(ram_image) + image: np.ndarray = np.frombuffer(mem_view, np.uint8)[::3] + + assert image[0] == 0 diff --git a/smarts/core/utils/adapters/ego_centric_adapters.py b/smarts/core/utils/adapters/ego_centric_adapters.py index f10b0bc1d1..2ab0f1be9b 100644 --- a/smarts/core/utils/adapters/ego_centric_adapters.py +++ b/smarts/core/utils/adapters/ego_centric_adapters.py @@ -55,7 +55,7 @@ def adjust_heading(h): wpps = obs.waypoint_paths or [] def _replace_via(via: Union[Via, ViaPoint]): - return _replace(via, position=transform(via.position)) + return _replace(via, position=transform(via.position + (0.0,))) vd = None if obs.via_data: @@ -63,7 +63,6 @@ def _replace_via(via: Union[Via, ViaPoint]): vd = _replace( obs.via_data, near_via_points=rpvp(obs.via_data.near_via_points), - hit_via_points=rpvp(obs.via_data.hit_via_points), ) replace_wps = lambda lwps: [ [ diff --git a/smarts/core/utils/dummy.py b/smarts/core/utils/dummy.py index b714dcc78a..f9d2cf83b1 100644 --- a/smarts/core/utils/dummy.py +++ b/smarts/core/utils/dummy.py @@ -26,7 +26,7 @@ import numpy as np import smarts.sstudio.sstypes as t -from smarts.core.coordinates import Dimensions, Heading, RefLinePoint +from smarts.core.coordinates import Dimensions, Heading, Point, RefLinePoint from smarts.core.events import Events from smarts.core.observations import ( DrivableAreaGridMap, @@ -35,12 +35,15 @@ Observation, OccupancyGridMap, RoadWaypoints, + SignalObservation, TopDownRGB, VehicleObservation, + ViaPoint, Vias, ) from smarts.core.plan import EndlessGoal, Mission, Start from smarts.core.road_map import Waypoint +from smarts.core.signals import SignalLightState def dummy_observation() -> Observation: @@ -50,7 +53,7 @@ def dummy_observation() -> Observation: step_count=1, elapsed_sim_time=0.2, events=Events( - collisions=[], + collisions=(2,), off_road=False, off_route=False, on_shoulder=False, @@ -99,7 +102,7 @@ def dummy_observation() -> Observation: angular_jerk=(0.0, 0.0, 0.0), ), under_this_agent_control=True, - neighborhood_vehicle_states=[ + neighborhood_vehicle_states=( VehicleObservation( id="car-west_0_0-east_0_max-784511-726648-0-0.0", position=(-1.33354215, -3.2, 0.0), @@ -122,7 +125,7 @@ def dummy_observation() -> Observation: lane_index=1, lane_position=RefLinePoint(-1.47159011, 0.0, 0.0), ), - ], + ), waypoint_paths=[ [ Waypoint( @@ -316,6 +319,9 @@ def dummy_observation() -> Observation: ], } ), - via_data=Vias(near_via_points=[]), + signals=( + SignalObservation(SignalLightState.GO, Point(181.0, 0.0), ("east_1",), None) + ), + via_data=Vias(near_via_points=(ViaPoint((181.0, 0.0), 1, "east", 5.0, False),)), steps_completed=4, ) diff --git a/smarts/env/utils/observation_conversion.py b/smarts/env/utils/observation_conversion.py index f164f91fef..0aaaf2384d 100644 --- a/smarts/env/utils/observation_conversion.py +++ b/smarts/env/utils/observation_conversion.py @@ -183,7 +183,7 @@ def _format_signals(signals: List[SignalObservation]): def _format_neighborhood_vehicle_states( - neighborhood_vehicle_states: List[VehicleObservation], + neighborhood_vehicle_states: Tuple[VehicleObservation], ): des_shp = _NEIGHBOR_SHP rcv_shp = len(neighborhood_vehicle_states) diff --git a/smarts/p3d/renderer.py b/smarts/p3d/renderer.py index cc80f60d22..4d997124bf 100644 --- a/smarts/p3d/renderer.py +++ b/smarts/p3d/renderer.py @@ -178,7 +178,8 @@ def set_rendering_backend( if "__it__" not in cls.__dict__: cls._rendering_backend = rendering_backend else: - warnings.warn("Cannot apply rendering backend after setup.") + if cls._rendering_backend != rendering_backend: + warnings.warn("Cannot apply rendering backend after setup.") def destroy(self): """Destroy this renderer and clean up all remaining resources.""" @@ -330,6 +331,7 @@ class _BufferAccessor: BufferID.ROAD_WAYPOINTS_LANE_OFFSET, BufferID.ROAD_WAYPOINTS_LANE_WIDTH, BufferID.ROAD_WAYPOINTS_LANE_INDEX, + BufferID.ROAD_WAYPOINTS_SPEED_LIMIT, } _via_near_set = { BufferID.VIA_DATA_NEAR_VIA_POINTS_HIT, @@ -345,7 +347,7 @@ class _BufferAccessor: BufferID.LIDAR_POINT_CLOUD_POINTS, } _signals_set = { - BufferID.SIGNALS_CONTROLLED_LANES, + # BufferID.SIGNALS_CONTROLLED_LANES, BufferID.SIGNALS_LAST_CHANGED, BufferID.SIGNALS_LIGHT_STATE, BufferID.SIGNALS_STOP_POINT, @@ -419,15 +421,18 @@ def _waypoint_paths_flattened(self, o: Observation): def _gen_methods_for_buffer(cls): cls._static_methods[BufferID.DELTA_TIME] = lambda o, m: o.dt cls._static_methods[BufferID.ELAPSED_SIM_TIME] = lambda o, m: o.elapsed_sim_time - cls._static_methods[ - BufferID.EGO_VEHICLE_STATE_HEADING - ] = lambda o, m: o.ego_vehicle_state.heading + cls._static_methods[BufferID.EGO_VEHICLE_STATE_HEADING] = lambda o, m: float( + o.ego_vehicle_state.heading + ) cls._static_methods[ BufferID.EGO_VEHICLE_STATE_SPEED ] = lambda o, m: o.ego_vehicle_state.speed cls._static_methods[ BufferID.EGO_VEHICLE_STATE_YAW_RATE ] = lambda o, m: o.ego_vehicle_state.yaw_rate + cls._static_methods[ + BufferID.EGO_VEHICLE_STATE_STEERING + ] = lambda o, m: o.ego_vehicle_state.steering cls._static_methods[ BufferID.EGO_VEHICLE_STATE_LANE_INDEX ] = lambda o, m: o.ego_vehicle_state.yaw_rate @@ -443,6 +448,9 @@ def _gen_methods_for_buffer(cls): else -1 ) + cls._static_methods[BufferID.EVENTS_COLLISIONS] = lambda o, m: len( + o.events.collisions + ) cls._static_methods[BufferID.EVENTS_OFF_ROAD] = lambda o, m: int( o.events.off_road ) @@ -639,7 +647,13 @@ class P3DShaderStep(_P3DCameraMixin, ShaderStep): fullscreen_quad_node: NodePath - def update(self, pose: Pose = None, height: float = None, **kwargs): + def update( + self, + pose: Pose = None, + height: float = None, + observation: Optional[Observation] = None, + **kwargs, + ): """Update the location of the shader directional values. Args: pose: @@ -648,7 +662,7 @@ def update(self, pose: Pose = None, height: float = None, **kwargs): The height of the camera above the camera target. """ inputs = {} - if pose: + if pose is not None: self.fullscreen_quad_node.setShaderInputs( iHeading=pose.heading, iTranslation=(pose.point.x, pose.point.y), @@ -659,14 +673,13 @@ def update(self, pose: Pose = None, height: float = None, **kwargs): # self.fullscreen_quad_node.setShaderInput( # "iTranslation", n1=pose.point.x, n2=pose.point.y # ) - if height: + if height is not None: inputs["iElevation"] = height if len(self.buffer_dependencies) == 0: return buffers = set(self.buffer_dependencies) - if observation := kwargs.get("observation"): - observation: Observation + if observation is not None: ba = _BufferAccessor() for b in buffers: if ba.should_get_data(b.buffer_id, observation): @@ -852,10 +865,12 @@ def render(self): def reset(self): """Reset the render back to initialized state.""" - self._vehicles_np.removeNode() - self._vehicles_np = self._root_np.attachNewNode("vehicles") - self._signals_np.removeNode() - self._signals_np = self._root_np.attachNewNode("signals") + if self._vehicles_np is not None: + self._vehicles_np.removeNode() + self._vehicles_np = self._root_np.attachNewNode("vehicles") + if self._signals_np is not None: + self._signals_np.removeNode() + self._signals_np = self._root_np.attachNewNode("signals") self._vehicle_nodes = {} self._signal_nodes = {} @@ -1245,7 +1260,6 @@ def _set_shader_inputs( BufferID.EGO_VEHICLE_STATE_SPEED, BufferID.EGO_VEHICLE_STATE_STEERING, BufferID.EGO_VEHICLE_STATE_YAW_RATE, - BufferID.EGO_VEHICLE_STATE_LANE_INDEX, BufferID.DISTANCE_TRAVELLED, ): initial_value = float() @@ -1256,6 +1270,7 @@ def _set_shader_inputs( ): initial_value = int() elif bn in ( + BufferID.EVENTS_COLLISIONS, BufferID.EVENTS_OFF_ROAD, BufferID.EVENTS_OFF_ROUTE, BufferID.EVENTS_ON_SHOULDER, @@ -1286,6 +1301,7 @@ def _set_shader_inputs( elif bn in ( BufferID.EGO_VEHICLE_STATE_ROAD_ID, BufferID.EGO_VEHICLE_STATE_LANE_ID, + BufferID.EGO_VEHICLE_STATE_LANE_INDEX, ): initial_value = int() @@ -1397,10 +1413,9 @@ def _set_shader_inputs( initial_value = [ int(), ] - elif bn in ( - BufferID.SIGNALS_STOP_POINT, - BufferID.SIGNALS_LAST_CHANGED, - ): + elif bn in (BufferID.SIGNALS_STOP_POINT,): + initial_value = [float(), float()] + elif bn in (BufferID.SIGNALS_LAST_CHANGED,): initial_value = [ float(), ]