From a0cbf1dfb233117aa3ffd22816032f383b82c671 Mon Sep 17 00:00:00 2001 From: marcgpuig Date: Mon, 10 Dec 2018 20:14:07 +0100 Subject: [PATCH] Added elevation in OpenDriveActor --- LibCarla/source/carla/opendrive/OpenDrive.cpp | 23 +++++-- LibCarla/source/carla/road/element/RoadInfo.h | 64 +++++++++++++++++++ .../carla/road/element/RoadInfoVisitor.h | 2 + .../Carla/Source/Carla/OpenDriveActor.cpp | 10 ++- 4 files changed, 91 insertions(+), 8 deletions(-) diff --git a/LibCarla/source/carla/opendrive/OpenDrive.cpp b/LibCarla/source/carla/opendrive/OpenDrive.cpp index 7fcf5a5ac..885e542de 100644 --- a/LibCarla/source/carla/opendrive/OpenDrive.cpp +++ b/LibCarla/source/carla/opendrive/OpenDrive.cpp @@ -124,20 +124,29 @@ namespace opendrive { // Transforma data for the MapBuilder for (road_data_t::iterator it = roadData.begin(); it != roadData.end(); ++it) { carla::road::element::RoadSegmentDefinition roadSegment(it->first); - carla::road::element::RoadInfoLane *roadInfoLanes = + carla::road::element::RoadInfoLane *RoadInfoLanes = roadSegment.MakeInfo(); - carla::road::element::RoadGeneralInfo *roadGeneralInfo = + carla::road::element::RoadGeneralInfo *RoadGeneralInfo = roadSegment.MakeInfo(); - roadGeneralInfo->SetJunctionId(it->second->attributes.junction); + RoadGeneralInfo->SetJunctionId(it->second->attributes.junction); for (size_t i = 0; i < it->second->lanes.lane_offset.size(); ++i) { double s = it->second->lanes.lane_offset[i].s; double a = it->second->lanes.lane_offset[i].a; - roadGeneralInfo->SetLanesOffset(s, a); + RoadGeneralInfo->SetLanesOffset(s, a); } - ///////////////////////////////////////////////////////////////////////// + for(auto &&elevation : it->second->road_profiles.elevation_profile) { + roadSegment.MakeInfo( + elevation.start_position, + elevation.start_position, + elevation.elevation, + elevation.slope, + elevation.vertical_curvature, + elevation.curvature_change + ); + } std::map leftLanesGoToSuccessor, leftLanesGoToPredecessor; std::map rightLanesGoToSuccessor, rightLanesGoToPredecessor; @@ -204,12 +213,12 @@ namespace opendrive { &it->second->lanes.lane_sections.front().right; for (size_t i = 0; i < lanesLeft->size(); ++i) { - roadInfoLanes->addLaneInfo(lanesLeft->at(i).attributes.id, + RoadInfoLanes->addLaneInfo(lanesLeft->at(i).attributes.id, lanesLeft->at(i).lane_width[0].width, lanesLeft->at(i).attributes.type); } for (size_t i = 0; i < lanesRight->size(); ++i) { - roadInfoLanes->addLaneInfo(lanesRight->at(i).attributes.id, + RoadInfoLanes->addLaneInfo(lanesRight->at(i).attributes.id, lanesRight->at(i).lane_width[0].width, lanesRight->at(i).attributes.type); } diff --git a/LibCarla/source/carla/road/element/RoadInfo.h b/LibCarla/source/carla/road/element/RoadInfo.h index d833eb9de..8bbc832de 100644 --- a/LibCarla/source/carla/road/element/RoadInfo.h +++ b/LibCarla/source/carla/road/element/RoadInfo.h @@ -70,6 +70,70 @@ namespace element { } }; + class RoadElevationInfo : public RoadInfo { + public: + + void AcceptVisitor(RoadInfoVisitor &v) final { + v.Visit(*this); + } + + RoadElevationInfo(double d, + double start_position, + double elevation, + double slope, + double vertical_curvature, + double curvature_change) + : RoadInfo(d), + _start_position(start_position), + _elevation(elevation), + _slope(slope), + _vertical_curvature(vertical_curvature), + _curvature_change(curvature_change) {} + + double Evaluate(const double dist, double *out_tan) const { + const double t = dist - _start_position; + const double pos = _elevation + + _slope * t + + _vertical_curvature * t * t + + _curvature_change * t * t * t; + + if (out_tan) { + *out_tan = _slope + t * + (2 * _vertical_curvature + t * 3 * _curvature_change); + } + + return pos; + } + + double GetStartPosition() const { + return _start_position; + } + + double GetElevation() const { + return _elevation; + } + + double GetSlope() const { + return _slope; + } + + double GetVerticalCurvature() const { + return _vertical_curvature; + } + + double GetCurvatureChange() const { + return _curvature_change; + } + + private: + + double _start_position; // (S) start position(s - offset)[meters] + double _elevation; // (A) elevation [meters] + double _slope; // (B) + double _vertical_curvature; // (C) + double _curvature_change; // (D) + }; + class RoadInfoVelocity : public RoadInfo { public: diff --git a/LibCarla/source/carla/road/element/RoadInfoVisitor.h b/LibCarla/source/carla/road/element/RoadInfoVisitor.h index 19c9715be..13dba20c0 100644 --- a/LibCarla/source/carla/road/element/RoadInfoVisitor.h +++ b/LibCarla/source/carla/road/element/RoadInfoVisitor.h @@ -16,6 +16,7 @@ namespace element { class RoadInfoLane; class RoadGeneralInfo; class RoadInfoVelocity; + class RoadElevationInfo; class RoadInfoVisitor { public: @@ -23,6 +24,7 @@ namespace element { virtual void Visit(RoadInfoLane &) {} virtual void Visit(RoadGeneralInfo &) {} virtual void Visit(RoadInfoVelocity &) {} + virtual void Visit(RoadElevationInfo &) {} }; template diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/OpenDriveActor.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/OpenDriveActor.cpp index 6b10931b0..986d4403f 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/OpenDriveActor.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/OpenDriveActor.cpp @@ -64,7 +64,7 @@ AOpenDriveActor::AOpenDriveActor(const FObjectInitializer& ObjectInitializer) : SpriteComponent->SpriteInfo.DisplayName = ConstructorStatics.Name; // Assign sprite display name SpriteComponent->SetupAttachment(RootComponent); // Attach sprite to scene component SpriteComponent->Mobility = EComponentMobility::Static; - SpriteComponent->SetEditorScale(1.5f); + SpriteComponent->SetEditorScale(0.8f); } #endif // WITH_EDITORONLY_DATA } @@ -241,6 +241,8 @@ void AOpenDriveActor::RemoveRoutes() TArray AOpenDriveActor::GenerateLaneZeroPoints( const RoadSegment *road) { + using RoadElevationInfo = carla::road::element::RoadElevationInfo; + size_t LanesOffsetIndex = 0; TArray LaneZeroPoints; @@ -261,6 +263,12 @@ TArray AOpenDriveActor::GenerateLaneZeroPoints( DirectedPoint Waypoint = road->GetDirectedPointIn(WaypointsOffset); Waypoint.location.z = 1; + const RoadElevationInfo *ElevInfo = road->GetInfo(WaypointsOffset); + + if(ElevInfo) { + Waypoint.location.z += (float)ElevInfo->Evaluate(WaypointsOffset, nullptr); + } + // NOTE(Andrei): Applyed the laneOffset of the lane section Waypoint.ApplyLateralOffset(LanesOffset[LanesOffsetIndex].second);