From 8781712eb816e051309fa3350d086d2b8ae796a3 Mon Sep 17 00:00:00 2001 From: Marc Garcia Puig Date: Thu, 2 May 2019 14:38:18 +0200 Subject: [PATCH 1/2] Updated OpenDriveActor (#1593) * Fixed OpenDriveActor * Fixed bebug lines of OpenDriveActor in UE4 editor * Updated changelog --- CHANGELOG.md | 1 + LibCarla/source/carla/road/Map.cpp | 53 +++ LibCarla/source/carla/road/Map.h | 4 + LibCarla/source/carla/road/Road.h | 3 +- .../Carla/Source/Carla/OpenDriveActor.cpp | 327 ++++-------------- .../Carla/Source/Carla/OpenDriveActor.h | 27 -- 6 files changed, 129 insertions(+), 286 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 675ada91a..bb6984951 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -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 diff --git a/LibCarla/source/carla/road/Map.cpp b/LibCarla/source/carla/road/Map.cpp index f88f90b70..4156ecaf3 100644 --- a/LibCarla/source/carla/road/Map.cpp +++ b/LibCarla/source/carla/road/Map.cpp @@ -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 static void ForEachDrivableLaneImpl( @@ -377,6 +385,24 @@ namespace road { return result; } + std::vector Map::GetPredecessors(const Waypoint waypoint) const { + const auto &prev_lanes = GetLane(waypoint).GetPreviousLanes(); + std::vector 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 Map::GetNext( const Waypoint waypoint, const double distance) const { @@ -445,6 +471,33 @@ namespace road { return result; } + std::vector Map::GenerateWaypointsOnRoadEntries() const { + std::vector 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> Map::GenerateTopology() const { std::vector> result; for (const auto &pair : _data.GetRoads()) { diff --git a/LibCarla/source/carla/road/Map.h b/LibCarla/source/carla/road/Map.h index 984d12e18..360b8591a 100644 --- a/LibCarla/source/carla/road/Map.h +++ b/LibCarla/source/carla/road/Map.h @@ -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 GetSuccessors(Waypoint waypoint) const; + std::vector 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 GenerateWaypoints(double approx_distance) const; + /// Generate waypoints on each @a lane at the start of each @a road + std::vector 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> GenerateTopology() const; diff --git a/LibCarla/source/carla/road/Road.h b/LibCarla/source/carla/road/Road.h index 70589d08c..3ac955291 100644 --- a/LibCarla/source/carla/road/Road.h +++ b/LibCarla/source/carla/road/Road.h @@ -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 GetNexts() const; diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/OpenDriveActor.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/OpenDriveActor.cpp index 635f9c8d7..d32ed98e9 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/OpenDriveActor.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/OpenDriveActor.cpp @@ -16,57 +16,13 @@ #include #include +#include #include -/*static TArray WaypointVector2FVectorArray( - const std::vector &Waypoints, - const float TriggersHeight) -{ - TArray 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 TrafficLightBP(TEXT( - "Blueprint'/Game/Carla/Static/TrafficSigns/Streetlights_01/BP_TrafficLightPole.BP_TrafficLightPole'")); - TrafficLightBlueprintClass = (UClass *) TrafficLightBP.Object->GeneratedClass; - - static ConstructorHelpers::FObjectFinder TrafficGroupBP(TEXT( - "Blueprint'/Game/Carla/Static/TrafficSigns/Streetlights_01/BP_TrafficLightPoleGroup.BP_TrafficLightPoleGroup'")); - TrafficGroupBlueprintClass = (UClass *) TrafficGroupBP.Object->GeneratedClass; - - static ConstructorHelpers::FObjectFinder TrafficSign30BP(TEXT( - "Blueprint'/Game/Carla/Static/TrafficSigns/PostSigns/Round/SpeedLimiters/BP_SpeedLimit30.BP_SpeedLimit30'")); - TrafficSign30BlueprintClass = (UClass *) TrafficSign30BP.Object->GeneratedClass; - - /*static ConstructorHelpers::FObjectFinder TrafficSign40BP(TEXT( - "Blueprint'/Game/Carla/Static/TrafficSigns/PostSigns/Round/SpeedLimiters/BP_SpeedLimit40.BP_SpeedLimit40'")); - TrafficSign40BlueprintClass = (UClass *) TrafficSign40BP.Object->GeneratedClass;*/ - - static ConstructorHelpers::FObjectFinder TrafficSign60BP(TEXT( - "Blueprint'/Game/Carla/Static/TrafficSigns/PostSigns/Round/SpeedLimiters/BP_SpeedLimit60.BP_SpeedLimit60'")); - TrafficSign60BlueprintClass = (UClass *) TrafficSign60BP.Object->GeneratedClass; - - static ConstructorHelpers::FObjectFinder TrafficSign90BP(TEXT( - "Blueprint'/Game/Carla/Static/TrafficSigns/PostSigns/Round/SpeedLimiters/BP_SpeedLimit90.BP_SpeedLimit90'")); - TrafficSign90BlueprintClass = (UClass *) TrafficSign90BP.Object->GeneratedClass; - - static ConstructorHelpers::FObjectFinder 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> Topology = - map->GenerateTopology(); + const std::vector 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 AlreadyVisited; + std::unordered_map> PredecessorMap; - std::unordered_set 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 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 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 Waypoints; - // if(RoutePlanner==nullptr) { - // AlreadyVisited.emplace_back(Wp); - // Waypoints.emplace_back(Wp); - // } - Waypoints.emplace_back(Successor); + if (Waypoints.size() >= 2) + { + TArray 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(); + 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 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(); - 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 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(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(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 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(TrafficSign30BlueprintClass, - // TSLoc, - // TSRot, - // SpawnParams); - // break; - // case 60: - // SignActor = GetWorld()->SpawnActor(TrafficSign60BlueprintClass, - // TSLoc, - // TSRot, - // SpawnParams); - // break; - // case 90: - // SignActor = GetWorld()->SpawnActor(TrafficSign90BlueprintClass, - // TSLoc, - // TSRot, - // SpawnParams); - // break; - // case 100: - // SignActor = GetWorld()->SpawnActor(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() diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/OpenDriveActor.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/OpenDriveActor.h index c948b1028..29fea249a 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/OpenDriveActor.h +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/OpenDriveActor.h @@ -39,33 +39,6 @@ private: UPROPERTY() TArray VehicleSpawners; - UPROPERTY() - TArray PersistentTrafficLights; - - UPROPERTY() - TArray PersistentTrafficSigns; - - UPROPERTY() - TSubclassOf TrafficLightBlueprintClass; - - UPROPERTY() - TSubclassOf TrafficGroupBlueprintClass; - - UPROPERTY() - TSubclassOf TrafficSign30BlueprintClass; - - UPROPERTY() - TSubclassOf TrafficSign40BlueprintClass; - - UPROPERTY() - TSubclassOf TrafficSign60BlueprintClass; - - UPROPERTY() - TSubclassOf TrafficSign90BlueprintClass; - - UPROPERTY() - TSubclassOf TrafficSign100BlueprintClass; - #if WITH_EDITORONLY_DATA /// Generate the road network using an OpenDrive file (named as the current /// .umap) From 538b4e04d6a65e2d5c195569cbb72a8f35b313f1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tomasz=20Su=C5=82kowski?= Date: Thu, 2 May 2019 16:01:47 +0200 Subject: [PATCH 2/2] Client-side bounding boxes (#1383) * BasicSynchronousClient for client side bounding boxes done * client-side bounding boxes done * Updated CHANGELOG.md and client_bounding_boxes with appropriate imports for development build * moved client_bounding_boxes.py to examples/ * attempt to fix strange pylint error * fix attempt for pylint2 * Disabled pylint E1126 as it is getting confused with numpy matrix slice --- CHANGELOG.md | 1 + PythonAPI/.pylintrc | 2 +- PythonAPI/examples/client_bounding_boxes.py | 399 ++++++++++++++++++++ 3 files changed, 401 insertions(+), 1 deletion(-) create mode 100755 PythonAPI/examples/client_bounding_boxes.py diff --git a/CHANGELOG.md b/CHANGELOG.md index bb6984951..84aba56f8 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -22,6 +22,7 @@ ## CARLA 0.9.5 + * Added `client_bounding_boxes.py` to show bounding boxes client-side * New Town07, rural environment with narrow roads * Reworked OpenDRIVE parser and waypoints API - Fixed several situations in which the XODR was incorrectly parsed diff --git a/PythonAPI/.pylintrc b/PythonAPI/.pylintrc index b69ea1655..a7983ff4d 100644 --- a/PythonAPI/.pylintrc +++ b/PythonAPI/.pylintrc @@ -1,6 +1,6 @@ [MESSAGES CONTROL] max-line-length=120 [MASTER] -disable=I0011,I0013,E1121 +disable=I0011,I0013,E1121,E1126 [TYPECHECK] ignored-modules=carla,carla.command,libcarla,pygame,numpy,configparser,ConfigParser diff --git a/PythonAPI/examples/client_bounding_boxes.py b/PythonAPI/examples/client_bounding_boxes.py new file mode 100755 index 000000000..3df29c2c2 --- /dev/null +++ b/PythonAPI/examples/client_bounding_boxes.py @@ -0,0 +1,399 @@ +#!/usr/bin/env python + +# Copyright (c) 2019 Aptiv +# +# This work is licensed under the terms of the MIT license. +# For a copy, see . + +""" +An example of client-side bounding boxes with basic car controls. + +Controls: + + W : throttle + S : brake + AD : steer + Space : hand-brake + + ESC : quit +""" + +# ============================================================================== +# -- find carla module --------------------------------------------------------- +# ============================================================================== + + +import glob +import os +import sys + +try: + sys.path.append(glob.glob('**/carla-*%d.%d-%s.egg' % ( + sys.version_info.major, + sys.version_info.minor, + 'win-amd64' if os.name == 'nt' else 'linux-x86_64'))[0]) +except IndexError: + pass + + +# ============================================================================== +# -- imports ------------------------------------------------------------------- +# ============================================================================== + +import carla + +import weakref +import random + +try: + import pygame + from pygame.locals import K_ESCAPE + from pygame.locals import K_SPACE + from pygame.locals import K_a + from pygame.locals import K_d + from pygame.locals import K_s + from pygame.locals import K_w +except ImportError: + raise RuntimeError('cannot import pygame, make sure pygame package is installed') + +try: + import numpy as np +except ImportError: + raise RuntimeError('cannot import numpy, make sure numpy package is installed') + +VIEW_WIDTH = 1920//2 +VIEW_HEIGHT = 1080//2 +VIEW_FOV = 90 + +BB_COLOR = (248, 64, 24) + +# ============================================================================== +# -- ClientSideBoundingBoxes --------------------------------------------------- +# ============================================================================== + + +class ClientSideBoundingBoxes(object): + """ + This is a module responsible for creating 3D bounding boxes and drawing them + client-side on pygame surface. + """ + + @staticmethod + def get_bounding_boxes(vehicles, camera): + """ + Creates 3D bounding boxes based on carla vehicle list and camera. + """ + + bounding_boxes = [ClientSideBoundingBoxes.get_bounding_box(vehicle, camera) for vehicle in vehicles] + # filter objects behind camera + bounding_boxes = [bb for bb in bounding_boxes if all(bb[:, 2] > 0)] + return bounding_boxes + + @staticmethod + def draw_bounding_boxes(display, bounding_boxes): + """ + Draws bounding boxes on pygame display. + """ + + bb_surface = pygame.Surface((VIEW_WIDTH, VIEW_HEIGHT)) + bb_surface.set_colorkey((0, 0, 0)) + for bbox in bounding_boxes: + points = [(int(bbox[i, 0]), int(bbox[i, 1])) for i in range(8)] + # draw lines + # base + pygame.draw.line(bb_surface, BB_COLOR, points[0], points[1]) + pygame.draw.line(bb_surface, BB_COLOR, points[0], points[1]) + pygame.draw.line(bb_surface, BB_COLOR, points[1], points[2]) + pygame.draw.line(bb_surface, BB_COLOR, points[2], points[3]) + pygame.draw.line(bb_surface, BB_COLOR, points[3], points[0]) + # top + pygame.draw.line(bb_surface, BB_COLOR, points[4], points[5]) + pygame.draw.line(bb_surface, BB_COLOR, points[5], points[6]) + pygame.draw.line(bb_surface, BB_COLOR, points[6], points[7]) + pygame.draw.line(bb_surface, BB_COLOR, points[7], points[4]) + # base-top + pygame.draw.line(bb_surface, BB_COLOR, points[0], points[4]) + pygame.draw.line(bb_surface, BB_COLOR, points[1], points[5]) + pygame.draw.line(bb_surface, BB_COLOR, points[2], points[6]) + pygame.draw.line(bb_surface, BB_COLOR, points[3], points[7]) + display.blit(bb_surface, (0, 0)) + + @staticmethod + def get_bounding_box(vehicle, camera): + """ + Returns 3D bounding box for a vehicle based on camera view. + """ + + bb_cords = ClientSideBoundingBoxes._create_bb_points(vehicle) + cords_x_y_z = ClientSideBoundingBoxes._vehicle_to_sensor(bb_cords, vehicle, camera)[:3, :] + cords_y_minus_z_x = np.concatenate([cords_x_y_z[1, :], -cords_x_y_z[2, :], cords_x_y_z[0, :]]) + bbox = np.transpose(np.dot(camera.calibration, cords_y_minus_z_x)) + camera_bbox = np.concatenate([bbox[:, 0] / bbox[:, 2], bbox[:, 1] / bbox[:, 2], bbox[:, 2]], axis=1) + return camera_bbox + + @staticmethod + def _create_bb_points(vehicle): + """ + Returns 3D bounding box for a vehicle. + """ + + cords = np.zeros((8, 4)) + extent = vehicle.bounding_box.extent + cords[0, :] = np.array([extent.x, extent.y, -extent.z, 1]) + cords[1, :] = np.array([-extent.x, extent.y, -extent.z, 1]) + cords[2, :] = np.array([-extent.x, -extent.y, -extent.z, 1]) + cords[3, :] = np.array([extent.x, -extent.y, -extent.z, 1]) + cords[4, :] = np.array([extent.x, extent.y, extent.z, 1]) + cords[5, :] = np.array([-extent.x, extent.y, extent.z, 1]) + cords[6, :] = np.array([-extent.x, -extent.y, extent.z, 1]) + cords[7, :] = np.array([extent.x, -extent.y, extent.z, 1]) + return cords + + @staticmethod + def _vehicle_to_sensor(cords, vehicle, sensor): + """ + Transforms coordinates of a vehicle bounding box to sensor. + """ + + world_cord = ClientSideBoundingBoxes._vehicle_to_world(cords, vehicle) + sensor_cord = ClientSideBoundingBoxes._world_to_sensor(world_cord, sensor) + return sensor_cord + + @staticmethod + def _vehicle_to_world(cords, vehicle): + """ + Transforms coordinates of a vehicle bounding box to world. + """ + + bb_transform = carla.Transform(vehicle.bounding_box.location) + bb_vehicle_matrix = ClientSideBoundingBoxes.get_matrix(bb_transform) + vehicle_world_matrix = ClientSideBoundingBoxes.get_matrix(vehicle.get_transform()) + bb_world_matrix = np.dot(vehicle_world_matrix, bb_vehicle_matrix) + world_cords = np.dot(bb_world_matrix, np.transpose(cords)) + return world_cords + + @staticmethod + def _world_to_sensor(cords, sensor): + """ + Transforms world coordinates to sensor. + """ + + sensor_world_matrix = ClientSideBoundingBoxes.get_matrix(sensor.get_transform()) + world_sensor_matrix = np.linalg.inv(sensor_world_matrix) + sensor_cords = np.dot(world_sensor_matrix, cords) + return sensor_cords + + @staticmethod + def get_matrix(transform): + """ + Creates matrix from carla transform. + """ + + rotation = transform.rotation + location = transform.location + c_y = np.cos(np.radians(rotation.yaw)) + s_y = np.sin(np.radians(rotation.yaw)) + c_r = np.cos(np.radians(rotation.roll)) + s_r = np.sin(np.radians(rotation.roll)) + c_p = np.cos(np.radians(rotation.pitch)) + s_p = np.sin(np.radians(rotation.pitch)) + matrix = np.matrix(np.identity(4)) + matrix[0, 3] = location.x + matrix[1, 3] = location.y + matrix[2, 3] = location.z + matrix[0, 0] = c_p * c_y + matrix[0, 1] = c_y * s_p * s_r - s_y * c_r + matrix[0, 2] = -c_y * s_p * c_r - s_y * s_r + matrix[1, 0] = s_y * c_p + matrix[1, 1] = s_y * s_p * s_r + c_y * c_r + matrix[1, 2] = -s_y * s_p * c_r + c_y * s_r + matrix[2, 0] = s_p + matrix[2, 1] = -c_p * s_r + matrix[2, 2] = c_p * c_r + return matrix + + +# ============================================================================== +# -- BasicSynchronousClient ---------------------------------------------------- +# ============================================================================== + + +class BasicSynchronousClient(object): + """ + Basic implementation of a synchronous client. + """ + + def __init__(self): + self.client = None + self.world = None + self.camera = None + self.car = None + + self.display = None + self.image = None + self.capture = True + + def camera_blueprint(self): + """ + Returns camera blueprint. + """ + + camera_bp = self.world.get_blueprint_library().find('sensor.camera.rgb') + camera_bp.set_attribute('image_size_x', str(VIEW_WIDTH)) + camera_bp.set_attribute('image_size_y', str(VIEW_HEIGHT)) + camera_bp.set_attribute('fov', str(VIEW_FOV)) + return camera_bp + + def set_synchronous_mode(self, synchronous_mode): + """ + Sets synchronous mode. + """ + + settings = self.world.get_settings() + settings.synchronous_mode = synchronous_mode + self.world.apply_settings(settings) + + def setup_car(self): + """ + Spawns actor-vehicle to be controled. + """ + + car_bp = self.world.get_blueprint_library().filter('vehicle.*')[0] + location = random.choice(self.world.get_map().get_spawn_points()) + self.car = self.world.spawn_actor(car_bp, location) + + def setup_camera(self): + """ + Spawns actor-camera to be used to render view. + Sets calibration for client-side boxes rendering. + """ + + camera_transform = carla.Transform(carla.Location(x=-5.5, z=2.8), carla.Rotation(pitch=-15)) + self.camera = self.world.spawn_actor(self.camera_blueprint(), camera_transform, attach_to=self.car) + weak_self = weakref.ref(self) + self.camera.listen(lambda image: weak_self().set_image(weak_self, image)) + + calibration = np.identity(3) + calibration[0, 2] = VIEW_WIDTH / 2.0 + calibration[1, 2] = VIEW_HEIGHT / 2.0 + calibration[0, 0] = calibration[1, 1] = VIEW_WIDTH / (2.0 * np.tan(VIEW_FOV * np.pi / 360.0)) + self.camera.calibration = calibration + + def control(self, car): + """ + Applies control to main car based on pygame pressed keys. + Will return True If ESCAPE is hit, otherwise False to end main loop. + """ + + keys = pygame.key.get_pressed() + if keys[K_ESCAPE]: + return True + + control = car.get_control() + control.throttle = 0 + if keys[K_w]: + control.throttle = 1 + control.reverse = False + elif keys[K_s]: + control.throttle = 1 + control.reverse = True + if keys[K_a]: + control.steer = max(-1., min(control.steer - 0.05, 0)) + elif keys[K_d]: + control.steer = min(1., max(control.steer + 0.05, 0)) + else: + control.steer = 0 + control.hand_brake = keys[K_SPACE] + + car.apply_control(control) + return False + + @staticmethod + def set_image(weak_self, img): + """ + Sets image coming from camera sensor. + The self.capture flag is a mean of synchronization - once the flag is + set, next coming image will be stored. + """ + + self = weak_self() + if self.capture: + self.image = img + self.capture = False + + def render(self, display): + """ + Transforms image from camera sensor and blits it to main pygame display. + """ + + if self.image is not None: + array = np.frombuffer(self.image.raw_data, dtype=np.dtype("uint8")) + array = np.reshape(array, (self.image.height, self.image.width, 4)) + array = array[:, :, :3] + array = array[:, :, ::-1] + surface = pygame.surfarray.make_surface(array.swapaxes(0, 1)) + display.blit(surface, (0, 0)) + + def game_loop(self): + """ + Main program loop. + """ + + try: + pygame.init() + + self.client = carla.Client('127.0.0.1', 2000) + self.client.set_timeout(2.0) + self.world = self.client.get_world() + + self.setup_car() + self.setup_camera() + + self.display = pygame.display.set_mode((VIEW_WIDTH, VIEW_HEIGHT), pygame.HWSURFACE | pygame.DOUBLEBUF) + pygame_clock = pygame.time.Clock() + + self.set_synchronous_mode(True) + vehicles = self.world.get_actors().filter('vehicle.*') + + while True: + self.world.tick() + + self.capture = True + pygame_clock.tick_busy_loop(20) + + self.render(self.display) + bounding_boxes = ClientSideBoundingBoxes.get_bounding_boxes(vehicles, self.camera) + ClientSideBoundingBoxes.draw_bounding_boxes(self.display, bounding_boxes) + + pygame.display.flip() + + pygame.event.pump() + if self.control(self.car): + return + + finally: + self.set_synchronous_mode(False) + self.camera.destroy() + self.car.destroy() + pygame.quit() + + +# ============================================================================== +# -- main() -------------------------------------------------------------------- +# ============================================================================== + + +def main(): + """ + Initializes the client-side bounding box demo. + """ + + try: + client = BasicSynchronousClient() + client.game_loop() + finally: + print('EXIT') + + +if __name__ == '__main__': + main()