Merge pull request #1014 from carla-simulator/usable_open_drive
Improved OpenDriveActor
This commit is contained in:
Néstor Subirón
GitHub
commit
a985cabc78
Binary file not shown.
|
|
@ -7,69 +7,172 @@
|
|||
#include "Carla.h"
|
||||
|
||||
#include "OpenDriveActor.h"
|
||||
#include "Algo/Reverse.h"
|
||||
|
||||
#include "Util/OpenDrive.h"
|
||||
|
||||
#include <compiler/disable-ue4-macros.h>
|
||||
#include <carla/rpc/String.h>
|
||||
#include <carla/geom/Math.h>
|
||||
#include <compiler/enable-ue4-macros.h>
|
||||
|
||||
AOpenDriveActor::AOpenDriveActor()
|
||||
{
|
||||
PrimaryActorTick.bCanEverTick = true;
|
||||
}
|
||||
#include <functional>
|
||||
|
||||
void AOpenDriveActor::BeginPlay()
|
||||
TArray<FVector> DirectedPointArray2FVectorArray(
|
||||
const TArray<AOpenDriveActor::DirectedPoint> &DirectedPoints)
|
||||
{
|
||||
Super::BeginPlay();
|
||||
}
|
||||
|
||||
void AOpenDriveActor::BeginDestroy()
|
||||
{
|
||||
for (int i = 0; i < RoutePlanners.Num(); ++i)
|
||||
TArray<FVector> Positions;
|
||||
Positions.Reserve(DirectedPoints.Num());
|
||||
for (int i = 0; i < DirectedPoints.Num(); ++i)
|
||||
{
|
||||
RoutePlanners[i]->Destroy();
|
||||
Positions.Add(DirectedPoints[i].location);
|
||||
}
|
||||
|
||||
RoutePlanners.Empty();
|
||||
Super::BeginDestroy();
|
||||
return Positions;
|
||||
}
|
||||
|
||||
void AOpenDriveActor::OnConstruction(const FTransform &transform)
|
||||
AOpenDriveActor::AOpenDriveActor(const FObjectInitializer& ObjectInitializer) :
|
||||
Super(ObjectInitializer)
|
||||
{
|
||||
Super::OnConstruction(transform);
|
||||
BuildRoutes();
|
||||
PrimaryActorTick.bCanEverTick = false;
|
||||
|
||||
// Structure to hold one-time initialization
|
||||
static struct FConstructorStatics
|
||||
{
|
||||
// A helper class object we use to find target UTexture2D object in resource package
|
||||
ConstructorHelpers::FObjectFinderOptional<UTexture2D> TextureObject;
|
||||
FName Category;
|
||||
FText Name;
|
||||
FConstructorStatics()
|
||||
// Use helper class object to find the texture resource path
|
||||
: TextureObject(TEXT("/Carla/Icons/OpenDriveActorIcon"))
|
||||
, Category(TEXT("OpenDriveActor"))
|
||||
, Name(NSLOCTEXT("SpriteCategory", "OpenDriveActor", "OpenDriveActor"))
|
||||
{
|
||||
}
|
||||
} ConstructorStatics;
|
||||
|
||||
// We need a scene component to attach Icon sprite
|
||||
USceneComponent* SceneComponent = ObjectInitializer.CreateDefaultSubobject<USceneComponent>(this, TEXT("SceneComp"));
|
||||
RootComponent = SceneComponent;
|
||||
RootComponent->Mobility = EComponentMobility::Static;
|
||||
|
||||
#if WITH_EDITORONLY_DATA
|
||||
SpriteComponent = ObjectInitializer.CreateEditorOnlyDefaultSubobject<UBillboardComponent>(this, TEXT("Sprite"));
|
||||
if (SpriteComponent)
|
||||
{
|
||||
SpriteComponent->Sprite = ConstructorStatics.TextureObject.Get(); // Get the sprite texture from helper class object
|
||||
SpriteComponent->SpriteInfo.Category = ConstructorStatics.Category; // Assign sprite category name
|
||||
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);
|
||||
}
|
||||
#endif // WITH_EDITORONLY_DATA
|
||||
}
|
||||
|
||||
#if WITH_EDITOR
|
||||
void AOpenDriveActor::PostEditChangeProperty(struct FPropertyChangedEvent& Event)
|
||||
{
|
||||
Super::PostEditChangeProperty(Event);
|
||||
|
||||
const FName PropertyName = (Event.Property != NULL ? Event.Property->GetFName() : NAME_None);
|
||||
if (PropertyName == GET_MEMBER_NAME_CHECKED(AOpenDriveActor, bGenerateRoutes))
|
||||
{
|
||||
if (bGenerateRoutes)
|
||||
{
|
||||
bGenerateRoutes = false;
|
||||
|
||||
RemoveRoutes(); // Avoid OpenDrive overlapping
|
||||
RemoveSpawners(); // Restart the spawners in case OpenDrive has changed
|
||||
BuildRoutes();
|
||||
|
||||
if (bAddSpawners)
|
||||
{
|
||||
AddSpawners();
|
||||
}
|
||||
if (bShowDebug)
|
||||
{
|
||||
DebugRoutes();
|
||||
}
|
||||
}
|
||||
}
|
||||
if (PropertyName == GET_MEMBER_NAME_CHECKED(AOpenDriveActor, bRemoveRoutes))
|
||||
{
|
||||
if (bRemoveRoutes)
|
||||
{
|
||||
bRemoveRoutes = false;
|
||||
|
||||
RemoveDebugRoutes();
|
||||
RemoveSpawners();
|
||||
RemoveRoutes();
|
||||
}
|
||||
}
|
||||
if (PropertyName == GET_MEMBER_NAME_CHECKED(AOpenDriveActor, bShowDebug))
|
||||
{
|
||||
if (bShowDebug)
|
||||
{
|
||||
DebugRoutes();
|
||||
}
|
||||
else
|
||||
{
|
||||
RemoveDebugRoutes();
|
||||
}
|
||||
}
|
||||
if (PropertyName == GET_MEMBER_NAME_CHECKED(AOpenDriveActor, bRemoveCurrentSpawners))
|
||||
{
|
||||
if (bRemoveCurrentSpawners)
|
||||
{
|
||||
bRemoveCurrentSpawners = false;
|
||||
|
||||
RemoveSpawners();
|
||||
}
|
||||
}
|
||||
}
|
||||
#endif // WITH_EDITOR
|
||||
|
||||
ARoutePlanner *AOpenDriveActor::GenerateRoutePlanner(const TArray<DirectedPoint> &DirectedPoints)
|
||||
{
|
||||
using CarlaMath = carla::geom::Math;
|
||||
|
||||
TArray<FVector> Positions = DirectedPointArray2FVectorArray(DirectedPoints);
|
||||
ARoutePlanner *RoutePlanner = GetWorld()->SpawnActor<ARoutePlanner>();
|
||||
|
||||
RoutePlanner->SetActorRotation(FRotator(0.0f, CarlaMath::to_degrees(DirectedPoints[0].tangent), 0.0f));
|
||||
RoutePlanner->SetActorLocation(DirectedPoints[0].location);
|
||||
RoutePlanner->SetBoxExtent(FVector(70.0f, 70.0f, 50.0f));
|
||||
RoutePlanner->AddRoute(1.0f, Positions);
|
||||
RoutePlanner->Init();
|
||||
RoutePlanners.Add(RoutePlanner);
|
||||
return RoutePlanner;
|
||||
}
|
||||
|
||||
void AOpenDriveActor::BuildRoutes()
|
||||
{
|
||||
std::string parseError;
|
||||
using IdType = carla::road::element::id_type;
|
||||
|
||||
std::string ParseError;
|
||||
|
||||
// NOTE(Andrei): As the OpenDrive file has the same name as level,
|
||||
// build the path to the xodr file using the lavel name and the
|
||||
// game content directory.
|
||||
FString mapName = GetWorld()->GetMapName();
|
||||
FString xodrContent = FOpenDrive::Load(mapName);
|
||||
FString MapName = GetWorld()->GetMapName();
|
||||
FString XodrContent = FOpenDrive::Load(MapName);
|
||||
|
||||
auto map_ptr = carla::opendrive::OpenDrive::Load(TCHAR_TO_UTF8(*xodrContent),
|
||||
auto map_ptr = carla::opendrive::OpenDrive::Load(TCHAR_TO_UTF8(*XodrContent),
|
||||
XmlInputType::CONTENT,
|
||||
&parseError);
|
||||
&ParseError);
|
||||
|
||||
if (parseError.size())
|
||||
if (ParseError.size())
|
||||
{
|
||||
UE_LOG(LogCarla, Error, TEXT("OpenDrive parsing error: '%s'."), *carla::rpc::ToFString(parseError));
|
||||
UE_LOG(LogCarla, Error, TEXT("OpenDrive parsing error: '%s'."), *carla::rpc::ToFString(ParseError));
|
||||
return;
|
||||
}
|
||||
|
||||
const auto &map = map_ptr->GetData();
|
||||
std::vector<carla::road::lane_junction_t> junctionInfo = map.GetJunctionInformation();
|
||||
std::vector<carla::road::lane_junction_t> JunctionInfo = map.GetJunctionInformation();
|
||||
|
||||
///////////////////////////////////////////////////////////////////////////
|
||||
// NOTE(Andrei): Build the roads that are not junctions
|
||||
|
||||
auto RoadIDsView = map.GetAllIds();
|
||||
std::vector<carla::road::element::id_type> roadIDs(RoadIDsView.begin(), RoadIDsView.end());
|
||||
std::vector<IdType> roadIDs(RoadIDsView.begin(), RoadIDsView.end());
|
||||
std::sort(roadIDs.begin(), roadIDs.end());
|
||||
|
||||
for (auto &&id : roadIDs)
|
||||
|
|
@ -77,193 +180,193 @@ void AOpenDriveActor::BuildRoutes()
|
|||
GenerateWaypointsRoad(map.GetRoad(id));
|
||||
}
|
||||
|
||||
///////////////////////////////////////////////////////////////////////////
|
||||
// NOTE(Andrei): Build the roads that are junctions as one RoutePlanner
|
||||
// can have more than one path that can be taken
|
||||
|
||||
// junctionId roadID laneID
|
||||
std::map<int, std::map<int, std::map<int, ARoutePlanner *>>> junctions;
|
||||
std::map<int, std::map<int, std::map<int, ARoutePlanner *>>> Junctions;
|
||||
|
||||
for (size_t i = 0; i < junctionInfo.size(); ++i)
|
||||
for (auto && Junction : JunctionInfo)
|
||||
{
|
||||
TArray<TArray<FVector>> waypoints;
|
||||
TArray<TArray<DirectedPoint>> Waypoints;
|
||||
|
||||
int fromRoadID = junctionInfo[i].incomming_road;
|
||||
int toRoadID = junctionInfo[i].connection_road;
|
||||
int junctonID = junctionInfo[i].junction_id;
|
||||
int FromRoadID = Junction.incomming_road;
|
||||
int ToRoadID = Junction.connection_road;
|
||||
int JunctonID = Junction.junction_id;
|
||||
|
||||
GenerateWaypointsJunction(map.GetRoad(toRoadID), waypoints);
|
||||
GenerateWaypointsJunction(map.GetRoad(ToRoadID), Waypoints);
|
||||
ARoutePlanner *routePlanner = nullptr;
|
||||
|
||||
std::sort(junctionInfo[i].from_lane.begin(), junctionInfo[i].from_lane.end());
|
||||
if (junctionInfo[i].from_lane[0] < 0)
|
||||
std::sort(Junction.from_lane.begin(), Junction.from_lane.end(), std::greater<int>());
|
||||
|
||||
if (Junction.from_lane[0] < 0)
|
||||
{
|
||||
std::reverse(junctionInfo[i].from_lane.begin(), junctionInfo[i].from_lane.end());
|
||||
std::reverse(Junction.from_lane.begin(), Junction.from_lane.end());
|
||||
}
|
||||
|
||||
for (size_t n = 0; n < junctionInfo[i].from_lane.size(); ++n)
|
||||
for (size_t n = 0; n < Junction.from_lane.size(); ++n)
|
||||
{
|
||||
int fromLaneID = junctionInfo[i].from_lane[n];
|
||||
routePlanner = junctions[junctonID][fromRoadID][fromLaneID];
|
||||
int FromLaneID = Junction.from_lane[n];
|
||||
routePlanner = Junctions[JunctonID][FromRoadID][FromLaneID];
|
||||
|
||||
if (routePlanner == nullptr)
|
||||
{
|
||||
routePlanner = GenerateRoutePlanner(waypoints[n]);
|
||||
routePlanner = GenerateRoutePlanner(Waypoints[n]);
|
||||
routePlanner->SetSplineColor(FColor::MakeRandomColor());
|
||||
junctions[junctonID][fromRoadID][fromLaneID] = routePlanner;
|
||||
Junctions[JunctonID][FromRoadID][FromLaneID] = routePlanner;
|
||||
}
|
||||
else
|
||||
{
|
||||
routePlanner->AddRoute(1.0, waypoints[n]);
|
||||
routePlanner->AddRoute(1.0, DirectedPointArray2FVectorArray(Waypoints[n]));
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
///////////////////////////////////////////////////////////////////////////
|
||||
|
||||
for (int i = 0; i < RoutePlanners.Num(); ++i)
|
||||
/// Remove all the existing ARoutePlanner and VehicleSpawners previously
|
||||
/// generated by this class to avoid overlapping
|
||||
void AOpenDriveActor::RemoveRoutes()
|
||||
{
|
||||
const int rp_num = RoutePlanners.Num();
|
||||
for (int i = 0; i < rp_num; i++)
|
||||
{
|
||||
RoutePlanners[i]->DrawRoutes();
|
||||
if (RoutePlanners[i] != nullptr)
|
||||
{
|
||||
RoutePlanners[i]->Destroy();
|
||||
}
|
||||
}
|
||||
RoutePlanners.Empty();
|
||||
}
|
||||
|
||||
ARoutePlanner *AOpenDriveActor::GenerateRoutePlanner(const TArray<FVector> &waypoints)
|
||||
TArray<AOpenDriveActor::DirectedPoint> AOpenDriveActor::GenerateLaneZeroPoints(
|
||||
const RoadSegment *road)
|
||||
{
|
||||
ARoutePlanner *junctionRoutePlanner = nullptr;
|
||||
size_t LanesOffsetIndex = 0;
|
||||
TArray<DirectedPoint> LaneZeroPoints;
|
||||
|
||||
ARoutePlanner *routePlanner = GetWorld()->SpawnActor<ARoutePlanner>();
|
||||
routePlanner->SetActorLocation(waypoints[0]);
|
||||
const RoadGeneralInfo *generalInfo =
|
||||
road->GetInfo<RoadGeneralInfo>(0.0);
|
||||
std::vector<std::pair<double, double>> LanesOffset = generalInfo->GetLanesOffset();
|
||||
|
||||
routePlanner->SetBoxExtent(FVector(70.0f, 70.0f, 50.0f));
|
||||
routePlanner->AddRoute(1.0f, waypoints);
|
||||
routePlanner->Init();
|
||||
|
||||
RoutePlanners.Add(routePlanner);
|
||||
return routePlanner;
|
||||
}
|
||||
|
||||
TArray<carla::road::element::DirectedPoint> AOpenDriveActor::GenerateLaneZeroPoints(
|
||||
const carla::road::element::RoadSegment *road)
|
||||
{
|
||||
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)
|
||||
for (float WaypointsOffset = 0.0f; WaypointsOffset < road->GetLength() + RoadAccuracy; WaypointsOffset += RoadAccuracy)
|
||||
{
|
||||
// NOTE(Andrei): Calculate the which laneOffset has to be used
|
||||
if (lanesOffsetIndex < lanesOffset.size() - 1 &&
|
||||
waypointsOffset >= lanesOffset[lanesOffsetIndex + 1].first)
|
||||
if (LanesOffsetIndex < LanesOffset.size() - 1 &&
|
||||
WaypointsOffset >= LanesOffset[LanesOffsetIndex + 1].first)
|
||||
{
|
||||
++lanesOffsetIndex;
|
||||
++LanesOffsetIndex;
|
||||
}
|
||||
|
||||
// NOTE(Andrei): Get waypoin at the offset, and invert the y axis
|
||||
carla::road::element::DirectedPoint waypoint = road->GetDirectedPointIn(waypointsOffset);
|
||||
waypoint.location.z = 1;
|
||||
DirectedPoint Waypoint = road->GetDirectedPointIn(WaypointsOffset);
|
||||
Waypoint.location.z = 1;
|
||||
|
||||
// NOTE(Andrei): Applyed the laneOffset of the lane section
|
||||
waypoint.ApplyLateralOffset(lanesOffset[lanesOffsetIndex].second);
|
||||
Waypoint.ApplyLateralOffset(LanesOffset[LanesOffsetIndex].second);
|
||||
|
||||
laneZeroPoints.Add(waypoint);
|
||||
LaneZeroPoints.Add(Waypoint);
|
||||
}
|
||||
|
||||
return laneZeroPoints;
|
||||
return LaneZeroPoints;
|
||||
}
|
||||
|
||||
TArray<TArray<FVector>> AOpenDriveActor::GenerateRightLaneWaypoints(
|
||||
const carla::road::element::RoadSegment *road,
|
||||
const TArray<carla::road::element::DirectedPoint> &laneZeroPoints)
|
||||
TArray<TArray<AOpenDriveActor::DirectedPoint>> AOpenDriveActor::GenerateRightLaneWaypoints(
|
||||
const RoadSegment *road,
|
||||
const TArray<DirectedPoint> &laneZeroPoints)
|
||||
{
|
||||
const carla::road::element::RoadInfoLane *lanesInfo =
|
||||
road->GetInfo<carla::road::element::RoadInfoLane>(0.0);
|
||||
const RoadInfoLane *lanesInfo =
|
||||
road->GetInfo<RoadInfoLane>(0.0);
|
||||
std::vector<int> rightLanes =
|
||||
lanesInfo->getLanesIDs(carla::road::element::RoadInfoLane::which_lane_e::Right);
|
||||
lanesInfo->getLanesIDs(RoadInfoLane::which_lane_e::Right);
|
||||
|
||||
TArray<TArray<FVector>> retWaypoints;
|
||||
TArray<TArray<DirectedPoint>> retWaypoints;
|
||||
double currentOffset = 0.0;
|
||||
|
||||
for (size_t j = 0; j < rightLanes.size(); ++j)
|
||||
{
|
||||
const carla::road::element::LaneInfo *laneInfo = lanesInfo->getLane(rightLanes[j]);
|
||||
currentOffset += laneInfo->_width * 0.5;
|
||||
TArray<FVector> roadWaypoints;
|
||||
const LaneInfo *laneInfo = lanesInfo->getLane(rightLanes[j]);
|
||||
const float HalfWidth = laneInfo->_width * 0.5;
|
||||
|
||||
currentOffset += HalfWidth;
|
||||
if (laneInfo->_type == "driving")
|
||||
{
|
||||
TArray<DirectedPoint> roadWaypoints;
|
||||
for (int i = 0; i < laneZeroPoints.Num(); ++i)
|
||||
{
|
||||
carla::road::element::DirectedPoint currentPoint = laneZeroPoints[i];
|
||||
DirectedPoint currentPoint = laneZeroPoints[i];
|
||||
currentPoint.ApplyLateralOffset(-currentOffset);
|
||||
roadWaypoints.Add(currentPoint.location);
|
||||
roadWaypoints.Add(currentPoint);
|
||||
}
|
||||
|
||||
if (roadWaypoints.Num() >= 2)
|
||||
{
|
||||
retWaypoints.Add(roadWaypoints);
|
||||
}
|
||||
}
|
||||
|
||||
currentOffset += laneInfo->_width * 0.5;
|
||||
currentOffset += HalfWidth;
|
||||
}
|
||||
|
||||
return retWaypoints;
|
||||
}
|
||||
|
||||
TArray<TArray<FVector>> AOpenDriveActor::GenerateLeftLaneWaypoints(
|
||||
const carla::road::element::RoadSegment *road,
|
||||
const TArray<carla::road::element::DirectedPoint> &laneZeroPoints)
|
||||
TArray<TArray<AOpenDriveActor::DirectedPoint>> AOpenDriveActor::GenerateLeftLaneWaypoints(
|
||||
const RoadSegment *road,
|
||||
const TArray<DirectedPoint> &laneZeroPoints)
|
||||
{
|
||||
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);
|
||||
using CarlaMath = carla::geom::Math;
|
||||
|
||||
TArray<TArray<FVector>> retWaypoints;
|
||||
const RoadInfoLane *lanesInfo =
|
||||
road->GetInfo<RoadInfoLane>(0.0);
|
||||
std::vector<int> leftLanes = lanesInfo->getLanesIDs(RoadInfoLane::which_lane_e::Left);
|
||||
|
||||
TArray<TArray<DirectedPoint>> retWaypoints;
|
||||
double currentOffset = 0.0;
|
||||
|
||||
for (size_t j = 0; j < leftLanes.size(); ++j)
|
||||
{
|
||||
const carla::road::element::LaneInfo *laneInfo = lanesInfo->getLane(leftLanes[j]);
|
||||
currentOffset += laneInfo->_width * 0.5;
|
||||
TArray<FVector> roadWaypoints;
|
||||
const LaneInfo *laneInfo = lanesInfo->getLane(leftLanes[j]);
|
||||
const float HalfWidth = laneInfo->_width * 0.5;
|
||||
|
||||
currentOffset += HalfWidth;
|
||||
if (laneInfo->_type == "driving")
|
||||
{
|
||||
TArray<DirectedPoint> roadWaypoints;
|
||||
for (int i = 0; i < laneZeroPoints.Num(); ++i)
|
||||
{
|
||||
carla::road::element::DirectedPoint currentPoint = laneZeroPoints[i];
|
||||
DirectedPoint currentPoint = laneZeroPoints[i];
|
||||
currentPoint.ApplyLateralOffset(currentOffset);
|
||||
roadWaypoints.Add(currentPoint.location);
|
||||
if (currentPoint.tangent + CarlaMath::pi() < CarlaMath::pi_double())
|
||||
{
|
||||
currentPoint.tangent += CarlaMath::pi();
|
||||
}
|
||||
else
|
||||
{
|
||||
currentPoint.tangent -= CarlaMath::pi();
|
||||
}
|
||||
roadWaypoints.Add(currentPoint);
|
||||
}
|
||||
|
||||
if (roadWaypoints.Num() >= 2)
|
||||
{
|
||||
Algo::Reverse(roadWaypoints);
|
||||
retWaypoints.Add(roadWaypoints);
|
||||
}
|
||||
}
|
||||
|
||||
currentOffset += laneInfo->_width * 0.5;
|
||||
currentOffset += HalfWidth;
|
||||
}
|
||||
return retWaypoints;
|
||||
}
|
||||
|
||||
void AOpenDriveActor::GenerateWaypointsRoad(const carla::road::element::RoadSegment *road)
|
||||
void AOpenDriveActor::GenerateWaypointsRoad(const RoadSegment *road)
|
||||
{
|
||||
const carla::road::element::RoadGeneralInfo *generalInfo =
|
||||
road->GetInfo<carla::road::element::RoadGeneralInfo>(0.0);
|
||||
const RoadGeneralInfo *generalInfo =
|
||||
road->GetInfo<RoadGeneralInfo>(0.0);
|
||||
if (generalInfo->GetJunctionId() > -1)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
TArray<carla::road::element::DirectedPoint> laneZeroPoints = GenerateLaneZeroPoints(road);
|
||||
TArray<DirectedPoint> laneZeroPoints = GenerateLaneZeroPoints(road);
|
||||
|
||||
TArray<TArray<FVector>> rightLaneWaypoints = GenerateRightLaneWaypoints(road, laneZeroPoints);
|
||||
TArray<TArray<FVector>> leftLaneWaypoints = GenerateLeftLaneWaypoints(road, laneZeroPoints);
|
||||
TArray<TArray<DirectedPoint>> rightLaneWaypoints = GenerateRightLaneWaypoints(road, laneZeroPoints);
|
||||
TArray<TArray<DirectedPoint>> leftLaneWaypoints = GenerateLeftLaneWaypoints(road, laneZeroPoints);
|
||||
|
||||
for (int i = 0; i < rightLaneWaypoints.Num(); ++i)
|
||||
{
|
||||
|
|
@ -277,17 +380,17 @@ void AOpenDriveActor::GenerateWaypointsRoad(const carla::road::element::RoadSegm
|
|||
}
|
||||
|
||||
void AOpenDriveActor::GenerateWaypointsJunction(
|
||||
const carla::road::element::RoadSegment *road,
|
||||
TArray<TArray<FVector>> &out_waypoints)
|
||||
const RoadSegment *road,
|
||||
TArray<TArray<DirectedPoint>> &out_waypoints)
|
||||
{
|
||||
const carla::road::element::RoadGeneralInfo *generalInfo =
|
||||
road->GetInfo<carla::road::element::RoadGeneralInfo>(0.0);
|
||||
const RoadGeneralInfo *generalInfo =
|
||||
road->GetInfo<RoadGeneralInfo>(0.0);
|
||||
if (generalInfo->GetJunctionId() == -1)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
TArray<carla::road::element::DirectedPoint> laneZeroPoints = GenerateLaneZeroPoints(road);
|
||||
TArray<DirectedPoint> laneZeroPoints = GenerateLaneZeroPoints(road);
|
||||
out_waypoints = GenerateRightLaneWaypoints(road, laneZeroPoints);
|
||||
|
||||
if (out_waypoints.Num() == 0)
|
||||
|
|
@ -295,3 +398,49 @@ void AOpenDriveActor::GenerateWaypointsJunction(
|
|||
out_waypoints = GenerateLeftLaneWaypoints(road, laneZeroPoints);
|
||||
}
|
||||
}
|
||||
|
||||
void AOpenDriveActor::DebugRoutes() const
|
||||
{
|
||||
for (int i = 0; i < RoutePlanners.Num(); ++i)
|
||||
{
|
||||
if (RoutePlanners[i] != nullptr)
|
||||
{
|
||||
RoutePlanners[i]->DrawRoutes();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void AOpenDriveActor::RemoveDebugRoutes() const
|
||||
{
|
||||
#if WITH_EDITOR
|
||||
FlushPersistentDebugLines(GetWorld());
|
||||
#endif // WITH_EDITOR
|
||||
}
|
||||
|
||||
void AOpenDriveActor::AddSpawners()
|
||||
{
|
||||
for (int i = 0; i < RoutePlanners.Num(); ++i)
|
||||
{
|
||||
if (RoutePlanners[i] != nullptr)
|
||||
{
|
||||
FTransform Trans = RoutePlanners[i]->GetActorTransform();
|
||||
AVehicleSpawnPoint *Spawner = GetWorld()->SpawnActor<AVehicleSpawnPoint>();
|
||||
Spawner->SetActorRotation(Trans.GetRotation());
|
||||
Spawner->SetActorLocation(Trans.GetTranslation() + FVector(0.0f, 0.0f, SpawnersHeight));
|
||||
VehicleSpawners.Add(Spawner);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void AOpenDriveActor::RemoveSpawners()
|
||||
{
|
||||
const int vs_num = VehicleSpawners.Num();
|
||||
for (int i = 0; i < vs_num; i++)
|
||||
{
|
||||
if (VehicleSpawners[i] != nullptr)
|
||||
{
|
||||
VehicleSpawners[i]->Destroy();
|
||||
}
|
||||
}
|
||||
VehicleSpawners.Empty();
|
||||
}
|
||||
|
|
|
|||
|
|
@ -9,10 +9,14 @@
|
|||
#include "CoreMinimal.h"
|
||||
#include "GameFramework/Actor.h"
|
||||
|
||||
#include "Components/SceneComponent.h"
|
||||
#include "Components/BillboardComponent.h"
|
||||
|
||||
#include "Traffic/RoutePlanner.h"
|
||||
|
||||
#include "Vehicle/VehicleSpawnPoint.h"
|
||||
|
||||
#include <compiler/disable-ue4-macros.h>
|
||||
#include <carla/geom/Math.h>
|
||||
#include <carla/opendrive/OpenDrive.h>
|
||||
#include <compiler/enable-ue4-macros.h>
|
||||
|
||||
|
|
@ -23,39 +27,98 @@ class CARLA_API AOpenDriveActor : public AActor
|
|||
{
|
||||
GENERATED_BODY()
|
||||
|
||||
protected:
|
||||
// A UBillboardComponent to hold Icon sprite
|
||||
UBillboardComponent* SpriteComponent;
|
||||
// Sprite for the Billboard Component
|
||||
UTexture2D* SpriteTexture;
|
||||
|
||||
private:
|
||||
|
||||
UPROPERTY()
|
||||
TArray<ARoutePlanner *> RoutePlanners;
|
||||
|
||||
UPROPERTY()
|
||||
TArray<AVehicleSpawnPoint *> VehicleSpawners;
|
||||
|
||||
#if WITH_EDITORONLY_DATA
|
||||
/// Generate the road network using an OpenDrive file (named as the current .umap)
|
||||
UPROPERTY(Category = "Generate", EditAnywhere)
|
||||
bool bGenerateRoutes = false;
|
||||
#endif // WITH_EDITORONLY_DATA
|
||||
|
||||
/// Distance between waypoints where the cars will drive
|
||||
UPROPERTY(Category = "Generate", EditAnywhere, meta = (ClampMin = "0.01", UIMin = "0.01"))
|
||||
float RoadAccuracy = 2.0f;
|
||||
|
||||
#if WITH_EDITORONLY_DATA
|
||||
/// Remove the previously generated road network. Also, it will remove spawners if necessary
|
||||
UPROPERTY(Category = "Generate", EditAnywhere)
|
||||
bool bRemoveRoutes = false;
|
||||
#endif // WITH_EDITORONLY_DATA
|
||||
|
||||
/// If true, spawners will be placed when generating the routes
|
||||
UPROPERTY(Category = "Spawners", EditAnywhere)
|
||||
bool bAddSpawners = true;
|
||||
|
||||
/// Determine the height where the spawners will be placed, relative to each RoutePlanner
|
||||
UPROPERTY(Category = "Spawners", EditAnywhere)
|
||||
float SpawnersHeight = 300.0;
|
||||
|
||||
#if WITH_EDITORONLY_DATA
|
||||
/// Remove already placed spawners if necessary
|
||||
UPROPERTY(Category = "Spawners", EditAnywhere)
|
||||
bool bRemoveCurrentSpawners = false;
|
||||
#endif // WITH_EDITORONLY_DATA
|
||||
|
||||
#if WITH_EDITORONLY_DATA
|
||||
/// Show / Hide additional debug information
|
||||
UPROPERTY(Category = "Debug", EditAnywhere)
|
||||
bool bShowDebug = true;
|
||||
#endif // WITH_EDITORONLY_DATA
|
||||
|
||||
public:
|
||||
|
||||
// Sets default values for this actor's properties
|
||||
AOpenDriveActor();
|
||||
using RoadSegment = carla::road::element::RoadSegment;
|
||||
using DirectedPoint = carla::road::element::DirectedPoint;
|
||||
using LaneInfo = carla::road::element::LaneInfo;
|
||||
using RoadGeneralInfo = carla::road::element::RoadGeneralInfo;
|
||||
using RoadInfoLane = carla::road::element::RoadInfoLane;
|
||||
|
||||
AOpenDriveActor(const FObjectInitializer& ObjectInitializer);
|
||||
|
||||
void BuildRoutes();
|
||||
|
||||
virtual void BeginPlay() override;
|
||||
void RemoveRoutes();
|
||||
|
||||
virtual void BeginDestroy() override;
|
||||
void DebugRoutes() const;
|
||||
|
||||
virtual void OnConstruction(const FTransform &transform) override;
|
||||
void RemoveDebugRoutes() const;
|
||||
|
||||
ARoutePlanner *GenerateRoutePlanner(const TArray<FVector> &waypoints);
|
||||
#if WITH_EDITOR
|
||||
void PostEditChangeProperty(struct FPropertyChangedEvent& PropertyChangedEvent);
|
||||
#endif // WITH_EDITOR
|
||||
|
||||
TArray<carla::road::element::DirectedPoint> GenerateLaneZeroPoints(
|
||||
const carla::road::element::RoadSegment *road);
|
||||
ARoutePlanner *GenerateRoutePlanner(const TArray<DirectedPoint> &waypoints);
|
||||
|
||||
TArray<TArray<FVector>> GenerateRightLaneWaypoints(
|
||||
const carla::road::element::RoadSegment *road,
|
||||
const TArray<carla::road::element::DirectedPoint> &laneZeroPoints);
|
||||
TArray<DirectedPoint> GenerateLaneZeroPoints(
|
||||
const RoadSegment *road);
|
||||
|
||||
TArray<TArray<FVector>> GenerateLeftLaneWaypoints(
|
||||
const carla::road::element::RoadSegment *road,
|
||||
const TArray<carla::road::element::DirectedPoint> &laneZeroPoints);
|
||||
TArray<TArray<DirectedPoint>> GenerateRightLaneWaypoints(
|
||||
const RoadSegment *road,
|
||||
const TArray<DirectedPoint> &laneZeroPoints);
|
||||
|
||||
TArray<TArray<DirectedPoint>> GenerateLeftLaneWaypoints(
|
||||
const RoadSegment *road,
|
||||
const TArray<DirectedPoint> &laneZeroPoints);
|
||||
|
||||
void GenerateWaypointsJunction(
|
||||
const carla::road::element::RoadSegment *road,
|
||||
TArray<TArray<FVector>> &waypoints);
|
||||
const RoadSegment *road,
|
||||
TArray<TArray<DirectedPoint>> &waypoints);
|
||||
|
||||
void GenerateWaypointsRoad(const carla::road::element::RoadSegment *road);
|
||||
void GenerateWaypointsRoad(const RoadSegment *road);
|
||||
|
||||
void AddSpawners();
|
||||
|
||||
void RemoveSpawners();
|
||||
};
|
||||
|
|
|
|||
|
|
@ -65,6 +65,12 @@ ARoutePlanner::ARoutePlanner(const FObjectInitializer &ObjectInitializer)
|
|||
SplineColor = FColor::Black;
|
||||
}
|
||||
|
||||
void ARoutePlanner::BeginDestroy()
|
||||
{
|
||||
CleanRoute();
|
||||
Super::BeginDestroy();
|
||||
}
|
||||
|
||||
#if WITH_EDITOR
|
||||
void ARoutePlanner::PostEditChangeProperty(FPropertyChangedEvent &PropertyChangedEvent)
|
||||
{
|
||||
|
|
@ -94,6 +100,10 @@ void ARoutePlanner::AddRoute(float probability, const TArray<FVector> &routePoin
|
|||
USplineComponent *NewSpline = NewObject<USplineComponent>(this);
|
||||
NewSpline->bHiddenInGame = true;
|
||||
|
||||
#if WITH_EDITOR
|
||||
NewSpline->EditorUnselectedSplineSegmentColor = FLinearColor(0.15f, 0.15f, 0.15f);
|
||||
#endif // WITH_EDITOR
|
||||
|
||||
NewSpline->SetLocationAtSplinePoint(0, routePoints[0], ESplineCoordinateSpace::World, true);
|
||||
NewSpline->SetLocationAtSplinePoint(1, routePoints[1], ESplineCoordinateSpace::World, true);
|
||||
|
||||
|
|
@ -191,10 +201,6 @@ void ARoutePlanner::DrawRoutes()
|
|||
#if WITH_EDITOR
|
||||
for (int i = 0, lenRoutes = Routes.Num(); i < lenRoutes; ++i)
|
||||
{
|
||||
FVector boxCenter = Routes[i]->GetLocationAtSplinePoint(0, ESplineCoordinateSpace::World);
|
||||
boxCenter.Z += i * 101.0f;
|
||||
DrawDebugBox(GetWorld(), boxCenter, TriggerVolume->GetUnscaledBoxExtent() - FVector(10.0f, 10.0f, 10.0f), SplineColor, true);
|
||||
|
||||
for (int j = 0, lenNumPoints = Routes[i]->GetNumberOfSplinePoints() - 1; j < lenNumPoints; ++j)
|
||||
{
|
||||
FVector p0 = Routes[i]->GetLocationAtSplinePoint(j + 0, ESplineCoordinateSpace::World);
|
||||
|
|
|
|||
|
|
@ -26,6 +26,8 @@ public:
|
|||
|
||||
ARoutePlanner(const FObjectInitializer &ObjectInitializer);
|
||||
|
||||
virtual void BeginDestroy() override;
|
||||
|
||||
void Init();
|
||||
|
||||
void SetBoxExtent(const FVector &Extent)
|
||||
|
|
|
|||
Loading…
Reference in New Issue