diff --git a/CHANGELOG.md b/CHANGELOG.md index 40bc6ce4d..febf85733 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,5 +1,8 @@ ## Latest Changes + * LaneInvasionSensor stabilization + - Fix naming: Use 'LaneInvasionSensor'/'lane_invasion' instead of mixture with 'LaneDetector'/'lane_detector' + - Create server-side LaneInvasionSensor (to be able to access it via ROS bridge) * Fix ActorList returned by ActorList.Filter(...) * Add --rolename to manual_control.py * Migrate Content to AWS diff --git a/Docs/cameras_and_sensors.md b/Docs/cameras_and_sensors.md index 50e3c0460..0f6cf9ce2 100644 --- a/Docs/cameras_and_sensors.md +++ b/Docs/cameras_and_sensors.md @@ -48,7 +48,7 @@ This is the list of sensors currently available * [sensor.camera.semantic_segmentation](#sensorcamerasemantic_segmentation) * [sensor.lidar.ray_cast](#sensorlidarray_cast) * [sensor.other.collision](#sensorothercollision) - * [sensor.other.lane_detector](#sensorotherlane_detector) + * [sensor.other.lane_invasion](#sensorotherlane_invasion) * [sensor.other.obstacle](#sensorotherobstacle) sensor.camera.rgb @@ -268,14 +268,14 @@ object for each collision registered Note that several collision events might be registered during a single simulation update. -sensor.other.lane_detector +sensor.other.lane_invasion -------------------------- > _This sensor is a work in progress, currently very limited._ This sensor, when attached to an actor, it registers an event each time the actor crosses a lane marking. This sensor is somehow special as it works fully -on the client-side. The lane detector uses the road data of the active map to +on the client-side. The lane invasion uses the road data of the active map to determine whether a vehicle is invading another lane. This information is based on the OpenDrive file provided by the map, therefore it is subject to the fidelity of the OpenDrive description. In some places there might be diff --git a/LibCarla/source/carla/client/GnssSensor.cpp b/LibCarla/source/carla/client/GnssSensor.cpp index af49e8d9d..49220927b 100644 --- a/LibCarla/source/carla/client/GnssSensor.cpp +++ b/LibCarla/source/carla/client/GnssSensor.cpp @@ -60,7 +60,7 @@ namespace client { _geo_reference.Transform(GetLocation())); } catch (const std::exception &e) { /// @todo We need to unsubscribe the sensor. - // log_error("LaneDetector:", e.what()); + // log_error("GnssSensor:", e.what()); return nullptr; } } diff --git a/LibCarla/source/carla/client/LaneDetector.cpp b/LibCarla/source/carla/client/LaneInvasionSensor.cpp similarity index 82% rename from LibCarla/source/carla/client/LaneDetector.cpp rename to LibCarla/source/carla/client/LaneInvasionSensor.cpp index d0ce1d2b3..8bc723963 100644 --- a/LibCarla/source/carla/client/LaneDetector.cpp +++ b/LibCarla/source/carla/client/LaneInvasionSensor.cpp @@ -4,7 +4,7 @@ // This work is licensed under the terms of the MIT license. // For a copy, see . -#include "carla/client/LaneDetector.h" +#include "carla/client/LaneInvasionSensor.h" #include "carla/Logging.h" #include "carla/client/Map.h" @@ -40,9 +40,9 @@ namespace client { location + Rotate(yaw, geom::Location(-box.extent.x, -box.extent.y, 0.0f))}; } - LaneDetector::~LaneDetector() = default; + LaneInvasionSensor::~LaneInvasionSensor() = default; - void LaneDetector::Listen(CallbackFunctionType callback) { + void LaneInvasionSensor::Listen(CallbackFunctionType callback) { if (_is_listening) { log_error(GetDisplayId(), ": already listening"); return; @@ -57,15 +57,15 @@ namespace client { _map = GetWorld().GetMap(); DEBUG_ASSERT(_map != nullptr); - auto self = boost::static_pointer_cast(shared_from_this()); + auto self = boost::static_pointer_cast(shared_from_this()); log_debug(GetDisplayId(), ": subscribing to tick event"); GetEpisode().Lock()->RegisterOnTickEvent([ cb=std::move(callback), - weak_self=WeakPtr(self)](const auto ×tamp) { + weak_self=WeakPtr(self)](const auto ×tamp) { auto self = weak_self.lock(); if (self != nullptr) { - auto data = self->TickLaneDetector(timestamp); + auto data = self->TickLaneInvasionSensor(timestamp); if (data != nullptr) { cb(std::move(data)); } @@ -74,11 +74,12 @@ namespace client { _is_listening = true; } - void LaneDetector::Stop() { - throw_exception(std::runtime_error("LaneDetector::Stop(): not implemented.")); + void LaneInvasionSensor::Stop() { + /// @todo We need unsubscribe from the world on tick. + _is_listening = false; } - SharedPtr LaneDetector::TickLaneDetector( + SharedPtr LaneInvasionSensor::TickLaneInvasionSensor( const Timestamp ×tamp) { try { const auto new_bounds = GetVehicleBounds(*_vehicle); @@ -98,7 +99,7 @@ namespace client { crossed_lanes); } catch (const std::exception &e) { /// @todo We need to unsubscribe the sensor. - // log_error("LaneDetector:", e.what()); + // log_error("LaneInvasionSensor:", e.what()); return nullptr; } } diff --git a/LibCarla/source/carla/client/LaneDetector.h b/LibCarla/source/carla/client/LaneInvasionSensor.h similarity index 87% rename from LibCarla/source/carla/client/LaneDetector.h rename to LibCarla/source/carla/client/LaneInvasionSensor.h index b58f1edfc..71b1e5f44 100644 --- a/LibCarla/source/carla/client/LaneDetector.h +++ b/LibCarla/source/carla/client/LaneInvasionSensor.h @@ -17,12 +17,12 @@ namespace client { class Map; class Vehicle; - class LaneDetector final : public ClientSideSensor { + class LaneInvasionSensor final : public ClientSideSensor { public: using ClientSideSensor::ClientSideSensor; - ~LaneDetector(); + ~LaneInvasionSensor(); /// Register a @a callback to be executed each time a new measurement is /// received. @@ -43,7 +43,7 @@ namespace client { private: - SharedPtr TickLaneDetector(const Timestamp ×tamp); + SharedPtr TickLaneInvasionSensor(const Timestamp ×tamp); bool _is_listening = false; diff --git a/LibCarla/source/carla/client/detail/ActorFactory.cpp b/LibCarla/source/carla/client/detail/ActorFactory.cpp index e97ec5efd..f89c7347f 100644 --- a/LibCarla/source/carla/client/detail/ActorFactory.cpp +++ b/LibCarla/source/carla/client/detail/ActorFactory.cpp @@ -10,7 +10,7 @@ #include "carla/StringUtil.h" #include "carla/client/Actor.h" #include "carla/client/GnssSensor.h" -#include "carla/client/LaneDetector.h" +#include "carla/client/LaneInvasionSensor.h" #include "carla/client/ServerSideSensor.h" #include "carla/client/TrafficLight.h" #include "carla/client/TrafficSign.h" @@ -73,8 +73,8 @@ namespace detail { SharedPtr parent, GarbageCollectionPolicy gc) { auto init = ActorInitializer{description, episode, parent}; - if (description.description.id == "sensor.other.lane_detector") { /// @todo - return MakeActorImpl(std::move(init), gc); + if (description.description.id == "sensor.other.lane_invasion") { /// @todo + return MakeActorImpl(std::move(init), gc); } else if (description.description.id == "sensor.other.gnss") { /// @todo return MakeActorImpl(std::move(init), gc); } else if (description.HasAStream()) { diff --git a/LibCarla/source/carla/client/detail/Simulator.cpp b/LibCarla/source/carla/client/detail/Simulator.cpp index d0bc5c23a..c6be42c0f 100644 --- a/LibCarla/source/carla/client/detail/Simulator.cpp +++ b/LibCarla/source/carla/client/detail/Simulator.cpp @@ -111,12 +111,6 @@ namespace detail { SharedPtr Simulator::GetBlueprintLibrary() { auto defs = _client.GetActorDefinitions(); - { /// @todo - rpc::ActorDefinition def; - def.id = "sensor.other.lane_detector"; - def.tags = "sensor,other,lane_detector"; - defs.emplace_back(def); - } return MakeShared(std::move(defs)); } @@ -138,9 +132,7 @@ namespace detail { Actor *parent, GarbageCollectionPolicy gc) { rpc::Actor actor; - if (blueprint.GetId() == "sensor.other.lane_detector") { /// @todo - actor.description = blueprint.MakeActorDescription(); - } else if (parent != nullptr) { + if (parent != nullptr) { actor = _client.SpawnActorWithParent( blueprint.MakeActorDescription(), transform, @@ -165,9 +157,7 @@ namespace detail { bool Simulator::DestroyActor(Actor &actor) { bool success = true; - if (actor.GetTypeId() != "sensor.other.lane_detector") { /// @todo - success = _client.DestroyActor(actor.GetId()); - } + success = _client.DestroyActor(actor.GetId()); if (success) { // Remove it's persistent state so it cannot access the client anymore. actor.GetEpisode().Clear(); diff --git a/LibCarla/source/carla/sensor/SensorRegistry.h b/LibCarla/source/carla/sensor/SensorRegistry.h index d37492c43..b0c727d94 100644 --- a/LibCarla/source/carla/sensor/SensorRegistry.h +++ b/LibCarla/source/carla/sensor/SensorRegistry.h @@ -24,6 +24,7 @@ class ACollisionSensor; class ADepthCamera; class AGnssSensor; +class ALaneInvasionSensor; class AObstacleDetectionSensor; class ARayCastLidar; class ASceneCaptureCamera; @@ -45,6 +46,7 @@ namespace sensor { std::pair, std::pair, std::pair, + std::pair, std::pair >; @@ -63,6 +65,7 @@ namespace sensor { #include "Carla/Sensor/SemanticSegmentationCamera.h" #include "Carla/Sensor/WorldObserver.h" #include "Carla/Sensor/GnssSensor.h" +#include "Carla/Sensor/LaneInvasionSensor.h" #include "Carla/Sensor/ObstacleDetectionSensor.h" #endif // LIBCARLA_SENSOR_REGISTRY_WITH_SENSOR_INCLUDES diff --git a/PythonAPI/automatic_control.py b/PythonAPI/automatic_control.py index c0f8bc354..87c207c0d 100755 --- a/PythonAPI/automatic_control.py +++ b/PythonAPI/automatic_control.py @@ -572,7 +572,7 @@ class LaneInvasionSensor(object): self._parent = parent_actor self.hud = hud world = self._parent.get_world() - bp = world.get_blueprint_library().find('sensor.other.lane_detector') + bp = world.get_blueprint_library().find('sensor.other.lane_invasion') self.sensor = world.spawn_actor(bp, carla.Transform(), attach_to=self._parent) # We need to pass the lambda a weak reference to self to avoid circular # reference. diff --git a/PythonAPI/manual_control.py b/PythonAPI/manual_control.py index 497ba60a7..20ebabcc3 100755 --- a/PythonAPI/manual_control.py +++ b/PythonAPI/manual_control.py @@ -605,7 +605,7 @@ class LaneInvasionSensor(object): self._parent = parent_actor self.hud = hud world = self._parent.get_world() - bp = world.get_blueprint_library().find('sensor.other.lane_detector') + bp = world.get_blueprint_library().find('sensor.other.lane_invasion') self.sensor = world.spawn_actor(bp, carla.Transform(), attach_to=self._parent) # We need to pass the lambda a weak reference to self to avoid circular # reference. diff --git a/PythonAPI/manual_control_steeringwheel.py b/PythonAPI/manual_control_steeringwheel.py index b88128a76..bd042605b 100755 --- a/PythonAPI/manual_control_steeringwheel.py +++ b/PythonAPI/manual_control_steeringwheel.py @@ -617,7 +617,7 @@ class LaneInvasionSensor(object): self._parent = parent_actor self.hud = hud world = self._parent.get_world() - bp = world.get_blueprint_library().find('sensor.other.lane_detector') + bp = world.get_blueprint_library().find('sensor.other.lane_invasion') self.sensor = world.spawn_actor(bp, carla.Transform(), attach_to=self._parent) # We need to pass the lambda a weak reference to self to avoid circular # reference. diff --git a/PythonAPI/source/libcarla/Sensor.cpp b/PythonAPI/source/libcarla/Sensor.cpp index 06706ffa0..6b5bce135 100644 --- a/PythonAPI/source/libcarla/Sensor.cpp +++ b/PythonAPI/source/libcarla/Sensor.cpp @@ -7,7 +7,7 @@ #include #include #include -#include +#include #include #include @@ -36,8 +36,8 @@ void export_sensor() { .def(self_ns::str(self_ns::self)) ; - class_, boost::noncopyable, boost::shared_ptr> - ("LaneDetector", no_init) + class_, boost::noncopyable, boost::shared_ptr> + ("LaneInvasionSensor", no_init) .def(self_ns::str(self_ns::self)) ; diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/LaneInvasionSensor.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/LaneInvasionSensor.cpp new file mode 100644 index 000000000..120b3b9b3 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/LaneInvasionSensor.cpp @@ -0,0 +1,26 @@ +// Copyright (c) 2019 Intel Labs. +// +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#include "Carla.h" +#include "Carla/Sensor/LaneInvasionSensor.h" +#include "StaticMeshResources.h" + +FActorDefinition ALaneInvasionSensor::GetSensorDefinition() +{ + return UActorBlueprintFunctionLibrary::MakeGenericSensorDefinition(TEXT("other"), TEXT("lane_invasion")); +} + +ALaneInvasionSensor::ALaneInvasionSensor(const FObjectInitializer &ObjectInitializer) + : Super(ObjectInitializer) +{ + PrimaryActorTick.bCanEverTick = false; + + auto MeshComp = CreateDefaultSubobject(TEXT("RootComponent")); + MeshComp->SetCollisionProfileName(UCollisionProfile::NoCollision_ProfileName); + MeshComp->bHiddenInGame = true; + MeshComp->CastShadow = false; + MeshComp->PostPhysicsComponentTick.bCanEverTick = false; + RootComponent = MeshComp; +} \ No newline at end of file diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/LaneInvasionSensor.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/LaneInvasionSensor.h new file mode 100644 index 000000000..4c97c894f --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/LaneInvasionSensor.h @@ -0,0 +1,27 @@ +// Copyright (c) 2019 Intel Labs. +// +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#pragma once + +#include "Carla/Sensor/ShaderBasedSensor.h" + +#include "Carla/Actor/ActorDefinition.h" + +#include "LaneInvasionSensor.generated.h" + +/// LaneInvasion sensor representation +/// The actual position calculation is done one client side +UCLASS() +class CARLA_API ALaneInvasionSensor : public ASensor +{ + GENERATED_BODY() + +public: + + static FActorDefinition GetSensorDefinition(); + + ALaneInvasionSensor(const FObjectInitializer &ObjectInitializer); + +};