Skip to content
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 2 additions & 0 deletions CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -1,6 +1,8 @@
cmake_minimum_required(VERSION 3.18 FATAL_ERROR)
cmake_policy(VERSION 3.18)

set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -w")

include("${CMAKE_CURRENT_SOURCE_DIR}/external/madrona/cmake/madrona_init.cmake")

project(Madrona3DExample LANGUAGES C CXX)
Expand Down
6 changes: 6 additions & 0 deletions external/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -1,3 +1,9 @@
if(CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-w)
endif()

set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -w")

set(MADRONA_ENABLE_TESTS ON)
add_subdirectory("${MADRONA_DIR}" madrona EXCLUDE_FROM_ALL)
add_subdirectory(json)
Expand Down
8 changes: 4 additions & 4 deletions src/json_serialization.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -234,7 +234,7 @@ namespace gpudrive
road.mapType = MapType::UNKNOWN;
}

for (int i = 0; i < road.numPoints; i++)
for (uint32_t i = 0; i < road.numPoints; i++)
{
road.mean.x += (road.geometry[i].x - road.mean.x)/(i+1);
road.mean.y += (road.geometry[i].y - road.mean.y)/(i+1);
Expand Down Expand Up @@ -297,8 +297,8 @@ namespace gpudrive
const auto& metadata = j.at("metadata");

// Set SDC
int sdc_index = metadata.at("sdc_track_index").get<int>();
if (sdc_index < 0 || sdc_index >= j.at("objects").size()) {
size_t sdc_index = metadata.at("sdc_track_index").get<int>();
if (sdc_index >= j.at("objects").size()) {
std::cerr << "Warning: Invalid sdc_track_index " << sdc_index << " in scene " << j.at("name").get<std::string>() << std::endl;
} else {
int sdc_id = j.at("objects")[sdc_index].at("id").get<int>();
Expand All @@ -318,7 +318,7 @@ namespace gpudrive
// Set tracks to predict
for (const auto& track : metadata.at("tracks_to_predict")) {
int track_index = track.at("track_index").get<int>();
if (track_index < 0 || track_index >= j.at("objects").size()) {
if (track_index < 0 || static_cast<size_t>(track_index) >= j.at("objects").size()) {
std::cerr << "Warning: Invalid track_index " << track_index << " in scene " << j.at("name").get<std::string>() << std::endl;
} else {
int track_id = j.at("objects")[track_index].at("id").get<int>();
Expand Down
8 changes: 6 additions & 2 deletions src/knn.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -23,7 +23,9 @@ void fillZeros(gpudrive::MapObservation *begin,
gpudrive::MapObservation{.position = {0, 0},
.scale = madrona::math::Diag3x3{0, 0, 0},
.heading = 0.f,
.type = (float)gpudrive::EntityType::None};
.type = (float)gpudrive::EntityType::None,
.id = 0.f,
.mapType = (float)gpudrive::MapType::UNKNOWN};
}
}

Expand All @@ -42,7 +44,9 @@ relativeObservation(const gpudrive::MapObservation &absoluteObservation,
.xy(),
.scale = absoluteObservation.scale,
.heading = gpudrive::utils::quatToYaw(referenceRotation.inv() * madrona::math::Quat::angleAxis(absoluteObservation.heading,madrona::math::up)),
.type = absoluteObservation.type};
.type = absoluteObservation.type,
.id = absoluteObservation.id,
.mapType = absoluteObservation.mapType};
}


Expand Down
10 changes: 0 additions & 10 deletions src/level_gen.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -280,16 +280,6 @@ static inline void createRoadEntities(Engine &ctx, const MapRoad &roadInit, Coun
}
}

static void createFloorPlane(Engine &ctx)
{
ctx.data().floorPlane = ctx.makeRenderableEntity<PhysicsEntity>();
setRoadEntitiesProps(ctx, ctx.data().floorPlane, Vector3{.x = 0, .y = 0, .z = 0},
Quat::angleAxis(0, madrona::math::up),
Diag3x3{.d0 = 100, .d1 = 100, .d2 = 0.1},
EntityType::None, ObjectID{(int32_t)SimObject::Plane}, ResponseType::Static, 0, MapType::UNKNOWN);
registerRigidBodyEntity(ctx, ctx.data().floorPlane, SimObject::Plane);
}

void createPaddingEntities(Engine &ctx) {
for (CountT agentIdx = ctx.data().numAgents;
agentIdx < consts::kMaxAgentCount; ++agentIdx) {
Expand Down
5 changes: 5 additions & 0 deletions src/mgr.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -345,10 +345,15 @@ static void loadPhysicsObjects(PhysicsLoader &loader)
.muD = 0.5f,
});

#pragma clang diagnostic push
#pragma clang diagnostic ignored "-Wmissing-field-initializers"

SourceCollisionPrimitive plane_prim {
.type = CollisionPrimitive::Type::Plane,
};

#pragma clang diagnostic pop

src_objs[(CountT)SimObject::Plane] = {
.prims = Span<const SourceCollisionPrimitive>(&plane_prim, 1),
.invMass = 0.f,
Expand Down
6 changes: 2 additions & 4 deletions src/sim.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -375,8 +375,7 @@ static inline float encodeType(EntityType type)
// This system is specially optimized in the GPU version:
// a warp of threads is dispatched for each invocation of the system
// and each thread in the warp traces one lidar ray for the agent.
inline void lidarSystem(Engine &ctx, Entity e, const AgentInterfaceEntity &agent_iface,
EntityType &entityType) {
inline void lidarSystem(Engine &ctx, Entity e, const AgentInterfaceEntity &agent_iface) {
Lidar &lidar = ctx.get<Lidar>(agent_iface.e);
const Action &action = ctx.get<Action>(agent_iface.e);

Expand Down Expand Up @@ -789,8 +788,7 @@ void setupRestOfTasks(TaskGraphBuilder &builder, const Sim::Config &cfg,
lidarSystem,
#endif
Entity,
AgentInterfaceEntity,
EntityType
AgentInterfaceEntity
>>({clear_tmp});
}

Expand Down
2 changes: 1 addition & 1 deletion src/utils.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -60,7 +60,7 @@ class ReferenceFrame {
return gpudrive::utils::quatToYaw(referenceRotation.inv() * absoluteRot);
}

madrona::math::Vector2 referencePosition;
madrona::base::Rotation referenceRotation;
madrona::math::Vector2 referencePosition;
};
}}
32 changes: 15 additions & 17 deletions src/viewer.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -77,7 +77,8 @@ int main(int argc, char *argv[])
.params = {
.polylineReductionThreshold = 1.0,
.observationRadius = 100.0,
.maxNumControlledAgents = 0
.rewardParams = RewardParams(),
.maxNumControlledAgents = 0,
},
.enableBatchRenderer = enable_batch_renderer,
.extRenderAPI = wm.gpuAPIManager().backend(),
Expand Down Expand Up @@ -133,28 +134,27 @@ int main(int argc, char *argv[])
auto steps_remaining_printer = mgr.stepsRemainingTensor().makePrinter();
auto reward_printer = mgr.rewardTensor().makePrinter();

auto printObs = [&]() {
printf("Self\n");
self_printer.print();
// auto printObs = [&]() {
// printf("Self\n");
// self_printer.print();

printf("Partner\n");
partner_printer.print();
// printf("Partner\n");
// partner_printer.print();

printf("Lidar\n");
lidar_printer.print();
// printf("Lidar\n");
// lidar_printer.print();

printf("Steps Remaining\n");
steps_remaining_printer.print();
// printf("Steps Remaining\n");
// steps_remaining_printer.print();

printf("Reward\n");
reward_printer.print();
// printf("Reward\n");
// reward_printer.print();

printf("\n");
};
// printf("\n");
// };

viewer.loop(
[&mgr](CountT world_idx, const Viewer::UserInput &input) {

using Key = Viewer::KeyboardKey;
if (input.keyHit(Key::R)) {
mgr.reset({(int)world_idx});
Expand All @@ -171,8 +171,6 @@ int main(int argc, char *argv[])
float acceleration{0};
const float accelerationDelta{1};

bool shift_pressed = input.keyPressed(Key::Shift);

if (input.keyPressed(Key::W)) {
acceleration += accelerationDelta;
}
Expand Down
26 changes: 12 additions & 14 deletions tests/bicyclemodel.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -24,6 +24,7 @@ class BicycleKinematicModelTest : public ::testing::Test {
.params = {
.polylineReductionThreshold = 0.0,
.observationRadius = 100.0,
.rewardParams = gpudrive::RewardParams(),
.collisionBehaviour = gpudrive::CollisionBehaviour::Ignore,
.initOnlyValidAgentsAtFirstStep = false,
.dynamicsModel = gpudrive::DynamicsModel::Classic
Expand Down Expand Up @@ -73,9 +74,6 @@ class BicycleKinematicModelTest : public ::testing::Test {
acc_distribution = std::uniform_real_distribution<float>(-3.0, 2.0);
steering_distribution = std::uniform_real_distribution<float>(-0.7, 0.7);
generator = std::default_random_engine(42);

auto shape_tensor = mgr.shapeTensor();
int32_t *ptr = static_cast<int32_t *>(shape_tensor.devicePtr());
num_agents = 1;
}
};
Expand All @@ -102,30 +100,30 @@ std::tuple<float, float, float, float> StepBicycleModel(float x, float y, float
std::pair<bool, std::string> validateBicycleModel(const py::Tensor &abs_obs, const py::Tensor &self_obs, const std::vector<float> &expected, const uint32_t num_agents)
{
int64_t num_elems = 1;
for (int i = 0; i < abs_obs.numDims(); i++)
for (int64_t i = 0; i < abs_obs.numDims(); i++)
{
num_elems *= abs_obs.dims()[i];
}

if (num_agents * gpudrive::AbsoluteSelfObservationExportSize > num_elems)
if (static_cast<int64_t>(num_agents * gpudrive::AbsoluteSelfObservationExportSize) > num_elems)
{
return {false, "Expected number of elements is less than the number of agents."};
}

num_elems = 1;
for (int i = 0; i < self_obs.numDims(); i++)
for (int64_t i = 0; i < self_obs.numDims(); i++)
{
num_elems *= self_obs.dims()[i];
}

if (num_agents * gpudrive::SelfObservationExportSize > num_elems)
if (static_cast<int64_t>(num_agents * gpudrive::SelfObservationExportSize) > num_elems)
{
return {false, "Expected number of elements is less than the number of agents."};
}

float *ptr = static_cast<float *>(abs_obs.devicePtr());

for (int64_t i = 0, agent_idx = 0; i < num_agents * gpudrive::AbsoluteSelfObservationExportSize;)
for (int64_t i = 0, agent_idx = 0; i < static_cast<int64_t>(num_agents * gpudrive::AbsoluteSelfObservationExportSize);)
{
auto x = static_cast<float>(ptr[i]);
auto y = static_cast<float>(ptr[i + 1]);
Expand All @@ -144,7 +142,7 @@ std::pair<bool, std::string> validateBicycleModel(const py::Tensor &abs_obs, con
}

ptr = static_cast<float *>(self_obs.devicePtr());
for (int64_t i = 0, agent_idx = 0; i < num_agents * gpudrive::SelfObservationExportSize;)
for (int64_t i = 0, agent_idx = 0; i < static_cast<int64_t>(num_agents * gpudrive::SelfObservationExportSize);)
{
auto speed = static_cast<float>(ptr[i]);
auto speed_exp = expected[agent_idx+3];
Expand All @@ -166,7 +164,7 @@ std::vector<float> parseBicycleModel(const py::Tensor &abs_obs, const py::Tensor
std::vector<float> obs;
obs.resize(num_agents * 4);
float *ptr = static_cast<float *>(abs_obs.devicePtr());
for (int i = 0, agent_idx = 0; i < num_agents * gpudrive::AbsoluteSelfObservationExportSize;)
for (size_t i = 0, agent_idx = 0; i < num_agents * gpudrive::AbsoluteSelfObservationExportSize;)
{
obs[agent_idx] = static_cast<float>(ptr[i]);
obs[agent_idx+1] = static_cast<float>(ptr[i+1]);
Expand All @@ -175,7 +173,7 @@ std::vector<float> parseBicycleModel(const py::Tensor &abs_obs, const py::Tensor
i+=gpudrive::AbsoluteSelfObservationExportSize;
}
ptr = static_cast<float *>(self_obs.devicePtr());
for (int i = 0, agent_idx = 0; i < num_agents * gpudrive::SelfObservationExportSize;)
for (size_t i = 0, agent_idx = 0; i < num_agents * gpudrive::SelfObservationExportSize;)
{
obs[agent_idx+3] = static_cast<float>(ptr[i]);
agent_idx += 4;
Expand All @@ -201,7 +199,7 @@ TEST_F(BicycleKinematicModelTest, TestModelEvolution) {
};
std::vector<float> expected;
//Check first step -
for(int i = 0; i < num_agents; i++)
for(uint32_t i = 0; i < num_agents; i++)
{
auto [x_next, y_next, theta_next, speed_next] = StepBicycleModel(initialState[4*i], initialState[4*i+1], initialState[4*i+2], initialState[4*i+3], 0, 0, 0.1, agent_length_map[i]);
expected.push_back(x_next);
Expand All @@ -215,13 +213,13 @@ TEST_F(BicycleKinematicModelTest, TestModelEvolution) {
ASSERT_TRUE(valid);
printObs();

for(int i = 0; i < num_steps; i++)
for(int64_t i = 0; i < num_steps; i++)
{
expected.clear();
printObs();
auto prev_state = parseBicycleModel(abs_obs, self_obs, num_agents); // Due to floating point errors, we cannot use the expected values from the previous step so as not to accumulate errors.
printVector(prev_state);
for(int j = 0; j < num_agents; j++)
for(uint32_t j = 0; j < num_agents; j++)
{
float acc = acc_distribution(generator);
float steering = steering_distribution(generator);
Expand Down
11 changes: 7 additions & 4 deletions tests/observationTest.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -24,6 +24,7 @@ class ObservationsTest : public ::testing::Test {
.params = {
.polylineReductionThreshold = 0.0,
.observationRadius = 100.0,
.rewardParams = gpudrive::RewardParams(),
.collisionBehaviour = gpudrive::CollisionBehaviour::Ignore,
.roadObservationAlgorithm = gpudrive::FindRoadObservationsWith::KNearestEntitiesWithRadiusFiltering
}
Expand All @@ -43,7 +44,6 @@ class ObservationsTest : public ::testing::Test {

std::cout<<"CTEST Mean x: "<<mean.first<<" Mean y: "<<mean.second<<std::endl;

int64_t n_roads = 0;
for (const auto &obj : rawJson["roads"]) {
std::vector<std::pair<float, float>> roadGeom;
for (const auto &point: obj["geometry"])
Expand Down Expand Up @@ -88,18 +88,21 @@ TEST_F(ObservationsTest, TestObservations) {
auto obs = mgr.mapObservationTensor();
auto flat_obs = test_utils::flatten_obs(obs);

int64_t idx = 0;
for(int64_t i = 0; i < roadGeoms.size(); i++)
size_t idx = 0;
for(size_t i = 0; i < roadGeoms.size(); i++)
{
std::vector<std::pair<float, float>> roadGeom = roadGeoms[i];
float roadType = roadTypes[i];
for(int64_t j = 0; j < roadGeom.size() - 1; j++)
for(size_t j = 0; j < roadGeom.size() - 1; j++)
{
if(roadType > (float)gpudrive::EntityType::RoadLane && roadType < (float)gpudrive::EntityType::StopSign)
{
float x = (roadGeom[j].first + roadGeom[j+1].first + roadGeom[j+2].first + roadGeom[j+3].first)/4 - mean.first;
float y = (roadGeom[j].second + roadGeom[j+1].second + roadGeom[j+2].second + roadGeom[j+3].second)/4 - mean.second;

if(idx >= flat_obs.size())
break;

ASSERT_NEAR(flat_obs[idx], x, test_utils::EPSILON);
ASSERT_NEAR(flat_obs[idx+1], y, test_utils::EPSILON);
ASSERT_EQ(flat_obs[idx+6], roadType);
Expand Down
Loading