Added elevation in OpenDriveActor

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::RoadInfoLane>();
 
-      carla::road::element::RoadGeneralInfo *roadGeneralInfo =
+      carla::road::element::RoadGeneralInfo *RoadGeneralInfo =
           roadSegment.MakeInfo<carla::road::element::RoadGeneralInfo>();
-      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<carla::road::element::RoadElevationInfo>(
+          elevation.start_position,
+          elevation.start_position,
+          elevation.elevation,
+          elevation.slope,
+          elevation.vertical_curvature,
+          elevation.curvature_change
+        );
+      }
 
       std::map<int, int> leftLanesGoToSuccessor, leftLanesGoToPredecessor;
       std::map<int, int> 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);
       }

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:
 

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 <typename T, typename IT>

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::DirectedPoint> AOpenDriveActor::GenerateLaneZeroPoints(
     const RoadSegment *road)
 {
+  using RoadElevationInfo = carla::road::element::RoadElevationInfo;
+
   size_t LanesOffsetIndex = 0;
   TArray<DirectedPoint> LaneZeroPoints;
 
@@ -261,6 +263,12 @@ TArray<AOpenDriveActor::DirectedPoint> AOpenDriveActor::GenerateLaneZeroPoints(
     DirectedPoint Waypoint = road->GetDirectedPointIn(WaypointsOffset);
     Waypoint.location.z = 1;
 
+    const RoadElevationInfo *ElevInfo = road->GetInfo<RoadElevationInfo>(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);