Updated OpenDriveActor (#1593)

* Fixed OpenDriveActor

* Fixed bebug lines of OpenDriveActor in UE4 editor

* Updated changelog
This commit is contained in:
Marc Garcia Puig 2019-05-02 14:38:18 +02:00 committed by Néstor Subirón
parent 413737887f
commit 8781712eb8
6 changed files with 129 additions and 286 deletions

View File

@ -16,6 +16,7 @@
- Walkers animation is simulated in playback (through speed of walker), so they walk properly.
* Fixed Lidar effectiveness bug in manual_control.py
* Added C++ client example using LibCarla
* Updated OpenDriveActor to use the new Waypoint API
* Fixed wrong units in VehiclePhysicsControl's center of mass
* Several optimizations to the RPC server, now supports a bigger load of async messages

View File

@ -49,6 +49,14 @@ namespace road {
}
}
static double GetDistanceAtEndOfLane(const Lane &lane) {
if (lane.GetId() > 0) {
return lane.GetDistance() + 10.0 * EPSILON;
} else {
return lane.GetDistance() + lane.GetLength() - 10.0 * EPSILON;
}
}
/// Return a waypoint for each drivable lane on @a lane_section.
template <typename FuncT>
static void ForEachDrivableLaneImpl(
@ -377,6 +385,24 @@ namespace road {
return result;
}
std::vector<Waypoint> Map::GetPredecessors(const Waypoint waypoint) const {
const auto &prev_lanes = GetLane(waypoint).GetPreviousLanes();
std::vector<Waypoint> result;
result.reserve(prev_lanes.size());
for (auto *next_lane : prev_lanes) {
RELEASE_ASSERT(next_lane != nullptr);
const auto lane_id = next_lane->GetId();
RELEASE_ASSERT(lane_id != 0);
const auto *section = next_lane->GetLaneSection();
RELEASE_ASSERT(section != nullptr);
const auto *road = next_lane->GetRoad();
RELEASE_ASSERT(road != nullptr);
const auto distance = GetDistanceAtEndOfLane(*next_lane);
result.emplace_back(Waypoint{road->GetId(), section->GetId(), lane_id, distance});
}
return result;
}
std::vector<Waypoint> Map::GetNext(
const Waypoint waypoint,
const double distance) const {
@ -445,6 +471,33 @@ namespace road {
return result;
}
std::vector<Waypoint> Map::GenerateWaypointsOnRoadEntries() const {
std::vector<Waypoint> result;
for (const auto &pair : _data.GetRoads()) {
const auto &road = pair.second;
// right lanes start at s 0
for (const auto &lane_section : road.GetLaneSectionsAt(0.0)) {
for (const auto &lane : lane_section.GetLanes()) {
// add only the right (negative) lanes
if (lane.first < 0 && lane.second.GetType() == Lane::LaneType::Driving) {
result.emplace_back(Waypoint{ road.GetId(), lane_section.GetId(), lane.second.GetId(), 0.0 });
}
}
}
// left lanes start at s max
const auto road_len = road.GetLength();
for (const auto &lane_section : road.GetLaneSectionsAt(road_len)) {
for (const auto &lane : lane_section.GetLanes()) {
// add only the left (positive) lanes
if (lane.first > 0 && lane.second.GetType() == Lane::LaneType::Driving) {
result.emplace_back(Waypoint{ road.GetId(), lane_section.GetId(), lane.second.GetId(), road_len });
}
}
}
}
return result;
}
std::vector<std::pair<Waypoint, Waypoint>> Map::GenerateTopology() const {
std::vector<std::pair<Waypoint, Waypoint>> result;
for (const auto &pair : _data.GetRoads()) {

View File

@ -83,6 +83,7 @@ namespace road {
/// successor lane; i.e., the list of each waypoint in the next road segment
/// that a vehicle could drive from @a waypoint.
std::vector<Waypoint> GetSuccessors(Waypoint waypoint) const;
std::vector<Waypoint> GetPredecessors(Waypoint waypoint) const;
/// Return the list of waypoints at @a distance such that a vehicle at @a
/// waypoint could drive to.
@ -97,6 +98,9 @@ namespace road {
/// Generate all the waypoints in @a map separated by @a approx_distance.
std::vector<Waypoint> GenerateWaypoints(double approx_distance) const;
/// Generate waypoints on each @a lane at the start of each @a road
std::vector<Waypoint> GenerateWaypointsOnRoadEntries() const;
/// Generate the minimum set of waypoints that define the topology of @a
/// map. The waypoints are placed at the entrance of each lane.
std::vector<std::pair<Waypoint, Waypoint>> GenerateTopology() const;

View File

@ -63,9 +63,10 @@ namespace road {
Lane *GetPrevLane(const double s, const LaneId lane_id);
// get the start and end section with a lan id
/// Get the start section (from road coordinates s) given a @a lane id
LaneSection *GetStartSection(LaneId id);
/// Get the end section (from road coordinates s) given a @a lane id
LaneSection *GetEndSection(LaneId id);
std::vector<Road *> GetNexts() const;

View File

@ -16,57 +16,13 @@
#include <carla/rpc/String.h>
#include <compiler/enable-ue4-macros.h>
#include <algorithm>
#include <unordered_set>
/*static TArray<FVector> WaypointVector2FVectorArray(
const std::vector<carla::road::element::Waypoint> &Waypoints,
const float TriggersHeight)
{
TArray<FVector> Positions;
Positions.Reserve(Waypoints.size());
for (int i = 0; i < Waypoints.size(); ++i)
{
// Add the trigger height because the z position of the points does not
// influence on the driver AI and is easy to visualize in the editor
Positions.Add(Waypoints[i].ComputeTransform().location +
FVector(0.f, 0.f, TriggersHeight));
}
return Positions;
}*/
AOpenDriveActor::AOpenDriveActor(const FObjectInitializer &ObjectInitializer)
: Super(ObjectInitializer)
{
PrimaryActorTick.bCanEverTick = false;
#if WITH_EDITORONLY_DATA
static ConstructorHelpers::FObjectFinder<UBlueprint> TrafficLightBP(TEXT(
"Blueprint'/Game/Carla/Static/TrafficSigns/Streetlights_01/BP_TrafficLightPole.BP_TrafficLightPole'"));
TrafficLightBlueprintClass = (UClass *) TrafficLightBP.Object->GeneratedClass;
static ConstructorHelpers::FObjectFinder<UBlueprint> TrafficGroupBP(TEXT(
"Blueprint'/Game/Carla/Static/TrafficSigns/Streetlights_01/BP_TrafficLightPoleGroup.BP_TrafficLightPoleGroup'"));
TrafficGroupBlueprintClass = (UClass *) TrafficGroupBP.Object->GeneratedClass;
static ConstructorHelpers::FObjectFinder<UBlueprint> TrafficSign30BP(TEXT(
"Blueprint'/Game/Carla/Static/TrafficSigns/PostSigns/Round/SpeedLimiters/BP_SpeedLimit30.BP_SpeedLimit30'"));
TrafficSign30BlueprintClass = (UClass *) TrafficSign30BP.Object->GeneratedClass;
/*static ConstructorHelpers::FObjectFinder<UBlueprint> TrafficSign40BP(TEXT(
"Blueprint'/Game/Carla/Static/TrafficSigns/PostSigns/Round/SpeedLimiters/BP_SpeedLimit40.BP_SpeedLimit40'"));
TrafficSign40BlueprintClass = (UClass *) TrafficSign40BP.Object->GeneratedClass;*/
static ConstructorHelpers::FObjectFinder<UBlueprint> TrafficSign60BP(TEXT(
"Blueprint'/Game/Carla/Static/TrafficSigns/PostSigns/Round/SpeedLimiters/BP_SpeedLimit60.BP_SpeedLimit60'"));
TrafficSign60BlueprintClass = (UClass *) TrafficSign60BP.Object->GeneratedClass;
static ConstructorHelpers::FObjectFinder<UBlueprint> TrafficSign90BP(TEXT(
"Blueprint'/Game/Carla/Static/TrafficSigns/PostSigns/Round/SpeedLimiters/BP_SpeedLimit90.BP_SpeedLimit90'"));
TrafficSign90BlueprintClass = (UClass *) TrafficSign90BP.Object->GeneratedClass;
static ConstructorHelpers::FObjectFinder<UBlueprint> TrafficSign100BP(TEXT(
"Blueprint'/Game/Carla/Static/TrafficSigns/PostSigns/Round/SpeedLimiters/BP_SpeedLimit100.BP_SpeedLimit100'"));
TrafficSign100BlueprintClass = (UClass *) TrafficSign100BP.Object->GeneratedClass;
#endif // WITH_EDITORONLY_DATA
// Structure to hold one-time initialization
static struct FConstructorStatics
@ -191,219 +147,90 @@ void AOpenDriveActor::BuildRoutes(FString MapName)
}
// List with waypoints, each one at the end of each lane of the map
const std::vector<std::pair<Waypoint, Waypoint>> Topology =
map->GenerateTopology();
const std::vector<Waypoint> LaneWaypoints =
map->GenerateWaypointsOnRoadEntries();
// Since we are going to iterate all the successors of all the lanes, we need
// a vector to store the already visited lanes. Lanes can be successors of
// multiple other lanes
std::vector<Waypoint> AlreadyVisited;
std::unordered_map<Waypoint, std::vector<Waypoint>> PredecessorMap;
std::unordered_set<Waypoint> WpLaneStartList;
for (auto &&WpPair : Topology)
for (auto &Wp : LaneWaypoints)
{
WpLaneStartList.emplace(WpPair.first);
WpLaneStartList.emplace(WpPair.second);
const auto PredecessorsList = map->GetPredecessors(Wp);
if (PredecessorsList.empty())
{
continue;
}
const auto MinRoadId = *std::min_element(
PredecessorsList.begin(),
PredecessorsList.end(),
[](const auto &WaypointA, const auto &WaypointB) {
return WaypointA.road_id < WaypointB.road_id;
});
PredecessorMap[MinRoadId].emplace_back(Wp);
}
for (auto &&Wp : WpLaneStartList)
for (auto &&PredecessorWp : PredecessorMap)
{
std::vector<Waypoint> Successors = map->GetSuccessors(Wp);
// // The RoutePlanner will be created only if some route must be added to it
// // so no one will be created unnecessarily
ARoutePlanner *RoutePlanner = nullptr;
// // Fill the RoutePlanner with all the needed roads
for (auto &&Successor : Successors)
{
const auto RoadId = Successor.road_id;
const auto LaneId = Successor.lane_id;
for (auto &&Wp : PredecessorWp.second)
{
std::vector<Waypoint> Waypoints;
auto CurrentWp = Wp;
// // Create an identifier of the current lane
//const auto Identifier = std::make_pair(RoadId, LaneId);
// // If Identifier does not exist in AlreadyVisited we haven't visited the
// // lane
if (!std::any_of(AlreadyVisited.begin(), AlreadyVisited.end(), [&](auto i) {
return (i.road_id == Successor.road_id && i.lane_id == Successor.lane_id && i.section_id == Successor.section_id);
}))
do
{
Waypoints.emplace_back(CurrentWp);
const auto Successors = map->GetNext(CurrentWp, RoadAccuracy);
if (Successors.empty())
{
// Add the identifier as visited
AlreadyVisited.emplace_back(Successor);
break;
}
if (Successors.front().road_id != Wp.road_id)
{
break;
}
CurrentWp = Successors.front();
} while (CurrentWp.road_id == Wp.road_id);
const double MaxDist = map->GetLane(Successor).GetLength();
//const double MaxDist = 0; // workarround while changing the WaypointAPI
// connect the last wp of the current toad to the first wp of the following road
const auto FollowingWp = map->GetSuccessors(CurrentWp);
if (!FollowingWp.empty())
{
Waypoints.emplace_back(FollowingWp.front());
}
std::vector<Waypoint> Waypoints;
// if(RoutePlanner==nullptr) {
// AlreadyVisited.emplace_back(Wp);
// Waypoints.emplace_back(Wp);
// }
Waypoints.emplace_back(Successor);
if (Waypoints.size() >= 2)
{
TArray<FVector> Positions;
Positions.Reserve(Waypoints.size());
for (int i = 0; i < Waypoints.size(); ++i)
{
// Add the trigger height because the z position of the points does not
// influence on the driver AI and is easy to visualize in the editor
Positions.Add(map->ComputeTransform(Waypoints[i]).location +
FVector(0.f, 0.f, TriggersHeight));
}
double Dist = RoadAccuracy;
while (Dist < MaxDist) {
auto NewWaypointList = map->GetNext(Successor, Dist);
for(Waypoint distWP : NewWaypointList) {
Waypoints.emplace_back(distWP);
AlreadyVisited.emplace_back(distWP);
}
Dist += RoadAccuracy;
}
auto NewWaypointList = map->GetNext(Successor, MaxDist);
// if((map->IsJunction(Successor.road_id) && NewWaypointList.size()>1) || (!map->IsJunction(Successor.road_id) && NewWaypointList.size()==1))
for(Waypoint distWP : NewWaypointList) {
Waypoints.emplace_back(NewWaypointList[0]);
}
// If the route planner does not exist, create it
if (RoutePlanner == nullptr )
{
const auto WpTransform = map->ComputeTransform(Wp);
RoutePlanner = GetWorld()->SpawnActor<ARoutePlanner>();
RoutePlanner->bIsIntersection = map->IsJunction(Wp.road_id);
RoutePlanner->SetBoxExtent(FVector(70.f, 70.f, 50.f));
RoutePlanner->SetActorRotation(WpTransform.rotation);
RoutePlanner->SetActorLocation(WpTransform.location +
FVector(0.f, 0.f, TriggersHeight));
}
if(Waypoints.size() >= 2) {
TArray<FVector> Positions;
Positions.Reserve(Waypoints.size());
for (int i = 0; i < Waypoints.size(); ++i)
{
// Add the trigger height because the z position of the points does not
// influence on the driver AI and is easy to visualize in the editor
Positions.Add(map->ComputeTransform(Waypoints[i]).location +
FVector(0.f, 0.f, TriggersHeight));
}
// If the route planner does not exist, create it
if (RoutePlanner == nullptr )
{
RoutePlanner = GetWorld()->SpawnActor<ARoutePlanner>();
RoutePlanner->bIsIntersection = std::any_of(Successors.begin(), Successors.end(), [&map](auto w) {
return map->IsJunction(w.road_id);
});
RoutePlanner->SetBoxExtent(FVector(70.f, 70.f, 50.f));
RoutePlanner->SetActorRotation(map->ComputeTransform(Successor).rotation);
RoutePlanner->SetActorLocation(map->ComputeTransform(Successor).location +
FVector(0.f, 0.f, TriggersHeight));
}
if(RoutePlanner!=nullptr) RoutePlanner->AddRoute(1.f, Positions);
if(RoutePlanner!=nullptr) RoutePlanners.Add(RoutePlanner);
}
if (RoutePlanner != nullptr)
{
RoutePlanner->AddRoute(1.f, Positions);
RoutePlanners.Add(RoutePlanner);
}
}
}
}
// const std::vector<TrafficGroup> TrafficLightGroup = map.GetTrafficGroups();
// for (TrafficGroup CurrentTrafficLightGroup : TrafficLightGroup)
// {
// double RedTime = CurrentTrafficLightGroup.red_time;
// double YellowTime = CurrentTrafficLightGroup.yellow_time;
// double GreenTime = CurrentTrafficLightGroup.green_time;
// FActorSpawnParameters SpawnParams;
// FOutputDeviceNull ar;
// AActor *SpawnedTrafficGroup = GetWorld()->SpawnActor<AActor>(TrafficGroupBlueprintClass,
// FVector(0, 0, 0),
// FRotator(0, 0, 0),
// SpawnParams);
// FString SetTrafficTimesCommand = FString::Printf(TEXT("SetTrafficTimes %f %f %f"),
// RedTime, YellowTime, GreenTime);
// SpawnedTrafficGroup->CallFunctionByNameWithArguments(*SetTrafficTimesCommand, ar, NULL, true);
// for (TrafficLight CurrentTrafficLight : CurrentTrafficLightGroup.traffic_lights)
// {
// FVector TLPos =
// FVector(CurrentTrafficLight.x_pos, CurrentTrafficLight.y_pos, CurrentTrafficLight.z_pos);
// FRotator TLRot = FRotator(CurrentTrafficLight.x_rot,
// CurrentTrafficLight.z_rot,
// CurrentTrafficLight.y_rot);
// AActor *SpawnedTrafficLight = GetWorld()->SpawnActor<AActor>(TrafficLightBlueprintClass,
// TLPos,
// TLRot,
// SpawnParams);
// FString AddTrafficLightCommand = FString::Printf(TEXT("AddTrafficLightPole %s"),
// *SpawnedTrafficLight->GetName());
// SpawnedTrafficGroup->CallFunctionByNameWithArguments(*AddTrafficLightCommand, ar, NULL, true);
// PersistentTrafficLights.Push(SpawnedTrafficGroup);
// SpawnedTrafficLight->CallFunctionByNameWithArguments(TEXT("InitData"), ar, NULL, true);
// for (TrafficBoxComponent TfBoxComponent : CurrentTrafficLight.box_areas)
// {
// FVector TLBoxPos = FVector(TfBoxComponent.x_pos,
// TfBoxComponent.y_pos,
// TfBoxComponent.z_pos);
// FRotator TLBoxRot = FRotator(TfBoxComponent.x_rot,
// TfBoxComponent.z_rot,
// TfBoxComponent.y_rot);
// FString BoxCommand = FString::Printf(TEXT("SetBoxLocationAndRotation %f %f %f %f %f %f"),
// TLBoxPos.X,
// TLBoxPos.Y,
// TLBoxPos.Z,
// TLBoxRot.Pitch,
// TLBoxRot.Roll,
// TLBoxRot.Yaw);
// SpawnedTrafficLight->CallFunctionByNameWithArguments(*BoxCommand, ar, NULL, true);
// PersistentTrafficLights.Push(SpawnedTrafficLight);
// }
// }
// }
// const std::vector<TrafficSign> TrafficSigns = map.GetTrafficSigns();
// for (TrafficSign CurrentTrafficSign : TrafficSigns)
// {
// //switch()
// AActor* SignActor;
// FOutputDeviceNull ar;
// FVector TSLoc = FVector(CurrentTrafficSign.x_pos, CurrentTrafficSign.y_pos, CurrentTrafficSign.z_pos);
// FRotator TSRot = FRotator(CurrentTrafficSign.x_rot, CurrentTrafficSign.z_rot, CurrentTrafficSign.y_rot);
// FActorSpawnParameters SpawnParams;
// switch(CurrentTrafficSign.speed) {
// case 30:
// SignActor = GetWorld()->SpawnActor<AActor>(TrafficSign30BlueprintClass,
// TSLoc,
// TSRot,
// SpawnParams);
// break;
// case 60:
// SignActor = GetWorld()->SpawnActor<AActor>(TrafficSign60BlueprintClass,
// TSLoc,
// TSRot,
// SpawnParams);
// break;
// case 90:
// SignActor = GetWorld()->SpawnActor<AActor>(TrafficSign90BlueprintClass,
// TSLoc,
// TSRot,
// SpawnParams);
// break;
// case 100:
// SignActor = GetWorld()->SpawnActor<AActor>(TrafficSign100BlueprintClass,
// TSLoc,
// TSRot,
// SpawnParams);
// break;
// default:
// FString errorMessage = "Traffic Sign not found. Posibilities: 30, 60, 90, 100";
// UE_LOG(LogCarla, Warning, TEXT("%s"), *errorMessage);
// break;
// }
// PersistentTrafficSigns.Push(SignActor);
// for (TrafficBoxComponent TfBoxComponent : CurrentTrafficSign.box_areas)
// {
// FVector TLBoxPos = FVector(TfBoxComponent.x_pos,
// TfBoxComponent.y_pos,
// TfBoxComponent.z_pos);
// FRotator TLBoxRot = FRotator(TfBoxComponent.x_rot,
// TfBoxComponent.z_rot,
// TfBoxComponent.y_rot);
// FString BoxCommand = FString::Printf(TEXT("SetBoxLocationAndRotation %f %f %f %f %f %f"),
// TLBoxPos.X,
// TLBoxPos.Y,
// TLBoxPos.Z,
// TLBoxRot.Pitch,
// TLBoxRot.Roll,
// TLBoxRot.Yaw);
// SignActor->CallFunctionByNameWithArguments(*BoxCommand, ar, NULL, true);
// }
// }
}
void AOpenDriveActor::RemoveRoutes()
@ -417,22 +244,6 @@ void AOpenDriveActor::RemoveRoutes()
}
}
RoutePlanners.Empty();
const int tl_num = PersistentTrafficLights.Num();
for (int i = 0; i < tl_num; i++)
{
if(PersistentTrafficLights[i] != nullptr) {
PersistentTrafficLights[i]->Destroy();
}
}
PersistentTrafficLights.Empty();
const int ts_num = PersistentTrafficSigns.Num();
for (int i = 0; i < ts_num; i++)
{
if(PersistentTrafficSigns[i] != nullptr) {
PersistentTrafficSigns[i]->Destroy();
}
}
PersistentTrafficSigns.Empty();
}
void AOpenDriveActor::DebugRoutes() const
@ -450,7 +261,7 @@ void AOpenDriveActor::RemoveDebugRoutes() const
{
#if WITH_EDITOR
FlushPersistentDebugLines(GetWorld());
#endif // WITH_EDITOR
#endif // WITH_EDITOR
}
void AOpenDriveActor::AddSpawners()

View File

@ -39,33 +39,6 @@ private:
UPROPERTY()
TArray<AVehicleSpawnPoint *> VehicleSpawners;
UPROPERTY()
TArray<AActor *> PersistentTrafficLights;
UPROPERTY()
TArray<AActor *> PersistentTrafficSigns;
UPROPERTY()
TSubclassOf<class AActor> TrafficLightBlueprintClass;
UPROPERTY()
TSubclassOf<class AActor> TrafficGroupBlueprintClass;
UPROPERTY()
TSubclassOf<class AActor> TrafficSign30BlueprintClass;
UPROPERTY()
TSubclassOf<class AActor> TrafficSign40BlueprintClass;
UPROPERTY()
TSubclassOf<class AActor> TrafficSign60BlueprintClass;
UPROPERTY()
TSubclassOf<class AActor> TrafficSign90BlueprintClass;
UPROPERTY()
TSubclassOf<class AActor> TrafficSign100BlueprintClass;
#if WITH_EDITORONLY_DATA
/// Generate the road network using an OpenDrive file (named as the current
/// .umap)