diff --git a/nocturne/cpp/src/simulation.cc b/nocturne/cpp/src/simulation.cc index bc7c6c22..ae305c74 100644 --- a/nocturne/cpp/src/simulation.cc +++ b/nocturne/cpp/src/simulation.cc @@ -14,7 +14,8 @@ void Simulation::Render() { constexpr int64_t kWinHeight = 800; sf::ContextSettings settings; - settings.antialiasingLevel = std::min(sf::RenderTexture::getMaximumAntialiasingLevel(), 4u); + settings.antialiasingLevel = + std::min(sf::RenderTexture::getMaximumAntialiasingLevel(), 4u); render_window_ = std::make_unique( sf::VideoMode(kWinWidth, kWinHeight), "Nocturne", sf::Style::Default, settings); @@ -35,7 +36,7 @@ void Simulation::Render() { // draw scenario render_window_->draw(*scenario_); - + // draw frames per seconds on screen sf::Time elapsed = clock_.restart(); float fps = 1.0f / elapsed.asSeconds(); diff --git a/nocturne/pybind11/src/scenario.cc b/nocturne/pybind11/src/scenario.cc index f0891165..fb206045 100644 --- a/nocturne/pybind11/src/scenario.cc +++ b/nocturne/pybind11/src/scenario.cc @@ -40,28 +40,44 @@ void DefineScenario(py::module& m) { .def("objects", &Scenario::objects, py::return_value_policy::reference) .def("moving_objects", &Scenario::moving_objects, py::return_value_policy::reference) - .def("remove_object", &Scenario::RemoveObject) .def("road_lines", &Scenario::road_lines) - + .def("remove_object", &Scenario::RemoveObject, + py::call_guard()) .def("ego_state", [](const Scenario& scenario, const Object& src) { - return utils::AsNumpyArray(scenario.EgoState(src)); + NdArray ret; + { + py::gil_scoped_release release; + ret = scenario.EgoState(src); + } + return utils::AsNumpyArray(std::move(ret)); }) .def( "visible_state", [](const Scenario& scenario, const Object& src, float view_dist, float view_angle, float head_angle, bool padding) { - return utils::AsNumpyArrayDict( - scenario.VisibleState(src, view_dist, view_angle, head_angle, padding)); + std::unordered_map> ret; + { + py::gil_scoped_release release; + ret = scenario.VisibleState(src, view_dist, view_angle, + head_angle, padding); + } + return utils::AsNumpyArrayDict(std::move(ret)); }, py::arg("object"), py::arg("view_dist") = 60, - py::arg("view_angle") = kHalfPi, py::arg("head_angle") = 0.0, py::arg("padding") = false) + py::arg("view_angle") = kHalfPi, py::arg("head_angle") = 0.0, + py::arg("padding") = false) .def( "flattened_visible_state", [](const Scenario& scenario, const Object& src, float view_dist, float view_angle, float head_angle) { - return utils::AsNumpyArray(scenario.FlattenedVisibleState( - src, view_dist, view_angle, head_angle)); + NdArray ret; + { + py::gil_scoped_release release; + ret = scenario.FlattenedVisibleState(src, view_dist, view_angle, + head_angle); + } + return utils::AsNumpyArray(std::move(ret)); }, py::arg("object"), py::arg("view_dist") = 60, py::arg("view_angle") = kHalfPi, py::arg("head_angle") = 0.0) @@ -71,6 +87,69 @@ void DefineScenario(py::module& m) { .def("expert_action", &Scenario::ExpertAction) .def("expert_pos_shift", &Scenario::ExpertPosShift) .def("expert_heading_shift", &Scenario::ExpertHeadingShift) + .def( + "image", + [](Scenario& scenario, uint64_t img_height, uint64_t img_width, + bool draw_target_positions, float padding, Object* source, + uint64_t view_height, uint64_t view_width, + bool rotate_with_source) { + NdArray ret; + { + py::gil_scoped_release release; + ret = scenario.Image(img_height, img_width, draw_target_positions, + padding, source, view_height, view_width, + rotate_with_source); + } + return utils::AsNumpyArray(std::move(ret)); + }, + "Return a numpy array of dimension (img_height, img_width, 4) " + "representing an image of the scene.", + py::arg("img_height") = 1000, py::arg("img_width") = 1000, + py::arg("draw_target_positions") = true, py::arg("padding") = 50.0f, + py::arg("source") = nullptr, py::arg("view_height") = 200, + py::arg("view_width") = 200, py::arg("rotate_with_source") = true) + .def( + "feature_image", + [](Scenario& scenario, const Object& source, float view_dist, + float view_angle, float head_angle, uint64_t img_height, + uint64_t img_width, float padding, bool draw_target_position) { + NdArray ret; + { + py::gil_scoped_release release; + ret = scenario.EgoVehicleFeaturesImage( + source, view_dist, view_angle, head_angle, img_height, + img_width, padding, draw_target_position); + } + return utils::AsNumpyArray(std::move(ret)); + }, + "Return a numpy array of dimension (img_height, img_width, 4) " + "representing an image of what is returned by getVisibleState(?).", + py::arg("source"), py::arg("view_dist") = 120.0f, + py::arg("view_angle") = geometry::utils::kPi * 0.8f, + py::arg("head_angle") = 0.0f, py::arg("img_height") = 1000, + py::arg("img_width") = 1000, py::arg("padding") = 0.0f, + py::arg("draw_target_position") = true) + .def( + "cone_image", + [](Scenario& scenario, const Object& source, float view_dist, + float view_angle, float head_angle, uint64_t img_height, + uint64_t img_width, float padding, bool draw_target_position) { + NdArray ret; + { + py::gil_scoped_release release; + ret = scenario.EgoVehicleConeImage( + source, view_dist, view_angle, head_angle, img_height, + img_width, padding, draw_target_position); + } + return utils::AsNumpyArray(std::move(ret)); + }, + "Return a numpy array of dimension (img_height, img_width, 4) " + "representing a cone of what the agent sees.", + py::arg("source"), py::arg("view_dist") = 120.0f, + py::arg("view_angle") = geometry::utils::kPi * 0.8f, + py::arg("head_angle") = 0.0f, py::arg("img_height") = 1000, + py::arg("img_width") = 1000, py::arg("padding") = 0.0f, + py::arg("draw_target_position") = true) // TODO: Deprecate the legacy interfaces below. .def("getVehicles", &Scenario::vehicles, @@ -89,9 +168,14 @@ void DefineScenario(py::module& m) { bool draw_target_positions, float padding, Object* source, uint64_t view_height, uint64_t view_width, bool rotate_with_source) { - return utils::AsNumpyArray(scenario.Image( - img_height, img_width, draw_target_positions, padding, source, - view_height, view_width, rotate_with_source)); + NdArray ret; + { + py::gil_scoped_release release; + ret = scenario.Image(img_height, img_width, draw_target_positions, + padding, source, view_height, view_width, + rotate_with_source); + } + return utils::AsNumpyArray(std::move(ret)); }, "Return a numpy array of dimension (img_height, img_width, 4) " "representing an image of the scene.", @@ -104,10 +188,14 @@ void DefineScenario(py::module& m) { [](Scenario& scenario, const Object& source, float view_dist, float view_angle, float head_angle, uint64_t img_height, uint64_t img_width, float padding, bool draw_target_position) { - return utils::AsNumpyArray( - scenario.EgoVehicleFeaturesImage( - source, view_dist, view_angle, head_angle, img_height, - img_width, padding, draw_target_position)); + NdArray ret; + { + py::gil_scoped_release release; + ret = scenario.EgoVehicleFeaturesImage( + source, view_dist, view_angle, head_angle, img_height, + img_width, padding, draw_target_position); + } + return utils::AsNumpyArray(std::move(ret)); }, "Return a numpy array of dimension (img_height, img_width, 4) " "representing an image of what is returned by getVisibleState(?).", @@ -121,10 +209,14 @@ void DefineScenario(py::module& m) { [](Scenario& scenario, const Object& source, float view_dist, float view_angle, float head_angle, uint64_t img_height, uint64_t img_width, float padding, bool draw_target_position) { - return utils::AsNumpyArray( - scenario.EgoVehicleConeImage(source, view_dist, view_angle, - head_angle, img_height, img_width, - padding, draw_target_position)); + NdArray ret; + { + py::gil_scoped_release release; + ret = scenario.EgoVehicleConeImage( + source, view_dist, view_angle, head_angle, img_height, + img_width, padding, draw_target_position); + } + return utils::AsNumpyArray(std::move(ret)); }, "Return a numpy array of dimension (img_height, img_width, 4) " "representing a cone of what the agent sees.", diff --git a/nocturne/pybind11/src/simulation.cc b/nocturne/pybind11/src/simulation.cc index ad0eb4e5..bff13e12 100644 --- a/nocturne/pybind11/src/simulation.cc +++ b/nocturne/pybind11/src/simulation.cc @@ -25,11 +25,12 @@ void DefineSimulation(py::module& m) { py::arg("config") = std::unordered_map>()) - .def("reset", &Simulation::Reset) - .def("step", &Simulation::Step) - .def("render", &Simulation::Render) + .def("reset", &Simulation::Reset, + py::call_guard()) + .def("step", &Simulation::Step, py::call_guard()) .def("scenario", &Simulation::GetScenario, py::return_value_policy::reference) + .def("render", &Simulation::Render) .def("save_screenshot", &Simulation::SaveScreenshot) // TODO: Deprecate the legacy methods below. diff --git a/tests/test_dynamics.py b/tests/test_dynamics.py index 431696f3..6fc9a5b9 100644 --- a/tests/test_dynamics.py +++ b/tests/test_dynamics.py @@ -38,7 +38,7 @@ def test_inverse_dynamics(): id2obj_ground_truth = {obj.id: obj for obj in objects_ground_truth} # create a test sim that will replay actions from inverse dynamics sim_test, objects_test = _create_sim(file_path, expert_control=False) - scenario_test = sim_test.getScenario() + scenario_test = sim_test.scenario() # step simulation for time in range(SIM_N_STEPS): diff --git a/tests/test_rl_env.py b/tests/test_rl_env.py index 2e4ab5fb..77f24e01 100644 --- a/tests/test_rl_env.py +++ b/tests/test_rl_env.py @@ -28,15 +28,15 @@ def test_rl_env(): times = [] _ = env.reset() # quick check that rendering works - _ = env.scenario.getConeImage(env.scenario.getVehicles()[0], - 80.0, - 120 * (np.pi / 180), - 0.0, - draw_target_position=False) + _ = env.scenario.cone_image(env.scenario.vehicles()[0], + 80.0, + 120 * (np.pi / 180), + 0.0, + draw_target_position=False) for _ in range(90): - vehs = env.scenario.getObjectsThatMoved() + vehs = env.scenario.moving_objects() prev_position = { - veh.getID(): [veh.position.x, veh.position.y] + veh.id: [veh.position.x, veh.position.y] for veh in vehs } t = time.perf_counter() @@ -45,7 +45,7 @@ def test_rl_env(): for veh in vehs}) times.append(time.perf_counter() - t) for veh in vehs: - if veh in env.scenario.getObjectsThatMoved(): + if veh in env.scenario.moving_objects(): new_position = [veh.position.x, veh.position.y] assert prev_position[veh.getID( )] != new_position, f'veh {veh.getID()} was in position \ diff --git a/tests/test_simulation_functions.py b/tests/test_simulation_functions.py index d9f2a693..1226ab57 100644 --- a/tests/test_simulation_functions.py +++ b/tests/test_simulation_functions.py @@ -27,42 +27,42 @@ def test_scenario_functions(): # now lets test for collisions # grab a vehicle and place it on top of another vehicle sim = Simulation(scenario_path=file_path, config=get_scenario_dict(cfg)) - scenario = sim.getScenario() - veh0 = scenario.getVehicles()[0] - veh1 = scenario.getVehicles()[1] - veh2 = scenario.getVehicles()[2] + scenario = sim.scenario() + veh0 = scenario.vehicles()[0] + veh1 = scenario.vehicles()[1] + veh2 = scenario.vehicles()[2] # TODO(ev this fails unless the shift is non-zero) - veh1.setPosition(veh0.getPosition().x + 0.001, veh0.getPosition().y) + veh1.set_position(veh0.position.x + 0.001, veh0.position.y) sim.step(0.000001) - assert veh1.getCollided( - ), 'vehicle1 should have collided after being placed on vehicle 0' - assert veh0.getCollided( - ), 'vehicle0 should have collided after vehicle 0 was placed on it' - assert not veh2.getCollided(), 'vehicle2 should not have collided' + assert veh0.collided, \ + 'vehicle0 should have collided after vehicle 0 was placed on it' + assert veh1.collided, \ + 'vehicle1 should have collided after being placed on vehicle 0' + assert not veh2.collided, 'vehicle2 should not have collided' # confirm that this is still true a time-step later sim.step(0.000001) - assert veh1.getCollided( - ), 'vehicle1 should have collided after being placed on vehicle 0' - assert veh0.getCollided( - ), 'vehicle0 should have collided after vehicle 0 was placed on it' - assert not veh2.getCollided(), 'vehicle2 should not have collided' + assert veh0.collided, \ + 'vehicle0 should have collided after vehicle 0 was placed on it' + assert veh1.collided, \ + 'vehicle1 should have collided after being placed on vehicle 0' + assert not veh2.collided, 'vehicle2 should not have collided' # now offset them slightly and do the same thing again sim = Simulation(scenario_path=file_path, config=get_scenario_dict(cfg)) - scenario = sim.getScenario() - veh0 = scenario.getVehicles()[0] - veh1 = scenario.getVehicles()[1] - veh2 = scenario.getVehicles()[2] - veh0 = scenario.getVehicles()[0] - veh1 = scenario.getVehicles()[1] - veh1.setPosition(veh0.getPosition().x + 0.2, veh0.getPosition().y + 0.2) + scenario = sim.scenario() + veh0 = scenario.vehicles()[0] + veh1 = scenario.vehicles()[1] + veh2 = scenario.vehicles()[2] + veh0 = scenario.vehicles()[0] + veh1 = scenario.vehicles()[1] + veh1.set_position(veh0.position.x + 0.2, veh0.position.y + 0.2) sim.step(0.000001) - assert veh1.getCollided( - ), 'vehicle1 should have collided after being placed overlapping vehicle 0' - assert veh0.getCollided( - ), 'vehicle0 should have collided after vehicle 1 was placed on it' - assert not veh2.getCollided(), 'vehicle2 should not have collided' + assert veh0.collided, \ + 'vehicle0 should have collided after vehicle 1 was placed on it' + assert veh1.collided, \ + 'vehicle1 should have collided after being placed overlapping vehicle 0' + assert not veh2.collided, 'vehicle2 should not have collided' ################################ # Road Collision checking @@ -71,11 +71,11 @@ def test_scenario_functions(): print('entering road line - vehicle collision checking') # find a road edge colliding_road_line = None - for roadline in scenario.getRoadLines(): - if roadline.canCollide(): + for roadline in scenario.road_lines(): + if roadline.check_collision: colliding_road_line = roadline break - roadpoints = colliding_road_line.getGeometry() + roadpoints = colliding_road_line.geometry_points() start_point = np.array([roadpoints[0].x, roadpoints[0].y]) road_segment_dir = np.array([roadpoints[1].x, roadpoints[1].y]) - np.array( [roadpoints[0].x, roadpoints[0].y]) @@ -83,30 +83,31 @@ def test_scenario_functions(): road_segment_dir) < 1 # it should be able to fit inside the vehicle road_segment_angle = np.arctan2( road_segment_dir[1], road_segment_dir[0]) # atan2 is (y, x) not (x,y) - veh0.setHeading(road_segment_angle) + veh0.heading = road_segment_angle # place the vehicle so that the segment is contained inside of it new_center = start_point + 0.5 * road_segment_dir - veh0.setPosition(new_center[0], new_center[1]) + veh0.set_position(new_center[0], new_center[1]) sim.step(1e-6) - cone = scenario.getConeImage(veh0, view_angle=2 * np.pi, head_angle=0.0) + cone = scenario.cone_image(veh0, view_angle=2 * np.pi, head_angle=0.0) plt.figure() plt.imshow(cone) plt.savefig('line_veh_check.png') - assert veh0.getCollided( - ), 'vehicle0 should have collided after a road edge is placed inside it' + assert veh0.collided, \ + 'vehicle0 should have collided after a road edge is placed inside it' - # place the vehicle on one of the points so that the road segment intersects with a vehicle edge + # place the vehicle on one of the points so that the road segment intersects + # with a vehicle edge sim.reset() - scenario = sim.getScenario() - veh0 = scenario.getVehicles()[0] - veh0.setHeading(road_segment_angle) + scenario = sim.scenario() + veh0 = scenario.vehicles()[0] + veh0.heading = road_segment_angle veh_length = veh0.length new_center += veh_length / 2 * road_segment_dir - veh0.setPosition(new_center[0], new_center[1]) + veh0.set_position(new_center[0], new_center[1]) sim.step(1e-6) - assert veh0.getCollided( - ), 'vehicle0 should have collided since a road edge intersects it' + assert veh0.collided, \ + 'vehicle0 should have collided since a road edge intersects it' ###################### # Waymo Scene Construction @@ -115,44 +116,59 @@ def test_scenario_functions(): # image cfg['scenario'].update({'start_time': 20}) sim = Simulation(scenario_path=file_path, config=get_scenario_dict(cfg)) - scenario = sim.getScenario() + scenario = sim.scenario() - img1 = scenario.getConeImage(scenario.getVehicles()[4], 120.0, 2 * np.pi, - 0.0) + img1 = scenario.cone_image(scenario.vehicles()[4], 120.0, 2 * np.pi, 0.0) - # check that initializing things with and without pedestrians leads to a different - # image + # check that initializing things with and without pedestrians leads to a + # different image cfg['scenario'].update({'start_time': 20, 'allow_non_vehicles': False}) sim = Simulation(scenario_path=file_path, config=get_scenario_dict(cfg)) - scenario = sim.getScenario() + scenario = sim.scenario() - img2 = scenario.getConeImage(scenario.getVehicles()[4], 120.0, 2 * np.pi, - 0.0) + img2 = scenario.cone_image(scenario.getVehicles()[4], 120.0, 2 * np.pi, + 0.0) assert not np.isclose(np.sum(img1 - img2), 0.0), 'adding pedestrians should change the image' # check a variety of nocturne functions - _ = scenario.getPedestrians() - _ = scenario.getCyclists() + _ = scenario.pedestrians() + _ = scenario.cyclists() - # check that the padding function for visible state is returning the right thing. - visible_dict = scenario.visible_state(object=scenario.getVehicles()[0], + # check that the padding function for visible state is returning the right + # thing. + visible_dict = scenario.visible_state(object=scenario.vehicles()[0], view_dist=80, view_angle=120 * (np.pi / 180), padding=True) scenario_cfg = cfg['scenario'] - assert scenario_cfg['max_visible_objects'] == visible_dict['objects'].shape[0], \ - 'visible dict padding returned {} objects but should have been \ - {}'.format(visible_dict['objects'].shape[0], scenario_cfg['max_visible_objects']) - assert scenario_cfg['max_visible_road_points'] == visible_dict['road_points'].shape[0], \ - 'visible dict padding returned {} objects but should have been \ - {}'.format(visible_dict['road_points'].shape[0], scenario_cfg['max_visible_road_points']) - assert scenario_cfg['max_visible_traffic_lights'] == visible_dict['traffic_lights'].shape[0], \ - 'visible dict padding returned {} objects but should have been \ - {}'.format(visible_dict['traffic_lights'].shape[0], scenario_cfg['max_visible_traffic_lights']) - assert scenario_cfg['max_visible_stop_signs'] == visible_dict['stop_signs'].shape[0], \ - 'visible dict padding returned {} objects but should have been \ - {}'.format(visible_dict['stop_signs'].shape[0], scenario_cfg['max_visible_stop_signs']) + assert ( + scenario_cfg['max_visible_objects'] == visible_dict['objects'].shape[0] + ), \ + f"visible dict padding returned {visible_dict['objects'].shape[0]} " + \ + f"objects but should have been {scenario_cfg['max_visible_objects']}" + assert ( + scenario_cfg['max_visible_road_points'] == + visible_dict['road_points'].shape[0] + ), \ + f"visible dict padding returned {visible_dict['road_points']} " + \ + "road_points but should have been " + \ + str(scenario_cfg['max_visible_road_points']) + assert ( + scenario_cfg['max_visible_traffic_lights'] == + visible_dict['traffic_lights'].shape[0] + ), \ + f"visible dict padding returned {visible_dict['traffic_lights']} " + \ + "traffic_lights but should have been " + \ + str(scenario_cfg['max_visible_traffic_lights']) + + assert ( + scenario_cfg['max_visible_stop_signs'] == + visible_dict['stop_signs'].shape[0] + ), \ + f"visible dict padding returned {visible_dict['stop_signs']} " + \ + "stop_signs but should have been " + \ + str(scenario_cfg['max_visible_stop_signs']) def main():