Merge branch 'master' into roads
This commit is contained in:
commit
503fedeb6b
|
@ -102,8 +102,8 @@
|
|||
## `carla.Vehicle(carla.Actor)`
|
||||
|
||||
- `bounding_box`
|
||||
- `control`
|
||||
- `apply_control(vehicle_control)`
|
||||
- `get_vehicle_control()`
|
||||
- `set_autopilot(enabled=True)`
|
||||
|
||||
## `carla.TrafficLight(carla.Actor)`
|
||||
|
@ -272,7 +272,8 @@ Static presets
|
|||
|
||||
## `carla.TrafficLightState`
|
||||
|
||||
- `Unknown`
|
||||
- `Off`
|
||||
- `Red`
|
||||
- `Yellow`
|
||||
- `Green`
|
||||
- `Unknown`
|
||||
|
|
|
@ -11,14 +11,8 @@
|
|||
namespace carla {
|
||||
namespace client {
|
||||
|
||||
TrafficLightState TrafficLight::GetState() {
|
||||
auto state = GetEpisode().Lock()->GetActorDynamicState(*this).state;
|
||||
switch (state) {
|
||||
case 1u: return TrafficLightState::Red;
|
||||
case 2u: return TrafficLightState::Yellow;
|
||||
case 3u: return TrafficLightState::Green;
|
||||
default: return TrafficLightState::Unknown;
|
||||
}
|
||||
rpc::TrafficLightState TrafficLight::GetState() {
|
||||
return GetEpisode().Lock()->GetActorDynamicState(*this).state.traffic_light_state;
|
||||
}
|
||||
|
||||
} // namespace client
|
||||
|
|
|
@ -7,17 +7,11 @@
|
|||
#pragma once
|
||||
|
||||
#include "carla/client/Actor.h"
|
||||
#include "carla/rpc/TrafficLightState.h"
|
||||
|
||||
namespace carla {
|
||||
namespace client {
|
||||
|
||||
enum class TrafficLightState {
|
||||
Unknown,
|
||||
Red,
|
||||
Yellow,
|
||||
Green
|
||||
};
|
||||
|
||||
class TrafficLight : public Actor {
|
||||
public:
|
||||
|
||||
|
@ -27,7 +21,7 @@ namespace client {
|
|||
///
|
||||
/// @note This function does not call the simulator, it returns the
|
||||
/// traffic light state received in the last tick.
|
||||
TrafficLightState GetState();
|
||||
rpc::TrafficLightState GetState();
|
||||
};
|
||||
|
||||
} // namespace client
|
||||
|
|
|
@ -11,6 +11,10 @@
|
|||
namespace carla {
|
||||
namespace client {
|
||||
|
||||
void Vehicle::SetAutopilot(bool enabled) {
|
||||
GetEpisode().Lock()->SetVehicleAutopilot(*this, enabled);
|
||||
}
|
||||
|
||||
void Vehicle::ApplyControl(const Control &control) {
|
||||
if (control != _control) {
|
||||
GetEpisode().Lock()->ApplyControlToVehicle(*this, control);
|
||||
|
@ -18,8 +22,8 @@ namespace client {
|
|||
}
|
||||
}
|
||||
|
||||
void Vehicle::SetAutopilot(bool enabled) {
|
||||
GetEpisode().Lock()->SetVehicleAutopilot(*this, enabled);
|
||||
Vehicle::Control Vehicle::GetVehicleControl() const {
|
||||
return GetEpisode().Lock()->GetActorDynamicState(*this).state.vehicle_control;
|
||||
}
|
||||
|
||||
} // namespace client
|
||||
|
|
|
@ -27,12 +27,9 @@ namespace client {
|
|||
|
||||
/// Return the control last applied to this vehicle.
|
||||
///
|
||||
/// @warning This function only takes into account the control applied to
|
||||
/// this instance of Vehicle. Note that several instances of Vehicle (even
|
||||
/// in different processes) may point to the same vehicle in the simulator.
|
||||
const Control &GetControl() const {
|
||||
return _control;
|
||||
}
|
||||
/// @note This function does not call the simulator, it returns the Control
|
||||
/// received in the last tick.
|
||||
Control GetVehicleControl() const;
|
||||
|
||||
private:
|
||||
|
||||
|
|
|
@ -30,7 +30,7 @@ namespace detail {
|
|||
geom::Transform transform;
|
||||
geom::Vector3D velocity;
|
||||
geom::Vector3D acceleration;
|
||||
uint8_t state = 0u;
|
||||
sensor::data::ActorDynamicState::TypeDependentState state;
|
||||
};
|
||||
|
||||
const auto &GetTimestamp() const {
|
||||
|
|
|
@ -0,0 +1,29 @@
|
|||
// Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma
|
||||
// de Barcelona (UAB).
|
||||
//
|
||||
// This work is licensed under the terms of the MIT license.
|
||||
// For a copy, see <https://opensource.org/licenses/MIT>.
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "carla/MsgPack.h"
|
||||
|
||||
#include <cstdint>
|
||||
|
||||
namespace carla {
|
||||
namespace rpc {
|
||||
|
||||
enum class TrafficLightState : uint8_t {
|
||||
Off,
|
||||
Red,
|
||||
Yellow,
|
||||
Green,
|
||||
|
||||
Unknown,
|
||||
SIZE
|
||||
};
|
||||
|
||||
} // namespace rpc
|
||||
} // namespace carla
|
||||
|
||||
MSGPACK_ADD_ENUM(carla::rpc::TrafficLightState);
|
|
@ -12,14 +12,14 @@ namespace rpc {
|
|||
using WP = WeatherParameters;
|
||||
|
||||
// cloudyness precip. prec.dep. wind azimuth altitude
|
||||
WP WP::ClearNoon = { 0.0f, 0.0f, 00.0f, 0.35f, 0.0f, 75.0f};
|
||||
WP WP::ClearNoon = { 15.0f, 0.0f, 00.0f, 0.35f, 0.0f, 75.0f};
|
||||
WP WP::CloudyNoon = { 80.0f, 0.0f, 00.0f, 0.35f, 0.0f, 75.0f};
|
||||
WP WP::WetNoon = { 20.0f, 0.0f, 50.0f, 0.35f, 0.0f, 75.0f};
|
||||
WP WP::WetCloudyNoon = { 80.0f, 0.0f, 50.0f, 0.35f, 0.0f, 75.0f};
|
||||
WP WP::MidRainyNoon = { 80.0f, 30.0f, 50.0f, 0.40f, 0.0f, 75.0f};
|
||||
WP WP::HardRainNoon = { 90.0f, 60.0f, 100.0f, 1.0f, 0.0f, 75.0f};
|
||||
WP WP::SoftRainNoon = { 70.0f, 15.0f, 50.0f, 0.35f, 0.0f, 75.0f};
|
||||
WP WP::ClearSunset = { 0.0f, 0.0f, 00.0f, 0.35f, 0.0f, 15.0f};
|
||||
WP WP::ClearSunset = { 15.0f, 0.0f, 00.0f, 0.35f, 0.0f, 15.0f};
|
||||
WP WP::CloudySunset = { 80.0f, 0.0f, 00.0f, 0.35f, 0.0f, 15.0f};
|
||||
WP WP::WetSunset = { 20.0f, 0.0f, 50.0f, 0.35f, 0.0f, 15.0f};
|
||||
WP WP::WetCloudySunset = { 90.0f, 0.0f, 50.0f, 0.35f, 0.0f, 15.0f};
|
||||
|
|
|
@ -9,6 +9,8 @@
|
|||
#include "carla/geom/Transform.h"
|
||||
#include "carla/geom/Vector3D.h"
|
||||
#include "carla/rpc/ActorId.h"
|
||||
#include "carla/rpc/TrafficLightState.h"
|
||||
#include "carla/rpc/VehicleControl.h"
|
||||
|
||||
#include <cstdint>
|
||||
|
||||
|
@ -16,6 +18,37 @@ namespace carla {
|
|||
namespace sensor {
|
||||
namespace data {
|
||||
|
||||
namespace detail {
|
||||
|
||||
#pragma pack(push, 1)
|
||||
class PackedVehicleControl {
|
||||
public:
|
||||
|
||||
PackedVehicleControl() = default;
|
||||
|
||||
PackedVehicleControl(const rpc::VehicleControl &control)
|
||||
: throttle(control.throttle),
|
||||
steer(control.steer),
|
||||
brake(control.brake),
|
||||
hand_brake(control.hand_brake),
|
||||
reverse(control.reverse) {}
|
||||
|
||||
operator rpc::VehicleControl() const {
|
||||
return {throttle, steer, brake, hand_brake, reverse};
|
||||
}
|
||||
|
||||
private:
|
||||
|
||||
float throttle;
|
||||
float steer;
|
||||
float brake;
|
||||
bool hand_brake;
|
||||
bool reverse;
|
||||
};
|
||||
#pragma pack(pop)
|
||||
|
||||
} // namespace detail
|
||||
|
||||
#pragma pack(push, 1)
|
||||
|
||||
/// Dynamic state of an actor at a certain frame.
|
||||
|
@ -27,12 +60,17 @@ namespace data {
|
|||
|
||||
geom::Vector3D velocity;
|
||||
|
||||
uint8_t state;
|
||||
union TypeDependentState {
|
||||
rpc::TrafficLightState traffic_light_state;
|
||||
detail::PackedVehicleControl vehicle_control;
|
||||
} state;
|
||||
};
|
||||
|
||||
#pragma pack(pop)
|
||||
|
||||
static_assert(sizeof(ActorDynamicState) == 1u + 10u * sizeof(uint32_t), "Invalid ActorDynamicState size!");
|
||||
static_assert(
|
||||
sizeof(ActorDynamicState) == 10u * sizeof(uint32_t) + sizeof(detail::PackedVehicleControl),
|
||||
"Invalid ActorDynamicState size!");
|
||||
|
||||
} // namespace data
|
||||
} // namespace sensor
|
||||
|
|
|
@ -29,6 +29,7 @@ Use ARROWS or WASD keys for control.
|
|||
|
||||
R : toggle recording images to disk
|
||||
|
||||
F1 : toggle HUD
|
||||
H/? : toggle help
|
||||
ESC : quit
|
||||
"""
|
||||
|
@ -64,7 +65,10 @@ import carla
|
|||
from carla import ColorConverter as cc
|
||||
|
||||
import argparse
|
||||
import collections
|
||||
import datetime
|
||||
import logging
|
||||
import math
|
||||
import random
|
||||
import re
|
||||
import weakref
|
||||
|
@ -79,6 +83,7 @@ try:
|
|||
from pygame.locals import K_BACKSPACE
|
||||
from pygame.locals import K_DOWN
|
||||
from pygame.locals import K_ESCAPE
|
||||
from pygame.locals import K_F1
|
||||
from pygame.locals import K_LEFT
|
||||
from pygame.locals import K_RIGHT
|
||||
from pygame.locals import K_SLASH
|
||||
|
@ -115,10 +120,16 @@ def find_weather_presets():
|
|||
return [(getattr(carla.WeatherParameters, x), name(x)) for x in presets]
|
||||
|
||||
|
||||
def get_actor_display_name(actor, truncate=250):
|
||||
name = ' '.join(actor.type_id.replace('_', '.').title().split('.')[1:])
|
||||
return (name[:truncate-1] + u'\u2026') if len(name) > truncate else name
|
||||
|
||||
|
||||
class World(object):
|
||||
def __init__(self, carla_world, hud):
|
||||
self.world = carla_world
|
||||
self.hud = hud
|
||||
self.world.on_tick(hud.on_world_tick)
|
||||
blueprint = self._get_random_blueprint()
|
||||
spawn_points = self.world.get_map().get_spawn_points()
|
||||
spawn_point = random.choice(spawn_points) if spawn_points else carla.Transform()
|
||||
|
@ -196,6 +207,8 @@ class KeyboardControl(object):
|
|||
return True
|
||||
elif event.key == K_BACKSPACE:
|
||||
world.restart()
|
||||
elif event.key == K_F1:
|
||||
world.hud.toggle_info()
|
||||
elif event.key == K_h or (event.key == K_SLASH and pygame.key.get_mods() & KMOD_SHIFT):
|
||||
world.hud.help.toggle()
|
||||
elif event.key == K_TAB:
|
||||
|
@ -253,13 +266,70 @@ class HUD(object):
|
|||
self._font_mono = pygame.font.Font(mono, 14)
|
||||
self._notifications = FadingText(font, (width, 40), (0, height - 40))
|
||||
self.help = HelpText(pygame.font.Font(mono, 24), width, height)
|
||||
self.client_fps = 0
|
||||
self.server_fps = 0
|
||||
self.frame_number = 0
|
||||
self.simulation_time = 0
|
||||
self._show_info = True
|
||||
self._info_text = []
|
||||
self._server_clock = pygame.time.Clock()
|
||||
|
||||
def on_world_tick(self, timestamp):
|
||||
self._server_clock.tick()
|
||||
self.server_fps = self._server_clock.get_fps()
|
||||
self.frame_number = timestamp.frame_count
|
||||
self.simulation_time = timestamp.elapsed_seconds
|
||||
|
||||
def tick(self, world, clock):
|
||||
self.client_fps = clock.get_fps()
|
||||
if not self._show_info:
|
||||
return
|
||||
t = world.vehicle.get_transform()
|
||||
v = world.vehicle.get_velocity()
|
||||
c = world.vehicle.get_vehicle_control()
|
||||
heading = 'N' if abs(t.rotation.yaw) < 89.5 else ''
|
||||
heading += 'S' if abs(t.rotation.yaw) > 90.5 else ''
|
||||
heading += 'E' if 179.5 > t.rotation.yaw > 0.5 else ''
|
||||
heading += 'W' if -0.5 > t.rotation.yaw > -179.5 else ''
|
||||
colhist = world.collision_sensor.get_collision_history()
|
||||
collision = [colhist[x + self.frame_number - 200] for x in range(0, 200)]
|
||||
max_col = max(1.0, max(collision))
|
||||
collision = [x / max_col for x in collision]
|
||||
self._info_text = [
|
||||
'server: % 16d FPS' % self.server_fps,
|
||||
'client: % 16d FPS' % clock.get_fps(),
|
||||
'',
|
||||
'vehicle: % 20s' % get_actor_display_name(world.vehicle, truncate=20),
|
||||
'map: % 20s' % world.world.map_name,
|
||||
'simulation time: % 12s' % datetime.timedelta(seconds=int(self.simulation_time)),
|
||||
'',
|
||||
'speed: % 15.0f km/h' % (3.6 * math.sqrt(v.x**2 + v.y**2 + v.z**2)),
|
||||
u'heading:% 16.0f\N{DEGREE SIGN} % 2s' % (t.rotation.yaw, heading),
|
||||
'location:% 20s' % ('(% 5.1f, % 5.1f)' % (t.location.x, t.location.y)),
|
||||
'height: % 18.0f m' % t.location.z,
|
||||
'',
|
||||
('throttle:', c.throttle, 0.0, 1.0),
|
||||
('steer:', c.steer, -1.0, 1.0),
|
||||
('brake:', c.brake, 0.0, 1.0),
|
||||
('reverse:', c.reverse),
|
||||
('hand brake:', c.hand_brake),
|
||||
'',
|
||||
'collision:',
|
||||
collision
|
||||
]
|
||||
vehicles = world.world.get_actors().filter('vehicle.*')
|
||||
if len(vehicles) > 1:
|
||||
self._info_text += ['', 'nearby vehicles:']
|
||||
distance = lambda l: math.sqrt((l.x - t.location.x)**2 + (l.y - t.location.y)**2 + (l.z - t.location.z)**2)
|
||||
vehicles = [(distance(x.get_location()), x) for x in vehicles if x.id != world.vehicle.id]
|
||||
for d, vehicle in sorted(vehicles):
|
||||
if d > 200.0:
|
||||
break
|
||||
vehicle_type = get_actor_display_name(vehicle, truncate=22)
|
||||
self._info_text.append('% 4dm %s' % (d, vehicle_type))
|
||||
self._notifications.tick(world, clock)
|
||||
|
||||
def toggle_info(self):
|
||||
self._show_info = not self._show_info
|
||||
|
||||
def notification(self, text, seconds=2.0):
|
||||
self._notifications.set_text(text, seconds=seconds)
|
||||
|
||||
|
@ -267,11 +337,42 @@ class HUD(object):
|
|||
self._notifications.set_text('Error: %s' % text, (255, 0, 0))
|
||||
|
||||
def render(self, display):
|
||||
if self._show_info:
|
||||
info_surface = pygame.Surface((220, self.dim[1]))
|
||||
info_surface.set_alpha(100)
|
||||
display.blit(info_surface, (0, 0))
|
||||
v_offset = 4
|
||||
bar_h_offset = 100
|
||||
bar_width = 106
|
||||
for item in self._info_text:
|
||||
if v_offset + 18 > self.dim[1]:
|
||||
break
|
||||
if isinstance(item, list):
|
||||
if len(item) > 1:
|
||||
points = [(x + 8, v_offset + 8 + (1.0 - y) * 30) for x, y in enumerate(item)]
|
||||
pygame.draw.lines(display, (255, 136, 0), False, points, 2)
|
||||
item = None
|
||||
v_offset += 18
|
||||
elif isinstance(item, tuple):
|
||||
if isinstance(item[1], bool):
|
||||
rect = pygame.Rect((bar_h_offset, v_offset + 8), (6, 6))
|
||||
pygame.draw.rect(display, (255, 255, 255), rect, 0 if item[1] else 1)
|
||||
else:
|
||||
rect_border = pygame.Rect((bar_h_offset, v_offset + 8), (bar_width, 6))
|
||||
pygame.draw.rect(display, (255, 255, 255), rect_border, 1)
|
||||
f = (item[1] - item[2]) / (item[3] - item[2])
|
||||
if item[2] < 0.0:
|
||||
rect = pygame.Rect((bar_h_offset + f * (bar_width - 6), v_offset + 8), (6, 6))
|
||||
else:
|
||||
rect = pygame.Rect((bar_h_offset, v_offset + 8), (f * bar_width, 6))
|
||||
pygame.draw.rect(display, (255, 255, 255), rect)
|
||||
item = item[0]
|
||||
if item: # At this point has to be a str.
|
||||
surface = self._font_mono.render(item, True, (255, 255, 255))
|
||||
display.blit(surface, (8, v_offset))
|
||||
v_offset += 18
|
||||
self._notifications.render(display)
|
||||
self.help.render(display)
|
||||
fps_text = 'client: %02d FPS; server: %02d FPS' % (self.client_fps, self.server_fps)
|
||||
fps = self._font_mono.render(fps_text, True, (60, 60, 60))
|
||||
display.blit(fps, (6, 4))
|
||||
|
||||
|
||||
# ==============================================================================
|
||||
|
@ -339,6 +440,7 @@ class HelpText(object):
|
|||
class CollisionSensor(object):
|
||||
def __init__(self, parent_actor, hud):
|
||||
self.sensor = None
|
||||
self._history = []
|
||||
self._parent = parent_actor
|
||||
self._hud = hud
|
||||
world = self._parent.get_world()
|
||||
|
@ -349,6 +451,12 @@ class CollisionSensor(object):
|
|||
weak_self = weakref.ref(self)
|
||||
self.sensor.listen(lambda event: CollisionSensor._on_collision(weak_self, event))
|
||||
|
||||
def get_collision_history(self):
|
||||
history = collections.defaultdict(int)
|
||||
for frame, intensity in self._history:
|
||||
history[frame] += intensity
|
||||
return history
|
||||
|
||||
@staticmethod
|
||||
def _on_collision(weak_self, event):
|
||||
self = weak_self()
|
||||
|
@ -356,6 +464,11 @@ class CollisionSensor(object):
|
|||
return
|
||||
actor_type = ' '.join(event.other_actor.type_id.replace('_', '.').title().split('.')[1:])
|
||||
self._hud.notification('Collision with %r' % actor_type)
|
||||
impulse = event.normal_impulse
|
||||
intensity = math.sqrt(impulse.x**2 + impulse.y**2 + impulse.z**2)
|
||||
self._history.append((event.frame_number, intensity))
|
||||
if len(self._history) > 2000:
|
||||
self._history.pop(0)
|
||||
|
||||
|
||||
# ==============================================================================
|
||||
|
@ -391,7 +504,6 @@ class CameraManager(object):
|
|||
bp.set_attribute('image_size_y', str(hud.dim[1]))
|
||||
item.append(bp)
|
||||
self._index = None
|
||||
self._server_clock = pygame.time.Clock()
|
||||
|
||||
def toggle_camera(self):
|
||||
self._transform_index = (self._transform_index + 1) % len(self._camera_transforms)
|
||||
|
@ -433,8 +545,6 @@ class CameraManager(object):
|
|||
self = weak_self()
|
||||
if not self:
|
||||
return
|
||||
self._server_clock.tick()
|
||||
self._hud.server_fps = self._server_clock.get_fps()
|
||||
if self._sensors[self._index][0].startswith('sensor.lidar'):
|
||||
points = np.frombuffer(image.raw_data, dtype=np.dtype('f4'))
|
||||
points = np.reshape(points, (int(points.shape[0]/3), 3))
|
||||
|
|
|
@ -7,6 +7,7 @@
|
|||
#include <carla/client/Actor.h>
|
||||
#include <carla/client/TrafficLight.h>
|
||||
#include <carla/client/Vehicle.h>
|
||||
#include <carla/rpc/TrafficLightState.h>
|
||||
|
||||
#include <boost/python/suite/indexing/vector_indexing_suite.hpp>
|
||||
|
||||
|
@ -59,17 +60,18 @@ void export_actor() {
|
|||
|
||||
class_<cc::Vehicle, bases<cc::Actor>, boost::noncopyable, boost::shared_ptr<cc::Vehicle>>("Vehicle", no_init)
|
||||
.add_property("bounding_box", CALL_RETURNING_COPY(cc::Vehicle, GetBoundingBox))
|
||||
.add_property("control", CALL_RETURNING_COPY(cc::Vehicle, GetControl))
|
||||
.def("apply_control", &cc::Vehicle::ApplyControl, (arg("control")))
|
||||
.def("get_vehicle_control", &cc::Vehicle::GetVehicleControl)
|
||||
.def("set_autopilot", &cc::Vehicle::SetAutopilot, (arg("enabled")=true))
|
||||
.def(self_ns::str(self_ns::self))
|
||||
;
|
||||
|
||||
enum_<cc::TrafficLightState>("TrafficLightState")
|
||||
.value("Unknown", cc::TrafficLightState::Unknown)
|
||||
.value("Red", cc::TrafficLightState::Red)
|
||||
.value("Yellow", cc::TrafficLightState::Yellow)
|
||||
.value("Green", cc::TrafficLightState::Green)
|
||||
enum_<cr::TrafficLightState>("TrafficLightState")
|
||||
.value("Off", cr::TrafficLightState::Off)
|
||||
.value("Red", cr::TrafficLightState::Red)
|
||||
.value("Yellow", cr::TrafficLightState::Yellow)
|
||||
.value("Green", cr::TrafficLightState::Green)
|
||||
.value("Unknown", cr::TrafficLightState::Unknown)
|
||||
;
|
||||
|
||||
class_<cc::TrafficLight, bases<cc::Actor>, boost::noncopyable, boost::shared_ptr<cc::TrafficLight>>("TrafficLight", no_init)
|
||||
|
|
|
@ -12,12 +12,20 @@
|
|||
#include "Carla/Game/Tagger.h"
|
||||
#include "Carla/Traffic/TrafficLightBase.h"
|
||||
|
||||
static bool FActorRegistry_IsTrafficLight(const FActorView &View)
|
||||
static FActorView::ActorType FActorRegistry_GetActorType(const FActorView &View)
|
||||
{
|
||||
return
|
||||
View.IsValid() &&
|
||||
View.GetSemanticTags().Contains(ECityObjectLabel::TrafficSigns) &&
|
||||
(nullptr != Cast<ATrafficLightBase>(View.GetActor()));
|
||||
if (View.IsValid())
|
||||
{
|
||||
if (nullptr != Cast<ACarlaWheeledVehicle>(View.GetActor()))
|
||||
{
|
||||
return FActorView::ActorType::Vehicle;
|
||||
}
|
||||
else if (nullptr != Cast<ATrafficLightBase>(View.GetActor()))
|
||||
{
|
||||
return FActorView::ActorType::TrafficLight;
|
||||
}
|
||||
}
|
||||
return FActorView::ActorType::Other;
|
||||
}
|
||||
|
||||
static FString GetRelevantTagAsString(const FActorView &View)
|
||||
|
@ -51,7 +59,7 @@ FActorView FActorRegistry::Register(AActor &Actor, FActorDescription Description
|
|||
|
||||
auto View = FActorView(Id, Actor, std::move(Description));
|
||||
ATagger::GetTagsOfTaggedActor(Actor, View.SemanticTags);
|
||||
View.bIsTrafficLight = FActorRegistry_IsTrafficLight(View);
|
||||
View.Type = FActorRegistry_GetActorType(View);
|
||||
|
||||
auto Result = ActorDatabase.emplace(Id, View);
|
||||
check(Result.second);
|
||||
|
|
|
@ -18,6 +18,12 @@ public:
|
|||
|
||||
using IdType = uint32;
|
||||
|
||||
enum class ActorType : uint8 {
|
||||
Other,
|
||||
Vehicle,
|
||||
TrafficLight
|
||||
};
|
||||
|
||||
FActorView() = default;
|
||||
FActorView(const FActorView &) = default;
|
||||
|
||||
|
@ -31,6 +37,11 @@ public:
|
|||
return Id;
|
||||
}
|
||||
|
||||
ActorType GetActorType() const
|
||||
{
|
||||
return Type;
|
||||
}
|
||||
|
||||
AActor *GetActor()
|
||||
{
|
||||
return TheActor;
|
||||
|
@ -51,11 +62,6 @@ public:
|
|||
return SemanticTags;
|
||||
}
|
||||
|
||||
bool IsTrafficLight() const
|
||||
{
|
||||
return bIsTrafficLight;
|
||||
}
|
||||
|
||||
private:
|
||||
|
||||
friend class FActorRegistry;
|
||||
|
@ -67,12 +73,11 @@ private:
|
|||
|
||||
IdType Id = 0u;
|
||||
|
||||
ActorType Type = ActorType::Other;
|
||||
|
||||
AActor *TheActor = nullptr;
|
||||
|
||||
TSharedPtr<const FActorDescription> Description = nullptr;
|
||||
|
||||
TSet<ECityObjectLabel> SemanticTags;
|
||||
|
||||
/// @todo
|
||||
bool bIsTrafficLight = false;
|
||||
};
|
||||
|
|
|
@ -8,6 +8,8 @@
|
|||
|
||||
#include "Carla/Sensor/ShaderBasedSensor.h"
|
||||
|
||||
#include "Carla/Actor/ActorDefinition.h"
|
||||
|
||||
#include "DepthCamera.generated.h"
|
||||
|
||||
/// Sensor that produces "depth" images.
|
||||
|
|
|
@ -13,19 +13,34 @@
|
|||
|
||||
#include <compiler/disable-ue4-macros.h>
|
||||
#include <carla/sensor/SensorRegistry.h>
|
||||
#include <carla/sensor/data/ActorDynamicState.h>
|
||||
#include <compiler/enable-ue4-macros.h>
|
||||
|
||||
static uint8 AWorldObserver_GetActorState(const FActorView &View)
|
||||
static auto AWorldObserver_GetActorState(const FActorView &View)
|
||||
{
|
||||
if (View.IsTrafficLight())
|
||||
using AType = FActorView::ActorType;
|
||||
|
||||
carla::sensor::data::ActorDynamicState::TypeDependentState state;
|
||||
|
||||
if (AType::Vehicle == View.GetActorType())
|
||||
{
|
||||
auto Vehicle = Cast<ACarlaWheeledVehicle>(View.GetActor());
|
||||
if (Vehicle != nullptr)
|
||||
{
|
||||
state.vehicle_control = carla::rpc::VehicleControl{Vehicle->GetVehicleControl()};
|
||||
}
|
||||
}
|
||||
else if (AType::TrafficLight == View.GetActorType())
|
||||
{
|
||||
auto TrafficLight = Cast<ATrafficLightBase>(View.GetActor());
|
||||
if (TrafficLight != nullptr)
|
||||
{
|
||||
return static_cast<uint8>(TrafficLight->GetTrafficSignState());
|
||||
using TLS = carla::rpc::TrafficLightState;
|
||||
state.traffic_light_state = static_cast<TLS>(TrafficLight->GetTrafficSignState());
|
||||
}
|
||||
}
|
||||
return 0u;
|
||||
|
||||
return state;
|
||||
}
|
||||
|
||||
static carla::Buffer AWorldObserver_Serialize(
|
||||
|
@ -53,7 +68,7 @@ static carla::Buffer AWorldObserver_Serialize(
|
|||
for (auto &&pair : Registry) {
|
||||
auto &&actor_view = pair.second;
|
||||
check(actor_view.GetActor() != nullptr);
|
||||
constexpr float TO_METERS = 1e-3;
|
||||
constexpr float TO_METERS = 1e-2;
|
||||
const auto velocity = TO_METERS * actor_view.GetActor()->GetVelocity();
|
||||
ActorDynamicState info = {
|
||||
actor_view.GetActorId(),
|
||||
|
|
|
@ -14,18 +14,18 @@
|
|||
|
||||
UENUM(BlueprintType)
|
||||
enum class ETrafficSignState : uint8 {
|
||||
UNKNOWN UMETA(DisplayName = "UNKNOWN"),
|
||||
TrafficLightRed UMETA(DisplayName = "Traffic Light - Red"),
|
||||
TrafficLightYellow UMETA(DisplayName = "Traffic Light - Yellow"),
|
||||
TrafficLightGreen UMETA(DisplayName = "Traffic Light - Green"),
|
||||
SpeedLimit_30 UMETA(DisplayName = "Speed Limit - 30"),
|
||||
SpeedLimit_40 UMETA(DisplayName = "Speed Limit - 40"),
|
||||
SpeedLimit_50 UMETA(DisplayName = "Speed Limit - 50"),
|
||||
SpeedLimit_60 UMETA(DisplayName = "Speed Limit - 60"),
|
||||
SpeedLimit_90 UMETA(DisplayName = "Speed Limit - 90"),
|
||||
SpeedLimit_100 UMETA(DisplayName = "Speed Limit - 100"),
|
||||
SpeedLimit_120 UMETA(DisplayName = "Speed Limit - 120"),
|
||||
SpeedLimit_130 UMETA(DisplayName = "Speed Limit - 130")
|
||||
UNKNOWN = 0u UMETA(DisplayName = "UNKNOWN"),
|
||||
TrafficLightRed = 1u UMETA(DisplayName = "Traffic Light - Red"),
|
||||
TrafficLightYellow = 2u UMETA(DisplayName = "Traffic Light - Yellow"),
|
||||
TrafficLightGreen = 3u UMETA(DisplayName = "Traffic Light - Green"),
|
||||
SpeedLimit_30 UMETA(DisplayName = "Speed Limit - 30"),
|
||||
SpeedLimit_40 UMETA(DisplayName = "Speed Limit - 40"),
|
||||
SpeedLimit_50 UMETA(DisplayName = "Speed Limit - 50"),
|
||||
SpeedLimit_60 UMETA(DisplayName = "Speed Limit - 60"),
|
||||
SpeedLimit_90 UMETA(DisplayName = "Speed Limit - 90"),
|
||||
SpeedLimit_100 UMETA(DisplayName = "Speed Limit - 100"),
|
||||
SpeedLimit_120 UMETA(DisplayName = "Speed Limit - 120"),
|
||||
SpeedLimit_130 UMETA(DisplayName = "Speed Limit - 130")
|
||||
};
|
||||
|
||||
UCLASS()
|
||||
|
|
|
@ -86,29 +86,33 @@ void ACarlaWheeledVehicle::ApplyVehicleControl(const FVehicleControl &VehicleCon
|
|||
void ACarlaWheeledVehicle::SetThrottleInput(const float Value)
|
||||
{
|
||||
GetVehicleMovementComponent()->SetThrottleInput(Value);
|
||||
Control.Throttle = Value;
|
||||
}
|
||||
|
||||
void ACarlaWheeledVehicle::SetSteeringInput(const float Value)
|
||||
{
|
||||
GetVehicleMovementComponent()->SetSteeringInput(Value);
|
||||
Control.Steer = Value;
|
||||
}
|
||||
|
||||
void ACarlaWheeledVehicle::SetBrakeInput(const float Value)
|
||||
{
|
||||
GetVehicleMovementComponent()->SetBrakeInput(Value);
|
||||
Control.Brake = Value;
|
||||
}
|
||||
|
||||
void ACarlaWheeledVehicle::SetReverse(const bool Value)
|
||||
{
|
||||
if (Value != bIsInReverse) {
|
||||
bIsInReverse = Value;
|
||||
if (Value != Control.bReverse) {
|
||||
Control.bReverse = Value;
|
||||
auto MovementComponent = GetVehicleMovementComponent();
|
||||
MovementComponent->SetUseAutoGears(!bIsInReverse);
|
||||
MovementComponent->SetTargetGear(bIsInReverse ? -1 : 1, true);
|
||||
MovementComponent->SetUseAutoGears(!Control.bReverse);
|
||||
MovementComponent->SetTargetGear(Control.bReverse ? -1 : 1, true);
|
||||
}
|
||||
}
|
||||
|
||||
void ACarlaWheeledVehicle::SetHandbrakeInput(const bool Value)
|
||||
{
|
||||
GetVehicleMovementComponent()->SetHandbrakeInput(Value);
|
||||
Control.bHandBrake = Value;
|
||||
}
|
||||
|
|
|
@ -41,6 +41,13 @@ public:
|
|||
/// @{
|
||||
public:
|
||||
|
||||
/// Vehicle control currently applied to this vehicle.
|
||||
UFUNCTION(Category = "CARLA Wheeled Vehicle", BlueprintCallable)
|
||||
const FVehicleControl &GetVehicleControl() const
|
||||
{
|
||||
return Control;
|
||||
}
|
||||
|
||||
/// Transform of the vehicle. Location is shifted so it matches center of the
|
||||
/// vehicle bounds rather than the actor's location.
|
||||
UFUNCTION(Category = "CARLA Wheeled Vehicle", BlueprintCallable)
|
||||
|
@ -80,6 +87,12 @@ public:
|
|||
UFUNCTION(Category = "CARLA Wheeled Vehicle", BlueprintCallable)
|
||||
float GetMaximumSteerAngle() const;
|
||||
|
||||
UFUNCTION(Category = "CARLA Wheeled Vehicle", BlueprintCallable)
|
||||
ECarlaWheeledVehicleState GetAIVehicleState() const
|
||||
{
|
||||
return State;
|
||||
}
|
||||
|
||||
/// @}
|
||||
// ===========================================================================
|
||||
/// @name Set functions
|
||||
|
@ -105,7 +118,7 @@ public:
|
|||
UFUNCTION(Category = "CARLA Wheeled Vehicle", BlueprintCallable)
|
||||
void ToggleReverse()
|
||||
{
|
||||
SetReverse(!bIsInReverse);
|
||||
SetReverse(!Control.bReverse);
|
||||
}
|
||||
|
||||
UFUNCTION(Category = "CARLA Wheeled Vehicle", BlueprintCallable)
|
||||
|
@ -128,12 +141,6 @@ public:
|
|||
State = InState;
|
||||
}
|
||||
|
||||
UFUNCTION(Category = "AI Controller", BlueprintCallable)
|
||||
ECarlaWheeledVehicleState GetAIVehicleState() const
|
||||
{
|
||||
return State;
|
||||
}
|
||||
|
||||
private:
|
||||
|
||||
/// Current state of the vehicle controller (for debugging purposes).
|
||||
|
@ -146,6 +153,5 @@ private:
|
|||
UPROPERTY(Category = "CARLA Wheeled Vehicle", VisibleAnywhere)
|
||||
UVehicleAgentComponent *VehicleAgentComponent;
|
||||
|
||||
UPROPERTY()
|
||||
bool bIsInReverse = false;
|
||||
FVehicleControl Control;
|
||||
};
|
||||
|
|
Loading…
Reference in New Issue