Skip to content

Sim level z-axis integration #270

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Draft
nadarenator wants to merge 10 commits into
base: main
Choose a base branch
from
Draft

Sim level z-axis integration #270

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
10 commits
Select commit Hold shift + click to select a range
4db824d
partial changes
nadarenator Oct 15, 2024
3bd4fc2
partial changes
nadarenator Oct 15, 2024
73b9266
yet another partial change
nadarenator Nov 6, 2024
1123be5
json reading revisions
nadarenator Dec 5, 2024
f63e799
updated types and export sizes
nadarenator Dec 5, 2024
4029a93
rotation and other lvl_gen changes
nadarenator Dec 6, 2024
efc1869
Fix export sizes
aaravpandya Dec 6, 2024
0bb2bab
compiler error fixes
nadarenator Dec 6, 2024
98f82f1
Fix relative obs
aaravpandya Dec 6, 2024
a833ee2
make json z conditional
nadarenator Dec 6, 2024
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
19 changes: 10 additions & 9 deletions src/init.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -13,40 +13,41 @@ namespace gpudrive

// Cannot use Madrona::math::Vector2 because it is not a POD type.
// Getting all zeros if using any madrona types.
struct MapVector2
struct MapVector3
{
float x;
float y;
float z;
};

struct MapObject
{
MapVector2 position[MAX_POSITIONS];
MapVector3 position[MAX_POSITIONS];
float width;
float length;
float heading[MAX_POSITIONS];
MapVector2 velocity[MAX_POSITIONS];
MapVector3 velocity[MAX_POSITIONS];
bool valid[MAX_POSITIONS];
MapVector2 goalPosition;
MapVector3 goalPosition;
EntityType type;

uint32_t numPositions;
uint32_t numHeadings;
uint32_t numVelocities;
uint32_t numValid;
MapVector2 mean;
MapVector3 mean;
bool markAsExpert{false};
};

struct MapRoad
{
// std::array<MapPosition, MAX_POSITIONS> geometry;
MapVector2 geometry[MAX_GEOMETRY];
MapVector3 geometry[MAX_GEOMETRY];
uint32_t id;
MapType mapType;
EntityType type;
uint32_t numPoints;
MapVector2 mean;
MapVector3 mean;
};

struct Map
Expand All @@ -57,7 +58,7 @@ namespace gpudrive
uint32_t numObjects;
uint32_t numRoads;
uint32_t numRoadSegments;
MapVector2 mean;
MapVector3 mean;

// Constructor
Map() = default;
Expand Down Expand Up @@ -112,7 +113,7 @@ namespace gpudrive
uint32_t maxNumControlledAgents = 10000; // Arbitrary high number to by default control all vehicles
bool IgnoreNonVehicles = false; // Default: false
FindRoadObservationsWith roadObservationAlgorithm{
FindRoadObservationsWith::KNearestEntitiesWithRadiusFiltering};
FindRoadObservationsWith::AllEntitiesWithRadiusFiltering};
Copy link
Contributor

@eugenevinitsky eugenevinitsky Apr 29, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

probably do not want this change to do this?

Copy link
Contributor

@daphne-cornelisse daphne-cornelisse Apr 29, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yeah I think this branch contains a lot of changes we do not want to include. I am starting a different branch with only the minimal required changes to access the z logs

bool initOnlyValidAgentsAtFirstStep = true; // Default: true
bool isStaticAgentControlled = false; // Default: false
bool enableLidar = false;
Expand Down
64 changes: 47 additions & 17 deletions src/json_serialization.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -7,15 +7,21 @@

