Merge master into local_planner_debug_flag
This commit is contained in:
germanros1987
GitHub
commit
94bc06274b
|
|
@ -16,11 +16,13 @@
|
|||
- 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
|
||||
|
||||
## 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
|
||||
|
|
|
|||
|
|
@ -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()) {
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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 <https://opensource.org/licenses/MIT>.
|
||||
|
||||
"""
|
||||
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()
|
||||
|
|
@ -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()
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
|
|||
Loading…
Reference in New Issue