p71924506
/
carla
mirror of https://github.com/carla-simulator/carla.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Refactoring

This commit is contained in:
iFuSiiOnzZ 2018-10-25 16:15:12 +02:00
parent e414eb8e98
commit d95d8ee5ec
3 changed files with 151 additions and 171 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

288
Unreal/CarlaUE4/Plugins/Carla/Source/Carla/OpenDriveActor.cpp
View File

@ -7,13 +7,81 @@
#include "DrawDebugHelpers.h"
#include "Algo/Reverse.h"
void AOpenDriveActor::DrawWaypoints(const TArray<FVector>& points)
AOpenDriveActor::AOpenDriveActor()
{
for (int i = 0, len = points.Num() - 1; i < len; ++i)
PrimaryActorTick.bCanEverTick = true;
}
void AOpenDriveActor::BeginPlay()
{
Super::BeginPlay();
}
void AOpenDriveActor::BeginDestroy()
{
for (int i = 0; i < RoutePlanners.Num(); ++i)
{
float f = (float)i / (float)len;
FColor c(255 * f, 255 - 255 * f, 0);
DrawDebugLine(GetWorld(), points[i + 0], points[i + 1], c, true);
RoutePlanners[i]->Destroy();
}
RoutePlanners.Empty();
Super::BeginDestroy();
}
void AOpenDriveActor::OnConstruction(const FTransform &transform)
{
Super::OnConstruction(transform);
carla::road::Map map = carla::opendrive::OpenDrive::Load("C:\\Users\\ajianu\\Desktop\\xodr\\test_03_new.xodr");
std::vector<carla::road::lane_junction_t> junctionInfo = map.GetJunctionInformation();
std::vector<carla::road::id_type> roadIDs = map.GetAllIds();
std::sort(roadIDs.begin(), roadIDs.end());
for (auto &&id : roadIDs)
{
GenerateWaypointsRoad(map.GetRoad(id));
}
///////////////////////////////////////////////////////////////////////////
// junctionId roadID laneID
std::map<int, std::map<int, std::map<int, ARoutePlanner *>>>junctions;
for (size_t i = 0; i < junctionInfo.size(); ++i)
{
TArray<TArray<FVector>> waypoints;
int fromRoadID = junctionInfo[i].incomming_road;
int toRoadID = junctionInfo[i].connection_road;
int junctonID = junctionInfo[i].junction_id;
GenerateWaypointsJunction(map.GetRoad(toRoadID), waypoints);
ARoutePlanner *routePlanner = nullptr;
for (size_t n = 0; n < junctionInfo[i].from_lane.size(); ++n)
{
int fromLaneID = junctionInfo[i].from_lane[n];
routePlanner = junctions[junctonID][fromRoadID][fromLaneID];
if (routePlanner == nullptr)
{
routePlanner = GenerateRoutePlanner(waypoints[n]);
routePlanner->SetSplineColor(FColor::MakeRandomColor());
junctions[junctonID][fromRoadID][fromLaneID] = routePlanner;
}
else
{
routePlanner->AddRoute(1.0, waypoints[n]);
}
}
}
///////////////////////////////////////////////////////////////////////////
for (int i = 0; i < RoutePlanners.Num(); ++i)
{
RoutePlanners[i]->DrawRoutes();
}
}
@ -31,26 +99,17 @@ ARoutePlanner *AOpenDriveActor::GenerateRoutePlanner(const TArray<FVector> &wayp
return routePlanner;
}
void AOpenDriveActor::GenerateWaypoints(const carla::road::element::RoadSegment *road)
TArray<carla::road::element::DirectedPoint> AOpenDriveActor::GenerateLaneZeroPoints(const carla::road::element::RoadSegment * road)
{
const carla::road::element::RoadInfoLane *lanesInfo = road->GetInfo<carla::road::element::RoadInfoLane>(0.0);
const carla::road::element::RoadGeneralInfo *generalInfo = road->GetInfo<carla::road::element::RoadGeneralInfo>(0.0);
if (generalInfo->GetJunctionId() > -1)
{
return;
}
std::vector<std::pair<double, double>> lanesOffset = generalInfo->GetLanesOffset();
std::vector<int> rightLanes = lanesInfo->getLanesIDs(carla::road::element::RoadInfoLane::which_lane_e::Right);
std::vector<int> leftLanes = lanesInfo->getLanesIDs(carla::road::element::RoadInfoLane::which_lane_e::Left);
size_t lanesOffsetIndex = 0;
TArray<carla::road::element::DirectedPoint> laneZeroPoints;
const carla::road::element::RoadGeneralInfo *generalInfo = road->GetInfo<carla::road::element::RoadGeneralInfo>(0.0);
std::vector<std::pair<double, double>> lanesOffset = generalInfo->GetLanesOffset();
for (float waypointsOffset = 0.0f; waypointsOffset < road->GetLength() + 2.0; waypointsOffset += 2.0)
{
//NOTE(Andrei): Calculate the which laneOffset has to be used
if (lanesOffsetIndex < lanesOffset.size() - 1 && waypointsOffset >= lanesOffset[lanesOffsetIndex + 1].first)
{
++lanesOffsetIndex;
@ -60,18 +119,29 @@ void AOpenDriveActor::GenerateWaypoints(const carla::road::element::RoadSegment
carla::road::element::DirectedPoint waypoint = road->GetDirectedPointIn(waypointsOffset);
waypoint.location.y *= (-1); waypoint.location.z = 1;
// NOTE(Andrei): Applyed the offset of the lane section
// NOTE(Andrei): Applyed the laneOffset of the lane section
waypoint.ApplyLateralOffset(lanesOffset[lanesOffsetIndex].second);
laneZeroPoints.Add(waypoint);
}
return laneZeroPoints;
}
TArray<TArray<FVector>> AOpenDriveActor::GenerateRightLaneWaypoints(const carla::road::element::RoadSegment * road, const TArray<carla::road::element::DirectedPoint>&)
{
TArray<carla::road::element::DirectedPoint> laneZeroPoints = GenerateLaneZeroPoints(road);
const carla::road::element::RoadInfoLane *lanesInfo = road->GetInfo<carla::road::element::RoadInfoLane>(0.0);
std::vector<int> rightLanes = lanesInfo->getLanesIDs(carla::road::element::RoadInfoLane::which_lane_e::Right);
TArray<TArray<FVector>> retWaypoints;
double currentOffset = 0.0;
for (size_t j = 0; j < rightLanes.size(); ++j)
{
const carla::road::element::LaneInfo *laneInfo = lanesInfo->getLane(rightLanes[j]);
TArray<FVector> unrealRoadWaypoints;
currentOffset += laneInfo->_width * 0.5;
TArray<FVector> roadWaypoints;
if (laneInfo->_type == "driving")
{
@ -79,25 +149,35 @@ void AOpenDriveActor::GenerateWaypoints(const carla::road::element::RoadSegment
{
carla::road::element::DirectedPoint currentPoint = laneZeroPoints[i];
currentPoint.ApplyLateralOffset(-currentOffset);
unrealRoadWaypoints.Add(currentPoint.location);
roadWaypoints.Add(currentPoint.location);
}
if (unrealRoadWaypoints.Num() >= 2)
if (roadWaypoints.Num() >= 2)
{
DrawWaypoints(unrealRoadWaypoints);
GenerateRoutePlanner(unrealRoadWaypoints);
retWaypoints.Add(roadWaypoints);
}
}
currentOffset += laneInfo->_width * 0.5;
}
currentOffset = 0.0;
return retWaypoints;
}
TArray<TArray<FVector>> AOpenDriveActor::GenerateLeftLaneWaypoints(const carla::road::element::RoadSegment * road, const TArray<carla::road::element::DirectedPoint>&)
{
TArray<carla::road::element::DirectedPoint> laneZeroPoints = GenerateLaneZeroPoints(road);
const carla::road::element::RoadInfoLane *lanesInfo = road->GetInfo<carla::road::element::RoadInfoLane>(0.0);
std::vector<int> leftLanes = lanesInfo->getLanesIDs(carla::road::element::RoadInfoLane::which_lane_e::Left);
TArray<TArray<FVector>> retWaypoints;
double currentOffset = 0.0;
for (size_t j = 0; j < leftLanes.size(); ++j)
{
const carla::road::element::LaneInfo *laneInfo = lanesInfo->getLane(leftLanes[leftLanes.size() - 1 -j]);
TArray<FVector> unrealRoadWaypoints;
const carla::road::element::LaneInfo *laneInfo = lanesInfo->getLane(leftLanes[leftLanes.size() - 1 - j]);
currentOffset += laneInfo->_width * 0.5;
TArray<FVector> roadWaypoints;
if (laneInfo->_type == "driving")
{
@ -105,157 +185,47 @@ void AOpenDriveActor::GenerateWaypoints(const carla::road::element::RoadSegment
{
carla::road::element::DirectedPoint currentPoint = laneZeroPoints[i];
currentPoint.ApplyLateralOffset(currentOffset);
unrealRoadWaypoints.Add(currentPoint.location);
roadWaypoints.Add(currentPoint.location);
}
if (unrealRoadWaypoints.Num() >= 2)
if (roadWaypoints.Num() >= 2)
{
Algo::Reverse(unrealRoadWaypoints);
DrawWaypoints(unrealRoadWaypoints);
GenerateRoutePlanner(unrealRoadWaypoints);
Algo::Reverse(roadWaypoints);
retWaypoints.Add(roadWaypoints);
}
}
currentOffset += laneInfo->_width * 0.5;
}
return retWaypoints;
}
void AOpenDriveActor::GenerateWaypointsJunctions(const carla::road::element::RoadSegment *road, TArray<int> &out_lanesId, TArray<TArray<FVector>> &out_waypoints)
void AOpenDriveActor::GenerateWaypointsRoad(const carla::road::element::RoadSegment *road)
{
const carla::road::element::RoadInfoLane *lanesInfo = road->GetInfo<carla::road::element::RoadInfoLane>(0.0);
const carla::road::element::RoadGeneralInfo *generalInfo = road->GetInfo<carla::road::element::RoadGeneralInfo>(0.0);
if (generalInfo->GetJunctionId() > -1) return;
if (generalInfo->GetJunctionId() == -1)
TArray<carla::road::element::DirectedPoint> laneZeroPoints = GenerateLaneZeroPoints(road);
TArray<TArray<FVector>> rightLaneWaypoints = GenerateRightLaneWaypoints(road, laneZeroPoints);
TArray<TArray<FVector>> leftLaneWaypoints = GenerateLeftLaneWaypoints(road, laneZeroPoints);
for (int i = 0; i < rightLaneWaypoints.Num(); ++i)
{
return;
GenerateRoutePlanner(rightLaneWaypoints[i]);
}
std::vector<std::pair<double, double>> lanesOffset = generalInfo->GetLanesOffset();
std::vector<int> rightLanes = lanesInfo->getLanesIDs(carla::road::element::RoadInfoLane::which_lane_e::Right);
std::vector<int> leftLanes = lanesInfo->getLanesIDs(carla::road::element::RoadInfoLane::which_lane_e::Left);
size_t lanesOffsetIndex = 0;
TArray<carla::road::element::DirectedPoint> laneZeroPoints;
for (float waypointsOffset = 0.0f; waypointsOffset < road->GetLength() + 2.0; waypointsOffset += 2.0)
for (int i = 0; i < leftLaneWaypoints.Num(); ++i)
{
if (lanesOffsetIndex < lanesOffset.size() - 1 && waypointsOffset >= lanesOffset[lanesOffsetIndex + 1].first)
{
++lanesOffsetIndex;
}
// NOTE(Andrei): Get waypoin at the offset, and invert the y axis
carla::road::element::DirectedPoint waypoint = road->GetDirectedPointIn(waypointsOffset);
waypoint.location.y *= (-1); waypoint.location.z = 1;
// NOTE(Andrei): Applyed the offset of the lane section
waypoint.ApplyLateralOffset(lanesOffset[lanesOffsetIndex].second);
laneZeroPoints.Add(waypoint);
}
double currentOffset = 0.0;
for (size_t j = 0; j < rightLanes.size(); ++j)
{
const carla::road::element::LaneInfo *laneInfo = lanesInfo->getLane(rightLanes[j]);
TArray<FVector> unrealRoadWaypoints;
currentOffset += laneInfo->_width * 0.5;
if (laneInfo->_type == "driving")
{
for (int i = 0; i < laneZeroPoints.Num(); ++i)
{
carla::road::element::DirectedPoint currentPoint = laneZeroPoints[i];
currentPoint.ApplyLateralOffset(-currentOffset);
unrealRoadWaypoints.Add(currentPoint.location);
}
if (unrealRoadWaypoints.Num() >= 2)
{
out_lanesId.Add(laneInfo->_id);
out_waypoints.Add(unrealRoadWaypoints);
}
}
currentOffset += laneInfo->_width * 0.5;
GenerateRoutePlanner(leftLaneWaypoints[i]);
}
}
void AOpenDriveActor::BeginDestroy()
void AOpenDriveActor::GenerateWaypointsJunction(const carla::road::element::RoadSegment *road, TArray<TArray<FVector>> &out_waypoints)
{
for (int i = 0; i < RoutePlanners.Num(); ++i)
{
RoutePlanners[i]->Destroy();
}
const carla::road::element::RoadGeneralInfo *generalInfo = road->GetInfo<carla::road::element::RoadGeneralInfo>(0.0);
if (generalInfo->GetJunctionId() == -1) return;
RoutePlanners.Empty();
Super::BeginDestroy();
}
void AOpenDriveActor::BeginPlay()
{
Super::BeginPlay();
}
AOpenDriveActor::AOpenDriveActor()
{
PrimaryActorTick.bCanEverTick = true;
}
void AOpenDriveActor::OnConstruction(const FTransform &transform)
{
Super::OnConstruction(transform);
carla::road::Map map = carla::opendrive::OpenDrive::Load("C:\\Users\\ajianu\\Desktop\\xodr\\test_03_new.xodr");
std::vector<carla::road::lane_junction_t> junctionInfo = map.GetJunctionInformation();
std::vector<carla::road::id_type> roadIDs = map.GetAllIds();
std::sort(roadIDs.begin(), roadIDs.end());
for (auto &&id : roadIDs)
{
GenerateWaypoints(map.GetRoad(id));
}
// junctionId roadID laneID
std::map<int, std::map<int, std::map<int, ARoutePlanner *>>>junctions;
for (size_t i = 0; i < junctionInfo.size(); ++i)
{
TArray<int> lanesId;
TArray<TArray<FVector>> waypoints;
int fromRoadID = junctionInfo[i].incomming_road;
int toRoadID = junctionInfo[i].connection_road;
int junctonID = junctionInfo[i].junction_id;
GenerateWaypointsJunctions(map.GetRoad(toRoadID), lanesId, waypoints);
ARoutePlanner *routePlanner = nullptr;
for (size_t n = 0; n < junctionInfo[i].from_lane.size(); ++n)
{
int fromLaneID = junctionInfo[i].from_lane[n];
routePlanner = junctions[junctonID][fromRoadID][fromLaneID];
if (routePlanner == nullptr)
{
char r = rand() % (255 - 100 + 1) + 100;
char g = rand() % (255 - 100 + 1) + 100;
char b = rand() % (255 - 100 + 1) + 100;
routePlanner = GenerateRoutePlanner(waypoints[n]);
routePlanner->SetSplineColor(FColor(r, g, b));
junctions[junctonID][fromRoadID][fromLaneID] = routePlanner;
routePlanner->DrawRoutes();
}
else
{
routePlanner->AddRoute(1.0, waypoints[n]);
routePlanner->DrawRoutes();
}
}
}
TArray<carla::road::element::DirectedPoint> laneZeroPoints = GenerateLaneZeroPoints(road);
out_waypoints = GenerateRightLaneWaypoints(road, laneZeroPoints);
}

10
Unreal/CarlaUE4/Plugins/Carla/Source/Carla/OpenDriveActor.h
View File

@ -21,21 +21,21 @@ class CARLA_API AOpenDriveActor : public AActor
private:
TArray<ARoutePlanner *> RoutePlanners;
;
public:
// Sets default values for this actor's properties
AOpenDriveActor();
virtual void BeginPlay() override;
virtual void BeginDestroy() override;
virtual void OnConstruction(const FTransform &transform) override;
ARoutePlanner *GenerateRoutePlanner(const TArray<FVector> &waypoints);
TArray<carla::road::element::DirectedPoint> GenerateLaneZeroPoints(const carla::road::element::RoadSegment *road);
void GenerateWaypointsJunctions(const carla::road::element::RoadSegment *road, TArray<int> &lanesId, TArray<TArray<FVector>> &waypoints);
void GenerateWaypoints(const carla::road::element::RoadSegment *road);
TArray<TArray<FVector>> GenerateRightLaneWaypoints(const carla::road::element::RoadSegment *road, const TArray<carla::road::element::DirectedPoint> &);
TArray<TArray<FVector>> GenerateLeftLaneWaypoints(const carla::road::element::RoadSegment *road, const TArray<carla::road::element::DirectedPoint> &);
void DrawWaypoints(const TArray<FVector> &points);
void GenerateWaypointsJunction(const carla::road::element::RoadSegment *road, TArray<TArray<FVector>> &waypoints);
void GenerateWaypointsRoad(const carla::road::element::RoadSegment *road);
};

24
Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Traffic/RoutePlanner.cpp
View File

@ -58,6 +58,8 @@ ARoutePlanner::ARoutePlanner(const FObjectInitializer& ObjectInitializer) :
TriggerVolume->SetCollisionProfileName(FName("OverlapAll"));
TriggerVolume->SetBoxExtent(FVector{50.0f, 50.0f, 50.0f});
TriggerVolume->bGenerateOverlapEvents = true;
_spline_color = FColor::Black;
}
#if WITH_EDITOR
@ -134,13 +136,13 @@ void ARoutePlanner::BeginPlay()
void ARoutePlanner::EndPlay(const EEndPlayReason::Type EndPlayReason)
{
// Deregister the delegate.
if (TriggerVolume->OnComponentBeginOverlap.IsAlreadyBound(this, &ARoutePlanner::OnTriggerBeginOverlap))
{
TriggerVolume->OnComponentBeginOverlap.RemoveDynamic(this, &ARoutePlanner::OnTriggerBeginOverlap);
}
// Deregister the delegate.
if (TriggerVolume->OnComponentBeginOverlap.IsAlreadyBound(this, &ARoutePlanner::OnTriggerBeginOverlap))
{
TriggerVolume->OnComponentBeginOverlap.RemoveDynamic(this, &ARoutePlanner::OnTriggerBeginOverlap);
}
Super::EndPlay(EndPlayReason);
Super::EndPlay(EndPlayReason);
}
void ARoutePlanner::OnTriggerBeginOverlap(
@ -179,7 +181,15 @@ void ARoutePlanner::DrawRoutes()
FVector p0 = Routes[i]->GetLocationAtSplinePoint(j + 0, ESplineCoordinateSpace::World);
FVector p1 = Routes[i]->GetLocationAtSplinePoint(j + 1, ESplineCoordinateSpace::World);
DrawDebugLine(GetWorld(), p0, p1, _spline_color, true);
if (_spline_color == FColor::Black)
{
float f = (float)j / (float)lenNumPoints;
DrawDebugLine(GetWorld(), p0, p1, FColor(255 * f, 255 - 255 * f, 0), true);
}
else
{
DrawDebugLine(GetWorld(), p0, p1, _spline_color, true);
}
}
}
}