namespace gpudrive
{
void from_json(const nlohmann::json &j, MapVector2 &p)
void from_json(const nlohmann::json &j, MapVector3 &p)
{
p.x = j.at("x").get<float>();
p.y = j.at("y").get<float>();
if (j.contains("z")) {
Copy link
Contributor

@eugenevinitsky eugenevinitsky Apr 29, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

does it sometimes not contain it?

p.z = j.at("z").get<float>();
}
else {
p.z = 0.0;
}
}

void from_json(const nlohmann::json &j, MapObject &obj)
{
obj.mean = {0,0};
obj.mean = {0,0,0};
uint32_t i = 0;
for (const auto &pos : j.at("position"))
{
Expand All @@ -24,6 +30,7 @@ namespace gpudrive
from_json(pos, obj.position[i]);
obj.mean.x += (obj.position[i].x - obj.mean.x)/(i+1);
obj.mean.y += (obj.position[i].y - obj.mean.y)/(i+1);
obj.mean.z += (obj.position[i].z - obj.mean.z)/(i+1);
++i;
}
else
Expand Down Expand Up @@ -100,7 +107,7 @@ namespace gpudrive

void from_json(const nlohmann::json &j, MapRoad &road, float polylineReductionThreshold = 0.0)
{
road.mean = {0,0};
road.mean = {0,0,0};
std::string type = j.at("type");
if(type == "road_edge")
road.type = EntityType::RoadEdge;
Expand All @@ -118,10 +125,10 @@ namespace gpudrive
road.type = EntityType::None;


std::vector<MapVector2> geometry_points_;
std::vector<MapVector3> geometry_points_;
for(const auto &point: j.at("geometry"))
{
MapVector2 p;
MapVector3 p;
from_json(point, p);
geometry_points_.push_back(p);
}
Expand Down Expand Up @@ -154,10 +161,24 @@ namespace gpudrive
}
if (k_2 >= num_sampled_points)
break;
//Using 3D space area calculation
auto point1 = geometry_points_[k * sample_every_n_];
auto point2 = geometry_points_[k_1 * sample_every_n_];
auto point3 = geometry_points_[k_2 * sample_every_n_];
float_t area = 0.5 * std::abs((point1.x - point3.x) * (point2.y - point1.y) - (point1.x - point2.x) * (point3.y - point1.y));
// Vector 1: point1 -> point2
float vx1 = point2.x - point1.x;
float vy1 = point2.y - point1.y;
float vz1 = point2.z - point1.z;
// Vector 2: point1 -> point3
float vx2 = point3.x - point1.x;
float vy2 = point3.y - point1.y;
float vz2 = point3.z - point1.z;
// Cross product to get the normal vector
float nx = vy1 * vz2 - vz1 * vy2;
float ny = vz1 * vx2 - vx1 * vz2;
float nz = vx1 * vy2 - vy1 * vx2;
// Magnitude of the cross product
float_t area = 0.5 * std::sqrt(nx * nx + ny * ny + nz * nz);
if (area < polylineReductionThreshold)
{ // If the area is less than the threshold, then we skip the middle point
skip[k_1] = true; // Mark the middle point as skipped
Expand All @@ -175,7 +196,7 @@ namespace gpudrive
k = 0;
skip[0] = false;
skip[num_sampled_points - 1] = false;
std::vector<MapVector2> new_geometry_points; // This list stores the points that are not skipped
std::vector<MapVector3> new_geometry_points; // This list stores the points that are not skipped
while (k < num_sampled_points)
{
if (!skip[k])
Expand Down Expand Up @@ -229,14 +250,16 @@ namespace gpudrive
{
road.mean.x += (road.geometry[i].x - road.mean.x)/(i+1);
road.mean.y += (road.geometry[i].y - road.mean.y)/(i+1);
road.mean.z += (road.geometry[i].z - road.mean.z)/(i+1);
}

}

std::pair<float, float> calc_mean(const nlohmann::json &j)
std::tuple<float, float, float> calc_mean(const nlohmann::json &j)
{
std::pair<float, float> mean = {0, 0};
std::tuple<float, float, float> mean = {0, 0, 0};
int64_t numEntities = 0;

for (const auto &obj : j["objects"])
{
int i = 0;
Expand All @@ -247,32 +270,39 @@ namespace gpudrive
numEntities++;
float newX = pos["x"];
float newY = pos["y"];
float newZ = pos["z"];

// Update mean incrementally
mean.first += (newX - mean.first) / numEntities;
mean.second += (newY - mean.second) / numEntities;
std::get<0>(mean) += (newX - std::get<0>(mean)) / numEntities;
std::get<1>(mean) += (newY - std::get<1>(mean)) / numEntities;
std::get<2>(mean) += (newZ - std::get<2>(mean)) / numEntities;
}
}

for (const auto &obj : j["roads"])
{
for (const auto &point : obj["geometry"])
{
numEntities++;
float newX = point["x"];
float newY = point["y"];

// Update mean incrementally
mean.first += (newX - mean.first) / numEntities;
mean.second += (newY - mean.second) / numEntities;
float newZ = point["z"];

std::get<0>(mean) += (newX - std::get<0>(mean)) / numEntities;
std::get<1>(mean) += (newY - std::get<1>(mean)) / numEntities;
std::get<2>(mean) += (newZ - std::get<2>(mean)) / numEntities;
}
}

return mean;
}


void from_json(const nlohmann::json &j, Map &map, float polylineReductionThreshold)
{
auto mean = calc_mean(j);
map.mean = {mean.first, mean.second};
map.mean = {std::get<0>(mean), std::get<1>(mean), std::get<2>(mean)}; // Updated to handle all 3 components

map.numObjects = std::min(j.at("objects").size(), static_cast<size_t>(MAX_OBJECTS));
size_t idx = 0;
for (const auto &obj : j.at("objects"))
Expand All @@ -281,7 +311,7 @@ namespace gpudrive
break;
obj.get_to(map.objects[idx++]);
}

map.numRoads = std::min(j.at("roads").size(), static_cast<size_t>(MAX_ROADS));
size_t countRoadPoints = 0;
idx = 0;
Expand Down
16 changes: 8 additions & 8 deletions src/knn.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -20,7 +20,7 @@ void fillZeros(gpudrive::MapObservation *begin,
gpudrive::MapObservation *beyond) {
while (begin < beyond) {
*begin++ =
gpudrive::MapObservation{.position = {0, 0},
gpudrive::MapObservation{.position = {0, 0, 0},
.scale = madrona::math::Diag3x3{0, 0, 0},
.heading = 0.f,
.type = (float)gpudrive::EntityType::None};
Expand All @@ -30,16 +30,16 @@ void fillZeros(gpudrive::MapObservation *begin,
gpudrive::MapObservation
relativeObservation(const gpudrive::MapObservation &absoluteObservation,
const madrona::base::Rotation &referenceRotation,
const madrona::math::Vector2 &referencePosition) {
const madrona::math::Vector3 &referencePosition) {
auto relativePosition =
madrona::math::Vector2{.x = absoluteObservation.position.x,
.y = absoluteObservation.position.y} -
madrona::math::Vector3{.x = absoluteObservation.position.x,
.y = absoluteObservation.position.y,
.z = absoluteObservation.position.z} -
referencePosition;

return gpudrive::MapObservation{
.position = referenceRotation.inv()
.rotateVec({relativePosition.x, relativePosition.y, 0})
.xy(),
.rotateVec({relativePosition.x, relativePosition.y, relativePosition.z}),
.scale = absoluteObservation.scale,
.heading = gpudrive::utils::quatToYaw(referenceRotation.inv() * madrona::math::Quat::angleAxis(absoluteObservation.heading,madrona::math::up)),
.type = absoluteObservation.type};
Expand All @@ -50,7 +50,7 @@ bool isObservationsValid(gpudrive::Engine &ctx,
gpudrive::MapObservation *observations,
madrona::CountT K,
const madrona::base::Rotation &referenceRotation,
const madrona::math::Vector2 &referencePosition) {
const madrona::math::Vector3 &referencePosition) {
#ifdef MADRONA_GPU_MODE
return true;
#else
Expand Down Expand Up @@ -102,7 +102,7 @@ namespace gpudrive {

template <madrona::CountT K>
void selectKNearestRoadEntities(Engine &ctx, const Rotation &referenceRotation,
const madrona::math::Vector2 &referencePosition,
const madrona::math::Vector3 &referencePosition,
gpudrive::MapObservation *heap) {
const Entity *roads = ctx.data().roads;
const auto roadCount = ctx.data().numRoads;
Expand Down
Loading
Loading