From 88ae9d04ae49e347d970153ab2633d9dc57e9152 Mon Sep 17 00:00:00 2001 From: "Pasch, Frederik" Date: Fri, 3 Jul 2020 15:54:04 +0200 Subject: [PATCH] Update to AD RSS Library 4.0.0 - Added support for unstructured scenes and pedestrians. - Rework rss example folder structure - Python based debug drawing - Handle negative speeds - Fixes #2871 by linking libproj statically. - Fixes #2958 by using sequential colcon build - Allow setting log level - Add ActorConstellationCallback - Move setting of RssDynamics to python code --- CHANGELOG.md | 1 + Docs/adv_rss.md | 10 +- Docs/core_concepts.md | 4 +- Docs/index.md | 2 +- Docs/python_api.md | 72 ++ Docs/ref_sensors.md | 38 + LibCarla/cmake/client/CMakeLists.txt | 2 + LibCarla/source/carla/rss/RssCheck.cpp | 670 +++++----- LibCarla/source/carla/rss/RssCheck.h | 243 ++-- LibCarla/source/carla/rss/RssRestrictor.cpp | 25 +- LibCarla/source/carla/rss/RssRestrictor.h | 8 +- LibCarla/source/carla/rss/RssSensor.cpp | 195 ++- LibCarla/source/carla/rss/RssSensor.h | 37 +- .../source/carla/sensor/data/RssResponse.h | 24 +- PythonAPI/carla/setup.py | 5 +- PythonAPI/carla/source/libcarla/AdRss.cpp | 118 +- PythonAPI/docs/sensor.yml | 89 +- PythonAPI/docs/sensor_data.yml | 80 ++ PythonAPI/examples/manual_control_rss.py | 1096 ----------------- PythonAPI/examples/rss/__init__.py | 0 PythonAPI/examples/rss/manual_control_rss.py | 884 +++++++++++++ PythonAPI/examples/rss/rss_sensor.py | 402 ++++++ PythonAPI/examples/rss/rss_visualization.py | 701 +++++++++++ .../rss/scenarios/FollowLeadingVehicle.xosc | 218 ++++ .../scenarios/unstructured_blocked_road.xosc | 187 +++ .../unstructured_pedestrian_crossing.xosc | 196 +++ .../unstructured_pedestrian_on_road.xosc | 223 ++++ Util/BuildTools/Ad-rss.sh | 55 +- Util/BuildTools/BuildLibCarla.sh | 2 +- Util/BuildTools/Package.sh | 1 + Util/BuildTools/Setup.sh | 3 +- mkdocs.yml | 2 +- 32 files changed, 3860 insertions(+), 1733 deletions(-) delete mode 100755 PythonAPI/examples/manual_control_rss.py create mode 100644 PythonAPI/examples/rss/__init__.py create mode 100755 PythonAPI/examples/rss/manual_control_rss.py create mode 100644 PythonAPI/examples/rss/rss_sensor.py create mode 100644 PythonAPI/examples/rss/rss_visualization.py create mode 100644 PythonAPI/examples/rss/scenarios/FollowLeadingVehicle.xosc create mode 100644 PythonAPI/examples/rss/scenarios/unstructured_blocked_road.xosc create mode 100644 PythonAPI/examples/rss/scenarios/unstructured_pedestrian_crossing.xosc create mode 100644 PythonAPI/examples/rss/scenarios/unstructured_pedestrian_on_road.xosc diff --git a/CHANGELOG.md b/CHANGELOG.md index 014f7ec13..53babbcc7 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,5 +1,6 @@ ## Latest + * Upgraded to AD RSS v4.0.0 supporting unstructured scenes and pedestrians * Fixed a bug where `get_traffic_light` would always return `None` * Changed frozen behavior for traffic lights. It now affects to all traffic lights at the same time * Added API function `freeze_all_traffic_lights` and `reset_group` diff --git a/Docs/adv_rss.md b/Docs/adv_rss.md index d14527c2c..7ea7a0b25 100644 --- a/Docs/adv_rss.md +++ b/Docs/adv_rss.md @@ -1,4 +1,4 @@ -# RSS Sensor +# RSS CARLA integrates the [C++ Library for Responsibility Sensitive Safety](https://github.com/intel/ad-rss-lib) in the client library. This feature allows users to investigate behaviours of RSS without having to implement anything. CARLA will take care of providing the input, and applying the output to the AD systems on the fly. @@ -37,13 +37,9 @@ __2. The client.__ __3. The RssSensor.__      __-__ Uses the *ad-rss-lib* to extract situations, do safety checks, and generate a response.      __-__ Sends the *RssRestrictor* a response containing the proper response and aceleration restrictions to be applied. -     __-__ Asks the server to do some debug drawings to visualize the results of the calculations. __4. The RssRestrictor__      __-__ If the client asks for it, applies the response to the [carla.VehicleControl](python_api.md#carla.VehicleControl), and returns the resulting one. -!!! Important - Debug drawings can delay the RSS response, so they should be disabled during automated RSS evaluations. Use [carla.RssVisualizationMode](python_api.md#carla.RssVisualizationMode) to change the visualization settings. - [![RSS sensor in CARLA](img/rss_carla_integration.png)](https://www.youtube.com/watch?v=UxKPXPT2T8Q)
Visualization of the RssSensor results.
@@ -101,9 +97,9 @@ make package.rss ### RssSensor -[__carla.RssSensor__](python_api.md#carla.RssSensor) supports [ad-rss-lib v3.0.0 feature set](https://intel.github.io/ad-rss-lib/RELEASE_NOTES_AND_DISCLAIMERS) completely, including intersections and [stay on road](https://intel.github.io/ad-rss-lib/ad_rss_map_integration/HandleRoadBoundaries/) support. +[__carla.RssSensor__](python_api.md#carla.RssSensor) supports [ad-rss-lib v4.0.0 feature set](https://intel.github.io/ad-rss-lib/RELEASE_NOTES_AND_DISCLAIMERS) completely, including intersections, [stay on road](https://intel.github.io/ad-rss-lib/ad_rss_map_integration/HandleRoadBoundaries/) support and unstructured scenes (e.g. with pedestrians). -So far, the server provides the sensor with ground truth data of the surroundings that includes the state of other vehicles and traffic lights. Future improvements of this feature will add to the equation pedestrians, and more information of the OpenDRIVE map among others. +So far, the server provides the sensor with ground truth data of the surroundings that includes the state of other traffic participants and traffic lights. ### RssRestrictor diff --git a/Docs/core_concepts.md b/Docs/core_concepts.md index 39337ece7..f6c6a4891 100644 --- a/Docs/core_concepts.md +++ b/Docs/core_concepts.md @@ -68,7 +68,7 @@ CARLA offers a wide range of features that go beyond the scope of this introduct * [__PTV-Vissim co-simulation__](adv_ptv.md). Run a synchronous simulation between CARLA and PTV-Vissim traffic simulator. * [__Recorder__](adv_recorder.md). Saves snapshots of the simulation state to reenact a simulation with exact precision. * [__Rendering options__](adv_rendering_options.md). Graphics quality settings, off-screen rendering and a no-rendering mode. -* [__RSS sensor__](adv_rss.md). This sensor is quite different from the rest. It integrates the [C++ Library for Responsibility Sensitive Safety](https://github.com/intel/ad-rss-lib), and modifies a vehicle's trajectory using safety checks. +* [__RSS__](adv_rss.md). Integration of the [C++ Library for Responsibility Sensitive Safety](https://github.com/intel/ad-rss-lib) to modify a vehicle's trajectory using safety checks. * [__Simulation time and synchrony__](adv_synchrony_timestep.md). Everything regarding the simulation time and server-client communication. * [__SUMO co-simulation__](adv_sumo.md). Run a synchronous simulation between CARLA and SUMO traffic simulator. * [__Traffic manager__](adv_traffic_manager.md). This module is in charge of every vehicle set to autopilot mode. It simulates traffic in the city for the simulation to look like a real urban environment. @@ -91,4 +91,4 @@ CARLA forum 1st. World and client

- \ No newline at end of file + diff --git a/Docs/index.md b/Docs/index.md index 33d6ecce3..f8bb8b983 100644 --- a/Docs/index.md +++ b/Docs/index.md @@ -67,7 +67,7 @@ CARLA forum — Register the events in a simulation and play it again. [__Rendering options__](adv_rendering_options.md) — From quality settings to no-render or off-screen modes. - [__RSS sensor__](adv_rss.md) + [__RSS__](adv_rss.md) — An implementation of RSS in the CARLA client library. [__SUMO co-simulation__](adv_sumo.md) — Run a synchronous simulation between CARLA and SUMO. diff --git a/Docs/python_api.md b/Docs/python_api.md index b4869c160..2390fba7e 100644 --- a/Docs/python_api.md +++ b/Docs/python_api.md @@ -1488,6 +1488,50 @@ Parses the axis' orientations to string. --- +## carla.RssActorConstellationData +Data structure that is provided within the callback registered by RssSensor.register_actor_constellation_callback(). + +

Instance Variables

+- **ego_match_object** (_libad_map_access_python.Object_) +The ego map matched information. +- **ego_route** (_libad_map_access_python.FullRoute_) +The ego route. +- **ego_dynamics_on_route** (_[carla.RssEgoDynamicsOnRoute](#carla.RssEgoDynamicsOnRoute)_) +Current ego vehicle dynamics regarding the route. +- **other_match_object** (_libad_map_access_python.Object_) +The other object's map matched information. +- **other_actor** (_[carla.Actor](#carla.Actor)_) +The other actor. + +

Methods

+ +
Dunder methods
+
- **\__str__**(**self**) + +--- + +## carla.RssActorConstellationResult +Data structure that should be returned by the callback registered by RssSensor.register_actor_constellation_callback(). + +

Instance Variables

+- **rss_calculation_mode** (_libad_rss_map_integration_python.RssMode_TODO_) +The calculation mode to be applied with the actor. +- **restrict_speed_limit_mode** (_libad_rss_map_integration_python.RestrictSpeedLimitMode_TODO_) +The mode for restricting speed limit. +- **ego_vehicle_dynamics** (_libad_rss_python.RssDynamics_) +The RSS dynamics to be applied for the ego vehicle. +- **actor_object_type** (_libad_rss_python.ObjectType_) +The RSS object type to be used for the actor. +- **actor_dynamics** (_libad_rss_python.RssDynamics_) +The RSS dynamics to be applied for the actor. + +

Methods

+ +
Dunder methods
+
- **\__str__**(**self**) + +--- + ## carla.RssEgoDynamicsOnRoute Part of the data contained inside a [carla.RssResponse](#carla.RssResponse) describing the state of the vehicle. The parameters include its current dynamics, and how it is heading regarding the target route. @@ -1530,6 +1574,20 @@ The ego acceleration component _lon_ regarding the route smoothened by an averag --- +## carla.RssLogLevel +Enum declaration used in [carla.RssSensor](#carla.RssSensor) to set the log level. + +

Instance Variables

+- **trace** +- **debug** +- **info** +- **warn** +- **err** +- **critical** +- **off** + +--- + ## carla.RssResponse
Inherited from _[carla.SensorData](#carla.SensorData)_

Class that contains the output of a [carla.RssSensor](#carla.RssSensor). This is the result of the RSS calculations performed for the parent vehicle of the sensor. @@ -1546,6 +1604,10 @@ Acceleration restrictions to be applied, according to the RSS calculation. Detailed RSS states at the current moment in time. - **ego_dynamics_on_route** (_[carla.RssEgoDynamicsOnRoute](#carla.RssEgoDynamicsOnRoute)_) Current ego vehicle dynamics regarding the route. +- **world_model** (_libad_rss_python.WorldModel_) +World model used for calculations. +- **situation_snapshot** (_libad_rss_python.SituationSnapshot_) +Detailed RSS situations extracted from the world model.

Methods

@@ -1606,6 +1668,16 @@ Appends a new target position to the current route of the vehicle. Erases the targets that have been appended to the route. - **drop_route**(**self**) Discards the current route. If there are targets remaining in **routing_targets**, creates a new route using those. Otherwise, a new route is created at random. +- **register_actor_constellation_callback**(**self**, **callback**) +Register a callback to customize a [carla.RssActorConstellationResult](#carla.RssActorConstellationResult). + - **Parameters:** + - `callback` – The function to be called whenever a RSS situation is about to be calculated. + +
Setters
+
- **set_log_level**(**self**, **log_level**) +Sets the log level. + - **Parameters:** + - `log_level` (_[carla.RssLogLevel](#carla.RssLogLevel)_) – New log level.
Dunder methods
- **\__str__**(**self**) diff --git a/Docs/ref_sensors.md b/Docs/ref_sensors.md index efb0494c5..cf2882896 100644 --- a/Docs/ref_sensors.md +++ b/Docs/ref_sensors.md @@ -1183,6 +1183,7 @@ The sensor allows to control the considered route by providing some key points, Description + routing_targets Get the current list of routing targets used for route. @@ -1194,6 +1195,12 @@ The sensor allows to control the considered route by providing some key points, drop_route Discards the current route and creates a new one. + +register_actor_constellation_callback +Register a callback to customize the calculations. + +set_log_level +Sets the log level.
@@ -1236,9 +1243,40 @@ if routing_targets: ego_dynamics_on_route carla.RssEgoDynamicsOnRoute Current ego vehicle dynamics regarding the route. + +situation_snapshot +carla.RssEgoDynamicsOnRoute +Current situation snapshot extracted from the world model. +In case a actor_constellation_callback is registered, a call is triggered for: + +1. default calculation (`actor_constellation_data.other_actor=None`) +2. per-actor calculation + +```py +# Fragment of manual_control_rss.py +# The function is registered as actor_constellation_callback +def _on_actor_constellation_request(self, actor_constellation_data): + actor_constellation_result = carla.RssActorConstellationResult() + actor_constellation_result.rss_calculation_mode = rssmap.RssMode.NotRelevant + actor_constellation_result.restrict_speed_limit_mode = rssmap.RssSceneCreation.RestrictSpeedLimitMode.IncreasedSpeedLimit10 + actor_constellation_result.ego_vehicle_dynamics = self.current_vehicle_parameters + actor_constellation_result.actor_object_type = rss.ObjectType.Invalid + actor_constellation_result.actor_dynamics = self.current_vehicle_parameters + + actor_id = -1 + actor_type_id = "none" + if actor_constellation_data.other_actor != None: + # customize actor_constellation_result for specific actor + ... + else: + # default + ... + return actor_constellation_result +``` + --- ## Semantic segmentation camera diff --git a/LibCarla/cmake/client/CMakeLists.txt b/LibCarla/cmake/client/CMakeLists.txt index 094ca720f..c0b9de355 100644 --- a/LibCarla/cmake/client/CMakeLists.txt +++ b/LibCarla/cmake/client/CMakeLists.txt @@ -20,6 +20,8 @@ if (BUILD_RSS_VARIANT) install(FILES ${spdlog_file} DESTINATION lib) list(APPEND ADRSS_LIBS ${spdlog_file}) + install(FILES ${ADRSS_INSTALL_DIR}/PROJ4/lib/libproj.a DESTINATION lib) + foreach(ad_lib ad_physics ad_rss ad_map_access ad_map_opendrive_reader ad_rss_map_integration) set(${ad_lib}_file ${ADRSS_INSTALL_DIR}/${ad_lib}/lib/lib${ad_lib}.a) install(FILES ${${ad_lib}_file} DESTINATION lib) diff --git a/LibCarla/source/carla/rss/RssCheck.cpp b/LibCarla/source/carla/rss/RssCheck.cpp index bd6064ed1..ccfa8b1aa 100644 --- a/LibCarla/source/carla/rss/RssCheck.cpp +++ b/LibCarla/source/carla/rss/RssCheck.cpp @@ -22,12 +22,14 @@ #include #include #include +#include #include #include #include "carla/client/Map.h" #include "carla/client/TrafficLight.h" #include "carla/client/Vehicle.h" +#include "carla/client/Walker.h" #include "carla/client/Waypoint.h" #define DEBUG_TIMING 0 @@ -51,8 +53,19 @@ void printRoute(std::string const &route_descr, ::ad::map::route::FullRoute cons constexpr float to_radians = static_cast(M_PI) / 180.0f; EgoDynamicsOnRoute::EgoDynamicsOnRoute() - : ego_speed(0.), + : time_since_epoch_check_start_ms(0.), + time_since_epoch_check_end_ms(0.), + ego_speed(0.), + min_stopping_distance(0.), + ego_center({0., 0., 0.}), + ego_heading(0.), + ego_heading_change(0.), + ego_steering_angle(0.), + ego_center_within_route(false), + crossing_border(false), route_heading(0.), + route_nominal_center({0., 0., 0.}), + heading_diff(0.), route_speed_lat(0.), route_speed_lon(0.), route_accel_lat(0.), @@ -72,57 +85,170 @@ std::shared_ptr getTimingLogger() { return logger; } -RssCheck::RssCheck() - : _ego_vehicle_dynamics(GetDefaultVehicleDynamics()), - _other_vehicle_dynamics(GetDefaultVehicleDynamics()), - _road_boundaries_mode(GetDefaultRoadBoundariesMode()), - _visualization_mode(GetDefaultVisualizationMode()) { +::ad::rss::world::RssDynamics RssCheck::GetDefaultVehicleDynamics() { + ::ad::rss::world::RssDynamics default_ego_vehicle_dynamics; + default_ego_vehicle_dynamics.alphaLon.accelMax = ::ad::physics::Acceleration(3.5); + default_ego_vehicle_dynamics.alphaLon.brakeMax = ::ad::physics::Acceleration(-8.); + default_ego_vehicle_dynamics.alphaLon.brakeMin = ::ad::physics::Acceleration(-4.); + default_ego_vehicle_dynamics.alphaLon.brakeMinCorrect = ::ad::physics::Acceleration(-3); + default_ego_vehicle_dynamics.alphaLat.accelMax = ::ad::physics::Acceleration(0.2); + default_ego_vehicle_dynamics.alphaLat.brakeMin = ::ad::physics::Acceleration(-0.8); + default_ego_vehicle_dynamics.lateralFluctuationMargin = ::ad::physics::Distance(0.1); + default_ego_vehicle_dynamics.responseTime = ::ad::physics::Duration(1.0); + default_ego_vehicle_dynamics.maxSpeedOnAcceleration = ::ad::physics::Speed(100.); + default_ego_vehicle_dynamics.unstructuredSettings.pedestrianTurningRadius = ad::physics::Distance(2.0); + default_ego_vehicle_dynamics.unstructuredSettings.driveAwayMaxAngle = ad::physics::Angle(2.4); + default_ego_vehicle_dynamics.unstructuredSettings.vehicleYawRateChange = ad::physics::AngularAcceleration(0.3); + default_ego_vehicle_dynamics.unstructuredSettings.vehicleMinRadius = ad::physics::Distance(3.5); + default_ego_vehicle_dynamics.unstructuredSettings.vehicleTrajectoryCalculationStep = ad::physics::Duration(0.2); + return default_ego_vehicle_dynamics; +} - _other_vehicle_dynamics.responseTime = ::ad::physics::Duration(2.0); +::ad::rss::world::RssDynamics RssCheck::GetDefaultPedestrianDynamics() { + ::ad::rss::world::RssDynamics default_pedestrian_dynamics; + default_pedestrian_dynamics.alphaLon.accelMax = ::ad::physics::Acceleration(2.0); + default_pedestrian_dynamics.alphaLon.brakeMax = ::ad::physics::Acceleration(-4.); + default_pedestrian_dynamics.alphaLon.brakeMin = ::ad::physics::Acceleration(-2.); + default_pedestrian_dynamics.alphaLon.brakeMinCorrect = ::ad::physics::Acceleration(-2.); + default_pedestrian_dynamics.alphaLat.accelMax = ::ad::physics::Acceleration(0.001); + default_pedestrian_dynamics.alphaLat.brakeMin = ::ad::physics::Acceleration(-0.001); + default_pedestrian_dynamics.lateralFluctuationMargin = ::ad::physics::Distance(0.1); + default_pedestrian_dynamics.responseTime = ::ad::physics::Duration(0.5); + default_pedestrian_dynamics.maxSpeedOnAcceleration = ::ad::physics::Speed(10.); + default_pedestrian_dynamics.unstructuredSettings.pedestrianTurningRadius = ad::physics::Distance(2.0); + default_pedestrian_dynamics.unstructuredSettings.driveAwayMaxAngle = ad::physics::Angle(2.4); + default_pedestrian_dynamics.unstructuredSettings.vehicleYawRateChange = ad::physics::AngularAcceleration(0.3); + default_pedestrian_dynamics.unstructuredSettings.vehicleMinRadius = ad::physics::Distance(3.5); + default_pedestrian_dynamics.unstructuredSettings.vehicleTrajectoryCalculationStep = ad::physics::Duration(0.2); + return default_pedestrian_dynamics; +} + +RssCheck::RssCheck(float maximum_steering_angle) + : _maximum_steering_angle(maximum_steering_angle) + , _road_boundaries_mode(GetDefaultRoadBoundariesMode()) { _logger = getLogger(); - spdlog::set_level(spdlog::level::warn); - _logger->set_level(spdlog::level::warn); - ::ad::map::access::getLogger()->set_level(spdlog::level::warn); - ::ad::rss::map::getLogger()->set_level(spdlog::level::warn); - _timing_logger = getTimingLogger(); _timing_logger->set_level(spdlog::level::off); + SetLogLevel(spdlog::level::warn); + + _default_actor_constellation_callback_ego_vehicle_dynamics = GetDefaultVehicleDynamics(); + _default_actor_constellation_callback_other_vehicle_dynamics = GetDefaultVehicleDynamics(); + // set the response time of others vehicles to 2 seconds; the rest stays the same + _default_actor_constellation_callback_other_vehicle_dynamics.responseTime = ::ad::physics::Duration(2.0); + _default_actor_constellation_callback_pedestrian_dynamics = GetDefaultPedestrianDynamics(); + + // Create a default callback. + _actor_constellation_callback = + [this](carla::SharedPtr<::carla::rss::ActorConstellationData> actor_constellation_data) + -> ::carla::rss::ActorConstellationResult { + ::carla::rss::ActorConstellationResult actor_constellation_result; + + actor_constellation_result.rss_calculation_mode = ::ad::rss::map::RssMode::NotRelevant; + actor_constellation_result.restrict_speed_limit_mode = ::ad::rss::map::RssSceneCreation::RestrictSpeedLimitMode::IncreasedSpeedLimit10; + actor_constellation_result.actor_object_type = ad::rss::world::ObjectType::Invalid; + actor_constellation_result.ego_vehicle_dynamics = this->_default_actor_constellation_callback_ego_vehicle_dynamics; + actor_constellation_result.actor_dynamics = this->_default_actor_constellation_callback_other_vehicle_dynamics; + + if (actor_constellation_data->other_actor != nullptr) { + if (boost::dynamic_pointer_cast(actor_constellation_data->other_actor) != nullptr) { + actor_constellation_result.rss_calculation_mode = ::ad::rss::map::RssMode::Unstructured; + actor_constellation_result.actor_object_type = ad::rss::world::ObjectType::Pedestrian; + actor_constellation_result.actor_dynamics = this->_default_actor_constellation_callback_pedestrian_dynamics; + } else if (boost::dynamic_pointer_cast(actor_constellation_data->other_actor) != + nullptr) { + actor_constellation_result.rss_calculation_mode = ::ad::rss::map::RssMode::Structured; + actor_constellation_result.actor_object_type = ad::rss::world::ObjectType::OtherVehicle; + + if (GetSpeed(*actor_constellation_data->other_actor) == ::ad::physics::Speed(0.)) { + /* + special handling for vehicles standing still + to cope with not yet implemented lateral intersection checks in core RSS implementation + if the other is standing still, we don't assume that he will accelerate + otherwise if standing at the intersection the acceleration within reaction time + will allow to enter the intersection which current RSS implementation will immediately consider + as dangerous + */ + actor_constellation_result.actor_dynamics.alphaLon.accelMax = ::ad::physics::Acceleration(0.); + } + } + } + + /* since the ego vehicle is controlled manually, it is easy possible that the ego vehicle + accelerates far more in lateral direction than the ego_dynamics indicate + in an automated vehicle this would be considered by the low-level controller when the RSS restriction + is taken into account properly + but the simple RSS restrictor within CARLA is not able to do so... + So we should at least tell RSS about the fact that we acceleration more than this + to be able to react on this + */ + auto const abs_avg_route_accel_lat = std::fabs(actor_constellation_data->ego_dynamics_on_route.avg_route_accel_lat); + if (abs_avg_route_accel_lat > actor_constellation_result.ego_vehicle_dynamics.alphaLat.accelMax) { + actor_constellation_result.ego_vehicle_dynamics.alphaLat.accelMax = + std::min(ad::physics::Acceleration(20.), abs_avg_route_accel_lat); + } + return actor_constellation_result; + }; + + // set the default dynamics + _carla_rss_state.default_ego_vehicle_dynamics = _default_actor_constellation_callback_ego_vehicle_dynamics; + + _logger->debug("RssCheck with default actor constellation callback created"); +} + +RssCheck::RssCheck(float maximum_steering_angle, + ActorConstellationCallbackFunctionType rss_actor_constellation_callback, + carla::SharedPtr const &carla_ego_actor) + : _maximum_steering_angle(maximum_steering_angle), + _actor_constellation_callback(rss_actor_constellation_callback), + _road_boundaries_mode(GetDefaultRoadBoundariesMode()) { + _logger = getLogger(); + _timing_logger = getTimingLogger(); + _timing_logger->set_level(spdlog::level::off); + + SetLogLevel(spdlog::level::warn); + + _carla_rss_state.ego_match_object = GetMatchObject(carla_ego_actor, ::ad::physics::Distance(2.0)); + UpdateDefaultRssDynamics(_carla_rss_state); + _logger->debug("RssCheck created"); } RssCheck::~RssCheck() {} -::ad::rss::world::RssDynamics RssCheck::GetDefaultVehicleDynamics() { - ::ad::rss::world::RssDynamics default_dynamics; - - default_dynamics.alphaLon.accelMax = ::ad::physics::Acceleration(3.5); - default_dynamics.alphaLon.brakeMax = ::ad::physics::Acceleration(-8.); - default_dynamics.alphaLon.brakeMin = ::ad::physics::Acceleration(-4.); - default_dynamics.alphaLon.brakeMinCorrect = ::ad::physics::Acceleration(-3); - default_dynamics.alphaLat.accelMax = ::ad::physics::Acceleration(0.2); - default_dynamics.alphaLat.brakeMin = ::ad::physics::Acceleration(-0.8); - default_dynamics.lateralFluctuationMargin = ::ad::physics::Distance(0.1); - default_dynamics.responseTime = ::ad::physics::Duration(1.0); - - return default_dynamics; +void RssCheck::SetLogLevel(const spdlog::level::level_enum &log_level) { + spdlog::set_level(log_level); + _logger->set_level(log_level); + ::ad::map::access::getLogger()->set_level(log_level); + ::ad::rss::map::getLogger()->set_level(log_level); } -const ::ad::rss::world::RssDynamics &RssCheck::GetEgoVehicleDynamics() const { - return _ego_vehicle_dynamics; +const ::ad::rss::world::RssDynamics &RssCheck::GetDefaultActorConstellationCallbackEgoVehicleDynamics() const { + return _default_actor_constellation_callback_ego_vehicle_dynamics; } -void RssCheck::SetEgoVehicleDynamics(const ::ad::rss::world::RssDynamics &ego_vehicle_dynamics) { - _ego_vehicle_dynamics = ego_vehicle_dynamics; +void RssCheck::SetDefaultActorConstellationCallbackEgoVehicleDynamics( + const ::ad::rss::world::RssDynamics &ego_vehicle_dynamics) { + _default_actor_constellation_callback_ego_vehicle_dynamics = ego_vehicle_dynamics; } -const ::ad::rss::world::RssDynamics &RssCheck::GetOtherVehicleDynamics() const { - return _other_vehicle_dynamics; +const ::ad::rss::world::RssDynamics &RssCheck::GetDefaultActorConstellationCallbackOtherVehicleDynamics() const { + return _default_actor_constellation_callback_other_vehicle_dynamics; } -void RssCheck::SetOtherVehicleDynamics(const ::ad::rss::world::RssDynamics &other_vehicle_dynamics) { - _other_vehicle_dynamics = other_vehicle_dynamics; +void RssCheck::SetDefaultActorConstellationCallbackOtherVehicleDynamics( + const ::ad::rss::world::RssDynamics &other_vehicle_dynamics) { + _default_actor_constellation_callback_other_vehicle_dynamics = other_vehicle_dynamics; +} + +const ::ad::rss::world::RssDynamics &RssCheck::GetDefaultActorConstellationCallbackPedestrianDynamics() const { + return _default_actor_constellation_callback_pedestrian_dynamics; +} + +void RssCheck::SetDefaultActorConstellationCallbackPedestrianDynamics( + const ::ad::rss::world::RssDynamics &pedestrian_dynamics) { + _default_actor_constellation_callback_pedestrian_dynamics = pedestrian_dynamics; } const ::carla::rss::RoadBoundariesMode &RssCheck::GetRoadBoundariesMode() const { @@ -157,14 +283,6 @@ void RssCheck::ResetRoutingTargets() { _routing_targets_to_append.clear(); } -void RssCheck::SetVisualizationMode(const ::carla::rss::VisualizationMode &visualization_mode) { - _visualization_mode = visualization_mode; -} - -const ::carla::rss::VisualizationMode &RssCheck::GetVisualizationMode() const { - return _visualization_mode; -} - void RssCheck::DropRoute() { _logger->debug("Dropping Route:: {}", _carla_rss_state.ego_route); _carla_rss_state.ego_route = ::ad::map::route::FullRoute(); @@ -174,8 +292,9 @@ bool RssCheck::CheckObjects(carla::client::Timestamp const ×tamp, carla::SharedPtr const &actors, carla::SharedPtr const &carla_ego_actor, ::ad::rss::state::ProperResponse &output_response, - ::ad::rss::world::AccelerationRestriction &output_acceleration_restriction, ::ad::rss::state::RssStateSnapshot &output_rss_state_snapshot, + ::ad::rss::situation::SituationSnapshot &output_situation_snapshot, + ::ad::rss::world::WorldModel &output_world_model, EgoDynamicsOnRoute &output_rss_ego_dynamics_on_route) { try { double const time_since_epoch_check_start_ms = @@ -189,6 +308,9 @@ bool RssCheck::CheckObjects(carla::client::Timestamp const ×tamp, #endif const auto carla_ego_vehicle = boost::dynamic_pointer_cast(carla_ego_actor); + if (carla_ego_vehicle == nullptr) { + _logger->error("RSS Sensor only support vehicles as ego."); + } #if DEBUG_TIMING t_end = std::chrono::high_resolution_clock::now(); @@ -198,7 +320,7 @@ bool RssCheck::CheckObjects(carla::client::Timestamp const ×tamp, // allow the vehicle to be at least 2.0 m away form the route to not lose // the contact to the route - auto const ego_match_object = GetMatchObject(*carla_ego_vehicle, ::ad::physics::Distance(2.0)); + auto const ego_match_object = GetMatchObject(carla_ego_actor, ::ad::physics::Distance(2.0)); if (::ad::map::point::isValid(_carla_rss_state.ego_match_object.enuPosition.centerPoint, false)) { // check for bigger position jumps of the ego vehicle @@ -232,7 +354,10 @@ bool RssCheck::CheckObjects(carla::client::Timestamp const ×tamp, _carla_rss_state.ego_dynamics_on_route = CalculateEgoDynamicsOnRoute( timestamp, time_since_epoch_check_start_ms, *carla_ego_vehicle, _carla_rss_state.ego_match_object, - _carla_rss_state.ego_route, _carla_rss_state.ego_dynamics_on_route); + _carla_rss_state.ego_route, _carla_rss_state.default_ego_vehicle_dynamics, + _carla_rss_state.ego_dynamics_on_route); + + UpdateDefaultRssDynamics(_carla_rss_state); CreateWorldModel(timestamp, *actors, *carla_ego_vehicle, _carla_rss_state); @@ -254,15 +379,7 @@ bool RssCheck::CheckObjects(carla::client::Timestamp const ×tamp, #if DEBUG_TIMING t_end = std::chrono::high_resolution_clock::now(); - std::cout << "-> VI " << std::chrono::duration(t_end - t_start).count() << " start store viz" - << std::endl; -#endif - - StoreVisualizationResults(_carla_rss_state); - -#if DEBUG_TIMING - t_end = std::chrono::high_resolution_clock::now(); - std::cout << "-> VI " << std::chrono::duration(t_end - t_start).count() << " end store viz" + std::cout << "-> AN " << std::chrono::duration(t_end - t_start).count() << " end analyze results" << std::endl; #endif @@ -271,8 +388,9 @@ bool RssCheck::CheckObjects(carla::client::Timestamp const ×tamp, // store result output_response = _carla_rss_state.proper_response; - output_acceleration_restriction = _carla_rss_state.acceleration_restriction; output_rss_state_snapshot = _carla_rss_state.rss_state_snapshot; + output_situation_snapshot = _carla_rss_state.situation_snapshot; + output_world_model = _carla_rss_state.world_model; output_rss_ego_dynamics_on_route = _carla_rss_state.ego_dynamics_on_route; if (_carla_rss_state.dangerous_state) { _logger->debug("===== ROUTE NOT SAFE ====="); @@ -293,36 +411,67 @@ bool RssCheck::CheckObjects(carla::client::Timestamp const ×tamp, } } -::ad::map::match::Object RssCheck::GetMatchObject(carla::client::Vehicle const &carla_vehicle, - ::ad::physics::Distance const &match_distance) const { +::ad::map::match::Object RssCheck::GetMatchObject(carla::SharedPtr const &actor, + ::ad::physics::Distance const &sampling_distance) const { ::ad::map::match::Object match_object; - auto const vehicle_transform = carla_vehicle.GetTransform(); + auto const vehicle_transform = actor->GetTransform(); match_object.enuPosition.centerPoint.x = ::ad::map::point::ENUCoordinate(vehicle_transform.location.x); - match_object.enuPosition.centerPoint.y = ::ad::map::point::ENUCoordinate(-1 * vehicle_transform.location.y); - match_object.enuPosition.centerPoint.z = ::ad::map::point::ENUCoordinate(0); // vehicle_transform.location.z; + match_object.enuPosition.centerPoint.y = ::ad::map::point::ENUCoordinate(-1. * vehicle_transform.location.y); + match_object.enuPosition.centerPoint.z = ::ad::map::point::ENUCoordinate(0.); // vehicle_transform.location.z; match_object.enuPosition.heading = - ::ad::map::point::createENUHeading((-1 * vehicle_transform.rotation.yaw) * to_radians); + ::ad::map::point::createENUHeading(-1 * vehicle_transform.rotation.yaw * to_radians); - const auto &bounding_box = carla_vehicle.GetBoundingBox(); - match_object.enuPosition.dimension.length = ::ad::physics::Distance(2 * bounding_box.extent.x); - match_object.enuPosition.dimension.width = ::ad::physics::Distance(2 * bounding_box.extent.y); - match_object.enuPosition.dimension.height = ::ad::physics::Distance(2 * bounding_box.extent.z); + auto const vehicle = boost::dynamic_pointer_cast(actor); + auto const walker = boost::dynamic_pointer_cast(actor); + if (vehicle != nullptr) { + const auto &bounding_box = vehicle->GetBoundingBox(); + match_object.enuPosition.dimension.length = ::ad::physics::Distance(2 * bounding_box.extent.x); + match_object.enuPosition.dimension.width = ::ad::physics::Distance(2 * bounding_box.extent.y); + match_object.enuPosition.dimension.height = ::ad::physics::Distance(2 * bounding_box.extent.z); + } else if (walker != nullptr) { + const auto &bounding_box = walker->GetBoundingBox(); + match_object.enuPosition.dimension.length = ::ad::physics::Distance(2 * bounding_box.extent.x); + match_object.enuPosition.dimension.width = ::ad::physics::Distance(2 * bounding_box.extent.y); + match_object.enuPosition.dimension.height = ::ad::physics::Distance(2 * bounding_box.extent.z); + } else { + _logger->error("Could not get bounding box of actor {}", actor->GetId()); + } match_object.enuPosition.enuReferencePoint = ::ad::map::access::getENUReferencePoint(); ::ad::map::match::AdMapMatching map_matching; match_object.mapMatchedBoundingBox = - map_matching.getMapMatchedBoundingBox(match_object.enuPosition, match_distance, ::ad::physics::Distance(2.)); + map_matching.getMapMatchedBoundingBox(match_object.enuPosition, sampling_distance); return match_object; } -::ad::physics::Speed RssCheck::GetSpeed(carla::client::Vehicle const &carla_vehicle) const { - auto const velocity = carla_vehicle.GetVelocity(); +::ad::physics::Speed RssCheck::GetSpeed(carla::client::Actor const &actor) const { + auto velocity = actor.GetVelocity(); + auto const actor_transform = actor.GetTransform(); + actor_transform.rotation.InverseRotateVector(velocity); + ::ad::physics::Speed speed(std::sqrt(velocity.x * velocity.x + velocity.y * velocity.y)); + if ( velocity.x < 0. ) + { + speed = -speed; + } + return speed; } +::ad::physics::AngularVelocity RssCheck::GetHeadingChange(carla::client::Actor const &actor) const { + auto const angular_velocity = actor.GetAngularVelocity(); + ::ad::physics::AngularVelocity heading_change(-1. * angular_velocity.z * to_radians); + return heading_change; +} + +::ad::physics::Angle RssCheck::GetSteeringAngle(carla::client::Vehicle const &actor) const { + auto const steer_ratio = actor.GetControl().steer; + ::ad::physics::Angle steering_angle(_maximum_steering_angle * steer_ratio); + return steering_angle; +} + void RssCheck::UpdateRoute(CarlaRssState &carla_rss_state) { _logger->debug("Update route start: {}", carla_rss_state.ego_route); @@ -444,23 +593,23 @@ void RssCheck::UpdateRoute(CarlaRssState &carla_rss_state) { _logger->debug("Update route result: {}", carla_rss_state.ego_route); } -EgoDynamicsOnRoute RssCheck::CalculateEgoDynamicsOnRoute(carla::client::Timestamp const ¤t_timestamp, - double const &time_since_epoch_check_start_ms, - carla::client::Vehicle const &carla_vehicle, - ::ad::map::match::Object match_object, - ::ad::map::route::FullRoute const &route, - EgoDynamicsOnRoute const &last_dynamics) const { +EgoDynamicsOnRoute RssCheck::CalculateEgoDynamicsOnRoute( + carla::client::Timestamp const ¤t_timestamp, double const &time_since_epoch_check_start_ms, + carla::client::Vehicle const &carla_vehicle, ::ad::map::match::Object match_object, + ::ad::map::route::FullRoute const &route, ::ad::rss::world::RssDynamics const &default_ego_vehicle_dynamics, + EgoDynamicsOnRoute const &last_dynamics) const { EgoDynamicsOnRoute new_dynamics; new_dynamics.timestamp = current_timestamp; new_dynamics.time_since_epoch_check_start_ms = time_since_epoch_check_start_ms; new_dynamics.ego_speed = GetSpeed(carla_vehicle); new_dynamics.ego_center = match_object.enuPosition.centerPoint; new_dynamics.ego_heading = match_object.enuPosition.heading; + new_dynamics.ego_heading_change = GetHeadingChange(carla_vehicle); + new_dynamics.ego_steering_angle = GetSteeringAngle(carla_vehicle); auto object_route = ::ad::map::route::getRouteSection(match_object, route, ::ad::map::route::RouteSectionCreationMode::AllRouteLanes); auto border = ::ad::map::route::getENUBorderOfRoute(object_route); - StoreDebugVisualization(object_route, border); new_dynamics.route_heading = ::ad::map::lane::getENUHeading(border, match_object.enuPosition.centerPoint); auto const object_center = ::ad::map::route::findCenterWaypoint(match_object, object_route); @@ -544,14 +693,23 @@ EgoDynamicsOnRoute RssCheck::CalculateEgoDynamicsOnRoute(carla::client::Timestam new_dynamics.crossing_border = false; // calculate the ego stopping distance, to be able to reduce the effort - ad::rss::map::RssObjectConversion object_conversion(::ad::rss::world::ObjectId(0u), - ::ad::rss::world::ObjectType::EgoVehicle, match_object, - new_dynamics.ego_speed, GetEgoVehicleDynamics()); + + ::ad::rss::map::RssObjectData ego_object_data; + ego_object_data.id = ::ad::rss::world::ObjectId(0u); + ego_object_data.type = ::ad::rss::world::ObjectType::EgoVehicle; + ego_object_data.matchObject = match_object; + ego_object_data.speed = new_dynamics.ego_speed; + ego_object_data.yawRate = new_dynamics.ego_heading_change; + ego_object_data.steeringAngle = new_dynamics.ego_steering_angle; + ego_object_data.rssDynamics = default_ego_vehicle_dynamics; + + ad::rss::map::RssObjectConversion object_conversion(ego_object_data); if (!object_conversion.calculateMinStoppingDistance(new_dynamics.min_stopping_distance)) { _logger->error( "CalculateEgoDynamicsOnRoute: calculation of min stopping distance " - "failed. Setting to 100. ({} {} {})", - match_object, new_dynamics.ego_speed, GetEgoVehicleDynamics()); + "failed. Setting to 100. ({} {} {} {})", + match_object, new_dynamics.ego_speed, new_dynamics.ego_speed, new_dynamics.ego_heading_change, + default_ego_vehicle_dynamics); new_dynamics.min_stopping_distance = ::ad::physics::Distance(100.); } @@ -559,6 +717,15 @@ EgoDynamicsOnRoute RssCheck::CalculateEgoDynamicsOnRoute(carla::client::Timestam return new_dynamics; } +void RssCheck::UpdateDefaultRssDynamics(CarlaRssState &carla_rss_state) { + ::ad::map::match::Object other_match_object; + carla::SharedPtr default_constellation_data{ + new ActorConstellationData{carla_rss_state.ego_match_object, carla_rss_state.ego_route, + carla_rss_state.ego_dynamics_on_route, other_match_object, nullptr}}; + auto const default_constellation_result = _actor_constellation_callback(default_constellation_data); + carla_rss_state.default_ego_vehicle_dynamics = default_constellation_result.ego_vehicle_dynamics; +} + ::ad::map::landmark::LandmarkIdSet RssCheck::GetGreenTrafficLightsOnRoute( std::vector> const &traffic_lights, ::ad::map::route::FullRoute const &route) const { @@ -649,28 +816,53 @@ RssCheck::RssObjectChecker::RssObjectChecker(RssCheck const &rss_check, _carla_rss_state(carla_rss_state), _green_traffic_lights(green_traffic_lights) {} -void RssCheck::RssObjectChecker::operator()(const carla::SharedPtr vehicle) const { +void RssCheck::RssObjectChecker::operator()( + const carla::SharedPtr other_traffic_participant) const { try { - auto other_dynamics = _rss_check.GetOtherVehicleDynamics(); - auto other_speed = _rss_check.GetSpeed(*vehicle); - auto ego_dynamics = _rss_check.GetEgoVehicleDynamics(); + auto other_match_object = _rss_check.GetMatchObject(other_traffic_participant, ::ad::physics::Distance(2.0)); - if (other_speed == ::ad::physics::Speed(0.)) { - // if the other is standing still, we don't assume that he will accelerate - other_dynamics.alphaLon.accelMax = ::ad::physics::Acceleration(0.); - } + _rss_check._logger->debug("OtherVehicleMapMatching: {} {}", other_traffic_participant->GetId(), + other_match_object.mapMatchedBoundingBox); - auto other_match_object = _rss_check.GetMatchObject(*vehicle, ::ad::physics::Distance(2.0)); + carla::SharedPtr actor_constellation_data{new ActorConstellationData{ + _carla_rss_state.ego_match_object, _carla_rss_state.ego_route, _carla_rss_state.ego_dynamics_on_route, + other_match_object, other_traffic_participant}}; + auto const actor_constellation_result = _rss_check._actor_constellation_callback(actor_constellation_data); - _rss_check._logger->debug("OtherVehicleMapMatching: {}", other_match_object.mapMatchedBoundingBox); + auto other_speed = _rss_check.GetSpeed(*other_traffic_participant); + auto other_heading_change = _rss_check.GetHeadingChange(*other_traffic_participant); + auto other_steering_angle = ::ad::physics::Angle(0.); + + ::ad::rss::map::RssObjectData ego_object_data; + ego_object_data.id = _carla_ego_vehicle.GetId(); + ego_object_data.type = ::ad::rss::world::ObjectType::EgoVehicle; + ego_object_data.matchObject = _carla_rss_state.ego_match_object; + ego_object_data.speed = _carla_rss_state.ego_dynamics_on_route.ego_speed; + ego_object_data.yawRate = _carla_rss_state.ego_dynamics_on_route.ego_heading_change; + ego_object_data.steeringAngle = _carla_rss_state.ego_dynamics_on_route.ego_steering_angle; + ego_object_data.rssDynamics = actor_constellation_result.ego_vehicle_dynamics; + + ::ad::rss::map::RssObjectData other_object_data; + other_object_data.id = ::ad::rss::world::ObjectId(other_traffic_participant->GetId()); + other_object_data.type = actor_constellation_result.actor_object_type; + other_object_data.matchObject = other_match_object; + other_object_data.speed = other_speed; + other_object_data.yawRate = other_heading_change; + other_object_data.steeringAngle = other_steering_angle; + other_object_data.rssDynamics = actor_constellation_result.actor_dynamics; + _scene_creation.appendScenes( - _carla_ego_vehicle.GetId(), _carla_rss_state.ego_match_object, _carla_rss_state.ego_dynamics_on_route.ego_speed, - ego_dynamics, _carla_rss_state.ego_route, ::ad::rss::world::ObjectId(vehicle->GetId()), - ::ad::rss::world::ObjectType::OtherVehicle, other_match_object, other_speed, other_dynamics, - ::ad::rss::map::RssSceneCreation::RestrictSpeedLimitMode::IncreasedSpeedLimit10, _green_traffic_lights); + ego_object_data, + _carla_rss_state.ego_route, + other_object_data, + actor_constellation_result.restrict_speed_limit_mode, + _green_traffic_lights, + actor_constellation_result.rss_calculation_mode); + } catch (...) { - _rss_check._logger->error("Exception processing vehicle {} -> Ignoring it", vehicle->GetId()); + _rss_check._logger->error("Exception processing other traffic participant {} -> Ignoring it", + other_traffic_participant->GetId()); } } @@ -678,7 +870,7 @@ void RssCheck::CreateWorldModel(carla::client::Timestamp const ×tamp, carla carla::client::Vehicle const &carla_ego_vehicle, CarlaRssState &carla_rss_state) const { // only loop once over the actors since always the respective objects are created std::vector> traffic_lights; - std::vector> other_vehicles; + std::vector> other_traffic_participants; for (const auto &actor : actors) { const auto traffic_light = boost::dynamic_pointer_cast(actor); if (traffic_light != nullptr) { @@ -686,15 +878,15 @@ void RssCheck::CreateWorldModel(carla::client::Timestamp const ×tamp, carla continue; } - const auto vehicle = boost::dynamic_pointer_cast(actor); - if (vehicle != nullptr) { - if (vehicle->GetId() == carla_ego_vehicle.GetId()) { + if ((boost::dynamic_pointer_cast(actor) != nullptr) || + (boost::dynamic_pointer_cast(actor) != nullptr)) { + if (actor->GetId() == carla_ego_vehicle.GetId()) { continue; } auto const relevant_distance = std::max(static_cast(carla_rss_state.ego_dynamics_on_route.min_stopping_distance), 100.); - if (vehicle->GetTransform().location.Distance(carla_ego_vehicle.GetTransform().location) < relevant_distance) { - other_vehicles.push_back(vehicle); + if (actor->GetTransform().location.Distance(carla_ego_vehicle.GetTransform().location) < relevant_distance) { + other_traffic_participants.push_back(actor); } } } @@ -702,58 +894,56 @@ void RssCheck::CreateWorldModel(carla::client::Timestamp const ×tamp, carla ::ad::map::landmark::LandmarkIdSet green_traffic_lights = GetGreenTrafficLightsOnRoute(traffic_lights, carla_rss_state.ego_route); - auto ego_vehicle_dynamics = GetEgoVehicleDynamics(); - auto const abs_avg_route_accel_lat = std::fabs(carla_rss_state.ego_dynamics_on_route.avg_route_accel_lat); - if (abs_avg_route_accel_lat > ego_vehicle_dynamics.alphaLat.accelMax) { - _logger->info( - "!! Route lateral dynamics exceed expectations: route:{} expected:{} " - "!!", - abs_avg_route_accel_lat, ego_vehicle_dynamics.alphaLat.accelMax); - ego_vehicle_dynamics.alphaLat.accelMax = std::min(::ad::physics::Acceleration(20.), abs_avg_route_accel_lat); - } - - ::ad::rss::map::RssSceneCreation scene_creation(timestamp.frame, ego_vehicle_dynamics); + ::ad::rss::map::RssSceneCreation scene_creation(timestamp.frame, carla_rss_state.default_ego_vehicle_dynamics); #ifdef RSS_USE_TBB tbb::parallel_for_each( - other_vehicles.begin(), other_vehicles.end(), + other_traffic_participants.begin(), other_traffic_participants.end(), RssObjectChecker(*this, scene_creation, carla_ego_vehicle, carla_rss_state, green_traffic_lights)); #else - for (auto const vehicle : other_vehicles) { + for (auto const traffic_participant : other_traffic_participants) { auto checker = RssObjectChecker(*this, scene_creation, carla_ego_vehicle, carla_rss_state, green_traffic_lights); - checker(vehicle); + checker(traffic_participant); } #endif if (_road_boundaries_mode != RoadBoundariesMode::Off) { // add artifical objects on the road boundaries for "stay-on-road" feature // use 'smart' dynamics + auto ego_vehicle_dynamics = carla_rss_state.default_ego_vehicle_dynamics; ego_vehicle_dynamics.alphaLat.accelMax = ::ad::physics::Acceleration(0.); - scene_creation.appendRoadBoundaries(carla_ego_vehicle.GetId(), carla_rss_state.ego_match_object, - carla_rss_state.ego_dynamics_on_route.ego_speed, ego_vehicle_dynamics, - carla_rss_state.ego_route, - // since the route always expanded, route isn't required to expand any - // more - ::ad::rss::map::RssSceneCreation::AppendRoadBoundariesMode::RouteOnly); + + ::ad::rss::map::RssObjectData ego_object_data; + ego_object_data.id = carla_ego_vehicle.GetId(); + ego_object_data.type = ::ad::rss::world::ObjectType::EgoVehicle; + ego_object_data.matchObject = _carla_rss_state.ego_match_object; + ego_object_data.speed = _carla_rss_state.ego_dynamics_on_route.ego_speed; + ego_object_data.yawRate = _carla_rss_state.ego_dynamics_on_route.ego_heading_change; + ego_object_data.steeringAngle = _carla_rss_state.ego_dynamics_on_route.ego_steering_angle; + ego_object_data.rssDynamics = ego_vehicle_dynamics; + scene_creation.appendRoadBoundaries(ego_object_data, carla_rss_state.ego_route, + // since the route always expanded, route isn't required to expand any + // more + ::ad::rss::map::RssSceneCreation::AppendRoadBoundariesMode::RouteOnly); } carla_rss_state.world_model = scene_creation.getWorldModel(); } void RssCheck::PerformCheck(CarlaRssState &carla_rss_state) const { - bool result = carla_rss_state.rss_check.calculateAccelerationRestriction( + bool result = carla_rss_state.rss_check.calculateProperResponse( carla_rss_state.world_model, carla_rss_state.situation_snapshot, carla_rss_state.rss_state_snapshot, - carla_rss_state.proper_response, carla_rss_state.acceleration_restriction); + carla_rss_state.proper_response); if (!result) { - _logger->warn("calculateAccelerationRestriction failed!"); + _logger->warn("calculateProperResponse failed!"); carla_rss_state.proper_response.lateralResponseRight = ::ad::rss::state::LateralResponse::None; carla_rss_state.proper_response.lateralResponseLeft = ::ad::rss::state::LateralResponse::None; carla_rss_state.proper_response.longitudinalResponse = ::ad::rss::state::LongitudinalResponse::None; } if (!carla_rss_state.proper_response.isSafe) { - _logger->info("Unsafe route: {}, {}", carla_rss_state.proper_response, carla_rss_state.acceleration_restriction); + _logger->info("Unsafe route: {}", carla_rss_state.proper_response); } } @@ -805,247 +995,29 @@ void RssCheck::AnalyseCheckResults(CarlaRssState &carla_rss_state) const { (carla_rss_state.proper_response.longitudinalResponse != ::ad::rss::state::LongitudinalResponse::None)) { _logger->debug("!! longitudinalResponse only triggered by borders: ignore !!"); carla_rss_state.proper_response.longitudinalResponse = ::ad::rss::state::LongitudinalResponse::None; - carla_rss_state.acceleration_restriction.longitudinalRange.maximum = GetEgoVehicleDynamics().alphaLon.accelMax; + carla_rss_state.proper_response.accelerationRestrictions.longitudinalRange.maximum = + carla_rss_state.default_ego_vehicle_dynamics.alphaLon.accelMax; } if (!vehicle_triggered_left_response && !left_border_is_dangerous && (carla_rss_state.proper_response.lateralResponseLeft != ::ad::rss::state::LateralResponse::None)) { _logger->debug("!! lateralResponseLeft only triggered by right border: ignore !!"); carla_rss_state.proper_response.lateralResponseLeft = ::ad::rss::state::LateralResponse::None; - carla_rss_state.acceleration_restriction.lateralLeftRange.maximum = GetEgoVehicleDynamics().alphaLat.accelMax; + carla_rss_state.proper_response.accelerationRestrictions.lateralLeftRange.maximum = + carla_rss_state.default_ego_vehicle_dynamics.alphaLat.accelMax; carla_rss_state.ego_dynamics_on_route.crossing_border = true; } if (!vehicle_triggered_right_response && !right_border_is_dangerous && (carla_rss_state.proper_response.lateralResponseRight != ::ad::rss::state::LateralResponse::None)) { _logger->debug("!! lateralResponseRight only triggered by left border: ignore !!"); carla_rss_state.proper_response.lateralResponseRight = ::ad::rss::state::LateralResponse::None; - carla_rss_state.acceleration_restriction.lateralRightRange.maximum = GetEgoVehicleDynamics().alphaLat.accelMax; + carla_rss_state.proper_response.accelerationRestrictions.lateralRightRange.maximum = + carla_rss_state.default_ego_vehicle_dynamics.alphaLat.accelMax; carla_rss_state.ego_dynamics_on_route.crossing_border = true; } - _logger->debug("RouteResponse: {}, AccelerationRestriction: {}", carla_rss_state.proper_response, - carla_rss_state.acceleration_restriction); + _logger->debug("RouteResponse: {}", carla_rss_state.proper_response); } -/// -/// visualization -/// - -void RssCheck::VisualizeResults(carla::client::World &world, - carla::SharedPtr const &carla_ego_actor) const { - if (_visualization_mode == VisualizationMode::Off) { - return; - } - - if (_visualization_mutex.try_lock()) { - carla::client::DebugHelper dh = world.MakeDebugHelper(); - - if ((_visualization_mode == VisualizationMode::RouteOnly) || - (_visualization_mode == VisualizationMode::VehicleStateAndRoute) || - (_visualization_mode == VisualizationMode::All)) { - VisualizeRouteLocked(dh, _visualization_route.first, carla_ego_actor, _visualization_route.second); - } - - if ((_visualization_mode == VisualizationMode::VehicleStateOnly) || - (_visualization_mode == VisualizationMode::VehicleStateAndRoute) || - (_visualization_mode == VisualizationMode::All)) { - VisualizeRssResultsLocked(dh, world, carla_ego_actor, _visualization_state_snapshot); - } - - if ((_visualization_mode == VisualizationMode::All) || - (_visualization_mode == VisualizationMode::DebugRouteOrientationObjectRoute) || - (_visualization_mode == VisualizationMode::DebugRouteOrientationBorders) || - (_visualization_mode == VisualizationMode::DebugRouteOrientationBoth)) { - VisualizeEgoDynamics(dh, carla_ego_actor, _visualization_ego_dynamics); - } - - if ((_visualization_mode == VisualizationMode::DebugRouteOrientationObjectRoute) || - (_visualization_mode == VisualizationMode::DebugRouteOrientationBoth)) { - VisualizeRouteLocked(dh, _visualization_debug_route, carla_ego_actor, false); - } - - if ((_visualization_mode == VisualizationMode::DebugRouteOrientationBorders) || - (_visualization_mode == VisualizationMode::DebugRouteOrientationBoth)) { - carla::sensor::data::Color color_left{0u, 255u, 255u}; - carla::sensor::data::Color color_right{122u, 255u, 255u}; - for (auto const &border : _visualization_debug_enu_border) { - VisualizeENUEdgeLocked(dh, border.left, color_left, carla_ego_actor->GetLocation().z); - VisualizeENUEdgeLocked(dh, border.right, color_right, carla_ego_actor->GetLocation().z); - } - } - - _visualization_mutex.unlock(); - } -} - -void RssCheck::StoreVisualizationResults(CarlaRssState const &carla_rss_state) { - if (_visualization_mutex.try_lock()) { - _visualization_state_snapshot = carla_rss_state.rss_state_snapshot; - _visualization_route = std::make_pair(carla_rss_state.ego_route, carla_rss_state.dangerous_state); - _visualization_ego_dynamics = carla_rss_state.ego_dynamics_on_route; - _visualization_mutex.unlock(); - } -} - -void RssCheck::StoreDebugVisualization(::ad::map::route::FullRoute const &debug_route, - std::vector<::ad::map::lane::ENUBorder> const &enu_border) const { - if (_visualization_mutex.try_lock()) { - _visualization_debug_route = debug_route; - _visualization_debug_enu_border = enu_border; - _visualization_mutex.unlock(); - } -} - -void RssCheck::VisualizeRssResultsLocked(carla::client::DebugHelper &dh, carla::client::World &world, - carla::SharedPtr const &carla_ego_actor, - ::ad::rss::state::RssStateSnapshot state_snapshot) const { - const auto carla_ego_vehicle = boost::dynamic_pointer_cast(carla_ego_actor); - carla::geom::Location ego_vehicle_location = carla_ego_vehicle->GetLocation(); - auto ego_vehicle_transform = carla_ego_vehicle->GetTransform(); - - for (auto const &state : state_snapshot.individualResponses) { - carla::rpc::ActorId vehicle_id = static_cast(state.objectId); - carla::SharedPtr vehicle_list = - world.GetActors(std::vector{vehicle_id}); - carla::geom::Location ego_point = ego_vehicle_location; - ego_point.z += 0.05f; - const auto yaw = ego_vehicle_transform.rotation.yaw; - const float cosine = std::cos(yaw * to_radians); - const float sine = std::sin(yaw * to_radians); - carla::geom::Location line_offset{-sine * 0.1f, cosine * 0.1f, 0.0f}; - for (const auto &actor : *vehicle_list) { - const auto vehicle = boost::dynamic_pointer_cast(actor); - carla::geom::Location point = vehicle->GetLocation(); - point.z += 0.05f; - carla::sensor::data::Color indicator_color{0u, 255u, 0u}; - bool dangerous = ::ad::rss::state::isDangerous(state); - if (dangerous) { - indicator_color = carla::sensor::data::Color{255u, 0u, 0u}; - } - if (_visualization_mode == VisualizationMode::All) { - // the car connections are only visualized if All is requested - carla::sensor::data::Color lon_color = indicator_color; - carla::sensor::data::Color lat_l_color = indicator_color; - carla::sensor::data::Color lat_r_color = indicator_color; - if (!state.longitudinalState.isSafe) { - lon_color.r = 255u; - if (dangerous) { - lon_color.g = 0u; - } else { - lon_color.g = 255u; - } - } - if (!state.lateralStateLeft.isSafe) { - lat_l_color.r = 255u; - if (dangerous) { - lat_l_color.g = 0u; - } else { - lat_l_color.g = 255u; - } - } - if (!state.lateralStateRight.isSafe) { - lat_r_color.r = 255u; - if (dangerous) { - lat_r_color.g = 0u; - } else { - lat_r_color.g = 255u; - } - } - dh.DrawLine(ego_point, point, 0.1f, lon_color, 0.02f, false); - dh.DrawLine(ego_point - line_offset, point - line_offset, 0.1f, lat_l_color, 0.02f, false); - dh.DrawLine(ego_point + line_offset, point + line_offset, 0.1f, lat_r_color, 0.02f, false); - } - point.z += 3.f; - dh.DrawPoint(point, 0.2f, indicator_color, 0.02f, false); - } - } -} - -void RssCheck::VisualizeENUEdgeLocked(carla::client::DebugHelper &dh, ::ad::map::point::ENUEdge const &edge, - carla::sensor::data::Color const &color, float const z_offset) const { - for (auto const &point : edge) { - carla::geom::Location carla_point(static_cast(static_cast(point.x)), - static_cast(static_cast(-1. * point.y)), - static_cast(static_cast(point.z))); - carla_point.z += z_offset; - dh.DrawPoint(carla_point, 0.1f, color, 0.1f, false); - } -} - -void RssCheck::VisualizeRouteLocked(carla::client::DebugHelper &dh, const ::ad::map::route::FullRoute &route, - carla::SharedPtr const &carla_ego_actor, - bool dangerous) const { - std::map<::ad::map::lane::LaneId, ::ad::map::point::ENUEdge> right_lane_edges; - std::map<::ad::map::lane::LaneId, ::ad::map::point::ENUEdge> left_lane_edges; - - carla::geom::Location ego_vehicle_location = carla_ego_actor->GetLocation(); - for (auto const &road_segment : route.roadSegments) { - { - auto &right_most_lane = road_segment.drivableLaneSegments.front(); - if (right_lane_edges.find(right_most_lane.laneInterval.laneId) == right_lane_edges.end()) { - ::ad::map::point::ENUEdge edge = ::ad::map::route::getRightProjectedENUEdge(right_most_lane.laneInterval); - right_lane_edges.insert({right_most_lane.laneInterval.laneId, edge}); - bool intersection_lane = - ::ad::map::intersection::Intersection::isLanePartOfAnIntersection(right_most_lane.laneInterval.laneId); - - carla::sensor::data::Color color((dangerous ? 128 : 255), 0, 0); - if (intersection_lane) { - color.b = (dangerous ? 128 : 255); - } - VisualizeENUEdgeLocked(dh, edge, color, ego_vehicle_location.z); - } - } - { - auto &left_most_lane = road_segment.drivableLaneSegments.back(); - if (left_lane_edges.find(left_most_lane.laneInterval.laneId) == left_lane_edges.end()) { - ::ad::map::point::ENUEdge edge = ::ad::map::route::getLeftProjectedENUEdge(left_most_lane.laneInterval); - left_lane_edges.insert({left_most_lane.laneInterval.laneId, edge}); - bool intersection_lane = - ::ad::map::intersection::Intersection::isLanePartOfAnIntersection(left_most_lane.laneInterval.laneId); - carla::sensor::data::Color color(0, (dangerous ? 128 : 255), 0); - if (intersection_lane) { - color.b = (dangerous ? 128 : 255); - } - VisualizeENUEdgeLocked(dh, edge, color, ego_vehicle_location.z); - } - } - } -} - -void RssCheck::VisualizeEgoDynamics(carla::client::DebugHelper &dh, - carla::SharedPtr const &carla_ego_actor, - EgoDynamicsOnRoute const &ego_dynamics_on_route) const { - const auto carla_ego_vehicle = boost::dynamic_pointer_cast(carla_ego_actor); - carla::geom::Location ego_vehicle_location = carla_ego_vehicle->GetLocation(); - - carla::sensor::data::Color color{0u, 0u, 255u}; - - auto sin_heading = static_cast(std::sin(static_cast(ego_dynamics_on_route.route_heading))); - auto cos_heading = static_cast(std::cos(static_cast(ego_dynamics_on_route.route_heading))); - - carla::geom::Location heading_location_start = ego_vehicle_location; - heading_location_start.x -= cos_heading * 10.f; - heading_location_start.y += sin_heading * 10.f; - heading_location_start.z += 0.5f; - carla::geom::Location heading_location_end = ego_vehicle_location; - heading_location_end.x += cos_heading * 10.f; - heading_location_end.y -= sin_heading * 10.f; - heading_location_end.z += 0.5f; - - dh.DrawArrow(heading_location_start, heading_location_end, 0.1f, 0.1f, color, 0.02f, false); - - auto sin_center = static_cast(std::sin(static_cast(ego_dynamics_on_route.route_heading) + M_PI_2)); - auto cos_center = static_cast(std::cos(static_cast(ego_dynamics_on_route.route_heading) + M_PI_2)); - carla::geom::Location center_location_start = ego_vehicle_location; - center_location_start.x -= cos_center * 2.f; - center_location_start.y += sin_center * 2.f; - center_location_start.z += 0.5f; - carla::geom::Location center_location_end = ego_vehicle_location; - center_location_end.x += cos_center * 2.f; - center_location_end.y -= sin_center * 2.f; - center_location_end.z += 0.5f; - - dh.DrawLine(center_location_start, center_location_end, 0.1f, color, 0.02f, false); -} } // namespace rss } // namespace carla diff --git a/LibCarla/source/carla/rss/RssCheck.h b/LibCarla/source/carla/rss/RssCheck.h index 45a0c227f..30e94980c 100644 --- a/LibCarla/source/carla/rss/RssCheck.h +++ b/LibCarla/source/carla/rss/RssCheck.h @@ -30,24 +30,6 @@ enum class RoadBoundariesMode { On /// The road boundaries of the current route are considered by RSS check }; -/// @brief struct defining the different supported handling of debug -/// visualization -enum class VisualizationMode { - Off, /// Nothing is visualized - RouteOnly, /// Only the route is visualized - DebugRouteOrientationObjectRoute, /// Only the route section around the - /// object - /// is visualized - DebugRouteOrientationBorders, /// Only the route section transformed into - /// borders around the object is visualized - DebugRouteOrientationBoth, /// The route section around the object and the - /// transformed borders are visualized - VehicleStateOnly, /// Only the states of the vehicles is visualized - VehicleStateAndRoute, /// The states of the vehicles and the route is - /// visualized - All /// All (except debug) is visualized -}; - /// @brief struct defining the ego vehicles current dynamics in respect to the /// current route /// @@ -73,6 +55,10 @@ struct EgoDynamicsOnRoute { ::ad::map::point::ENUPoint ego_center; /// @brief the considered heading of the ego vehicle ::ad::map::point::ENUHeading ego_heading; + /// @brief the considered heading change of the ego vehicle + ::ad::physics::AngularVelocity ego_heading_change; + /// @brief the considered steering angle of the ego vehicle + ::ad::physics::Angle ego_steering_angle; /// @brief check if the ego center is within route bool ego_center_within_route; /// @brief flag indicating if the current state is already crossing one of the @@ -102,12 +88,58 @@ struct EgoDynamicsOnRoute { ::ad::physics::Acceleration avg_route_accel_lon; }; +/// @brief Struct defining the configuration for RSS processing of a given actor +/// +/// The RssSensor implementation allows to configure the actors individually +/// for every frame +/// +struct ActorConstellationResult { + /// The calculation mode to be applied with the actor + ::ad::rss::map::RssMode rss_calculation_mode{::ad::rss::map::RssMode::NotRelevant}; + + /// The mode for restricting speed limit + ::ad::rss::map::RssSceneCreation::RestrictSpeedLimitMode restrict_speed_limit_mode{::ad::rss::map::RssSceneCreation::RestrictSpeedLimitMode::None}; + + /// The Rss dynamics to be applied for the ego vehicle + ::ad::rss::world::RssDynamics ego_vehicle_dynamics; + + /// The Rss object type to be used for the actor + ::ad::rss::world::ObjectType actor_object_type; + + /// The Rss dynamics to be applied for the actor + ::ad::rss::world::RssDynamics actor_dynamics; +}; + +struct ActorConstellationData { + /// @brief the ego map matched information + ::ad::map::match::Object ego_match_object; + + /// @brief the ego route + ::ad::map::route::FullRoute ego_route; + + /// @brief the ego dynamics on the route + EgoDynamicsOnRoute ego_dynamics_on_route; + + /// @brief the other object's map matched information + ::ad::map::match::Object other_match_object; + + carla::SharedPtr other_actor; +}; + /// @brief class implementing the actual RSS checks based on CARLA world /// description class RssCheck { public: - /// @brief constructor - RssCheck(); + using ActorConstellationCallbackFunctionType = + std::function<::carla::rss::ActorConstellationResult(carla::SharedPtr)>; + + /// @brief default constructor with default internal default actor constellation callback + RssCheck(float max_steering_angle); + + /// @brief constructor with actor constellation callback + RssCheck(float max_steering_angle, + ActorConstellationCallbackFunctionType rss_actor_constellation_callback, + carla::SharedPtr const &carla_ego_actor); /// @brief destructor ~RssCheck(); @@ -120,23 +152,28 @@ public: bool CheckObjects(carla::client::Timestamp const ×tamp, carla::SharedPtr const &actors, carla::SharedPtr const &carla_ego_actor, ::ad::rss::state::ProperResponse &output_response, - ::ad::rss::world::AccelerationRestriction &output_acceleration_restriction, ::ad::rss::state::RssStateSnapshot &output_rss_state_snapshot, + ::ad::rss::situation::SituationSnapshot &output_situation_snapshot, + ::ad::rss::world::WorldModel &output_world_model, EgoDynamicsOnRoute &output_rss_ego_dynamics_on_route); - /// @brief function to visualize the RSS check results - /// - void VisualizeResults(carla::client::World &world, - carla::SharedPtr const &carla_ego_actor) const; + /// @returns the used vehicle dynamics for ego vehicle + const ::ad::rss::world::RssDynamics &GetDefaultActorConstellationCallbackEgoVehicleDynamics() const; + /// @brief sets the vehicle dynamics to be used for the ego vehicle + void SetDefaultActorConstellationCallbackEgoVehicleDynamics( + const ::ad::rss::world::RssDynamics &ego_vehicle_dynamics); + /// @returns the used vehicle dynamics for other vehicles + const ::ad::rss::world::RssDynamics &GetDefaultActorConstellationCallbackOtherVehicleDynamics() const; + /// @brief sets the vehicle dynamics to be used for other vehicles + void SetDefaultActorConstellationCallbackOtherVehicleDynamics( + const ::ad::rss::world::RssDynamics &other_vehicle_dynamics); + /// @returns the used vehicle dynamics for pedestrians + const ::ad::rss::world::RssDynamics &GetDefaultActorConstellationCallbackPedestrianDynamics() const; + /// @brief sets the dynamics to be used for pedestrians + void SetDefaultActorConstellationCallbackPedestrianDynamics(const ::ad::rss::world::RssDynamics &pedestrian_dynamics); - /// @returns the used vehicle dynamics for ego vehcile - const ::ad::rss::world::RssDynamics &GetEgoVehicleDynamics() const; - /// @brief sets the vehicle dynamics to be used for the ego vehcile - void SetEgoVehicleDynamics(const ::ad::rss::world::RssDynamics &ego_vehicle_dynamics); - /// @returns the used vehicle dynamics for other vehciles - const ::ad::rss::world::RssDynamics &GetOtherVehicleDynamics() const; - /// @brief sets the vehicle dynamics to be used for other vehciles - void SetOtherVehicleDynamics(const ::ad::rss::world::RssDynamics &other_vehicle_dynamics); + /// @brief sets the current log level + void SetLogLevel(const spdlog::level::level_enum &log_level); /// @returns the current mode for respecting the road boundaries (@see also /// RssSensor::GetRoadBoundariesMode()) @@ -155,13 +192,6 @@ public: /// RssSensor::ResetRoutingTargets()) void ResetRoutingTargets(); - /// @brief sets the visualization mode (@see also - /// ::carla::rss::VisualizationMode) - void SetVisualizationMode(const ::carla::rss::VisualizationMode &visualization_mode); - /// @returns get the current visualization mode (@see also - /// ::carla::rss::VisualizationMode) - const ::carla::rss::VisualizationMode &GetVisualizationMode() const; - /// @brief drop the current route /// /// Afterwards a new route is selected randomly (if multiple routes are @@ -172,33 +202,39 @@ public: /// @returns the default vehicle dynamics static ::ad::rss::world::RssDynamics GetDefaultVehicleDynamics(); + /// @returns the default pedestrian dynamics + static ::ad::rss::world::RssDynamics GetDefaultPedestrianDynamics(); + /// @returns the default road boundaries mode static RoadBoundariesMode GetDefaultRoadBoundariesMode() { return RoadBoundariesMode::Off; } - /// @returns the default visualization mode - static VisualizationMode GetDefaultVisualizationMode() { - return VisualizationMode::All; - } - private: /// @brief standard logger std::shared_ptr _logger; /// @brief logger for timing log messages std::shared_ptr _timing_logger; - /// @brief current used vehicle dynamics for ego vehcile - ::ad::rss::world::RssDynamics _ego_vehicle_dynamics; - /// @brief current used vehicle dynamics for other vehciles - ::ad::rss::world::RssDynamics _other_vehicle_dynamics; + + /// @brief maximum steering angle + float _maximum_steering_angle; + + /// @brief current used vehicle dynamics for ego vehicle by the default actor constellation callback + ::ad::rss::world::RssDynamics _default_actor_constellation_callback_ego_vehicle_dynamics; + /// @brief current used vehicle dynamics for other vehicle by the default actor constellation callback + ::ad::rss::world::RssDynamics _default_actor_constellation_callback_other_vehicle_dynamics; + /// @brief current used vehicle dynamics for pedestrians by the default actor constellation callback + ::ad::rss::world::RssDynamics _default_actor_constellation_callback_pedestrian_dynamics; + + /// @brief the current actor constellation callback + ActorConstellationCallbackFunctionType _actor_constellation_callback; + /// @brief current used road boundaries mode ::carla::rss::RoadBoundariesMode _road_boundaries_mode; /// @brief current used routing targets std::vector<::ad::map::point::ENUPoint> _routing_targets; /// @brief routing targets to be appended next run std::vector<::ad::map::point::ENUPoint> _routing_targets_to_append; - /// @brief current used routing target - ::carla::rss::VisualizationMode _visualization_mode; /// @brief struct collecting the rss states required struct CarlaRssState { @@ -214,6 +250,9 @@ private: /// @brief the ego dynamics on the route EgoDynamicsOnRoute ego_dynamics_on_route; + /// @brief current used default vehicle dynamics for ego vehicle + ::ad::rss::world::RssDynamics default_ego_vehicle_dynamics; + /// @brief check input: the RSS world model ::ad::rss::world::WorldModel world_model; @@ -223,8 +262,6 @@ private: ::ad::rss::state::RssStateSnapshot rss_state_snapshot; /// @brief check result: the proper response ::ad::rss::state::ProperResponse proper_response; - /// @brief check result: the acceleration restrictions - ::ad::rss::world::AccelerationRestriction acceleration_restriction; /// @brief flag indicating if the current state is overall dangerous bool dangerous_state; /// @brief flag indicating if the current state is dangerous because of a @@ -240,7 +277,7 @@ private: RssObjectChecker(RssCheck const &rss_check, ::ad::rss::map::RssSceneCreation &scene_creation, carla::client::Vehicle const &carla_ego_vehicle, CarlaRssState const &carla_rss_state, ::ad::map::landmark::LandmarkIdSet const &green_traffic_lights); - void operator()(const carla::SharedPtr vehicle) const; + void operator()(const carla::SharedPtr other_traffic_participant) const; private: RssCheck const &_rss_check; @@ -255,12 +292,18 @@ private: /// @brief the current state of the ego vehicle CarlaRssState _carla_rss_state; - /// @brief calculate the map matched object from the carla_vehicle - ::ad::map::match::Object GetMatchObject(carla::client::Vehicle const &carla_vehicle, - ::ad::physics::Distance const &match_distance) const; + /// @brief calculate the map matched object from the actor + ::ad::map::match::Object GetMatchObject(carla::SharedPtr const &actor, + ::ad::physics::Distance const &sampling_distance) const; - /// @brief calculate the speed from the carla_vehicle - ::ad::physics::Speed GetSpeed(carla::client::Vehicle const &carla_vehicle) const; + /// @brief calculate the speed from the actor + ::ad::physics::Speed GetSpeed(carla::client::Actor const &actor) const; + + /// @brief calculate the heading change from the actor + ::ad::physics::AngularVelocity GetHeadingChange(carla::client::Actor const &actor) const; + + /// @brief calculate the steering angle from the actor + ::ad::physics::Angle GetSteeringAngle(carla::client::Vehicle const &actor) const; /// @brief update the desired ego vehicle route void UpdateRoute(CarlaRssState &carla_rss_state); @@ -271,8 +314,11 @@ private: carla::client::Vehicle const &carla_vehicle, ::ad::map::match::Object match_object, ::ad::map::route::FullRoute const &route, + ::ad::rss::world::RssDynamics const &default_ego_vehicle_dynamics, EgoDynamicsOnRoute const &last_dynamics) const; + void UpdateDefaultRssDynamics(CarlaRssState &carla_rss_state); + /// @brief collect the green traffic lights on the current route ::ad::map::landmark::LandmarkIdSet GetGreenTrafficLightsOnRoute( std::vector> const &traffic_lights, @@ -287,39 +333,6 @@ private: /// @brief Analyse the RSS check results void AnalyseCheckResults(CarlaRssState &carla_rss_state) const; - - /// - /// visualization - /// - void StoreVisualizationResults(CarlaRssState const &carla_rss_state); - - void StoreDebugVisualization(::ad::map::route::FullRoute const &debug_route, - std::vector<::ad::map::lane::ENUBorder> const &enu_border) const; - - /// mutex to protect the visualization content - mutable std::mutex _visualization_mutex; - /// the RssStateSnapshot to be visualized - ::ad::rss::state::RssStateSnapshot _visualization_state_snapshot; - void VisualizeRssResultsLocked(carla::client::DebugHelper &dh, carla::client::World &world, - carla::SharedPtr const &carla_ego_actor, - ::ad::rss::state::RssStateSnapshot state_snapshot) const; - - /// the FullRoute to be visualized - std::pair<::ad::map::route::FullRoute, bool> _visualization_route; - void VisualizeRouteLocked(carla::client::DebugHelper &dh, ::ad::map::route::FullRoute const &route, - carla::SharedPtr const &carla_ego_actor, bool dangerous) const; - - void VisualizeENUEdgeLocked(carla::client::DebugHelper &dh, ::ad::map::point::ENUEdge const &edge, - carla::sensor::data::Color const &color, float const z_offset) const; - - /// the EgoDynamicsOnRoute to be visualized - EgoDynamicsOnRoute _visualization_ego_dynamics; - void VisualizeEgoDynamics(carla::client::DebugHelper &dh, - carla::SharedPtr const &carla_ego_actor, - EgoDynamicsOnRoute const &ego_dynamics_on_route) const; - - mutable ::ad::map::route::FullRoute _visualization_debug_route; - mutable std::vector<::ad::map::lane::ENUBorder> _visualization_debug_enu_border; }; } // namespace rss @@ -342,6 +355,8 @@ inline std::ostream &operator<<(std::ostream &out, const ::carla::rss::EgoDynami << ", ego_speed=" << ego_dynamics_on_route.ego_speed << ", min_stopping_distance=" << ego_dynamics_on_route.min_stopping_distance << ", ego_center=" << ego_dynamics_on_route.ego_center << ", ego_heading=" << ego_dynamics_on_route.ego_heading + << ", ego_heading_change=" << ego_dynamics_on_route.ego_heading_change + << ", ego_steering_angle=" << ego_dynamics_on_route.ego_steering_angle << ", ego_center_within_route=" << ego_dynamics_on_route.ego_center_within_route << ", crossing_border=" << ego_dynamics_on_route.crossing_border << ", route_heading=" << ego_dynamics_on_route.route_heading @@ -352,7 +367,49 @@ inline std::ostream &operator<<(std::ostream &out, const ::carla::rss::EgoDynami << ", route_accel_lat=" << ego_dynamics_on_route.route_accel_lat << ", route_accel_lon=" << ego_dynamics_on_route.route_accel_lon << ", avg_route_accel_lat=" << ego_dynamics_on_route.avg_route_accel_lat - << ", avg_route_accel_lon=" << ego_dynamics_on_route.avg_route_accel_lon << ')'; + << ", avg_route_accel_lon=" << ego_dynamics_on_route.avg_route_accel_lon << ")"; return out; } + +/** + * \brief standard ostream operator + * + * \param[in/out] os The output stream to write to + * \param[in] actor_constellation_result the actor constellation result to stream out + * + * \returns The stream object. + * + */ +inline std::ostream &operator<<(std::ostream &out, + const ::carla::rss::ActorConstellationResult &actor_constellation_result) { + out << "ActorConstellationResult(rss_calculation_mode=" << actor_constellation_result.rss_calculation_mode + << ", restrict_speed_limit_mode=" << actor_constellation_result.restrict_speed_limit_mode + << ", ego_vehicle_dynamics=" << actor_constellation_result.ego_vehicle_dynamics + << ", actor_object_type=" << actor_constellation_result.actor_object_type + << ", actor_dynamics=" << actor_constellation_result.actor_dynamics << ")"; + return out; +} + +/** + * \brief standard ostream operator + * + * \param[in/out] os The output stream to write to + * \param[in] actor_constellation_result the actor constellation result to stream out + * + * \returns The stream object. + * + */ +inline std::ostream &operator<<(std::ostream &out, + const ::carla::rss::ActorConstellationData &actor_constellation_data) { + out << "ActorConstellationData("; + if (actor_constellation_data.other_actor != nullptr) { + out << "actor_id=" << actor_constellation_data.other_actor->GetId() + << ", actor_dynamics=" << actor_constellation_data.other_match_object << ", "; + } + out << "ego_match_object=" << actor_constellation_data.ego_match_object + << ", ego_route=" << actor_constellation_data.ego_route + << ", ego_dynamics_on_route=" << actor_constellation_data.ego_dynamics_on_route << ")"; + return out; +} + } // namespace std diff --git a/LibCarla/source/carla/rss/RssRestrictor.cpp b/LibCarla/source/carla/rss/RssRestrictor.cpp index 34725ccc0..16d4d694d 100644 --- a/LibCarla/source/carla/rss/RssRestrictor.cpp +++ b/LibCarla/source/carla/rss/RssRestrictor.cpp @@ -9,7 +9,8 @@ #include "carla/rss/RssCheck.h" #include -#include +#include +#include #include namespace carla { @@ -27,7 +28,7 @@ RssRestrictor::RssRestrictor() { RssRestrictor::~RssRestrictor() = default; carla::rpc::VehicleControl RssRestrictor::RestrictVehicleControl( - const carla::rpc::VehicleControl &vehicle_control, const ::ad::rss::world::AccelerationRestriction &restriction, + const carla::rpc::VehicleControl &vehicle_control, const ::ad::rss::state::ProperResponse &proper_response, const carla::rss::EgoDynamicsOnRoute &ego_dynamics_on_route, const carla::rpc::VehiclePhysicsControl &vehicle_physics) { carla::rpc::VehicleControl restricted_vehicle_control(vehicle_control); @@ -54,10 +55,13 @@ carla::rpc::VehicleControl RssRestrictor::RestrictVehicleControl( // do not apply any restrictions when in reverse gear if (!vehicle_control.reverse) { - _logger->debug("Lon {}, L {}, R {}; LatSpeed {}, Accel {}, Avg {}", restriction.longitudinalRange, - restriction.lateralLeftRange, restriction.lateralRightRange, ego_dynamics_on_route.route_speed_lat, - ego_dynamics_on_route.route_accel_lat, ego_dynamics_on_route.avg_route_accel_lat); - if (restriction.lateralLeftRange.maximum <= ::ad::physics::Acceleration(0.0)) { + _logger->debug("Lon {}, L {}, R {}; LatSpeed {}, Accel {}, Avg {}, Hdg {}, AllowedHeadingRanges {}", + proper_response.accelerationRestrictions.longitudinalRange, + proper_response.accelerationRestrictions.lateralLeftRange, + proper_response.accelerationRestrictions.lateralRightRange, ego_dynamics_on_route.route_speed_lat, + ego_dynamics_on_route.route_accel_lat, ego_dynamics_on_route.avg_route_accel_lat, + ego_dynamics_on_route.ego_heading, proper_response.headingRanges); + if (proper_response.accelerationRestrictions.lateralLeftRange.maximum <= ::ad::physics::Acceleration(0.0)) { if (ego_dynamics_on_route.route_speed_lat < ::ad::physics::Speed(0.0)) { // driving to the left if (ego_dynamics_on_route.route_speed_lon != ::ad::physics::Speed(0.0)) { @@ -75,7 +79,7 @@ carla::rpc::VehicleControl RssRestrictor::RestrictVehicleControl( } } - if (restriction.lateralRightRange.maximum <= ::ad::physics::Acceleration(0.0)) { + if (proper_response.accelerationRestrictions.lateralRightRange.maximum <= ::ad::physics::Acceleration(0.0)) { if (ego_dynamics_on_route.route_speed_lat > ::ad::physics::Speed(0.0)) { // driving to the right if (ego_dynamics_on_route.route_speed_lon != ::ad::physics::Speed(0.0)) { @@ -94,15 +98,16 @@ carla::rpc::VehicleControl RssRestrictor::RestrictVehicleControl( } // restrict acceleration - if (restriction.longitudinalRange.maximum > zero_accel) { + if (proper_response.accelerationRestrictions.longitudinalRange.maximum > zero_accel) { // TODO: determine acceleration and limit throttle } // decelerate - if (restriction.longitudinalRange.maximum < zero_accel) { + if (proper_response.accelerationRestrictions.longitudinalRange.maximum < zero_accel) { restricted_vehicle_control.throttle = 0.0f; - double brake_acceleration = std::fabs(static_cast(restriction.longitudinalRange.minimum)); + double brake_acceleration = + std::fabs(static_cast(proper_response.accelerationRestrictions.longitudinalRange.minimum)); double sum_brake_torque = mass * brake_acceleration * radius / 100.0; restricted_vehicle_control.brake = std::min(static_cast(sum_brake_torque / sum_max_brake_torque), 1.0f); } diff --git a/LibCarla/source/carla/rss/RssRestrictor.h b/LibCarla/source/carla/rss/RssRestrictor.h index 8fb70b431..798c03e85 100644 --- a/LibCarla/source/carla/rss/RssRestrictor.h +++ b/LibCarla/source/carla/rss/RssRestrictor.h @@ -10,10 +10,10 @@ namespace ad { namespace rss { -namespace world { +namespace state { -/// @brief forward declararion for the RSS Acceleration restrictions -struct AccelerationRestriction; +/// @brief forward declararion for the RSS proper response +struct ProperResponse; } // namespace world } // namespace rss @@ -46,7 +46,7 @@ public: /// Lateral braking is achieved by counter-steering, so is only a very /// rough solution carla::rpc::VehicleControl RestrictVehicleControl(const carla::rpc::VehicleControl &vehicle_control, - const ::ad::rss::world::AccelerationRestriction &restriction, + const ::ad::rss::state::ProperResponse &proper_response, const carla::rss::EgoDynamicsOnRoute &ego_dynamics_on_route, const carla::rpc::VehiclePhysicsControl &vehicle_physics); diff --git a/LibCarla/source/carla/rss/RssSensor.cpp b/LibCarla/source/carla/rss/RssSensor.cpp index b3bcd8a77..5f6377874 100644 --- a/LibCarla/source/carla/rss/RssSensor.cpp +++ b/LibCarla/source/carla/rss/RssSensor.cpp @@ -7,7 +7,6 @@ #include #include -#include #include #include #include @@ -18,13 +17,28 @@ #include "carla/client/detail/Simulator.h" #include "carla/rss/RssCheck.h" #include "carla/sensor/data/RssResponse.h" +#include "carla/client/Vehicle.h" namespace carla { namespace client { RssSensor::RssSensor(ActorInitializer init) : Sensor(std::move(init)), _on_tick_register_id(0u), _drop_route(false) {} -RssSensor::~RssSensor() = default; +RssSensor::~RssSensor() { + // ensure there is no processing anymore when deleting rss_check object + const std::lock_guard lock(_processing_lock); + _rss_check.reset(); +} + +void RssSensor::RegisterActorConstellationCallback(ActorConstellationCallbackFunctionType callback) { + if (IsListening()) { + log_error(GetDisplayId(), + ": registering of the actor constellation callback has to be done before start listening. Register has " + "no effect."); + return; + } + _rss_actor_constellation_callback = callback; +} void RssSensor::Listen(CallbackFunctionType callback) { if (IsListening()) { @@ -33,21 +47,40 @@ void RssSensor::Listen(CallbackFunctionType callback) { } if (GetParent() == nullptr) { - throw_exception(std::runtime_error(GetDisplayId() + ": not attached to vehicle")); + throw_exception(std::runtime_error(GetDisplayId() + ": not attached to actor")); return; } - _rss_check = std::make_shared<::carla::rss::RssCheck>(); + auto vehicle = boost::dynamic_pointer_cast(GetParent()); + if (vehicle == nullptr) { + throw_exception(std::runtime_error(GetDisplayId() + ": parent is not a vehicle")); + return; + } + + // get maximum steering angle + float max_steer_angle_deg = 0.f; + for (auto const &wheel : vehicle->GetPhysicsControl().GetWheels()) { + max_steer_angle_deg = std::max(max_steer_angle_deg, wheel.max_steer_angle); + } + auto max_steering_angle = max_steer_angle_deg * static_cast(M_PI) / 180.0f; auto map = GetWorld().GetMap(); DEBUG_ASSERT(map != nullptr); std::string const open_drive_content = map->GetOpenDrive(); + _rss_check = nullptr; ::ad::map::access::cleanup(); + ::ad::map::access::initFromOpenDriveContent(open_drive_content, 0.2, ::ad::map::intersection::IntersectionType::TrafficLight, ::ad::map::landmark::TrafficLightType::LEFT_STRAIGHT_RED_YELLOW_GREEN); + if (_rss_actor_constellation_callback == nullptr) { + _rss_check = std::make_shared<::carla::rss::RssCheck>(max_steering_angle); + } else { + _rss_check = std::make_shared<::carla::rss::RssCheck>(max_steering_angle, _rss_actor_constellation_callback, GetParent()); + } + auto self = boost::static_pointer_cast(shared_from_this()); log_debug(GetDisplayId(), ": subscribing to tick event"); @@ -69,94 +102,153 @@ void RssSensor::Stop() { return; } - // ensure there is no processing anymore - const std::lock_guard lock(_processing_lock); - log_debug(GetDisplayId(), ": unsubscribing from tick event"); GetEpisode().Lock()->RemoveOnTickEvent(_on_tick_register_id); _on_tick_register_id = 0u; +} - ::ad::map::access::cleanup(); +void RssSensor::SetLogLevel(const uint8_t &log_level) { + if (!bool(_rss_check)) { + log_error(GetDisplayId(), ": not yet listening. SetLogLevel has no effect."); + return; + } + + if (log_level < spdlog::level::n_levels) { + _rss_check->SetLogLevel(spdlog::level::level_enum(log_level)); + } } const ::ad::rss::world::RssDynamics &RssSensor::GetEgoVehicleDynamics() const { - if (bool(_rss_check)) { - return _rss_check->GetEgoVehicleDynamics(); - } else { - static auto default_vehicle_dynamics = rss::RssCheck::GetDefaultVehicleDynamics(); + static auto default_vehicle_dynamics = rss::RssCheck::GetDefaultVehicleDynamics(); + if (!bool(_rss_check)) { + log_error(GetDisplayId(), ": not yet listening. GetEgoVehicleDynamics has no effect."); return default_vehicle_dynamics; } + + if (bool(_rss_actor_constellation_callback)) { + log_error(GetDisplayId(), ": Actor constellation callback registered. GetEgoVehicleDynamics has no effect."); + return default_vehicle_dynamics; + } + + return _rss_check->GetDefaultActorConstellationCallbackEgoVehicleDynamics(); } void RssSensor::SetEgoVehicleDynamics(const ::ad::rss::world::RssDynamics &ego_dynamics) { - if (bool(_rss_check)) { - _rss_check->SetEgoVehicleDynamics(ego_dynamics); + if (!bool(_rss_check)) { + log_error(GetDisplayId(), ": not yet listening. SetEgoVehicleDynamics has no effect."); + return; } + + if (bool(_rss_actor_constellation_callback)) { + log_error(GetDisplayId(), ": Actor constellation callback registered. SetEgoVehicleDynamics has no effect."); + return; + } + + _rss_check->SetDefaultActorConstellationCallbackEgoVehicleDynamics(ego_dynamics); } const ::ad::rss::world::RssDynamics &RssSensor::GetOtherVehicleDynamics() const { - if (bool(_rss_check)) { - return _rss_check->GetOtherVehicleDynamics(); - } else { - static auto default_vehicle_dynamics = rss::RssCheck::GetDefaultVehicleDynamics(); + static auto default_vehicle_dynamics = rss::RssCheck::GetDefaultVehicleDynamics(); + if (!bool(_rss_check)) { + log_error(GetDisplayId(), ": not yet listening. GetOtherVehicleDynamics has no effect."); return default_vehicle_dynamics; } + + if (bool(_rss_actor_constellation_callback)) { + log_error(GetDisplayId(), ": Actor constellation callback registered. GetOtherVehicleDynamics has no effect."); + return default_vehicle_dynamics; + } + + return _rss_check->GetDefaultActorConstellationCallbackOtherVehicleDynamics(); } void RssSensor::SetOtherVehicleDynamics(const ::ad::rss::world::RssDynamics &other_vehicle_dynamics) { - if (bool(_rss_check)) { - _rss_check->SetOtherVehicleDynamics(other_vehicle_dynamics); + if (!bool(_rss_check)) { + log_error(GetDisplayId(), ": not yet listening. SetOtherVehicleDynamics has no effect."); + return; } + + if (bool(_rss_actor_constellation_callback)) { + log_error(GetDisplayId(), ": Actor constellation callback registered. SetOtherVehicleDynamics has no effect."); + return; + } + + _rss_check->SetDefaultActorConstellationCallbackOtherVehicleDynamics(other_vehicle_dynamics); +} + +const ::ad::rss::world::RssDynamics &RssSensor::GetPedestrianDynamics() const { + static auto default_pedestrian_dynamics = rss::RssCheck::GetDefaultPedestrianDynamics(); + if (!bool(_rss_check)) { + log_error(GetDisplayId(), ": not yet listening. GetPedestrianDynamics has no effect."); + return default_pedestrian_dynamics; + } + + if (bool(_rss_actor_constellation_callback)) { + log_error(GetDisplayId(), ": Actor constellation callback registered. GetPedestrianDynamics has no effect."); + return default_pedestrian_dynamics; + } + + return _rss_check->GetDefaultActorConstellationCallbackPedestrianDynamics(); +} + +void RssSensor::SetPedestrianDynamics(const ::ad::rss::world::RssDynamics &pedestrian_dynamics) { + if (!bool(_rss_check)) { + log_error(GetDisplayId(), ": not yet listening. SetPedestrianDynamics has no effect."); + return; + } + + if (bool(_rss_actor_constellation_callback)) { + log_error(GetDisplayId(), ": Actor constellation callback registered. SetPedestrianDynamics has no effect."); + return; + } + + _rss_check->SetDefaultActorConstellationCallbackPedestrianDynamics(pedestrian_dynamics); } const ::carla::rss::RoadBoundariesMode &RssSensor::GetRoadBoundariesMode() const { - if (bool(_rss_check)) { - return _rss_check->GetRoadBoundariesMode(); - } else { + if (!bool(_rss_check)) { + log_error(GetDisplayId(), ": not yet listening. GetRoadBoundariesMode has no effect."); static auto default_road_boundaries_mode = rss::RssCheck::GetDefaultRoadBoundariesMode(); return default_road_boundaries_mode; } + + return _rss_check->GetRoadBoundariesMode(); } void RssSensor::SetRoadBoundariesMode(const ::carla::rss::RoadBoundariesMode &road_boundaries_mode) { - if (bool(_rss_check)) { - _rss_check->SetRoadBoundariesMode(road_boundaries_mode); + if (!bool(_rss_check)) { + log_error(GetDisplayId(), ": not yet listening. SetRoadBoundariesMode has no effect."); + return; } + + _rss_check->SetRoadBoundariesMode(road_boundaries_mode); } void RssSensor::AppendRoutingTarget(const ::carla::geom::Transform &routing_target) { - if (bool(_rss_check)) { - _rss_check->AppendRoutingTarget(routing_target); + if (!bool(_rss_check)) { + log_error(GetDisplayId(), ": not yet listening. AppendRoutingTarget has no effect."); + return; } + + _rss_check->AppendRoutingTarget(routing_target); } const std::vector<::carla::geom::Transform> RssSensor::GetRoutingTargets() const { - if (bool(_rss_check)) { - return _rss_check->GetRoutingTargets(); - } else { + if (!bool(_rss_check)) { + log_error(GetDisplayId(), ": not yet listening. GetRoutingTargets has no effect."); return std::vector<::carla::geom::Transform>(); } + + return _rss_check->GetRoutingTargets(); } void RssSensor::ResetRoutingTargets() { - if (bool(_rss_check)) { - _rss_check->ResetRoutingTargets(); + if (!bool(_rss_check)) { + log_error(GetDisplayId(), ": not yet listening. ResetRoutingTargets has no effect."); + return; } -} -void RssSensor::SetVisualizationMode(const ::carla::rss::VisualizationMode &visualization_mode) { - if (bool(_rss_check)) { - _rss_check->SetVisualizationMode(visualization_mode); - } -} - -const ::carla::rss::VisualizationMode &RssSensor::GetVisualizationMode() const { - if (bool(_rss_check)) { - return _rss_check->GetVisualizationMode(); - } else { - static auto default_visualization_mode = rss::RssCheck::GetDefaultVisualizationMode(); - return default_visualization_mode; - } + _rss_check->ResetRoutingTargets(); } void RssSensor::DropRoute() { @@ -169,8 +261,9 @@ SharedPtr RssSensor::TickRssSensor(const Timestamp ×tam try { bool result = false; ::ad::rss::state::ProperResponse response; - ::ad::rss::world::AccelerationRestriction acceleration_restriction; ::ad::rss::state::RssStateSnapshot rss_state_snapshot; + ::ad::rss::situation::SituationSnapshot situation_snapshot; + ::ad::rss::world::WorldModel world_model; carla::rss::EgoDynamicsOnRoute ego_dynamics_on_route; if (_processing_lock.try_lock()) { spdlog::debug("RssSensor tick: T={}", timestamp.frame); @@ -183,18 +276,16 @@ SharedPtr RssSensor::TickRssSensor(const Timestamp ×tam } // check all object<->ego pairs with RSS and calculate proper response - result = _rss_check->CheckObjects(timestamp, actors, GetParent(), response, acceleration_restriction, - rss_state_snapshot, ego_dynamics_on_route); + result = _rss_check->CheckObjects(timestamp, actors, GetParent(), response, rss_state_snapshot, + situation_snapshot, world_model, ego_dynamics_on_route); _processing_lock.unlock(); - _rss_check->VisualizeResults(world, GetParent()); - spdlog::debug( "RssSensor response: T={} S:{}->E:{} DeltaT:{}", timestamp.frame, ego_dynamics_on_route.time_since_epoch_check_start_ms, ego_dynamics_on_route.time_since_epoch_check_end_ms, ego_dynamics_on_route.time_since_epoch_check_end_ms - ego_dynamics_on_route.time_since_epoch_check_start_ms); return MakeShared(timestamp.frame, timestamp.elapsed_seconds, GetTransform(), result, - response, acceleration_restriction, rss_state_snapshot, + response, rss_state_snapshot, situation_snapshot, world_model, ego_dynamics_on_route); } else { spdlog::debug("RssSensor tick dropped: T={}", timestamp.frame); diff --git a/LibCarla/source/carla/rss/RssSensor.h b/LibCarla/source/carla/rss/RssSensor.h index fe7eefd40..02b949023 100644 --- a/LibCarla/source/carla/rss/RssSensor.h +++ b/LibCarla/source/carla/rss/RssSensor.h @@ -27,11 +27,12 @@ namespace rss { /// forward declaration of the RoadBoundariesMode enum class RoadBoundariesMode; -/// forward declaration of the VisualizationMode -enum class VisualizationMode; /// forward declaration of the RssCheck class class RssCheck; - +/// forward declaration of the ActorContellationResult struct +struct ActorConstellationResult; +/// forward declaration of the ActorContellationData struct +struct ActorConstellationData; } // namespace rss namespace client { @@ -42,12 +43,19 @@ class RssSensor : public Sensor { public: using Sensor::Sensor; + using ActorConstellationCallbackFunctionType = + std::function<::carla::rss::ActorConstellationResult(carla::SharedPtr<::carla::rss::ActorConstellationData>)>; + /// @brief constructor explicit RssSensor(ActorInitializer init); /// @brief destructor ~RssSensor(); + /// Register a @a callback to be executed for each actor within each measurement to be processed + /// to decide on the operation of the RSS sensor in respect to the ego vehicle to actor constellation + void RegisterActorConstellationCallback(ActorConstellationCallbackFunctionType callback); + /// Register a @a callback to be executed each time a new measurement is /// received. /// @@ -66,6 +74,9 @@ public: return _on_tick_register_id != 0u; } + /// @brief sets the current log level + void SetLogLevel(const uint8_t &log_level); + /// @returns the currently used dynamics of the ego vehicle (@see also /// RssCheck::GetEgoVehicleDynamics()) const ::ad::rss::world::RssDynamics &GetEgoVehicleDynamics() const; @@ -80,6 +91,13 @@ public: /// also RssCheck::SetOtherVehicleDynamics()) void SetOtherVehicleDynamics(const ::ad::rss::world::RssDynamics &other_vehicle_dynamics); + /// @returns the currently used dynamics of pedestrians (@see also + /// RssCheck::GetPedestrianDynamics()) + const ::ad::rss::world::RssDynamics &GetPedestrianDynamics() const; + /// @brief sets the ego vehicle dynamics to be used by pedestrians (@see + /// also RssCheck::SetPedestrianDynamics()) + void SetPedestrianDynamics(const ::ad::rss::world::RssDynamics &pedestrian_dynamics); + /// @returns the current mode for respecting the road boundaries (@see also /// RssCheck::GetRoadBoundariesMode()) const ::carla::rss::RoadBoundariesMode &GetRoadBoundariesMode() const; @@ -97,13 +115,6 @@ public: /// RssCheck::ResetRoutingTargets()) void ResetRoutingTargets(); - /// @brief sets the visualization mode (@see also - /// RssCheck::SetVisualizationMode()) - void SetVisualizationMode(const ::carla::rss::VisualizationMode &visualization_mode); - /// @returns get the current visualization mode (@see also - /// RssCheck::GetVisualizationMode()) - const ::carla::rss::VisualizationMode &GetVisualizationMode() const; - /// @brief drop the current route (@see also RssCheck::DropRoute()) void DropRoute(); @@ -114,13 +125,15 @@ private: /// the id got when registering for the on tick event std::size_t _on_tick_register_id; - /// the mutex to protect the actual RSS processing and decouple the (slow) - /// visualization + /// the mutex to protect the actual RSS processing and in case it takes too long to process ever frame std::mutex _processing_lock; //// the object actually performing the RSS processing std::shared_ptr<::carla::rss::RssCheck> _rss_check; + //// the rss actor constellation callback function + ActorConstellationCallbackFunctionType _rss_actor_constellation_callback; + /// reqired to store DropRoute() requests until next sensor tick bool _drop_route; }; diff --git a/LibCarla/source/carla/sensor/data/RssResponse.h b/LibCarla/source/carla/sensor/data/RssResponse.h index d1c1e6499..3e0ee03c9 100644 --- a/LibCarla/source/carla/sensor/data/RssResponse.h +++ b/LibCarla/source/carla/sensor/data/RssResponse.h @@ -17,14 +17,16 @@ class RssResponse : public SensorData { public: explicit RssResponse(size_t frame_number, double timestamp, const rpc::Transform &sensor_transform, const bool &response_valid, const ::ad::rss::state::ProperResponse &response, - const ::ad::rss::world::AccelerationRestriction &acceleration_restriction, const ::ad::rss::state::RssStateSnapshot &rss_state_snapshot, + const ::ad::rss::situation::SituationSnapshot &situation_snapshot, + const ::ad::rss::world::WorldModel &world_model, const carla::rss::EgoDynamicsOnRoute &ego_dynamics_on_route) : SensorData(frame_number, timestamp, sensor_transform), _response_valid(response_valid), _response(response), - _acceleration_restriction(acceleration_restriction), _rss_state_snapshot(rss_state_snapshot), + _situation_snapshot(situation_snapshot), + _world_model(world_model), _ego_dynamics_on_route(ego_dynamics_on_route) {} bool GetResponseValid() const { @@ -35,14 +37,18 @@ public: return _response; } - const ::ad::rss::world::AccelerationRestriction &GetAccelerationRestriction() const { - return _acceleration_restriction; - } - const ::ad::rss::state::RssStateSnapshot &GetRssStateSnapshot() const { return _rss_state_snapshot; } + const ::ad::rss::situation::SituationSnapshot &GetSituationSnapshot() const { + return _situation_snapshot; + } + + const ::ad::rss::world::WorldModel &GetWorldModel() const { + return _world_model; + } + const carla::rss::EgoDynamicsOnRoute &GetEgoDynamicsOnRoute() const { return _ego_dynamics_on_route; } @@ -55,10 +61,12 @@ private: ::ad::rss::state::ProperResponse _response; - ::ad::rss::world::AccelerationRestriction _acceleration_restriction; - ::ad::rss::state::RssStateSnapshot _rss_state_snapshot; + ::ad::rss::situation::SituationSnapshot _situation_snapshot; + + ::ad::rss::world::WorldModel _world_model; + carla::rss::EgoDynamicsOnRoute _ego_dynamics_on_route; }; diff --git a/PythonAPI/carla/setup.py b/PythonAPI/carla/setup.py index 904315480..d3def6a24 100755 --- a/PythonAPI/carla/setup.py +++ b/PythonAPI/carla/setup.py @@ -74,10 +74,9 @@ def get_libcarla_extensions(): extra_link_args += [os.path.join(pwd, 'dependencies/lib/libad_map_opendrive_reader.a')] extra_link_args += [os.path.join(pwd, 'dependencies/lib/libboost_program_options.a')] extra_link_args += [os.path.join(pwd, 'dependencies/lib/libspdlog.a')] - extra_link_args += [os.path.join(pwd, 'dependencies/lib/libboost_system.so')] - extra_link_args += ['-ltbb'] + extra_link_args += [os.path.join(pwd, 'dependencies/lib/libproj.a')] extra_link_args += ['-lrt'] - extra_link_args += ['-lproj'] + extra_link_args += ['-ltbb'] extra_link_args += [os.path.join(pwd, 'dependencies/lib', pylib)] diff --git a/PythonAPI/carla/source/libcarla/AdRss.cpp b/PythonAPI/carla/source/libcarla/AdRss.cpp index 4615b80a7..4c3686816 100644 --- a/PythonAPI/carla/source/libcarla/AdRss.cpp +++ b/PythonAPI/carla/source/libcarla/AdRss.cpp @@ -12,19 +12,19 @@ #ifdef LIBCARLA_PYTHON_MAJOR_2 extern "C" { -void initlibad_physics_python(); -void initlibad_rss_python(); -void initlibad_map_access_python(); -void initlibad_rss_map_integration_python(); +void initlibad_physics_python2(); +void initlibad_rss_python2(); +void initlibad_map_access_python2(); +void initlibad_rss_map_integration_python2(); } #endif #ifdef LIBCARLA_PYTHON_MAJOR_3 extern "C" { -void PyInit_libad_physics_python(); -void PyInit_libad_rss_python(); -void PyInit_libad_map_access_python(); -void PyInit_libad_rss_map_integration_python(); +void PyInit_libad_physics_python3(); +void PyInit_libad_rss_python3(); +void PyInit_libad_map_access_python3(); +void PyInit_libad_rss_map_integration_python3(); } #endif @@ -42,8 +42,15 @@ namespace sensor { namespace data { std::ostream &operator<<(std::ostream &out, const RssResponse &resp) { - out << "RssResponse(frame=" << resp.GetFrame() << ", timestamp=" << resp.GetTimestamp() - << ", valid=" << resp.GetResponseValid() << ')'; + out << "RssResponse(frame=" << resp.GetFrame() + << ", timestamp=" << resp.GetTimestamp() + << ", valid=" << resp.GetResponseValid() + << ", proper_response=" << resp.GetProperResponse() + << ", rss_state_snapshot=" << resp.GetRssStateSnapshot() + << ", situation_snapshot=" << resp.GetSituationSnapshot() + << ", world_model=" << resp.GetWorldModel() + << ", ego_dynamics_on_route=" << resp.GetEgoDynamicsOnRoute() + << ')'; return out; } @@ -61,6 +68,11 @@ static auto GetOtherVehicleDynamics(const carla::client::RssSensor &self) { return other_dynamics; } +static auto GetPedestrianDynamics(const carla::client::RssSensor &self) { + ad::rss::world::RssDynamics pedestrian_dynamics(self.GetPedestrianDynamics()); + return pedestrian_dynamics; +} + static auto GetRoadBoundariesMode(const carla::client::RssSensor &self) { carla::rss::RoadBoundariesMode road_boundaries_mode(self.GetRoadBoundariesMode()); return road_boundaries_mode; @@ -71,24 +83,48 @@ static auto GetRoutingTargets(const carla::client::RssSensor &self) { return routing_targets; } -static auto GetVisualizationMode(const carla::client::RssSensor &self) { - carla::rss::VisualizationMode visualization_mode(self.GetVisualizationMode()); - return visualization_mode; +static void RegisterActorConstellationCallback(carla::client::RssSensor &self, boost::python::object callback) { + namespace py = boost::python; + // Make sure the callback is actually callable. + if (!PyCallable_Check(callback.ptr())) { + PyErr_SetString(PyExc_TypeError, "callback argument must be callable!"); + py::throw_error_already_set(); + } + + // We need to delete the callback while holding the GIL. + using Deleter = carla::PythonUtil::AcquireGILDeleter; + auto callback_ptr = carla::SharedPtr{new py::object(callback), Deleter()}; + + // Make a lambda callback. + auto callback_function = [callback = std::move(callback_ptr)]( + carla::SharedPtr<::carla::rss::ActorConstellationData> actor_constellation_data) + ->::carla::rss::ActorConstellationResult { + carla::PythonUtil::AcquireGIL lock; + ::carla::rss::ActorConstellationResult actor_constellation_result; + try { + actor_constellation_result = + py::call<::carla::rss::ActorConstellationResult>(callback->ptr(), py::object(actor_constellation_data)); + } catch (const py::error_already_set &) { + PyErr_Print(); + } + return actor_constellation_result; + }; + self.RegisterActorConstellationCallback(callback_function); } void export_ad_rss() { #ifdef LIBCARLA_PYTHON_MAJOR_2 - initlibad_physics_python(); - initlibad_rss_python(); - initlibad_map_access_python(); - initlibad_rss_map_integration_python(); + initlibad_physics_python2(); + initlibad_rss_python2(); + initlibad_map_access_python2(); + initlibad_rss_map_integration_python2(); #endif #ifdef LIBCARLA_PYTHON_MAJOR_3 - PyInit_libad_physics_python(); - PyInit_libad_rss_python(); - PyInit_libad_map_access_python(); - PyInit_libad_rss_map_integration_python(); + PyInit_libad_physics_python3(); + PyInit_libad_rss_python3(); + PyInit_libad_map_access_python3(); + PyInit_libad_rss_map_integration_python3(); #endif using namespace boost::python; @@ -118,35 +154,57 @@ void export_ad_rss() { .def_readwrite("avg_route_accel_lon", &carla::rss::EgoDynamicsOnRoute::avg_route_accel_lon) .def(self_ns::str(self_ns::self)); + class_("RssActorConstellationResult") + .def_readwrite("rss_calculation_mode", &carla::rss::ActorConstellationResult::rss_calculation_mode) + .def_readwrite("restrict_speed_limit_mode", &carla::rss::ActorConstellationResult::restrict_speed_limit_mode) + .def_readwrite("ego_vehicle_dynamics", &carla::rss::ActorConstellationResult::ego_vehicle_dynamics) + .def_readwrite("actor_object_type", &carla::rss::ActorConstellationResult::actor_object_type) + .def_readwrite("actor_dynamics", &carla::rss::ActorConstellationResult::actor_dynamics) + .def(self_ns::str(self_ns::self)); + + class_>( + "RssActorConstellationData", no_init) + .def_readonly("ego_match_object", &carla::rss::ActorConstellationData::ego_match_object) + .def_readonly("ego_route", &carla::rss::ActorConstellationData::ego_route) + .def_readonly("ego_dynamics_on_route", &carla::rss::ActorConstellationData::ego_dynamics_on_route) + .def_readonly("other_match_object", &carla::rss::ActorConstellationData::other_match_object) + .def_readonly("other_actor", &carla::rss::ActorConstellationData::other_actor) + .def(self_ns::str(self_ns::self)); + + enum_("RssLogLevel") + .value("trace", spdlog::level::trace) + .value("debug", spdlog::level::debug) + .value("info", spdlog::level::info) + .value("warn", spdlog::level::warn) + .value("err", spdlog::level::err) + .value("critical", spdlog::level::critical) + .value("off", spdlog::level::off); + enum_("RssRoadBoundariesMode") .value("Off", carla::rss::RoadBoundariesMode::Off) .value("On", carla::rss::RoadBoundariesMode::On); - enum_("RssVisualizationMode") - .value("Off", carla::rss::VisualizationMode::Off) - .value("RouteOnly", carla::rss::VisualizationMode::RouteOnly) - .value("VehicleStateOnly", carla::rss::VisualizationMode::VehicleStateOnly) - .value("VehicleStateAndRoute", carla::rss::VisualizationMode::VehicleStateAndRoute) - .value("All", carla::rss::VisualizationMode::All); - class_, boost::noncopyable, boost::shared_ptr>( "RssResponse", no_init) .add_property("response_valid", &csd::RssResponse::GetResponseValid) .add_property("proper_response", CALL_RETURNING_COPY(csd::RssResponse, GetProperResponse)) - .add_property("acceleration_restriction", CALL_RETURNING_COPY(csd::RssResponse, GetAccelerationRestriction)) .add_property("rss_state_snapshot", CALL_RETURNING_COPY(csd::RssResponse, GetRssStateSnapshot)) + .add_property("situation_snapshot", CALL_RETURNING_COPY(csd::RssResponse, GetSituationSnapshot)) + .add_property("world_model", CALL_RETURNING_COPY(csd::RssResponse, GetWorldModel)) .add_property("ego_dynamics_on_route", CALL_RETURNING_COPY(csd::RssResponse, GetEgoDynamicsOnRoute)) .def(self_ns::str(self_ns::self)); class_, boost::noncopyable, boost::shared_ptr>("RssSensor", no_init) .add_property("ego_vehicle_dynamics", &GetEgoVehicleDynamics, &cc::RssSensor::SetEgoVehicleDynamics) .add_property("other_vehicle_dynamics", &GetOtherVehicleDynamics, &cc::RssSensor::SetOtherVehicleDynamics) + .add_property("pedestrian_dynamics", &GetPedestrianDynamics, &cc::RssSensor::SetPedestrianDynamics) .add_property("road_boundaries_mode", &GetRoadBoundariesMode, &cc::RssSensor::SetRoadBoundariesMode) - .add_property("visualization_mode", &GetVisualizationMode, &cc::RssSensor::SetVisualizationMode) .add_property("routing_targets", &GetRoutingTargets) + .def("register_actor_constellation_callback", &RegisterActorConstellationCallback, (arg("callback"))) .def("append_routing_target", &cc::RssSensor::AppendRoutingTarget, (arg("routing_target"))) .def("reset_routing_targets", &cc::RssSensor::ResetRoutingTargets) .def("drop_route", &cc::RssSensor::DropRoute) + .def("set_log_level", &cc::RssSensor::SetLogLevel, (arg("log_level"))) .def(self_ns::str(self_ns::self)); class_>("RssRestrictor", diff --git a/PythonAPI/docs/sensor.yml b/PythonAPI/docs/sensor.yml index d7f0027f2..370c67ca7 100644 --- a/PythonAPI/docs/sensor.yml +++ b/PythonAPI/docs/sensor.yml @@ -8,20 +8,20 @@ doc: > Sensors compound a specific family of actors quite diverse and unique. They are normally spawned as attachment/sons of a vehicle (take a look at carla.World to learn about actor spawning). Sensors are thoroughly designed to retrieve different types of data that they are listening to. The data they receive is shaped as different subclasses inherited from carla.SensorData (depending on the sensor). - Most sensors can be divided in two groups: those receiving data on every tick (cameras, point clouds and some specific sensors) and those who only receive under certain circumstances (trigger detectors). CARLA provides a specific set of sensors and their blueprint can be found in carla.BlueprintLibrary. All the information on their preferences and settlement can be found [here](ref_sensors.md), but the list of those available in CARLA so far goes as follow. - Receive data on every tick. - - [Depth camera](ref_sensors.md#depth-camera). - - [Gnss sensor](ref_sensors.md#gnss-sensor). - - [IMU sensor](ref_sensors.md#imu-sensor). - - [Lidar raycast](ref_sensors.md#lidar-raycast-sensor). - - [Radar](ref_sensors.md#radar-sensor). - - [RGB camera](ref_sensors.md#rgb-camera). - - [RSS sensor](ref_sensors.md#rss-sensor). - - [Semantic Segmentation camera](ref_sensors.md#semantic-segmentation-camera). - Only receive data when triggered. - - [Collision detector](ref_sensors.md#collision-detector). - - [Lane invasion detector](ref_sensors.md#lane-invasion-detector). - - [Obstacle detector](ref_sensors.md#obstacle-detector). + Most sensors can be divided in two groups: those receiving data on every tick (cameras, point clouds and some specific sensors) and those who only receive under certain circumstances (trigger detectors). CARLA provides a specific set of sensors and their blueprint can be found in carla.BlueprintLibrary. All the information on their preferences and settlement can be found [here](ref_sensors.md), but the list of those available in CARLA so far goes as follow. + Receive data on every tick. + - [Depth camera](ref_sensors.md#depth-camera). + - [Gnss sensor](ref_sensors.md#gnss-sensor). + - [IMU sensor](ref_sensors.md#imu-sensor). + - [Lidar raycast](ref_sensors.md#lidar-raycast-sensor). + - [Radar](ref_sensors.md#radar-sensor). + - [RGB camera](ref_sensors.md#rgb-camera). + - [RSS sensor](ref_sensors.md#rss-sensor). + - [Semantic Segmentation camera](ref_sensors.md#semantic-segmentation-camera). + Only receive data when triggered. + - [Collision detector](ref_sensors.md#collision-detector). + - [Lane invasion detector](ref_sensors.md#lane-invasion-detector). + - [Obstacle detector](ref_sensors.md#obstacle-detector). # - PROPERTIES ------------------------- instance_variables: @@ -51,28 +51,28 @@ parent: carla.Sensor # - DESCRIPTION ------------------------ doc: > - This sensor works a bit differently than the rest. Take look at the [specific documentation](adv_rss.md), and the [rss sensor reference](ref_sensors.md#rss-sensor) to gain full understanding of it. + This sensor works a bit differently than the rest. Take look at the [specific documentation](adv_rss.md), and the [rss sensor reference](ref_sensors.md#rss-sensor) to gain full understanding of it. - The RSS sensor uses world information, and a [RSS library](https://github.com/intel/ad-rss-lib) to make safety checks on a vehicle. The output retrieved by the sensor is a carla.RssResponse. This will be used by a carla.RssRestrictor to modify a carla.VehicleControl before applying it to a vehicle. + The RSS sensor uses world information, and a [RSS library](https://github.com/intel/ad-rss-lib) to make safety checks on a vehicle. The output retrieved by the sensor is a carla.RssResponse. This will be used by a carla.RssRestrictor to modify a carla.VehicleControl before applying it to a vehicle. # - PROPERTIES ------------------------- instance_variables: - var_name: ego_vehicle_dynamics type: libad_rss_python.RssDynamics doc: > - States the [RSS parameters](https://intel.github.io/ad-rss-lib/ad_rss/Appendix-ParameterDiscussion/) that the sensor will consider for the ego vehicle. + States the [RSS parameters](https://intel.github.io/ad-rss-lib/ad_rss/Appendix-ParameterDiscussion/) that the sensor will consider for the ego vehicle if no actor constellation callback is registered. - var_name: other_vehicle_dynamics type: libad_rss_python.RssDynamics doc: > - States the [RSS parameters](https://intel.github.io/ad-rss-lib/ad_rss/Appendix-ParameterDiscussion/) that the sensor will consider for the rest of vehicles. + States the [RSS parameters](https://intel.github.io/ad-rss-lib/ad_rss/Appendix-ParameterDiscussion/) that the sensor will consider for the rest of vehicles if no actor constellation callback is registered. + - var_name: pedestrian_dynamics + type: libad_rss_python.RssDynamics + doc: > + States the [RSS parameters](https://intel.github.io/ad-rss-lib/ad_rss/Appendix-ParameterDiscussion/) that the sensor will consider for pedestrians if no actor constellation callback is registered. - var_name: road_boundaries_mode type: carla.RssRoadBoundariesMode doc: > Switches the [stay on road](https://intel.github.io/ad-rss-lib/ad_rss_map_integration/HandleRoadBoundaries/) feature. By default is __On__. - - var_name: visualization_mode - type: carla.RssVisualizationMode - doc: > - Sets the visualization of the RSS on the server side. By default is __All__. These drawings may delay de RSS so it is best to set this to __Off__ when evaluating RSS performance. - var_name: routing_targets type: vector doc: > @@ -86,22 +86,37 @@ doc: > New target point for the route. Choose these after the intersections to force the route to take the desired turn. doc: > - Appends a new target position to the current route of the vehicle. + Appends a new target position to the current route of the vehicle. - def_name: reset_routing_targets doc: > - Erases the targets that have been appended to the route. + Erases the targets that have been appended to the route. - def_name: drop_route doc: > Discards the current route. If there are targets remaining in **routing_targets**, creates a new route using those. Otherwise, a new route is created at random. + - def_name: register_actor_constellation_callback + params: + - param_name: callback + doc: > + The function to be called whenever a RSS situation is about to be calculated. + doc: > + Register a callback to customize a carla.RssActorConstellationResult. By this callback the settings of RSS parameters are done per actor constellation and the settings (ego_vehicle_dynamics, other_vehicle_dynamics and pedestrian_dynamics) have no effect. + - def_name: set_log_level + params: + - param_name: log_level + type: carla.RssLogLevel + doc: > + New log level. + doc: > + Sets the log level. # -------------------------------------- - def_name: __str__ # -------------------------------------- - class_name: RssRestrictor - parent: + parent: # - DESCRIPTION ------------------------ doc: > - These objects apply restrictions to a carla.VehicleControl. It is part of the CARLA implementation of the [C++ Library for Responsibility Sensitive Safety](https://github.com/intel/ad-rss-lib). This class works hand in hand with a [rss sensor](ref_sensors.md#rss-sensor), which provides the data of the restrictions to be applied. + These objects apply restrictions to a carla.VehicleControl. It is part of the CARLA implementation of the [C++ Library for Responsibility Sensitive Safety](https://github.com/intel/ad-rss-lib). This class works hand in hand with a [rss sensor](ref_sensors.md#rss-sensor), which provides the data of the restrictions to be applied. # - PROPERTIES ------------------------- instance_variables: @@ -124,7 +139,7 @@ - param_name: vehicle_physics type: carla.RssEgoDynamicsOnRoute doc: > - The current physics of the vehicle. Used to apply the restrictions properly. + The current physics of the vehicle. Used to apply the restrictions properly. return: carla.VehicleControl doc: > @@ -134,7 +149,7 @@ - class_name: RssRoadBoundariesMode # - DESCRIPTION ------------------------ doc: > - Enum declaration used in carla.RssSensor to enable or disable the [stay on road](https://intel.github.io/ad-rss-lib/ad_rss_map_integration/HandleRoadBoundaries/) feature. In summary, this feature considers the road boundaries as virtual objects. The minimum safety distance check is applied to these virtual walls, in order to make sure the vehicle does not drive off the road. + Enum declaration used in carla.RssSensor to enable or disable the [stay on road](https://intel.github.io/ad-rss-lib/ad_rss_map_integration/HandleRoadBoundaries/) feature. In summary, this feature considers the road boundaries as virtual objects. The minimum safety distance check is applied to these virtual walls, in order to make sure the vehicle does not drive off the road. # - PROPERTIES ------------------------- instance_variables: - var_name: 'On' @@ -146,20 +161,24 @@ Disables the _stay on road_ feature. # -------------------------------------- - - class_name: RssVisualizationMode + - class_name: RssLogLevel # - DESCRIPTION ------------------------ doc: > - Enum declaration used to state the visualization RSS calculations server side. Depending on these, the carla.RssSensor will use a carla.DebugHelper to draw different elements. These drawings take some time and might delay the RSS responses. It is best to disable them when evaluating RSS performance. + Enum declaration used in carla.RssSensor to set the log level. # - PROPERTIES ------------------------- instance_variables: - - var_name: 'Off' + - var_name: 'trace' # -------------------------------------- - - var_name: RouteOnly + - var_name: 'debug' # -------------------------------------- - - var_name: VehicleStateOnly + - var_name: 'info' # -------------------------------------- - - var_name: VehicleStateAndRoute + - var_name: 'warn' # -------------------------------------- - - var_name: All + - var_name: 'err' + # -------------------------------------- + - var_name: 'critical' + # -------------------------------------- + - var_name: 'off' # -------------------------------------- ... diff --git a/PythonAPI/docs/sensor_data.yml b/PythonAPI/docs/sensor_data.yml index 3f43e8c84..ac1e6bf1f 100644 --- a/PythonAPI/docs/sensor_data.yml +++ b/PythonAPI/docs/sensor_data.yml @@ -399,6 +399,16 @@ type: carla.RssEgoDynamicsOnRoute doc: > Current ego vehicle dynamics regarding the route. + # -------------------------------------- + - var_name: world_model + type: libad_rss_python.WorldModel + doc: > + World model used for calculations. + # -------------------------------------- + - var_name: situation_snapshot + type: libad_rss_python.SituationSnapshot + doc: > + Detailed RSS situations extracted from the world model. # - METHODS ---------------------------- methods: - def_name: __str__ @@ -489,6 +499,76 @@ - def_name: __str__ # -------------------------------------- + - class_name: RssActorConstellationData + # - DESCRIPTION ------------------------ + doc: > + Data structure that is provided within the callback registered by RssSensor.register_actor_constellation_callback(). + # - PROPERTIES ------------------------- + instance_variables: + - var_name: ego_match_object + type: libad_map_access_python.Object + doc: > + The ego map matched information. + # -------------------------------------- + - var_name: ego_route + type: libad_map_access_python.FullRoute + doc: > + The ego route. + # -------------------------------------- + - var_name: ego_dynamics_on_route + type: carla.RssEgoDynamicsOnRoute + doc: > + Current ego vehicle dynamics regarding the route. + # -------------------------------------- + - var_name: other_match_object + type: libad_map_access_python.Object + doc: > + The other object's map matched information. + # -------------------------------------- + - var_name: other_actor + type: carla.Actor + doc: > + The other actor. + # - METHODS ---------------------------- + methods: + - def_name: __str__ + # -------------------------------------- + + - class_name: RssActorConstellationResult + # - DESCRIPTION ------------------------ + doc: > + Data structure that should be returned by the callback registered by RssSensor.register_actor_constellation_callback(). + # - PROPERTIES ------------------------- + instance_variables: + - var_name: rss_calculation_mode + type: libad_rss_map_integration_python.RssMode_TODO + doc: > + The calculation mode to be applied with the actor. + # -------------------------------------- + - var_name: restrict_speed_limit_mode + type: libad_rss_map_integration_python.RestrictSpeedLimitMode_TODO + doc: > + The mode for restricting speed limit. + # -------------------------------------- + - var_name: ego_vehicle_dynamics + type: libad_rss_python.RssDynamics + doc: > + The RSS dynamics to be applied for the ego vehicle. + # -------------------------------------- + - var_name: actor_object_type + type: libad_rss_python.ObjectType + doc: > + The RSS object type to be used for the actor. + # -------------------------------------- + - var_name: actor_dynamics + type: libad_rss_python.RssDynamics + doc: > + The RSS dynamics to be applied for the actor. + # - METHODS ---------------------------- + methods: + - def_name: __str__ + # -------------------------------------- + - class_name: DVSEvent # - DESCRIPTION ------------------------ doc: > diff --git a/PythonAPI/examples/manual_control_rss.py b/PythonAPI/examples/manual_control_rss.py deleted file mode 100755 index ce0f0c68c..000000000 --- a/PythonAPI/examples/manual_control_rss.py +++ /dev/null @@ -1,1096 +0,0 @@ -#!/usr/bin/env python - -# Copyright (c) 2019 Computer Vision Center (CVC) at the Universitat Autonoma de -# Barcelona (UAB). -# Copyright (c) 2019 Intel Corporation -# -# This work is licensed under the terms of the MIT license. -# For a copy, see . - -# Allows controlling a vehicle with a keyboard. For a simpler and more -# documented example, please take a look at tutorial.py. - -""" -Welcome to CARLA manual control. - -Use ARROWS or WASD keys for control. - - W : throttle - S : brake - AD : steer - Q : toggle reverse - Space : hand-brake - P : toggle autopilot - M : toggle manual transmission - ,/. : gear up/down - - TAB : change sensor position - ` : next sensor - [1-9] : change to sensor [1-9] - C : change weather (Shift+C reverse) - Backspace : change vehicle - - R : toggle recording images to disk - - F2 : toggle RSS visualization mode - B : toggle RSS Road Boundaries Mode - G : RSS check drop current route - T : toggle RSS parameters - - CTRL + R : toggle recording of simulation (replacing any previous) - CTRL + P : start replaying last recorded simulation - CTRL + + : increments the start time of the replay by 1 second (+SHIFT = 10 seconds) - CTRL + - : decrements the start time of the replay by 1 second (+SHIFT = 10 seconds) - - F1 : toggle HUD - H/? : toggle help - ESC : quit -""" - -from __future__ import print_function - - -# ============================================================================== -# -- find carla module --------------------------------------------------------- -# ============================================================================== - - -import glob -import os -import sys - -try: - sys.path.append(glob.glob('../carla/dist/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 - -from carla import ColorConverter as cc - -import argparse -import collections -import datetime -import inspect -import logging -import math -import random -import re -import weakref - -try: - import pygame - from pygame.locals import KMOD_CTRL - from pygame.locals import KMOD_SHIFT - from pygame.locals import K_0 - from pygame.locals import K_9 - from pygame.locals import K_BACKQUOTE - from pygame.locals import K_BACKSPACE - from pygame.locals import K_COMMA - from pygame.locals import K_DOWN - from pygame.locals import K_ESCAPE - from pygame.locals import K_F1 - from pygame.locals import K_F2 - from pygame.locals import K_LEFT - from pygame.locals import K_PERIOD - from pygame.locals import K_RIGHT - from pygame.locals import K_SLASH - from pygame.locals import K_SPACE - from pygame.locals import K_TAB - from pygame.locals import K_UP - from pygame.locals import K_a - from pygame.locals import K_b - from pygame.locals import K_c - from pygame.locals import K_d - from pygame.locals import K_g - from pygame.locals import K_h - from pygame.locals import K_m - from pygame.locals import K_p - from pygame.locals import K_q - from pygame.locals import K_r - from pygame.locals import K_s - from pygame.locals import K_t - from pygame.locals import K_w - from pygame.locals import K_MINUS - from pygame.locals import K_EQUALS -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') - - -# ============================================================================== -# -- Global functions ---------------------------------------------------------- -# ============================================================================== - - -def find_weather_presets(): - rgx = re.compile('.+?(?:(?<=[a-z])(?=[A-Z])|(?<=[A-Z])(?=[A-Z][a-z])|$)') - name = lambda x: ' '.join(m.group(0) for m in rgx.finditer(x)) - presets = [x for x in dir(carla.WeatherParameters) if re.match('[A-Z].+', x)] - 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 - - -# ============================================================================== -# -- World --------------------------------------------------------------------- -# ============================================================================== - - -class World(object): - - def __init__(self, carla_world, hud, actor_filter, actor_role_name='hero', external_actor=False): - self.world = carla_world - self.actor_role_name = actor_role_name - self.external_actor = external_actor - self.map = self.world.get_map() - self.hud = hud - self.player = None - self.collision_sensor = None - self.lane_invasion_sensor = None - self.gnss_sensor = None - self.rss_sensor = None - self.camera_manager = None - self._weather_presets = find_weather_presets() - self._weather_index = 0 - self._actor_filter = actor_filter - self.restart() - self.world.on_tick(hud.on_world_tick) - self.recording_enabled = False - self.recording_start = 0 - - def restart(self): - # Keep same camera config if the camera manager exists. - cam_index = self.camera_manager.index if self.camera_manager is not None else 0 - cam_pos_index = self.camera_manager.transform_index if self.camera_manager is not None else 0 - - if self.external_actor: - # Check whether there is already an actor with defined role name - for actor in self.world.get_actors(): - if actor.attributes.get('role_name') == self.actor_role_name: - self.player = actor - break - else: - # Get a random blueprint. - blueprint = random.choice(self.world.get_blueprint_library().filter(self._actor_filter)) - blueprint.set_attribute('role_name', self.actor_role_name) - if blueprint.has_attribute('color'): - color = random.choice(blueprint.get_attribute('color').recommended_values) - blueprint.set_attribute('color', color) - if blueprint.has_attribute('driver_id'): - driver_id = random.choice(blueprint.get_attribute('driver_id').recommended_values) - blueprint.set_attribute('driver_id', driver_id) - if blueprint.has_attribute('is_invincible'): - blueprint.set_attribute('is_invincible', 'true') - # Spawn the player. - if self.player is not None: - spawn_point = self.player.get_transform() - spawn_point.location.z += 2.0 - spawn_point.rotation.roll = 0.0 - spawn_point.rotation.pitch = 0.0 - self.destroy() - self.player = self.world.try_spawn_actor(blueprint, spawn_point) - while self.player is None: - spawn_points = self.map.get_spawn_points() - spawn_point = random.choice(spawn_points) if spawn_points else carla.Transform() - self.player = self.world.try_spawn_actor(blueprint, spawn_point) - - if self.external_actor: - ego_sensors = [] - for actor in self.world.get_actors(): - if actor.parent == self.player: - ego_sensors.append(actor) - - for ego_sensor in ego_sensors: - if ego_sensor is not None: - ego_sensor.destroy() - - # Set up the sensors. - self.collision_sensor = CollisionSensor(self.player, self.hud) - self.lane_invasion_sensor = LaneInvasionSensor(self.player, self.hud) - self.gnss_sensor = GnssSensor(self.player) - self.rss_sensor = RssSensor(self.player) - self.camera_manager = CameraManager(self.player, self.hud) - self.camera_manager.transform_index = cam_pos_index - self.camera_manager.set_sensor(cam_index, notify=False) - actor_type = get_actor_display_name(self.player) - self.hud.notification(actor_type) - - def next_weather(self, reverse=False): - self._weather_index += -1 if reverse else 1 - self._weather_index %= len(self._weather_presets) - preset = self._weather_presets[self._weather_index] - self.hud.notification('Weather: %s' % preset[1]) - self.player.get_world().set_weather(preset[0]) - - def tick(self, clock): - self.hud.tick(self, clock) - - def render(self, display): - self.camera_manager.render(display) - self.hud.render(display) - - def destroy_sensors(self): - self.camera_manager.sensor.destroy() - self.camera_manager.sensor = None - self.camera_manager.index = None - - def destroy(self): - actors = [ - self.camera_manager.sensor, - self.collision_sensor.sensor, - self.lane_invasion_sensor.sensor, - self.gnss_sensor.sensor] - if self.rss_sensor: - actors.append(self.rss_sensor.sensor) - actors.append(self.player) - for actor in actors: - if actor is not None: - actor.destroy() - -# ============================================================================== -# -- KeyboardControl ----------------------------------------------------------- -# ============================================================================== - - -class KeyboardControl(object): - - def __init__(self, world, args): - self._autopilot_enabled = args.autopilot - self._world = world - self._restrictor = None - if isinstance(world.player, carla.Vehicle): - self._control = carla.VehicleControl() - world.player.set_autopilot(self._autopilot_enabled) - self._restrictor = carla.RssRestrictor() - elif isinstance(world.player, carla.Walker): - self._control = carla.WalkerControl() - self._autopilot_enabled = False - self._rotation = world.player.get_transform().rotation - else: - raise NotImplementedError("Actor type not supported") - self._steer_cache = 0.0 - world.hud.notification("Press 'H' or '?' for help.", seconds=4.0) - - def parse_events(self, client, world, clock): - for event in pygame.event.get(): - if event.type == pygame.QUIT: - return True - elif event.type == pygame.KEYUP: - if self._is_quit_shortcut(event.key): - 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: - world.camera_manager.toggle_camera() - elif event.key == K_c and pygame.key.get_mods() & KMOD_SHIFT: - world.next_weather(reverse=True) - elif event.key == K_c: - world.next_weather() - elif event.key == K_BACKQUOTE: - world.camera_manager.next_sensor() - elif event.key > K_0 and event.key <= K_9: - world.camera_manager.set_sensor(event.key - 1 - K_0) - elif event.key == K_r and not (pygame.key.get_mods() & KMOD_CTRL): - world.camera_manager.toggle_recording() - elif event.key == K_r and (pygame.key.get_mods() & KMOD_CTRL): - if (world.recording_enabled): - client.stop_recorder() - world.recording_enabled = False - world.hud.notification("Recorder is OFF") - else: - client.start_recorder("manual_recording.rec") - world.recording_enabled = True - world.hud.notification("Recorder is ON") - elif event.key == K_p and (pygame.key.get_mods() & KMOD_CTRL): - # stop recorder - client.stop_recorder() - world.recording_enabled = False - # work around to fix camera at start of replaying - currentIndex = world.camera_manager.index - world.destroy_sensors() - # disable autopilot - self._autopilot_enabled = False - world.player.set_autopilot(self._autopilot_enabled) - world.hud.notification("Replaying file 'manual_recording.rec'") - # replayer - client.replay_file("manual_recording.rec", world.recording_start, 0, 0) - world.camera_manager.set_sensor(currentIndex) - elif event.key == K_MINUS and (pygame.key.get_mods() & KMOD_CTRL): - if pygame.key.get_mods() & KMOD_SHIFT: - world.recording_start -= 10 - else: - world.recording_start -= 1 - world.hud.notification("Recording start time is %d" % (world.recording_start)) - elif event.key == K_EQUALS and (pygame.key.get_mods() & KMOD_CTRL): - if pygame.key.get_mods() & KMOD_SHIFT: - world.recording_start += 10 - else: - world.recording_start += 1 - world.hud.notification("Recording start time is %d" % (world.recording_start)) - elif event.key == K_F2: - if self._world and self._world.rss_sensor: - visualization_mode = self._world.rss_sensor.sensor.visualization_mode - visualization_mode = visualization_mode + 1 - if visualization_mode > carla.RssVisualizationMode.All: - visualization_mode = carla.RssVisualizationMode.Off - self._world.rss_sensor.sensor.visualization_mode = carla.RssVisualizationMode(visualization_mode) - elif event.key == K_b: - if self._world and self._world.rss_sensor: - if self._world.rss_sensor.sensor.road_boundaries_mode == carla.RssRoadBoundariesMode.Off: - self._world.rss_sensor.sensor.road_boundaries_mode = carla.RssRoadBoundariesMode.On - print("carla.RssRoadBoundariesMode.On") - else: - self._world.rss_sensor.sensor.road_boundaries_mode = carla.RssRoadBoundariesMode.Off - print("carla.RssRoadBoundariesMode.Off") - elif event.key == K_g: - if self._world and self._world.rss_sensor: - self._world.rss_sensor.drop_route() - elif event.key == K_t: - if self._world and self._world.rss_sensor: - if self._world.rss_sensor.assertive_parameters: - self._world.rss_sensor.set_default_parameters() - else: - self._world.rss_sensor.set_assertive_parameters() - if isinstance(self._control, carla.VehicleControl): - if event.key == K_q: - self._control.gear = 1 if self._control.reverse else -1 - elif event.key == K_m: - self._control.manual_gear_shift = not self._control.manual_gear_shift - self._control.gear = world.player.get_control().gear - world.hud.notification('%s Transmission' % - ('Manual' if self._control.manual_gear_shift else 'Automatic')) - elif self._control.manual_gear_shift and event.key == K_COMMA: - self._control.gear = max(-1, self._control.gear - 1) - elif self._control.manual_gear_shift and event.key == K_PERIOD: - self._control.gear = self._control.gear + 1 - elif event.key == K_p and not (pygame.key.get_mods() & KMOD_CTRL): - self._autopilot_enabled = not self._autopilot_enabled - world.player.set_autopilot(self._autopilot_enabled) - world.hud.notification('Autopilot %s' % ('On' if self._autopilot_enabled else 'Off')) - if not self._autopilot_enabled: - if isinstance(self._control, carla.VehicleControl): - prev_steer_cache = self._steer_cache - self._parse_vehicle_keys(pygame.key.get_pressed(), clock.get_time()) - self._control.reverse = self._control.gear < 0 - vehicle_control = self._control - world.hud.original_vehicle_control = vehicle_control - world.hud.restricted_vehicle_control = vehicle_control - - # limit speed to 30kmh - v = self._world.player.get_velocity() - if (3.6 * math.sqrt(v.x**2 + v.y**2 + v.z**2)) > 30.0: - self._control.throttle = 0 - - # if self._world.rss_sensor and self._world.rss_sensor.ego_dynamics_on_route and not self._world.rss_sensor.ego_dynamics_on_route.ego_center_within_route: - # print ("Not on route!" + str(self._world.rss_sensor.ego_dynamics_on_route)) - if self._restrictor: - rss_restriction = self._world.rss_sensor.acceleration_restriction if self._world.rss_sensor and self._world.rss_sensor.response_valid else None - if rss_restriction: - rss_ego_dynamics_on_route = self._world.rss_sensor.ego_dynamics_on_route - vehicle_physics = world.player.get_physics_control() - - if not (pygame.key.get_mods() & KMOD_CTRL): - vehicle_control = self._restrictor.restrict_vehicle_control( - vehicle_control, rss_restriction, rss_ego_dynamics_on_route, vehicle_physics) - world.hud.restricted_vehicle_control = vehicle_control - if world.hud.original_vehicle_control.steer != world.hud.restricted_vehicle_control.steer: - self._steer_cache = prev_steer_cache - - world.player.apply_control(vehicle_control) - - elif isinstance(self._control, carla.WalkerControl): - self._parse_walker_keys(pygame.key.get_pressed(), clock.get_time()) - world.player.apply_control(self._control) - - def _parse_vehicle_keys(self, keys, milliseconds): - self._control.throttle = 1.0 if keys[K_UP] or keys[K_w] else 0.0 - steer_increment = 5e-4 * milliseconds - if keys[K_LEFT] or keys[K_a]: - self._steer_cache -= steer_increment - elif keys[K_RIGHT] or keys[K_d]: - self._steer_cache += steer_increment - else: - self._steer_cache = 0.0 - self._steer_cache = min(0.7, max(-0.7, self._steer_cache)) - self._control.steer = round(self._steer_cache, 1) - self._control.brake = 1.0 if keys[K_DOWN] or keys[K_s] else 0.0 - self._control.hand_brake = keys[K_SPACE] - - def _parse_walker_keys(self, keys, milliseconds): - self._control.speed = 0.0 - if keys[K_DOWN] or keys[K_s]: - self._control.speed = 0.0 - if keys[K_LEFT] or keys[K_a]: - self._control.speed = .01 - self._rotation.yaw -= 0.08 * milliseconds - if keys[K_RIGHT] or keys[K_d]: - self._control.speed = .01 - self._rotation.yaw += 0.08 * milliseconds - if keys[K_UP] or keys[K_w]: - self._control.speed = 3.333 if pygame.key.get_mods() & KMOD_SHIFT else 2.778 - self._control.jump = keys[K_SPACE] - self._rotation.yaw = round(self._rotation.yaw, 1) - self._control.direction = self._rotation.get_forward_vector() - - @staticmethod - def _is_quit_shortcut(key): - return (key == K_ESCAPE) or (key == K_q and pygame.key.get_mods() & KMOD_CTRL) - - -# ============================================================================== -# -- HUD ----------------------------------------------------------------------- -# ============================================================================== - - -class HUD(object): - - def __init__(self, width, height): - self.dim = (width, height) - font = pygame.font.Font(pygame.font.get_default_font(), 20) - fonts = [x for x in pygame.font.get_fonts() if 'mono' in x] - default_font = 'ubuntumono' - mono = default_font if default_font in fonts else fonts[0] - mono = pygame.font.match_font(mono) - 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.server_fps = 0 - self.frame = 0 - self.simulation_time = 0 - self.original_vehicle_control = None - self.restricted_vehicle_control = None - 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 = timestamp.frame - self.simulation_time = timestamp.elapsed_seconds - - def tick(self, world, clock): - self._notifications.tick(world, clock) - if not self._show_info: - return - t = world.player.get_transform() - v = world.player.get_velocity() - c = world.player.get_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 - 200] for x in range(0, 200)] - max_col = max(1.0, max(collision)) - collision = [x / max_col for x in collision] - vehicles = world.world.get_actors().filter('vehicle.*') - self._info_text = [ - 'Server: % 16.0f FPS' % self.server_fps, - 'Client: % 16.0f FPS' % clock.get_fps(), - '', - 'Vehicle: % 20s' % get_actor_display_name(world.player, truncate=20), - 'Map: % 20s' % 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)), - 'GNSS:% 24s' % ('(% 2.6f, % 3.6f)' % (world.gnss_sensor.lat, world.gnss_sensor.lon)), - 'Height: % 18.0f m' % t.location.z, - ''] - if isinstance(c, carla.VehicleControl): - if isinstance(self.original_vehicle_control, carla.VehicleControl): - orig_control = self.original_vehicle_control - restricted_control = self.restricted_vehicle_control - self._info_text += [ - ('Throttle:', orig_control.throttle, 0.0, 1.0, restricted_control.throttle), - ('Steer:', orig_control.steer, -1.0, 1.0, restricted_control.steer), - ('Brake:', orig_control.brake, 0.0, 1.0, restricted_control.brake)] - else: - self._info_text += [ - ('Throttle:', c.throttle, 0.0, 1.0), - ('Steer:', c.steer, -1.0, 1.0), - ('Brake:', c.brake, 0.0, 1.0)] - self._info_text += [ - ('Reverse:', c.reverse), - ('Hand brake:', c.hand_brake), - ('Manual:', c.manual_gear_shift), - 'Gear: %s' % {-1: 'R', 0: 'N'}.get(c.gear, c.gear)] - elif isinstance(c, carla.WalkerControl): - self._info_text += [ - ('Speed:', c.speed, 0.0, 5.556), - ('Jump:', c.jump)] - self._info_text += [ - '', - 'Collision:', - collision, - '', - 'Number of vehicles: % 8d' % len(vehicles)] - 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.player.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)) - - 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) - - def error(self, text): - 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: - text_color = (255, 255, 255) - 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) - if len(item) == 5: - if item[1] != item[4]: - pygame.draw.rect(display, (255, 0, 0), rect_border, 1) - f = (item[4] - 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, 0, 0), rect) - text_color = (255, 0, 0) - item = item[0] - if item and len(item) > 0: # At this point has to be a str. - surface = self._font_mono.render(item, True, text_color) - display.blit(surface, (8, v_offset)) - v_offset += 18 - self._notifications.render(display) - self.help.render(display) - - -# ============================================================================== -# -- FadingText ---------------------------------------------------------------- -# ============================================================================== - - -class FadingText(object): - - def __init__(self, font, dim, pos): - self.font = font - self.dim = dim - self.pos = pos - self.seconds_left = 0 - self.surface = pygame.Surface(self.dim) - - def set_text(self, text, color=(255, 255, 255), seconds=2.0): - text_texture = self.font.render(text, True, color) - self.surface = pygame.Surface(self.dim) - self.seconds_left = seconds - self.surface.fill((0, 0, 0, 0)) - self.surface.blit(text_texture, (10, 11)) - - def tick(self, _, clock): - delta_seconds = 1e-3 * clock.get_time() - self.seconds_left = max(0.0, self.seconds_left - delta_seconds) - self.surface.set_alpha(500.0 * self.seconds_left) - - def render(self, display): - display.blit(self.surface, self.pos) - - -# ============================================================================== -# -- HelpText ------------------------------------------------------------------ -# ============================================================================== - - -class HelpText(object): - - def __init__(self, font, width, height): - lines = __doc__.split('\n') - self.font = font - self.dim = (680, len(lines) * 22 + 12) - self.pos = (0.5 * width - 0.5 * self.dim[0], 0.5 * height - 0.5 * self.dim[1]) - self.seconds_left = 0 - self.surface = pygame.Surface(self.dim) - self.surface.fill((0, 0, 0, 0)) - for n, line in enumerate(lines): - text_texture = self.font.render(line, True, (255, 255, 255)) - self.surface.blit(text_texture, (22, n * 22)) - self._render = False - self.surface.set_alpha(220) - - def toggle(self): - self._render = not self._render - - def render(self, display): - if self._render: - display.blit(self.surface, self.pos) - - -# ============================================================================== -# -- CollisionSensor ----------------------------------------------------------- -# ============================================================================== - - -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() - bp = world.get_blueprint_library().find('sensor.other.collision') - 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. - 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() - if not self: - return - actor_type = get_actor_display_name(event.other_actor) - 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, intensity)) - if len(self.history) > 4000: - self.history.pop(0) - - -# ============================================================================== -# -- LaneInvasionSensor -------------------------------------------------------- -# ============================================================================== - - -class LaneInvasionSensor(object): - - def __init__(self, parent_actor, hud): - self.sensor = None - self._parent = parent_actor - self.hud = hud - world = self._parent.get_world() - 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. - weak_self = weakref.ref(self) - self.sensor.listen(lambda event: LaneInvasionSensor._on_invasion(weak_self, event)) - - @staticmethod - def _on_invasion(weak_self, event): - self = weak_self() - if not self: - return - lane_types = set(x.type for x in event.crossed_lane_markings) - text = ['%r' % str(x).split()[-1] for x in lane_types] - self.hud.notification('Crossed line %s' % ' and '.join(text)) - -# ============================================================================== -# -- GnssSensor -------------------------------------------------------- -# ============================================================================== - - -class GnssSensor(object): - - def __init__(self, parent_actor): - self.sensor = None - self._parent = parent_actor - self.lat = 0.0 - self.lon = 0.0 - world = self._parent.get_world() - bp = world.get_blueprint_library().find('sensor.other.gnss') - self.sensor = world.spawn_actor(bp, carla.Transform(carla.Location(x=1.0, z=2.8)), attach_to=self._parent) - # We need to pass the lambda a weak reference to self to avoid circular - # reference. - weak_self = weakref.ref(self) - self.sensor.listen(lambda event: GnssSensor._on_gnss_event(weak_self, event)) - - @staticmethod - def _on_gnss_event(weak_self, event): - self = weak_self() - if not self: - return - self.lat = event.latitude - self.lon = event.longitude - -# ============================================================================== -# -- RssSensor -------------------------------------------------------- -# ============================================================================== - - -class RssSensor(object): - - def __init__(self, parent_actor, routing_targets=None): - self.sensor = None - self._parent = parent_actor - self.timestamp = None - self.response_valid = False - self.proper_response = None - self.acceleration_restriction = None - self.ego_dynamics_on_route = None - self.current_display_parameters = None # for display - self.assertive_parameters = False - world = self._parent.get_world() - bp = world.get_blueprint_library().find('sensor.other.rss') - self.sensor = world.spawn_actor(bp, carla.Transform(carla.Location(x=0.0, z=0.0)), attach_to=self._parent) - # We need to pass the lambda a weak reference to self to avoid circular - # reference. - - def check_rss_class(clazz): - return inspect.isclass(clazz) and "RssSensor" in clazz.__name__ - - if not inspect.getmembers(carla, check_rss_class): - raise RuntimeError('CARLA PythonAPI not compiled in RSS variant, please "make PythonAPI.rss"') - weak_self = weakref.ref(self) - self.sensor.listen(lambda event: RssSensor._on_rss_response(weak_self, event)) - self.sensor.visualization_mode = carla.RssVisualizationMode.All - self.sensor.visualize_results = True - self.sensor.road_boundaries_mode = carla.RssRoadBoundariesMode.On - self.set_default_parameters() - self.sensor.reset_routing_targets() - if routing_targets: - for target in routing_targets: - self.sensor.append_routing_target(target) - - def get_assertive_parameters(self): - ego_dynamics = self.sensor.ego_vehicle_dynamics - ego_dynamics.alphaLon.accelMax = 4.1 - ego_dynamics.alphaLon.brakeMin = -4.64 - ego_dynamics.alphaLon.brakeMinCorrect = -1.76 - ego_dynamics.alphaLon.brakeMax = -8.03 - ego_dynamics.alphaLat.brakeMin = -0.96 - ego_dynamics.alphaLat.accelMax = 0.43 - ego_dynamics.lateralFluctuationMargin = 0.07 - ego_dynamics.responseTime = 0.53 - ego_dynamics.maxSpeed = 100 - return ego_dynamics - - def set_assertive_parameters(self): - print("Use 'assertive' Ego RSS Parameters") - ego_dynamics = self.get_assertive_parameters() - self.assertive_parameters = True - self.sensor.ego_vehicle_dynamics = ego_dynamics - self.current_display_parameters = ego_dynamics - - def get_default_parameters(self): - ego_dynamics = self.sensor.ego_vehicle_dynamics - # default, from ad_rss documentation - ego_dynamics.alphaLon.accelMax = 3.5 - ego_dynamics.alphaLon.brakeMin = -4 - ego_dynamics.alphaLon.brakeMax = -8 - ego_dynamics.alphaLon.brakeMinCorrect = -3 - ego_dynamics.alphaLat.brakeMin = -0.8 - ego_dynamics.alphaLat.accelMax = 0.2 - ego_dynamics.lateralFluctuationMargin = 0.1 - ego_dynamics.responseTime = 1.0 - ego_dynamics.maxSpeed = 100 - return ego_dynamics - - def set_default_parameters(self): - print("Use 'default' Ego RSS Parameters") - ego_dynamics = self.get_default_parameters() - self.assertive_parameters = False - self.sensor.ego_vehicle_dynamics = ego_dynamics - self.current_display_parameters = ego_dynamics - - @staticmethod - def _on_rss_response(weak_self, response): - self = weak_self() - if not self or not response: - return - delta_time = 0.1 - if self.timestamp: - delta_time = response.timestamp - self.timestamp - # debug drawing within the RssSensor takes quite some time - # while debug drawing is blocking a thread the respective response usually arrives later - if delta_time > -0.05: - self.timestamp = response.timestamp - self.response_valid = response.response_valid - self.proper_response = response.proper_response - self.acceleration_restriction = response.acceleration_restriction - self.ego_dynamics_on_route = response.ego_dynamics_on_route - # else: - # print("ignore outdated response {}".format(delta_time)) - - def drop_route(self): - self.sensor.drop_route() - -# ============================================================================== -# -- CameraManager ------------------------------------------------------------- -# ============================================================================== - - -class CameraManager(object): - - def __init__(self, parent_actor, hud): - self.sensor = None - self.surface = None - self._parent = parent_actor - self.hud = hud - self.recording = False - bound_y = 0.5 + self._parent.bounding_box.extent.y - Attachment = carla.AttachmentType - self._camera_transforms = [ - (carla.Transform(carla.Location(x=-5.5, z=2.5), carla.Rotation(pitch=8.0)), Attachment.SpringArm), - (carla.Transform(carla.Location(x=1.6, z=1.7)), Attachment.Rigid), - (carla.Transform(carla.Location(x=5.5, y=1.5, z=1.5)), Attachment.SpringArm), - (carla.Transform(carla.Location(x=-8.0, z=6.0), carla.Rotation(pitch=6.0)), Attachment.SpringArm), - (carla.Transform(carla.Location(x=-1, y=-bound_y, z=0.5)), Attachment.Rigid)] - self.transform_index = 1 - self.sensors = [ - ['sensor.camera.rgb', cc.Raw, 'Camera RGB'], - ['sensor.camera.depth', cc.Raw, 'Camera Depth (Raw)'], - ['sensor.camera.depth', cc.Depth, 'Camera Depth (Gray Scale)'], - ['sensor.camera.depth', cc.LogarithmicDepth, 'Camera Depth (Logarithmic Gray Scale)'], - ['sensor.camera.semantic_segmentation', cc.Raw, 'Camera Semantic Segmentation (Raw)'], - ['sensor.camera.semantic_segmentation', cc.CityScapesPalette, - 'Camera Semantic Segmentation (CityScapes Palette)'], - ['sensor.lidar.ray_cast', None, 'Lidar (Ray-Cast)']] - world = self._parent.get_world() - bp_library = world.get_blueprint_library() - for item in self.sensors: - bp = bp_library.find(item[0]) - if item[0].startswith('sensor.camera'): - bp.set_attribute('image_size_x', str(hud.dim[0])) - bp.set_attribute('image_size_y', str(hud.dim[1])) - elif item[0].startswith('sensor.lidar'): - bp.set_attribute('range', '5000') - item.append(bp) - self.index = None - - def toggle_camera(self): - self.transform_index = (self.transform_index + 1) % len(self._camera_transforms) - self.set_sensor(self.index, notify=False, force_respawn=True) - - def set_sensor(self, index, notify=True, force_respawn=False): - index = index % len(self.sensors) - needs_respawn = True if self.index is None else \ - (force_respawn or (self.sensors[index][0] != self.sensors[self.index][0])) - if needs_respawn: - if self.sensor is not None: - self.sensor.destroy() - self.surface = None - self.sensor = self._parent.get_world().spawn_actor( - self.sensors[index][-1], - self._camera_transforms[self.transform_index][0], - attach_to=self._parent, - attachment_type=self._camera_transforms[self.transform_index][1]) - # We need to pass the lambda a weak reference to self to avoid - # circular reference. - weak_self = weakref.ref(self) - self.sensor.listen(lambda image: CameraManager._parse_image(weak_self, image)) - if notify: - self.hud.notification(self.sensors[index][2]) - self.index = index - - def next_sensor(self): - self.set_sensor(self.index + 1) - - def toggle_recording(self): - self.recording = not self.recording - self.hud.notification('Recording %s' % ('On' if self.recording else 'Off')) - - def render(self, display): - if self.surface is not None: - display.blit(self.surface, (0, 0)) - - @staticmethod - def _parse_image(weak_self, image): - self = weak_self() - if not self: - return - 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] / 4), 4)) - lidar_data = np.array(points[:, :2]) - lidar_data *= min(self.hud.dim) / 100.0 - lidar_data += (0.5 * self.hud.dim[0], 0.5 * self.hud.dim[1]) - lidar_data = np.fabs(lidar_data) # pylint: disable=E1111 - lidar_data = lidar_data.astype(np.int32) - lidar_data = np.reshape(lidar_data, (-1, 2)) - lidar_img_size = (self.hud.dim[0], self.hud.dim[1], 3) - lidar_img = np.zeros((lidar_img_size), dtype=int) - lidar_img[tuple(lidar_data.T)] = (255, 255, 255) - self.surface = pygame.surfarray.make_surface(lidar_img) - else: - image.convert(self.sensors[self.index][1]) - array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) - array = np.reshape(array, (image.height, image.width, 4)) - array = array[:, :, :3] - array = array[:, :, ::-1] - self.surface = pygame.surfarray.make_surface(array.swapaxes(0, 1)) - if self.recording: - image.save_to_disk('_out/%08d' % image.frame) - - -# ============================================================================== -# -- game_loop() --------------------------------------------------------------- -# ============================================================================== - - -def game_loop(args): - pygame.init() - pygame.font.init() - world = None - - try: - client = carla.Client(args.host, args.port) - client.set_timeout(2.0) - - display = pygame.display.set_mode( - (args.width, args.height), - pygame.HWSURFACE | pygame.DOUBLEBUF) - - hud = HUD(args.width, args.height) - world = World(client.get_world(), hud, args.filter, args.rolename, args.externalActor) - controller = KeyboardControl(world, args) - - clock = pygame.time.Clock() - carla.VehicleLightState=None - while True: - clock.tick_busy_loop(60) - if controller.parse_events(client, world, clock): - return - world.tick(clock) - world.render(display) - pygame.display.flip() - - finally: - - if (world and world.recording_enabled): - client.stop_recorder() - - if world is not None: - world.destroy() - - pygame.quit() - - -# ============================================================================== -# -- main() -------------------------------------------------------------------- -# ============================================================================== - - -def main(): - argparser = argparse.ArgumentParser( - description='CARLA Manual Control Client RSS') - argparser.add_argument( - '-v', '--verbose', - action='store_true', - dest='debug', - help='print debug information') - argparser.add_argument( - '--host', - metavar='H', - default='127.0.0.1', - help='IP of the host server (default: 127.0.0.1)') - argparser.add_argument( - '-p', '--port', - metavar='P', - default=2000, - type=int, - help='TCP port to listen to (default: 2000)') - argparser.add_argument( - '-a', '--autopilot', - action='store_true', - help='enable autopilot') - argparser.add_argument( - '--res', - metavar='WIDTHxHEIGHT', - default='1280x720', - help='window resolution (default: 1280x720)') - argparser.add_argument( - '--filter', - metavar='PATTERN', - default='vehicle.*', - help='actor filter (default: "vehicle.*")') - argparser.add_argument( - '--rolename', - metavar='NAME', - default='hero', - help='actor role name (default: "hero")') - argparser.add_argument( - '--externalActor', - action='store_true', - help='attaches to externally created actor by role name') - args = argparser.parse_args() - - args.width, args.height = [int(x) for x in args.res.split('x')] - - log_level = logging.DEBUG if args.debug else logging.INFO - logging.basicConfig(format='%(levelname)s: %(message)s', level=log_level) - - logging.info('listening to server %s:%s', args.host, args.port) - - print(__doc__) - - try: - - game_loop(args) - - except KeyboardInterrupt: - print('\nCancelled by user. Bye!') - - -if __name__ == '__main__': - - main() diff --git a/PythonAPI/examples/rss/__init__.py b/PythonAPI/examples/rss/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/PythonAPI/examples/rss/manual_control_rss.py b/PythonAPI/examples/rss/manual_control_rss.py new file mode 100755 index 000000000..4fcbe1127 --- /dev/null +++ b/PythonAPI/examples/rss/manual_control_rss.py @@ -0,0 +1,884 @@ +#!/usr/bin/env python + +# Copyright (c) 2019 Computer Vision Center (CVC) at the Universitat Autonoma de +# Barcelona (UAB). +# Copyright (c) 2019-2020 Intel Corporation +# +# This work is licensed under the terms of the MIT license. +# For a copy, see . + +# Allows controlling a vehicle with a keyboard. For a simpler and more +# documented example, please take a look at tutorial.py. + +""" +Welcome to CARLA manual control. + +Use ARROWS or WASD keys for control. + + W : throttle + S : brake + AD : steer + Q : toggle reverse + Space : hand-brake + P : toggle autopilot + + Backspace : change vehicle + + R : toggle recording images to disk + + F2 : toggle RSS visualization mode + B : toggle RSS Road Boundaries Mode + G : RSS check drop current route + T : toggle RSS + N : pause simulation + + F1 : toggle HUD + H/? : toggle help + ESC : quit +""" + +from __future__ import print_function + + +# ============================================================================== +# -- find carla module --------------------------------------------------------- +# ============================================================================== + + +import glob +import os +import sys +import signal + +try: + sys.path.append(glob.glob('../carla/dist/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 + +from carla import ColorConverter as cc + +import argparse +import collections +import datetime +import inspect +import logging +import math +import random +import re +import weakref +from rss_sensor import RssSensor +from rss_visualization import RssUnstructuredSceneDrawer, RssBoundingBoxDrawer, render_rss_states + +try: + import pygame + from pygame.locals import KMOD_CTRL + from pygame.locals import KMOD_SHIFT + from pygame.locals import K_BACKQUOTE + 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_F2 + from pygame.locals import K_LEFT + from pygame.locals import K_RIGHT + from pygame.locals import K_SLASH + from pygame.locals import K_SPACE + from pygame.locals import K_UP + from pygame.locals import K_a + from pygame.locals import K_b + from pygame.locals import K_c + from pygame.locals import K_d + from pygame.locals import K_g + from pygame.locals import K_h + from pygame.locals import K_n + from pygame.locals import K_p + from pygame.locals import K_q + from pygame.locals import K_r + from pygame.locals import K_s + from pygame.locals import K_t + from pygame.locals import K_u + from pygame.locals import K_w + from pygame.locals import K_l + from pygame.locals import K_i + from pygame.locals import K_z + from pygame.locals import K_x + from pygame.locals import MOUSEMOTION + from pygame.locals import MOUSEBUTTONDOWN + from pygame.locals import MOUSEBUTTONUP +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') + + +# ============================================================================== +# -- World --------------------------------------------------------------------- +# ============================================================================== + + +class World(object): + + def __init__(self, carla_world, hud, args): + self.world = carla_world + self.actor_role_name = args.rolename + self.dim = (args.width, args.height) + try: + self.map = self.world.get_map() + except RuntimeError as error: + print('RuntimeError: {}'.format(error)) + print(' The server could not send the OpenDRIVE (.xodr) file:') + print(' Make sure it exists, has the same name of your town, and is correct.') + sys.exit(1) + self.external_actor = args.externalActor + self.hud = hud + self.recording_frame_num = 0 + self.recording = False + self.recording_dir_num = 0 + self.player = None + self.actors = [] + self.rss_sensor = None + self.rss_unstructured_scene_drawer = None + self.rss_bounding_box_drawer = None + self._actor_filter = args.filter + if not self._actor_filter.startswith("vehicle."): + print('Error: RSS only supports vehicles as ego.') + sys.exit(1) + + self.restart() + self.world_tick_id = self.world.on_tick(self.on_world_tick) + + def on_world_tick(self, world_snapshot): + self.hud.on_world_tick(world_snapshot) + + def toggle_pause(self): + settings = self.world.get_settings() + self.pause_simulation(not settings.synchronous_mode) + + def pause_simulation(self, pause): + settings = self.world.get_settings() + if pause and not settings.synchronous_mode: + settings.synchronous_mode = True + settings.fixed_delta_seconds = 0.05 + self.world.apply_settings(settings) + elif not pause and settings.synchronous_mode: + settings.synchronous_mode = False + settings.fixed_delta_seconds = None + self.world.apply_settings(settings) + + def restart(self): + + if self.external_actor: + # Check whether there is already an actor with defined role name + for actor in self.world.get_actors(): + if actor.attributes.get('role_name') == self.actor_role_name: + self.player = actor + break + else: + # Get a random blueprint. + blueprint = random.choice(self.world.get_blueprint_library().filter(self._actor_filter)) + blueprint.set_attribute('role_name', self.actor_role_name) + if blueprint.has_attribute('color'): + color = random.choice(blueprint.get_attribute('color').recommended_values) + blueprint.set_attribute('color', color) + if blueprint.has_attribute('driver_id'): + driver_id = random.choice(blueprint.get_attribute('driver_id').recommended_values) + blueprint.set_attribute('driver_id', driver_id) + if blueprint.has_attribute('is_invincible'): + blueprint.set_attribute('is_invincible', 'true') + # Spawn the player. + if self.player is not None: + spawn_point = self.player.get_transform() + spawn_point.location.z += 2.0 + spawn_point.rotation.roll = 0.0 + spawn_point.rotation.pitch = 0.0 + self.destroy() + self.player = self.world.try_spawn_actor(blueprint, spawn_point) + while self.player is None: + if not self.map.get_spawn_points(): + print('There are no spawn points available in your map/town.') + print('Please add some Vehicle Spawn Point to your UE4 scene.') + sys.exit(1) + spawn_points = self.map.get_spawn_points() + spawn_point = random.choice(spawn_points) if spawn_points else carla.Transform() + self.player = self.world.try_spawn_actor(blueprint, spawn_point) + + if self.external_actor: + ego_sensors = [] + for actor in self.world.get_actors(): + if actor.parent == self.player: + ego_sensors.append(actor) + + for ego_sensor in ego_sensors: + if ego_sensor is not None: + ego_sensor.destroy() + + # Set up the sensors. + self.camera = Camera(self.player, self.dim) + self.rss_unstructured_scene_drawer = RssUnstructuredSceneDrawer(self.player, self.world, self.dim) + self.rss_bounding_box_drawer = RssBoundingBoxDrawer(self.dim, self.world, self.camera.sensor) + self.rss_sensor = RssSensor(self.player, self.world, + self.rss_unstructured_scene_drawer, self.rss_bounding_box_drawer) + + def tick(self, clock): + self.hud.tick(self, clock) + + def toggle_recording(self): + if not self.recording: + dir_name = "_out%04d" % self.recording_dir_num + while os.path.exists(dir_name): + self.recording_dir_num += 1 + dir_name = "_out%04d" % self.recording_dir_num + self.recording_frame_num = 0 + os.mkdir(dir_name) + else: + self.hud.notification('Recording finished (folder: _out%04d)' % self.recording_dir_num) + + self.recording = not self.recording + + def render(self, display): + self.camera.render(display) + self.rss_bounding_box_drawer.render(display, self.camera.current_frame) + self.rss_unstructured_scene_drawer.render(display) + self.hud.render(display, self.rss_sensor) + + if self.recording: + pygame.image.save(display, "_out%04d/%08d.bmp" % (self.recording_dir_num, self.recording_frame_num)) + self.recording_frame_num += 1 + + def destroy(self): + # stop from ticking + if self.world_tick_id: + self.world.remove_on_tick(self.world_tick_id) + + if self.camera: + self.camera.destroy() + if self.rss_sensor: + self.rss_sensor.destroy() + if self.rss_unstructured_scene_drawer: + self.rss_unstructured_scene_drawer.destroy() + if self.player: + self.player.destroy() + + +# ============================================================================== +# -- Camera -------------------------------------------------------------------- +# ============================================================================== + +class Camera(object): + def __init__(self, parent_actor, display_dimensions): + self.surface = None + self._parent = parent_actor + self.current_frame = None + bp_library = self._parent.get_world().get_blueprint_library() + bp = bp_library.find('sensor.camera.rgb') + bp.set_attribute('image_size_x', str(display_dimensions[0])) + bp.set_attribute('image_size_y', str(display_dimensions[1])) + self.sensor = self._parent.get_world().spawn_actor(bp, carla.Transform(carla.Location( + x=-5.5, z=2.5), carla.Rotation(pitch=8.0)), attach_to=self._parent, attachment_type=carla.AttachmentType.SpringArm) + + # We need to pass the lambda a weak reference to self to avoid + # circular reference. + weak_self = weakref.ref(self) + self.sensor.listen(lambda image: Camera._parse_image(weak_self, image)) + + def destroy(self): + self.sensor.stop() + self.sensor.destroy() + self.sensor = None + + def render(self, display): + if self.surface is not None: + display.blit(self.surface, (0, 0)) + + @staticmethod + def _parse_image(weak_self, image): + self = weak_self() + if not self: + return + self.current_frame = image.frame + image.convert(cc.Raw) + array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) + array = np.reshape(array, (image.height, image.width, 4)) + array = array[:, :, :3] + array = array[:, :, ::-1] + self.surface = pygame.surfarray.make_surface(array.swapaxes(0, 1)) + +# ============================================================================== +# -- VehicleControl ----------------------------------------------------------- +# ============================================================================== + + +class VehicleControl(object): + + MOUSE_STEERING_RANGE = 200 + signal_received = False + + """Class that handles keyboard input.""" + + def __init__(self, world, start_in_autopilot): + self._autopilot_enabled = start_in_autopilot + self._world = world + self._control = carla.VehicleControl() + self._lights = carla.VehicleLightState.NONE + world.player.set_autopilot(self._autopilot_enabled) + self._restrictor = carla.RssRestrictor() + self._vehicle_physics = world.player.get_physics_control() + world.player.set_light_state(self._lights) + self._steer_cache = 0.0 + self._mouse_steering_center = None + + self._surface = pygame.Surface((self.MOUSE_STEERING_RANGE * 2, self.MOUSE_STEERING_RANGE * 2)) + self._surface.set_colorkey(pygame.Color('black')) + self._surface.set_alpha(60) + + line_width = 2 + pygame.draw.polygon(self._surface, + (0, 0, 255), + [ + (0, 0), + (0, self.MOUSE_STEERING_RANGE * 2 - line_width), + (self.MOUSE_STEERING_RANGE * 2 - line_width, self.MOUSE_STEERING_RANGE * 2 - line_width), + (self.MOUSE_STEERING_RANGE * 2 - line_width, 0), + (0, 0) + ], line_width) + pygame.draw.polygon(self._surface, + (0, 0, 255), + [ + (0, self.MOUSE_STEERING_RANGE), + (self.MOUSE_STEERING_RANGE * 2, self.MOUSE_STEERING_RANGE) + ], line_width) + pygame.draw.polygon(self._surface, + (0, 0, 255), + [ + (self.MOUSE_STEERING_RANGE, 0), + (self.MOUSE_STEERING_RANGE, self.MOUSE_STEERING_RANGE * 2) + ], line_width) + + world.hud.notification("Press 'H' or '?' for help.", seconds=4.0) + + def render(self, display): + if self._mouse_steering_center: + display.blit( + self._surface, (self._mouse_steering_center[0] - self.MOUSE_STEERING_RANGE, self._mouse_steering_center[1] - self.MOUSE_STEERING_RANGE)) + + @staticmethod + def signal_handler(signum, frame): + print('\nReceived signal {}. Trigger stopping...'.format(signum)) + VehicleControl.signal_received = True + + def parse_events(self, client, world, clock): + if VehicleControl.signal_received: + print('\nAccepted signal. Stopping loop...') + return True + if isinstance(self._control, carla.VehicleControl): + current_lights = self._lights + for event in pygame.event.get(): + if event.type == pygame.QUIT: + return True + elif event.type == pygame.KEYUP: + if self._is_quit_shortcut(event.key): + return True + elif event.key == K_BACKSPACE: + if self._autopilot_enabled: + world.player.set_autopilot(False) + world.restart() + world.player.set_autopilot(True) + else: + 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_u: + world.rss_unstructured_scene_drawer.toggle_camera() + elif event.key == K_n: + world.toggle_pause() + elif event.key == K_r: + world.toggle_recording() + elif event.key == K_F2: + if self._world and self._world.rss_sensor: + self._world.rss_sensor.toggle_debug_visualization_mode() + elif event.key == K_b: + if self._world and self._world.rss_sensor: + if self._world.rss_sensor.sensor.road_boundaries_mode == carla.RssRoadBoundariesMode.Off: + self._world.rss_sensor.sensor.road_boundaries_mode = carla.RssRoadBoundariesMode.On + print("carla.RssRoadBoundariesMode.On") + else: + self._world.rss_sensor.sensor.road_boundaries_mode = carla.RssRoadBoundariesMode.Off + print("carla.RssRoadBoundariesMode.Off") + elif event.key == K_g: + if self._world and self._world.rss_sensor: + self._world.rss_sensor.drop_route() + elif event.key == K_t: + if self._world and self._world.rss_sensor: + if self._world.rss_sensor.assertive_parameters: + self._world.rss_sensor.set_default_parameters() + else: + self._world.rss_sensor.set_assertive_parameters() + if isinstance(self._control, carla.VehicleControl): + if event.key == K_q: + self._control.gear = 1 if self._control.reverse else -1 + elif event.key == K_p and not pygame.key.get_mods() & KMOD_CTRL: + self._autopilot_enabled = not self._autopilot_enabled + world.player.set_autopilot(self._autopilot_enabled) + world.hud.notification( + 'Autopilot %s' % ('On' if self._autopilot_enabled else 'Off')) + elif event.key == K_l and pygame.key.get_mods() & KMOD_CTRL: + current_lights ^= carla.VehicleLightState.Special1 + elif event.key == K_l and pygame.key.get_mods() & KMOD_SHIFT: + current_lights ^= carla.VehicleLightState.HighBeam + elif event.key == K_l: + # Use 'L' key to switch between lights: + # closed -> position -> low beam -> fog + if not self._lights & carla.VehicleLightState.Position: + world.hud.notification("Position lights") + current_lights |= carla.VehicleLightState.Position + else: + world.hud.notification("Low beam lights") + current_lights |= carla.VehicleLightState.LowBeam + if self._lights & carla.VehicleLightState.LowBeam: + world.hud.notification("Fog lights") + current_lights |= carla.VehicleLightState.Fog + if self._lights & carla.VehicleLightState.Fog: + world.hud.notification("Lights off") + current_lights ^= carla.VehicleLightState.Position + current_lights ^= carla.VehicleLightState.LowBeam + current_lights ^= carla.VehicleLightState.Fog + elif event.key == K_i: + current_lights ^= carla.VehicleLightState.Interior + elif event.key == K_z: + current_lights ^= carla.VehicleLightState.LeftBlinker + elif event.key == K_x: + current_lights ^= carla.VehicleLightState.RightBlinker + elif event.type == pygame.MOUSEBUTTONDOWN: + # store current mouse position for mouse-steering + if event.button == 1: + self._mouse_steering_center = event.pos + elif event.type == pygame.MOUSEBUTTONUP: + if event.button == 1: + self._mouse_steering_center = None + if not self._autopilot_enabled: + prev_steer_cache = self._steer_cache + self._parse_vehicle_keys(pygame.key.get_pressed(), clock.get_time()) + if pygame.mouse.get_pressed()[0]: + self._parse_mouse(pygame.mouse.get_pos()) + self._control.reverse = self._control.gear < 0 + # Set automatic control-related vehicle lights + if self._control.brake: + current_lights |= carla.VehicleLightState.Brake + else: # Remove the Brake flag + current_lights &= carla.VehicleLightState.All ^ carla.VehicleLightState.Brake + if self._control.reverse: + current_lights |= carla.VehicleLightState.Reverse + else: # Remove the Reverse flag + current_lights &= carla.VehicleLightState.All ^ carla.VehicleLightState.Reverse + if current_lights != self._lights: # Change the light state only if necessary + self._lights = current_lights + world.player.set_light_state(carla.VehicleLightState(self._lights)) + + vehicle_control = self._control + world.hud.original_vehicle_control = vehicle_control + world.hud.restricted_vehicle_control = vehicle_control + + # limit speed to 30kmh + v = self._world.player.get_velocity() + if (3.6 * math.sqrt(v.x**2 + v.y**2 + v.z**2)) > 30.0: + self._control.throttle = 0 + + # if self._world.rss_sensor and self._world.rss_sensor.ego_dynamics_on_route and not self._world.rss_sensor.ego_dynamics_on_route.ego_center_within_route: + # print ("Not on route!" + str(self._world.rss_sensor.ego_dynamics_on_route)) + if self._restrictor: + rss_proper_response = self._world.rss_sensor.proper_response if self._world.rss_sensor and self._world.rss_sensor.response_valid else None + if rss_proper_response: + if not (pygame.key.get_mods() & KMOD_CTRL): + vehicle_control = self._restrictor.restrict_vehicle_control( + vehicle_control, rss_proper_response, self._world.rss_sensor.ego_dynamics_on_route, self._vehicle_physics) + world.hud.restricted_vehicle_control = vehicle_control + world.hud.allowed_steering_ranges = self._world.rss_sensor.get_steering_ranges() + if world.hud.original_vehicle_control.steer != world.hud.restricted_vehicle_control.steer: + self._steer_cache = prev_steer_cache + + world.player.apply_control(vehicle_control) + + def _parse_vehicle_keys(self, keys, milliseconds): + if keys[K_UP] or keys[K_w]: + self._control.throttle = min(self._control.throttle + 0.2, 1) + else: + self._control.throttle = max(self._control.throttle - 0.2, 0) + + if keys[K_DOWN] or keys[K_s]: + self._control.brake = min(self._control.brake + 0.2, 1) + else: + self._control.brake = max(self._control.brake - 0.2, 0) + + steer_increment = 5e-4 * milliseconds + if keys[K_LEFT] or keys[K_a]: + if self._steer_cache > 0: + self._steer_cache = 0 + else: + self._steer_cache -= steer_increment + elif keys[K_RIGHT] or keys[K_d]: + if self._steer_cache < 0: + self._steer_cache = 0 + else: + self._steer_cache += steer_increment + elif self._steer_cache > 0: + self._steer_cache = max(self._steer_cache - steer_increment, 0.0) + elif self._steer_cache < 0: + self._steer_cache = min(self._steer_cache + steer_increment, 0.0) + else: + self._steer_cache = 0 + + self._steer_cache = min(1.0, max(-1.0, self._steer_cache)) + self._control.steer = round(self._steer_cache, 1) + self._control.hand_brake = keys[K_SPACE] + + def _parse_mouse(self, pos): + if not self._mouse_steering_center: + return + + lateral = float(pos[0] - self._mouse_steering_center[0]) + longitudinal = float(pos[1] - self._mouse_steering_center[1]) + max_val = self.MOUSE_STEERING_RANGE + lateral = -max_val if lateral < -max_val else max_val if lateral > max_val else lateral + longitudinal = -max_val if longitudinal < -max_val else max_val if longitudinal > max_val else longitudinal + self._control.steer = lateral/max_val + if longitudinal < 0.0: + self._control.throttle = -longitudinal / max_val + self._control.brake = 0.0 + elif longitudinal > 0.0: + self._control.throttle = 0.0 + self._control.brake = longitudinal / max_val + + @staticmethod + def _is_quit_shortcut(key): + return (key == K_ESCAPE) or (key == K_q and pygame.key.get_mods() & KMOD_CTRL) + + +# ============================================================================== +# -- HUD ----------------------------------------------------------------------- +# ============================================================================== + + +class HUD(object): + + def __init__(self, width, height): + self.dim = (width, height) + font = pygame.font.Font(pygame.font.get_default_font(), 20) + font_name = 'courier' if os.name == 'nt' else 'mono' + fonts = [x for x in pygame.font.get_fonts() if font_name in x] + default_font = 'ubuntumono' + mono = default_font if default_font in fonts else fonts[0] + mono = pygame.font.match_font(mono) + self._font_mono = pygame.font.Font(mono, 12 if os.name == 'nt' else 14) + self._notifications = FadingText(font, (width, 40), (0, height - 40)) + self.help = HelpText(pygame.font.Font(mono, 16), width, height) + self.server_fps = 0 + self.frame = 0 + self.simulation_time = 0 + self.original_vehicle_control = None + self.restricted_vehicle_control = None + self.allowed_steering_ranges = [] + 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 = timestamp.frame + self.simulation_time = timestamp.elapsed_seconds + + def tick(self, world, clock): + self._notifications.tick(world, clock) + if not self._show_info: + return + t = world.player.get_transform() + v = world.player.get_velocity() + c = world.player.get_control() + + self._info_text = [ + 'Server: % 16.0f FPS' % self.server_fps, + 'Client: % 16.0f FPS' % clock.get_fps(), + 'Map: % 20s' % world.map.name, + '', + 'Speed: % 15.0f km/h' % (3.6 * math.sqrt(v.x**2 + v.y**2 + v.z**2)), + 'Location:% 20s' % ('(% 5.1f, % 5.1f)' % (t.location.x, t.location.y)), + 'Heading: % 20.2f' % math.radians(t.rotation.yaw), + ''] + if self.original_vehicle_control: + orig_control = self.original_vehicle_control + restricted_control = self.restricted_vehicle_control + allowed_steering_ranges = self.allowed_steering_ranges + self._info_text += [ + ('Throttle:', orig_control.throttle, 0.0, 1.0, restricted_control.throttle), + ('Steer:', orig_control.steer, -1.0, 1.0, restricted_control.steer, allowed_steering_ranges), + ('Brake:', orig_control.brake, 0.0, 1.0, restricted_control.brake)] + self._info_text += [ + ('Reverse:', c.reverse), + ''] + + 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) + + def error(self, text): + self._notifications.set_text('Error: %s' % text, (255, 0, 0)) + + def render(self, display, rss_sensor): + 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: + text_color = (255, 255, 255) + 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 + 2), (10, 10)) + pygame.draw.rect(display, (255, 255, 255), rect, 0 if item[1] else 1) + else: + # draw allowed steering ranges + if len(item) == 6 and item[2] < 0.0: + for range in item[5]: + min_value = min(range[0], range[1]) + max_value = max(range[0], range[1]) + starting_value = min_value + length = (max(range[0], range[1]) - min(range[0], range[1])) / 2 + rect = pygame.Rect( + (bar_h_offset + (starting_value + 1) * (bar_width / 2), v_offset + 2), (length * bar_width, 14)) + pygame.draw.rect(display, (0, 255, 0), rect) + + # draw border + rect_border = pygame.Rect((bar_h_offset, v_offset + 2), (bar_width, 14)) + pygame.draw.rect(display, (255, 255, 255), rect_border, 1) + + # draw value / restricted value + input_value_rect_fill = 0 + if len(item) >= 5: + if item[1] != item[4]: + input_value_rect_fill = 1 + f = (item[4] - item[2]) / (item[3] - item[2]) + if item[2] < 0.0: + rect = pygame.Rect( + (bar_h_offset + 1 + f * (bar_width - 6), v_offset + 3), (12, 12)) + else: + rect = pygame.Rect((bar_h_offset + 1, v_offset + 3), (f * bar_width, 12)) + pygame.draw.rect(display, (255, 0, 0), rect) + + f = (item[1] - item[2]) / (item[3] - item[2]) + rect = None + if item[2] < 0.0: + rect = pygame.Rect((bar_h_offset + 2 + f * (bar_width - 14), v_offset + 4), (10, 10)) + else: + if item[1] != 0: + rect = pygame.Rect((bar_h_offset + 2, v_offset + 4), (f * (bar_width - 4), 10)) + if rect: + pygame.draw.rect(display, (255, 255, 255), rect, input_value_rect_fill) + item = item[0] + if item: # At this point has to be a str. + surface = self._font_mono.render(item, True, text_color) + display.blit(surface, (8, v_offset)) + v_offset += 18 + + render_rss_states(display, v_offset, self._font_mono, rss_sensor.individual_rss_states) + self._notifications.render(display) + self.help.render(display) + + +# ============================================================================== +# -- FadingText ---------------------------------------------------------------- +# ============================================================================== + + +class FadingText(object): + def __init__(self, font, dim, pos): + self.font = font + self.dim = dim + self.pos = pos + self.seconds_left = 0 + self.surface = pygame.Surface(self.dim) + + def set_text(self, text, color=(255, 255, 255), seconds=2.0): + text_texture = self.font.render(text, True, color) + self.surface = pygame.Surface(self.dim) + self.seconds_left = seconds + self.surface.fill((0, 0, 0, 0)) + self.surface.blit(text_texture, (10, 11)) + + def tick(self, _, clock): + delta_seconds = 1e-3 * clock.get_time() + self.seconds_left = max(0.0, self.seconds_left - delta_seconds) + self.surface.set_alpha(500.0 * self.seconds_left) + + def render(self, display): + display.blit(self.surface, self.pos) + + +# ============================================================================== +# -- HelpText ------------------------------------------------------------------ +# ============================================================================== + + +class HelpText(object): + """Helper class to handle text output using pygame""" + + def __init__(self, font, width, height): + lines = __doc__.split('\n') + self.font = font + self.line_space = 18 + self.dim = (780, len(lines) * self.line_space + 12) + self.pos = (0.5 * width - 0.5 * self.dim[0], 0.5 * height - 0.5 * self.dim[1]) + self.seconds_left = 0 + self.surface = pygame.Surface(self.dim) + self.surface.fill((0, 0, 0, 0)) + for n, line in enumerate(lines): + text_texture = self.font.render(line, True, (255, 255, 255)) + self.surface.blit(text_texture, (22, n * self.line_space)) + self._render = False + self.surface.set_alpha(220) + + def toggle(self): + self._render = not self._render + + def render(self, display): + if self._render: + display.blit(self.surface, self.pos) + +# ============================================================================== +# -- game_loop() --------------------------------------------------------------- +# ============================================================================== + + +def game_loop(args): + pygame.init() + pygame.font.init() + world = None + + try: + client = carla.Client(args.host, args.port) + client.set_timeout(2.0) + + display = pygame.display.set_mode( + (args.width, args.height), + pygame.HWSURFACE | pygame.DOUBLEBUF) + + hud = HUD(args.width, args.height) + world = World(client.get_world(), hud, args) + controller = VehicleControl(world, args.autopilot) + + clock = pygame.time.Clock() + while True: + clock.tick_busy_loop(60) + if controller.parse_events(client, world, clock): + return + world.tick(clock) + world.render(display) + controller.render(display) + pygame.display.flip() + + finally: + + if world is not None: + print('Destroying the world...') + world.destroy() + print('Destroyed!') + + pygame.quit() + + +# ============================================================================== +# -- main() -------------------------------------------------------------------- +# ============================================================================== + +def main(): + argparser = argparse.ArgumentParser( + description='CARLA Manual Control Client RSS') + argparser.add_argument( + '-v', '--verbose', + action='store_true', + dest='debug', + help='print debug information') + argparser.add_argument( + '--host', + metavar='H', + default='127.0.0.1', + help='IP of the host server (default: 127.0.0.1)') + argparser.add_argument( + '-p', '--port', + metavar='P', + default=2000, + type=int, + help='TCP port to listen to (default: 2000)') + argparser.add_argument( + '-a', '--autopilot', + action='store_true', + help='enable autopilot') + argparser.add_argument( + '--res', + metavar='WIDTHxHEIGHT', + default='1280x720', + help='window resolution (default: 1280x720)') + argparser.add_argument( + '--filter', + metavar='PATTERN', + default='vehicle.*', + help='actor filter (default: "vehicle.*")') + argparser.add_argument( + '--rolename', + metavar='NAME', + default='hero', + help='actor role name (default: "hero")') + argparser.add_argument( + '--externalActor', + action='store_true', + help='attaches to externally created actor by role name') + args = argparser.parse_args() + + args.width, args.height = [int(x) for x in args.res.split('x')] + + log_level = logging.DEBUG if args.debug else logging.INFO + logging.basicConfig(format='%(levelname)s: %(message)s', level=log_level) + + logging.info('listening to server %s:%s', args.host, args.port) + + print(__doc__) + + signal.signal(signal.SIGINT, VehicleControl.signal_handler) + + try: + game_loop(args) + + except KeyboardInterrupt: + print('\nCancelled by user. Bye!') + + +if __name__ == '__main__': + + main() diff --git a/PythonAPI/examples/rss/rss_sensor.py b/PythonAPI/examples/rss/rss_sensor.py new file mode 100644 index 000000000..a44a38d4b --- /dev/null +++ b/PythonAPI/examples/rss/rss_sensor.py @@ -0,0 +1,402 @@ +#!/usr/bin/env python +# +# Copyright (c) 2020 Intel Corporation +# +import time +import carla +import inspect +import carla +import sys +import math +from rss_visualization import RssDebugDrawer + +if sys.version_info.major == 3: + import libad_physics_python3 as physics + import libad_rss_python3 as rss + import libad_map_access_python3 as admap + import libad_rss_map_integration_python3 as rssmap +else: + import libad_physics_python2 as physics + import libad_rss_python2 as rss + import libad_map_access_python2 as admap + import libad_rss_map_integration_python2 as rssmap + +# ============================================================================== +# -- RssSensor ----------------------------------------------------------------- +# ============================================================================== + + +class RssStateInfo(object): + def __init__(self, rss_state, ego_dynamics_on_route, actor_calculation_mode_map): + self.rss_state = rss_state + self.distance = -1 + self.is_safe = None + self.other_actor = None + + self.actor_calculation_mode = None + if rss_state.objectId in actor_calculation_mode_map: + self.actor_calculation_mode = actor_calculation_mode_map[rss_state.objectId][0] + + if self.actor_calculation_mode == rssmap.RssMode.Structured: + self.is_safe = rss_state.longitudinalState.isSafe or ( + rss_state.lateralStateLeft.isSafe and rss_state.lateralStateRight.isSafe) + elif self.actor_calculation_mode == rssmap.RssMode.Unstructured: + self.is_safe = rss_state.unstructuredSceneState.isSafe + + # calculate distance to other vehicle + self.other_actor = actor_calculation_mode_map[rss_state.objectId][1] + if self.other_actor: + self.distance = math.sqrt((float(ego_dynamics_on_route.ego_center.x) - self.other_actor.get_location().x)**2 + + (float(ego_dynamics_on_route.ego_center.y) + self.other_actor.get_location().y)**2) + + def __str__(self): + return "RssStateInfo: object=" + str(self.rss_state.objectId) + " is_safe=" + str(self.is_safe) + + +class RssSensor(object): + def __init__(self, parent_actor, world, unstructured_scene_drawer, bounding_box_drawer, routing_targets=None): + self.sensor = None + self.unstructured_scene_drawer = unstructured_scene_drawer + self.bounding_box_drawer = bounding_box_drawer + self._parent = parent_actor + self.timestamp = None + self.response_valid = False + self.proper_response = None + self.rss_state_snapshot = None + self.situation_snapshot = None + self.world_model = None + self.individual_rss_states = [] + self._allowed_heading_ranges = [] + self.ego_dynamics_on_route = None + self.current_vehicle_parameters = self.get_default_parameters() + self.actor_calculation_mode_map = dict() + self.route = None + self.debug_drawer = RssDebugDrawer(parent_actor, world) + self.change_to_unstructured_position_map = dict() + + # get max steering angle + physics_control = parent_actor.get_physics_control() + self._max_steer_angle = 0.0 + for wheel in physics_control.wheels: + if wheel.max_steer_angle > self._max_steer_angle: + self._max_steer_angle = wheel.max_steer_angle + self._max_steer_angle = math.radians(self._max_steer_angle) + + world = self._parent.get_world() + bp = world.get_blueprint_library().find('sensor.other.rss') + self.sensor = world.spawn_actor(bp, carla.Transform(carla.Location(x=0.0, z=0.0)), attach_to=self._parent) + # We need to pass the lambda a weak reference to self to avoid circular + # reference. + + def check_rss_class(clazz): + return inspect.isclass(clazz) and "RssSensor" in clazz.__name__ + + if not inspect.getmembers(carla, check_rss_class): + raise RuntimeError('CARLA PythonAPI not compiled in RSS variant, please "make PythonAPI.rss"') + + self.set_default_parameters() + + self.sensor.register_actor_constellation_callback( + lambda actor_constellation_data: self._on_actor_constellation_request(actor_constellation_data)) + + self.sensor.listen(lambda event: self._on_rss_response(event)) + + # only relevant if actor constellation callback is not registered + # self.sensor.ego_vehicle_dynamics = self.current_vehicle_parameters + + self.sensor.road_boundaries_mode = carla.RssRoadBoundariesMode.Off + + # self.sensor.set_log_level(carla.RssLogLevel.trace) + + self.sensor.reset_routing_targets() + if routing_targets: + for target in routing_targets: + self.sensor.append_routing_target(target) + + def _on_actor_constellation_request(self, actor_constellation_data): + #print("_on_actor_constellation_request: ", str(actor_constellation_data)) + + actor_constellation_result = carla.RssActorConstellationResult() + actor_constellation_result.rss_calculation_mode = rssmap.RssMode.NotRelevant + actor_constellation_result.restrict_speed_limit_mode = rssmap.RssSceneCreation.RestrictSpeedLimitMode.IncreasedSpeedLimit10 + actor_constellation_result.ego_vehicle_dynamics = self.current_vehicle_parameters + actor_constellation_result.actor_object_type = rss.ObjectType.Invalid + actor_constellation_result.actor_dynamics = self.current_vehicle_parameters + + actor_id = -1 + actor_type_id = "none" + if actor_constellation_data.other_actor != None: + actor_id = actor_constellation_data.other_actor.id + actor_type_id = actor_constellation_data.other_actor.type_id + + ego_on_the_sidewalk = False + ego_on_routeable_road = False + for occupied_region in actor_constellation_data.ego_match_object.mapMatchedBoundingBox.laneOccupiedRegions: + lane = admap.getLane(occupied_region.laneId) + if lane.type == admap.LaneType.PEDESTRIAN: + # if not ego_on_the_sidewalk: + # print ( "ego-{} on lane of lane type {} => sidewalk".format(actor_id, lane.type)) + ego_on_the_sidewalk = True + elif admap.isRouteable(lane): + # if not ego_on_routeable_road: + # print ( "ego-{} on lane of lane type {} => road".format(actor_id, lane.type)) + ego_on_routeable_road = True + + if 'walker.pedestrian' in actor_constellation_data.other_actor.type_id: + # determine if the pedestrian is walking on the sidewalk or on the road + pedestrian_on_the_road = False + pedestrian_on_the_sidewalk = False + for occupied_region in actor_constellation_data.other_match_object.mapMatchedBoundingBox.laneOccupiedRegions: + lane = admap.getLane(occupied_region.laneId) + if lane.type == admap.LaneType.PEDESTRIAN: + # if not pedestrian_on_the_sidewalk: + # print ( "pedestrian-{} on lane of lane type {} => sidewalk".format(actor_id, lane.type)) + pedestrian_on_the_sidewalk = True + else: + # if not pedestrian_on_the_road: + # print ( "pedestrian-{} on lane of lane type {} => road".format(actor_id, lane.type)) + pedestrian_on_the_road = True + if ego_on_routeable_road and not ego_on_the_sidewalk and not pedestrian_on_the_road and pedestrian_on_the_sidewalk: + # pedestrian is not on the road, but on the sidewalk: then common sense is that vehicle has priority + # This analysis can and should be done more detailed, but this is a basic starting point for the decision + # In addition, the road network has to be correct to work best + # (currently there are no sidewalks in intersection areas) + # print ( "pedestrian-{} Off".format(actor_id)) + actor_constellation_result.rss_calculation_mode = rssmap.RssMode.NotRelevant + else: + # print ( "pedestrian-{} Unstructured".format(actor_id)) + actor_constellation_result.rss_calculation_mode = rssmap.RssMode.Unstructured + actor_constellation_result.actor_object_type = rss.ObjectType.Pedestrian + actor_constellation_result.actor_dynamics = self.get_pedestrian_parameters() + elif 'vehicle' in actor_constellation_data.other_actor.type_id: + actor_constellation_result.actor_object_type = rss.ObjectType.OtherVehicle + + # set the response time of others vehicles to 2 seconds; the rest stays the same + actor_constellation_result.actor_dynamics.responseTime = 2.0 + + # per default, if ego is not on the road -> unstructured + if ego_on_routeable_road: + actor_constellation_result.rss_calculation_mode = rssmap.RssMode.Structured + else: + # print("vehicle-{} unstructured: reason other ego not on routeable road".format(actor_id)) + actor_constellation_result.rss_calculation_mode = rssmap.RssMode.Unstructured + + # special handling for vehicles standing still + actor_vel = actor_constellation_data.other_actor.get_velocity() + actor_speed = math.sqrt(actor_vel.x**2 + actor_vel.y**2 + actor_vel.z**2) + if actor_speed < 0.01: + # reduce response time + actor_constellation_result.actor_dynamics.responseTime = 1.0 + # still in structured? + if actor_constellation_result.rss_calculation_mode == rssmap.RssMode.Structured: + + actor_distance = math.sqrt(float(actor_constellation_data.ego_match_object.enuPosition.centerPoint.x - + actor_constellation_data.other_match_object.enuPosition.centerPoint.x)**2 + + float(actor_constellation_data.ego_match_object.enuPosition.centerPoint.y - + actor_constellation_data.other_match_object.enuPosition.centerPoint.y)**2) + # print("vehicle-{} unstructured check: other distance {}".format(actor_id, actor_distance)) + + if actor_constellation_data.ego_dynamics_on_route.ego_speed < 0.01: + # both vehicles stand still, so we have to analyze in detail if we possibly want to use + # unstructured mode to cope with blockades on the road... + + if actor_distance < 10: + # the other has to be near enough to trigger a switch to unstructured + other_outside_routeable_road = False + for occupied_region in actor_constellation_data.other_match_object.mapMatchedBoundingBox.laneOccupiedRegions: + lane = admap.getLane(occupied_region.laneId) + if not admap.isRouteable(lane): + other_outside_routeable_road = True + + if other_outside_routeable_road: + # if the other is somewhat outside the standard routeable road (e.g. parked at the side, ...) + # we immediately decide for unstructured + # print("vehicle-{} unstructured: reason other outside routeable road".format(actor_id)) + actor_constellation_result.rss_calculation_mode = rssmap.RssMode.Unstructured + else: + # otherwise we have to look in the orientation delta in addition to get some basic idea of the + # constellation (we don't want to go into unstructured if we both waiting behind a red light...) + heading_delta = abs(float(actor_constellation_data.ego_match_object.enuPosition.heading - + actor_constellation_data.other_match_object.enuPosition.heading)) + if heading_delta > 0.2: # around 11 degree + # print("vehicle-{} unstructured: reason heading delta {}".format(actor_id, heading_delta)) + actor_constellation_result.rss_calculation_mode = rssmap.RssMode.Unstructured + self.change_to_unstructured_position_map[actor_id] = actor_constellation_data.other_match_object.enuPosition + else: + # ego moves + if actor_distance < 10: + # if the ego moves, the other actor doesn't move an the mode was previously set to unstructured, keep it + try: + if self.change_to_unstructured_position_map[actor_id] == actor_constellation_data.other_match_object.enuPosition: + heading_delta = abs(float(actor_constellation_data.ego_match_object.enuPosition.heading - + actor_constellation_data.other_match_object.enuPosition.heading)) + if heading_delta > 0.2: + actor_constellation_result.rss_calculation_mode = rssmap.RssMode.Unstructured + else: + del self.change_to_unstructured_position_map[actor_id] + except (AttributeError, KeyError): + pass + else: + if actor_id in self.change_to_unstructured_position_map: + del self.change_to_unstructured_position_map[actor_id] + + # still in structured? + if actor_constellation_result.rss_calculation_mode == rssmap.RssMode.Structured: + # in structured case we have to cope with not yet implemented lateral intersection checks in core RSS implementation + # if the other is standing still, we don't assume that he will accelerate + # otherwise if standing at the intersection the acceleration within reaction time + # will allow to enter the intersection which current RSS implementation will immediately consider + # as dangerous + # print("_on_actor_constellation_result({}) setting accelMax to zero".format(actor_constellation_data.other_actor.id)) + actor_constellation_result.actor_dynamics.alphaLon.accelMax = 0. + # store values for visualization + self.actor_calculation_mode_map[actor_id] = ( + actor_constellation_result.rss_calculation_mode, actor_constellation_data.other_actor) + else: + # store route for debug drawings + self.route = actor_constellation_data.ego_route + # since the ego vehicle is controlled manually, it is easy possible that the ego vehicle + # accelerates far more in lateral direction than the ego_dynamics indicate + # in an automated vehicle this would be considered by the low-level controller when the RSS restriction + # is taken into account properly + # but the simple RSS restrictor within CARLA is not able to do so... + # So we should at least tell RSS about the fact that we acceleration more than this + # to be able to react on this + abs_avg_route_accel_lat = abs(float(actor_constellation_data.ego_dynamics_on_route.avg_route_accel_lat)) + if abs_avg_route_accel_lat > actor_constellation_result.ego_vehicle_dynamics.alphaLat.accelMax: + # print("!! Route lateral dynamics exceed expectations: route:{} expected:{} !!".format(abs_avg_route_accel_lat, + # actor_constellation_result.ego_vehicle_dynamics.alphaLat.accelMax)) + actor_constellation_result.ego_vehicle_dynamics.alphaLat.accelMax = min(20., abs_avg_route_accel_lat) + + # print("_on_actor_constellation_result({}-{}): ".format(actor_id, actor_type_id), str(actor_constellation_result)) + return actor_constellation_result + + def destroy(self): + if self.sensor: + print("Stopping RSS sensor") + self.sensor.stop() + print("Deleting Scene Drawer") + self.unstructured_scene_drawer = None + print("Destroying RSS sensor") + self.sensor.destroy() + print("Destroyed RSS sensor") + + def toggle_debug_visualization_mode(self): + self.debug_drawer.toggleMode() + + def get_assertive_parameters(self): + ego_dynamics = rss.RssDynamics() + ego_dynamics.alphaLon.accelMax = 4.1 + ego_dynamics.alphaLon.brakeMax = -8.03 + ego_dynamics.alphaLon.brakeMin = -4.64 + ego_dynamics.alphaLon.brakeMinCorrect = -1.76 + ego_dynamics.alphaLat.accelMax = 0.43 + ego_dynamics.alphaLat.brakeMin = -0.96 + ego_dynamics.lateralFluctuationMargin = 0.07 + ego_dynamics.responseTime = 0.25 # paper: 0.53 + ego_dynamics.maxSpeedOnAcceleration = 100 + ego_dynamics.unstructuredSettings.pedestrianTurningRadius = 2.0 + ego_dynamics.unstructuredSettings.driveAwayMaxAngle = 2.4 + ego_dynamics.unstructuredSettings.vehicleYawRateChange = 0.3 + ego_dynamics.unstructuredSettings.vehicleMinRadius = 3.5 + ego_dynamics.unstructuredSettings.vehicleTrajectoryCalculationStep = 0.2 + return ego_dynamics + + def set_assertive_parameters(self): + print("Use 'assertive' RSS Parameters") + self.current_vehicle_parameters = self.get_assertive_parameters() + + def get_default_parameters(self): + ego_dynamics = rss.RssDynamics() + ego_dynamics.alphaLon.accelMax = 3.5 + ego_dynamics.alphaLon.brakeMax = -8 + ego_dynamics.alphaLon.brakeMin = -4 + ego_dynamics.alphaLon.brakeMinCorrect = -3 + ego_dynamics.alphaLat.accelMax = 0.2 + ego_dynamics.alphaLat.brakeMin = -0.8 + ego_dynamics.lateralFluctuationMargin = 0.1 + ego_dynamics.responseTime = 1.0 + ego_dynamics.maxSpeedOnAcceleration = 100 + ego_dynamics.unstructuredSettings.pedestrianTurningRadius = 2.0 + ego_dynamics.unstructuredSettings.driveAwayMaxAngle = 2.4 + ego_dynamics.unstructuredSettings.vehicleYawRateChange = 0.3 + ego_dynamics.unstructuredSettings.vehicleMinRadius = 3.5 + ego_dynamics.unstructuredSettings.vehicleTrajectoryCalculationStep = 0.2 + return ego_dynamics + + def set_default_parameters(self): + print("Use 'default' RSS Parameters") + self.current_vehicle_parameters = self.get_default_parameters() + + def get_pedestrian_parameters(self): + pedestrian_dynamics = rss.RssDynamics() + pedestrian_dynamics.alphaLon.accelMax = 2.0 + pedestrian_dynamics.alphaLon.brakeMax = -4.0 + pedestrian_dynamics.alphaLon.brakeMin = -2.0 + pedestrian_dynamics.alphaLon.brakeMinCorrect = -2.0 + pedestrian_dynamics.alphaLat.accelMax = 0.001 + pedestrian_dynamics.alphaLat.brakeMin = -0.001 + pedestrian_dynamics.lateralFluctuationMargin = 0.1 + pedestrian_dynamics.responseTime = 0.5 + pedestrian_dynamics.maxSpeedOnAcceleration = 10 + pedestrian_dynamics.unstructuredSettings.pedestrianTurningRadius = 2.0 + pedestrian_dynamics.unstructuredSettings.driveAwayMaxAngle = 2.4 + pedestrian_dynamics.unstructuredSettings.vehicleYawRateChange = 0.3 + pedestrian_dynamics.unstructuredSettings.vehicleMinRadius = 3.5 + pedestrian_dynamics.unstructuredSettings.vehicleTrajectoryCalculationStep = 0.2 + return pedestrian_dynamics + + def get_steering_ranges(self): + ranges = [] + for range in self._allowed_heading_ranges: + ranges.append( + ( + (float(self.ego_dynamics_on_route.ego_heading) - float(range.begin)) / self._max_steer_angle, + (float(self.ego_dynamics_on_route.ego_heading) - float(range.end)) / self._max_steer_angle) + ) + return ranges + + def _on_rss_response(self, response): + if not self or not response: + return + delta_time = 0.1 + if self.timestamp: + delta_time = response.timestamp - self.timestamp + if delta_time > -0.05: + self.timestamp = response.timestamp + self.response_valid = response.response_valid + self.proper_response = response.proper_response + self.ego_dynamics_on_route = response.ego_dynamics_on_route + self.rss_state_snapshot = response.rss_state_snapshot + self.situation_snapshot = response.situation_snapshot + self.world_model = response.world_model + + new_states = [] + for rss_state in response.rss_state_snapshot.individualResponses: + new_states.append(RssStateInfo(rss_state, response.ego_dynamics_on_route, + self.actor_calculation_mode_map)) + if len(new_states) > 0: + new_states.sort(key=lambda rss_states: rss_states.distance) + self.individual_rss_states = new_states + + # calculate the allowed heading ranges: + if response.proper_response.headingRanges: + heading = float(response.ego_dynamics_on_route.ego_heading) + heading_ranges = response.proper_response.headingRanges + steering_range = rss.HeadingRange() + steering_range.begin = - self._max_steer_angle + heading + steering_range.end = self._max_steer_angle + heading + rss.getHeadingOverlap(steering_range, heading_ranges) + self._allowed_heading_ranges = heading_ranges + else: + self._allowed_heading_ranges = [] + if self.unstructured_scene_drawer: + self.unstructured_scene_drawer.tick(response.frame, response, self._allowed_heading_ranges) + if self.bounding_box_drawer: + self.bounding_box_drawer.tick(response.frame, self.individual_rss_states) + self.debug_drawer.tick(self.route, not response.proper_response.isSafe, + self.individual_rss_states, self.ego_dynamics_on_route) + + self.actor_calculation_mode_map = dict() + else: + print("ignore outdated response {}".format(delta_time)) diff --git a/PythonAPI/examples/rss/rss_visualization.py b/PythonAPI/examples/rss/rss_visualization.py new file mode 100644 index 000000000..6ab294857 --- /dev/null +++ b/PythonAPI/examples/rss/rss_visualization.py @@ -0,0 +1,701 @@ +#!/usr/bin/env python +# +# Copyright (c) 2020 Intel Corporation +# + +import sys +from enum import Enum +import numpy as np +import pygame +import weakref +import carla + +if sys.version_info.major == 3: + import libad_rss_python3 as rss + import libad_rss_map_integration_python3 as rssmap +else: + import libad_rss_python2 as rss + import libad_rss_map_integration_python2 as rssmap + + +def render_rss_states(display, v_offset, font, individual_rss_states): + if individual_rss_states: + surface = font.render('RSS States:', True, (255, 255, 255)) + display.blit(surface, (8, v_offset)) + v_offset += 26 + for state in individual_rss_states: + object_name = "Obj" + if state.rss_state.objectId == 18446744073709551614: + object_name = "Border Left" + elif state.rss_state.objectId == 18446744073709551615: + object_name = "Border Right" + else: + if state.other_actor: + li = list(state.other_actor.type_id.split(".")) + if li: + li.pop(0) + li = [element.capitalize() for element in li] + + object_name = " ".join(li).strip()[:18] + + mode = "?" + if state.actor_calculation_mode == rssmap.RssMode.Structured: + mode = "S" + elif state.actor_calculation_mode == rssmap.RssMode.Unstructured: + mode = "U" + elif state.actor_calculation_mode == rssmap.RssMode.NotRelevant: + mode = "-" + item = '%4s % 2dm %8s' % (mode, state.distance, object_name) + + surface = font.render(item, True, (255, 255, 255)) + display.blit(surface, (5, v_offset)) + color = (128, 128, 128) + if state.actor_calculation_mode != rssmap.RssMode.NotRelevant: + if state.is_safe: + color = (0, 255, 0) + else: + color = (255, 0, 0) + pygame.draw.circle(display, color, (12, v_offset+7), 5) + xpos = 184 + if state.actor_calculation_mode == rssmap.RssMode.Structured: + if not state.rss_state.longitudinalState.isSafe and ((state.rss_state.longitudinalState.rssStateInformation.evaluator == "LongitudinalDistanceSameDirectionOtherInFront") or (state.rss_state.longitudinalState.rssStateInformation.evaluator == "LongitudinalDistanceSameDirectionEgoFront")): + pygame.draw.polygon(display, (255, 255, 255), ((xpos+1, v_offset+1+4), (xpos+6, v_offset+1+0), (xpos+11, v_offset+1+4), + (xpos+7, v_offset+1+4), (xpos+7, v_offset+1+12), (xpos+5, v_offset+1+12), (xpos+5, v_offset+1+4))) + xpos += 14 + + if not state.rss_state.longitudinalState.isSafe and ((state.rss_state.longitudinalState.rssStateInformation.evaluator == "LongitudinalDistanceOppositeDirectionEgoCorrectLane") or (state.rss_state.longitudinalState.rssStateInformation.evaluator == "LongitudinalDistanceOppositeDirection")): + pygame.draw.polygon(display, (255, 255, 255), ((xpos+2, v_offset+1+8), (xpos+6, v_offset+1+12), (xpos+10, v_offset+1+8), + (xpos+7, v_offset+1+8), (xpos+7, v_offset+1+0), (xpos+5, v_offset+1+0), (xpos+5, v_offset+1+8))) + xpos += 14 + + if not state.rss_state.lateralStateRight.isSafe and not (state.rss_state.lateralStateRight.rssStateInformation.evaluator == "None"): + pygame.draw.polygon(display, (255, 255, 255), ((xpos+0, v_offset+1+4), (xpos+8, v_offset+1+4), (xpos+8, v_offset+1+1), + (xpos+12, v_offset+1+6), (xpos+8, v_offset+1+10), (xpos+8, v_offset+1+8), (xpos+0, v_offset+1+8))) + xpos += 14 + if not state.rss_state.lateralStateLeft.isSafe and not (state.rss_state.lateralStateLeft.rssStateInformation.evaluator == "None"): + pygame.draw.polygon(display, (255, 255, 255), ((xpos+0, v_offset+1+6), (xpos+4, v_offset+1+1), (xpos+4, v_offset+1+4), + (xpos+12, v_offset+1+4), (xpos+12, v_offset+1+8), (xpos+4, v_offset+1+8), (xpos+4, v_offset+1+10))) + xpos += 14 + elif state.actor_calculation_mode == rssmap.RssMode.Unstructured: + text = "" + if state.rss_state.unstructuredSceneState.response == rss.UnstructuredSceneResponse.DriveAway: + text = "D" + elif state.rss_state.unstructuredSceneState.response == rss.UnstructuredSceneResponse.ContinueForward: + text = "C" + elif state.rss_state.unstructuredSceneState.response == rss.UnstructuredSceneResponse.Brake: + text = "B" + surface = font.render(text, True, (255, 255, 255)) + display.blit(surface, (xpos, v_offset)) + + v_offset += 14 + + +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 + +# ============================================================================== +# -- RssUnstructuredSceneDrawer ------------------------------------------------ +# ============================================================================== + + +class RssUnstructuredSceneDrawerMode(Enum): + disabled = 1 + window = 2 + fullscreen = 3 + + +class RssUnstructuredSceneDrawer(object): + + def __init__(self, parent_actor, world, display_dimensions): + self._last_rendered_frame = -1 + self._surface = None + self._current_rss_surface = None + self._current_camera_surface = None + self._world = world + self._parent_actor = parent_actor + self._display_dimensions = display_dimensions + self._camera = None + self._mode = RssUnstructuredSceneDrawerMode.disabled + + self.restart(RssUnstructuredSceneDrawerMode.window) + + def destroy(self): + if self._camera: + self._camera.stop() + self._camera.destroy() + self._camera = None + + def restart(self, mode): + # setup up top down camera + self.destroy() + self._mode = mode + + spawn_sensor = False + if mode == RssUnstructuredSceneDrawerMode.window: + self._dim = (self._display_dimensions[0]/3, self._display_dimensions[1]/2) + spawn_sensor = True + elif mode == RssUnstructuredSceneDrawerMode.fullscreen: + self._dim = (self._display_dimensions[0], self._display_dimensions[1]) + spawn_sensor = True + else: + self._surface = None + + self._calibration = np.identity(3) + self._calibration[0, 2] = self._dim[0] / 2.0 + self._calibration[1, 2] = self._dim[1] / 2.0 + self._calibration[0, 0] = self._calibration[1, 1] = self._dim[0] / \ + (2.0 * np.tan(90.0 * np.pi / 360.0)) # fov default: 90.0 + + if spawn_sensor: + bp_library = self._world.get_blueprint_library() + bp = bp_library.find('sensor.camera.rgb') + bp.set_attribute('image_size_x', str(self._dim[0])) + bp.set_attribute('image_size_y', str(self._dim[1])) + + self._camera = self._world.spawn_actor( + bp, + carla.Transform(carla.Location(x=7.5, z=10), carla.Rotation(pitch=-90)), + attach_to=self._parent_actor) + # We need to pass the lambda a weak reference to self to avoid + # circular reference. + weak_self = weakref.ref(self) + self._camera.listen(lambda image: self._parse_image(weak_self, image)) + + def update_surface(self, cam_frame, rss_frame): + if self._mode == RssUnstructuredSceneDrawerMode.disabled: + return + render = False + + if cam_frame and self._current_rss_surface and self._current_rss_surface[0] == cam_frame: + render = True + + if rss_frame and self._current_camera_surface and self._current_camera_surface[0] == rss_frame: + render = True + + if render: + surface = self._current_camera_surface[1] + surface.blit(self._current_rss_surface[1], (0, 0)) + rect = pygame.Rect((0, 0), (2, surface.get_height())) + pygame.draw.rect(surface, (0, 0, 0), rect, 0) + rect = pygame.Rect((0, 0), (surface.get_width(), 2)) + pygame.draw.rect(surface, (0, 0, 0), rect, 0) + rect = pygame.Rect((0, surface.get_height()-2), (surface.get_width(), surface.get_height())) + pygame.draw.rect(surface, (0, 0, 0), rect, 0) + rect = pygame.Rect((surface.get_width()-2, 0), (surface.get_width(), surface.get_width())) + pygame.draw.rect(surface, (0, 0, 0), rect, 0) + self._surface = surface + + def toggle_camera(self): + print("Toggle RssUnstructuredSceneDrawer") + if self._mode == RssUnstructuredSceneDrawerMode.window: + self.restart(RssUnstructuredSceneDrawerMode.fullscreen) + elif self._mode == RssUnstructuredSceneDrawerMode.fullscreen: + self.restart(RssUnstructuredSceneDrawerMode.disabled) + elif self._mode == RssUnstructuredSceneDrawerMode.disabled: + self.restart(RssUnstructuredSceneDrawerMode.window) + + @staticmethod + def _parse_image(weak_self, image): + self = weak_self() + if not self: + return + image.convert(carla.ColorConverter.Raw) + array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) + array = np.reshape(array, (image.height, image.width, 4)) + array = array[:, :, :3] + array = array[:, :, ::-1] + surface = pygame.surfarray.make_surface(array.swapaxes(0, 1)) + self._current_camera_surface = (image.frame, surface) + self.update_surface(image.frame, None) + + @staticmethod + def rotate_around_point(xy, radians, origin): + """Rotate a point around a given point. + """ + x, y = xy + offset_x, offset_y = origin + adjusted_x = (x - offset_x) + adjusted_y = (y - offset_y) + cos_rad = math.cos(radians) + sin_rad = math.sin(radians) + qx = offset_x + cos_rad * adjusted_x - sin_rad * adjusted_y + qy = offset_y + sin_rad * adjusted_x + cos_rad * adjusted_y + + return qx, qy + + def tick(self, frame, rss_response, allowed_heading_ranges): + if not self._camera: + return + surface = pygame.Surface(self._dim) + surface.set_colorkey(pygame.Color('black')) + surface.set_alpha(180) + try: + lines = RssUnstructuredSceneDrawer.get_trajectory_sets( + rss_response.rss_state_snapshot, self._camera.get_transform(), self._calibration) + + polygons = [] + for heading_range in allowed_heading_ranges: + polygons.append((RssUnstructuredSceneDrawer.transform_points( + RssUnstructuredSceneDrawer._get_points_from_pairs( + RssUnstructuredSceneDrawer.draw_heading_range(heading_range, rss_response.ego_dynamics_on_route)), + self._camera.get_transform(), self._calibration), (0, 0, 255))) + + RssUnstructuredSceneDrawer.draw_lines(surface, lines) + RssUnstructuredSceneDrawer.draw_polygons(surface, polygons) + + except RuntimeError as e: + print("ERROR {}".format(e)) + self._current_rss_surface = (frame, surface) + self.update_surface(None, frame) + + def render(self, display): + if self._surface: + display.blit(self._surface, (display.get_width() - self._dim[0], 0)) + + @staticmethod + def draw_heading_range(heading_range, ego_dynamics_on_route): + line = [(float(ego_dynamics_on_route.ego_center.x), float(ego_dynamics_on_route.ego_center.y))] + length = 3.0 + current_angle = float(heading_range.begin) + max_angle = float(heading_range.end) + if heading_range.end < heading_range.begin: + max_angle += 2.0 * np.pi + + while current_angle < max_angle: + line.append((float(ego_dynamics_on_route.ego_center.x) + length * np.cos(current_angle), + float(ego_dynamics_on_route.ego_center.y) + length * np.sin(current_angle))) + current_angle += 0.2 + + if current_angle != max_angle: + line.append((float(ego_dynamics_on_route.ego_center.x) + length * np.cos(max_angle), + float(ego_dynamics_on_route.ego_center.y) + length * np.sin(max_angle))) + + line.append((float(ego_dynamics_on_route.ego_center.x), float(ego_dynamics_on_route.ego_center.y))) + return line + + @staticmethod + def get_trajectory_sets(rss_state_snapshot, camera_transform, calibration): + """ + Creates 3D bounding boxes based on carla vehicle list and camera. + """ + trajectory_sets = [] + + # ego + trajectory_sets.append((RssUnstructuredSceneDrawer.transform_points(RssUnstructuredSceneDrawer._get_trajectory_set_points( + rss_state_snapshot.unstructuredSceneEgoInformation.brakeTrajectorySet), camera_transform, calibration), (255, 0, 0))) + trajectory_sets.append((RssUnstructuredSceneDrawer.transform_points(RssUnstructuredSceneDrawer._get_trajectory_set_points( + rss_state_snapshot.unstructuredSceneEgoInformation.continueForwardTrajectorySet), camera_transform, calibration), (0, 255, 0))) + + # others + for state in rss_state_snapshot.individualResponses: + if state.unstructuredSceneState.rssStateInformation.brakeTrajectorySet: + trajectory_sets.append((RssUnstructuredSceneDrawer.transform_points(RssUnstructuredSceneDrawer._get_trajectory_set_points( + state.unstructuredSceneState.rssStateInformation.brakeTrajectorySet), camera_transform, calibration), (255, 0, 0))) + if state.unstructuredSceneState.rssStateInformation.continueForwardTrajectorySet: + trajectory_sets.append((RssUnstructuredSceneDrawer.transform_points(RssUnstructuredSceneDrawer._get_trajectory_set_points( + state.unstructuredSceneState.rssStateInformation.continueForwardTrajectorySet), camera_transform, calibration), (0, 255, 0))) + + return trajectory_sets + + @staticmethod + def draw_lines(surface, lines): + """ + Draws lines on pygame display. + """ + for line, color in lines: + if len(line) > 1: + pygame.draw.lines(surface, color, True, line, 2) + + @staticmethod + def draw_polygons(surface, polygons): + """ + Draws polygons on pygame display. + """ + for polygon, color in polygons: + if len(polygon) > 1: + pygame.draw.polygon(surface, color, polygon) + + @staticmethod + def transform_points(world_cords, camera_transform, calibration): + """ + Returns trajectory set projected to camera view + """ + world_cords = np.transpose(world_cords) + cords_x_y_z = RssUnstructuredSceneDrawer._world_to_sensor(world_cords, camera_transform)[:3, :] + cords_y_minus_z_x = np.concatenate([cords_x_y_z[1, :], -cords_x_y_z[2, :], cords_x_y_z[0, :]]) + ts = np.transpose(np.dot(calibration, cords_y_minus_z_x)) + camera_ts = np.concatenate([ts[:, 0] / ts[:, 2], ts[:, 1] / ts[:, 2], ts[:, 2]], axis=1) + line_to_draw = [] + for point in camera_ts: + line_to_draw.append((int(point[0, 0]), int(point[0, 1]))) + return line_to_draw + + @staticmethod + def _get_trajectory_set_points(trajectory_set): + """ + """ + cords = np.zeros((len(trajectory_set), 4)) + i = 0 + for pt in trajectory_set: + cords[i, :] = np.array([pt.x, -pt.y, 0, 1]) + i += 1 + return cords + + @staticmethod + def _get_points_from_pairs(trajectory_set): + """ + """ + cords = np.zeros((len(trajectory_set), 4)) + i = 0 + for pt in trajectory_set: + cords[i, :] = np.array([pt[0], -pt[1], 0, 1]) + i += 1 + return cords + + @staticmethod + def _world_to_sensor(cords, camera_transform): + """ + Transforms world coordinates to sensor. + """ + sensor_world_matrix = get_matrix(camera_transform) + world_sensor_matrix = np.linalg.inv(sensor_world_matrix) + sensor_cords = np.dot(world_sensor_matrix, cords) + return sensor_cords + +# ============================================================================== +# -- RssBoundingBoxDrawer ------------------------------------------------------ +# ============================================================================== + + +class RssBoundingBoxDrawer(object): + + def __init__(self, display_dimensions, world, camera): + self._last_camera_frame = 0 + self._surface_for_frame = [] + self._world = world + self._dim = display_dimensions + self._calibration = np.identity(3) + self._calibration[0, 2] = self._dim[0] / 2.0 + self._calibration[1, 2] = self._dim[1] / 2.0 + self._calibration[0, 0] = self._calibration[1, 1] = self._dim[0] / \ + (2.0 * np.tan(90.0 * np.pi / 360.0)) # fov default: 90.0 + self._camera = camera + + def tick(self, frame, individual_rss_states): + if len(self._surface_for_frame) > 0: + try: + while self._surface_for_frame[0][0] < self._last_camera_frame: + self._surface_for_frame.pop(0) + except IndexError: + return + + # only render on new frame + if len(self._surface_for_frame) > 0: + if self._surface_for_frame[0][0] == frame: + return + + surface = pygame.Surface(self._dim) + surface.set_colorkey(pygame.Color('black')) + surface.set_alpha(80) + try: + bounding_boxes = RssBoundingBoxDrawer.get_bounding_boxes( + individual_rss_states, self._camera.get_transform(), self._calibration) + RssBoundingBoxDrawer.draw_bounding_boxes(surface, bounding_boxes) + except RuntimeError: + pass + self._surface_for_frame.append((frame, surface)) + + def render(self, display, current_camera_frame): + rendered = False + for frame, surface in self._surface_for_frame: + if frame == current_camera_frame: + display.blit(surface, (0, 0)) + rendered = True + break + if not rendered: + print("Surface not rendered for frame {}".format(current_camera_frame)) + self._last_camera_frame = current_camera_frame + + @staticmethod + def get_bounding_boxes(individual_rss_states, camera_transform, calibration): + """ + Creates 3D bounding boxes based on carla vehicle list and camera. + """ + bounding_boxes = [] + for state in individual_rss_states: + if state.actor_calculation_mode != rssmap.RssMode.NotRelevant and not state.is_safe and state.other_actor: + bounding_boxes.append(RssBoundingBoxDrawer.get_bounding_box( + state.other_actor, camera_transform, calibration)) + # 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(surface, bounding_boxes, color=pygame.Color('red')): + """ + Draws bounding boxes on pygame display. + """ + for bbox in bounding_boxes: + points = [(int(bbox[i, 0]), int(bbox[i, 1])) for i in range(8)] + # draw lines + # base + polygon = [points[0], points[1], points[2], points[3]] + pygame.draw.polygon(surface, color, polygon) + # top + polygon = [points[4], points[5], points[6], points[7]] + pygame.draw.polygon(surface, color, polygon) + # base-top + polygon = [points[0], points[1], points[5], points[4]] + pygame.draw.polygon(surface, color, polygon) + polygon = [points[1], points[2], points[6], points[5]] + pygame.draw.polygon(surface, color, polygon) + polygon = [points[2], points[6], points[7], points[3]] + pygame.draw.polygon(surface, color, polygon) + polygon = [points[0], points[4], points[7], points[3]] + pygame.draw.polygon(surface, color, polygon) + + @staticmethod + def get_bounding_box(vehicle, camera_transform, calibration): + """ + Returns 3D bounding box for a vehicle based on camera view. + """ + + bb_cords = RssBoundingBoxDrawer._create_bb_points(vehicle) + cords_x_y_z = RssBoundingBoxDrawer._vehicle_to_sensor(bb_cords, vehicle, camera_transform)[: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(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, camera_transform): + """ + Transforms coordinates of a vehicle bounding box to sensor. + """ + + world_cord = RssBoundingBoxDrawer._vehicle_to_world(cords, vehicle) + sensor_cord = RssBoundingBoxDrawer._world_to_sensor(world_cord, camera_transform) + 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 = get_matrix(bb_transform) + vehicle_world_matrix = 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, camera_transform): + """ + Transforms world coordinates to sensor. + """ + + sensor_world_matrix = get_matrix(camera_transform) + world_sensor_matrix = np.linalg.inv(sensor_world_matrix) + sensor_cords = np.dot(world_sensor_matrix, cords) + return sensor_cords + +# ============================================================================== +# -- RssDebugDrawer ------------------------------------------------------------ +# ============================================================================== + + +class RssDebugVisualizationMode(Enum): + Off = 1 + RouteOnly = 2 + VehicleStateOnly = 3 + VehicleStateAndRoute = 4 + All = 5 + + +class RssDebugDrawer(object): + + def __init__(self, player, world): + self._world = world + self._player = player + self._visualization_mode = RssDebugVisualizationMode.Off + + def toggleMode(self): + if self._visualization_mode == RssDebugVisualizationMode.All: + self._visualization_mode = RssDebugVisualizationMode.Off + elif self._visualization_mode == RssDebugVisualizationMode.Off: + self._visualization_mode = RssDebugVisualizationMode.RouteOnly + elif self._visualization_mode == RssDebugVisualizationMode.RouteOnly: + self._visualization_mode = RssDebugVisualizationMode.VehicleStateOnly + elif self._visualization_mode == RssDebugVisualizationMode.VehicleStateOnly: + self._visualization_mode = RssDebugVisualizationMode.VehicleStateAndRoute + elif self._visualization_mode == RssDebugVisualizationMode.VehicleStateAndRoute: + self._visualization_mode = RssDebugVisualizationMode.All + print("New Debug Drawer Mode {}".format(self._visualization_mode)) + + def tick(self, route, dangerous, individual_rss_states, ego_dynamics_on_route): + if self._visualization_mode == RssDebugVisualizationMode.RouteOnly or \ + self._visualization_mode == RssDebugVisualizationMode.VehicleStateAndRoute or \ + self._visualization_mode == RssDebugVisualizationMode.All: + self.visualize_route(dangerous, route) + + if self._visualization_mode == RssDebugVisualizationMode.VehicleStateOnly or \ + self._visualization_mode == RssDebugVisualizationMode.VehicleStateAndRoute or \ + self._visualization_mode == RssDebugVisualizationMode.All: + self.visualize_rss_results(individual_rss_states) + + if self._visualization_mode == RssDebugVisualizationMode.All: + self.visualize_ego_dynamics(ego_dynamics_on_route) + + def visualize_route(self, dangerous, route): + if not route: + return + right_lane_edges = dict() + left_lane_edges = dict() + + for road_segment in route.roadSegments: + right_most_lane = road_segment.drivableLaneSegments[0] + if right_most_lane.laneInterval.laneId not in right_lane_edges: + edge = admap.getRightProjectedENUEdge(right_most_lane.laneInterval) + right_lane_edges[right_most_lane.laneInterval.laneId] = edge + intersection_lane = admap.Intersection.isLanePartOfAnIntersection(right_most_lane.laneInterval.laneId) + + color = carla.Color(r=(128 if dangerous else 255)) + if intersection_lane: + color.b = 128 if dangerous else 255 + color = carla.Color(r=255, g=0, b=255) + self.visualize_enu_edge(edge, color, self._player.get_location().z) + + left_most_lane = road_segment.drivableLaneSegments[-1] + if left_most_lane.laneInterval.laneId not in left_lane_edges: + edge = admap.getLeftProjectedENUEdge(left_most_lane.laneInterval) + left_lane_edges[left_most_lane.laneInterval.laneId] = edge + intersection_lane = admap.Intersection.isLanePartOfAnIntersection(left_most_lane.laneInterval.laneId) + color = carla.Color(g=(128 if dangerous else 255)) + if intersection_lane: + color.b = 128 if dangerous else 255 + + self.visualize_enu_edge(edge, color, self._player.get_location().z) + + def visualize_enu_edge(self, edge, color, z_offset): + for point in edge: + carla_point = carla.Location(x=float(point.x), y=float(-1 * point.y), z=float(point.z + z_offset)) + self._world.debug.draw_point(carla_point, 0.1, color, 0.1, False) + + def visualize_rss_results(self, state_snapshot): + for state in state_snapshot: + if not state.other_actor: + print("Actor not found. Skip visualizing state {}".format(state)) + continue + ego_point = self._player.get_location() + ego_point.z += 0.05 + yaw = self._player.get_transform().rotation.yaw + cosine = math.cos(math.radians(yaw)) + sine = math.sin(math.radians(yaw)) + line_offset = carla.Location(-sine * 0.1, cosine * 0.1, 0.0) + + point = state.other_actor.get_location() + point.z += 0.05 + indicator_color = carla.Color(0, 255, 0) + dangerous = rss.isDangerous(state.rss_state) + if dangerous: + indicator_color = carla.Color(255, 0, 0) + elif state.rss_state.situationType == rss.SituationType.NotRelevant: + indicator_color = carla.Color(150, 150, 150) + + # if (self._visualization_mode == VisualizationMode::All) { + if True: + # the connection lines are only visualized if All is requested + lon_color = indicator_color + lat_l_color = indicator_color + lat_r_color = indicator_color + if not state.rss_state.longitudinalState.isSafe: + lon_color.r = 255 + lon_color.g = 0 if dangerous else 255 + if not state.rss_state.lateralStateLeft.isSafe: + lat_l_color.r = 255 + lat_l_color.g = 0 if dangerous else 255 + if not state.rss_state.lateralStateRight.isSafe: + lat_r_color.r = 255 + lat_r_color.g = 0 if dangerous else 255 + self._world.debug.draw_line(ego_point, point, 0.1, lon_color, 0.02, False) + self._world.debug.draw_line(ego_point - line_offset, point - + line_offset, 0.1, lat_l_color, 0.02, False) + self._world.debug.draw_line(ego_point + line_offset, point + + line_offset, 0.1, lat_r_color, 0.02, False) + point.z += 3. + self._world.debug.draw_point(point, 0.2, indicator_color, 0.02, False) + + def visualize_ego_dynamics(self, ego_dynamics_on_route): + color = carla.Color(0, 0, 255) + + sin_heading = math.sin(float(ego_dynamics_on_route.route_heading)) + cos_heading = math.cos(float(ego_dynamics_on_route.route_heading)) + + heading_location_start = self._player.get_location() + heading_location_start.x -= cos_heading * 10. + heading_location_start.y += sin_heading * 10. + heading_location_start.z += 0.5 + heading_location_end = self._player.get_location() + heading_location_end.x += cos_heading * 10. + heading_location_end.y -= sin_heading * 10. + heading_location_end.z += 0.5 + + self._world.debug.draw_arrow(heading_location_start, heading_location_end, 0.1, 0.1, color, 0.02, False) + + sin_center = math.sin(float(ego_dynamics_on_route.route_heading) + math.pi/2.) + cos_center = math.cos(float(ego_dynamics_on_route.route_heading) + math.pi/2.) + center_location_start = self._player.get_location() + center_location_start.x -= cos_center * 2. + center_location_start.y += sin_center * 2. + center_location_start.z += 0.5 + center_location_end = self._player.get_location() + center_location_end.x += cos_center * 2. + center_location_end.y -= sin_center * 2. + center_location_end.z += 0.5 + + self._world.debug.draw_line(center_location_start, center_location_end, 0.1, color, 0.02, False) diff --git a/PythonAPI/examples/rss/scenarios/FollowLeadingVehicle.xosc b/PythonAPI/examples/rss/scenarios/FollowLeadingVehicle.xosc new file mode 100644 index 000000000..ab1231662 --- /dev/null +++ b/PythonAPI/examples/rss/scenarios/FollowLeadingVehicle.xosc @@ -0,0 +1,218 @@ + + + + + + + + + + + + + + + + + + +
+ + + + + + + + + + + + + + + + + +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/PythonAPI/examples/rss/scenarios/unstructured_blocked_road.xosc b/PythonAPI/examples/rss/scenarios/unstructured_blocked_road.xosc new file mode 100644 index 000000000..92b489940 --- /dev/null +++ b/PythonAPI/examples/rss/scenarios/unstructured_blocked_road.xosc @@ -0,0 +1,187 @@ + + + + + + + + + + + + + + + + + + +
+ + + + + + + + + + + + + + + + + +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/PythonAPI/examples/rss/scenarios/unstructured_pedestrian_crossing.xosc b/PythonAPI/examples/rss/scenarios/unstructured_pedestrian_crossing.xosc new file mode 100644 index 000000000..c5b2a37a6 --- /dev/null +++ b/PythonAPI/examples/rss/scenarios/unstructured_pedestrian_crossing.xosc @@ -0,0 +1,196 @@ + + + + + + + + + + + + + + + +
+ + + + + + + + + + + + + + + + +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/PythonAPI/examples/rss/scenarios/unstructured_pedestrian_on_road.xosc b/PythonAPI/examples/rss/scenarios/unstructured_pedestrian_on_road.xosc new file mode 100644 index 000000000..40230351a --- /dev/null +++ b/PythonAPI/examples/rss/scenarios/unstructured_pedestrian_on_road.xosc @@ -0,0 +1,223 @@ + + + + + + + + + + + + + + + +
+ + + + + + + + + + + + + + + + +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Util/BuildTools/Ad-rss.sh b/Util/BuildTools/Ad-rss.sh index c5e33bb0a..3e5ad517c 100755 --- a/Util/BuildTools/Ad-rss.sh +++ b/Util/BuildTools/Ad-rss.sh @@ -3,17 +3,13 @@ # ============================================================================== # -- Set up environment -------------------------------------------------------- # ============================================================================== - -#export CC=/usr/bin/clang-8 -#export CXX=/usr/bin/clang++-8 - source $(dirname "$0")/Environment.sh # ============================================================================== # -- Get and compile ad-rss ------------------------------------------- # ============================================================================== -ADRSS_BASENAME=ad-rss-3.0.0 +ADRSS_BASENAME=ad-rss-4.0.0 ADRSS_INSTALL_DIR="${CARLA_BUILD_FOLDER}/${ADRSS_BASENAME}/install" @@ -21,41 +17,44 @@ ADRSS_INSTALL_DIR="${CARLA_BUILD_FOLDER}/${ADRSS_BASENAME}/install" if [[ -d "${ADRSS_INSTALL_DIR}" ]]; then log "${ADRSS_BASENAME} already installed." else - # ad-rss is built inside a colcon workspace, therefore we have to setup the workspace first - if [[ -d "${CARLA_BUILD_FOLDER}/${ADRSS_BASENAME}/src" ]]; then - rm -rf "${CARLA_BUILD_FOLDER}/${ADRSS_BASENAME}/src" - fi - if [[ -d "${CARLA_BUILD_FOLDER}/${ADRSS_BASENAME}/build" ]]; then - rm -rf "${CARLA_BUILD_FOLDER}/${ADRSS_BASENAME}/build" - fi + if [[ ! -d "${CARLA_BUILD_FOLDER}/${ADRSS_BASENAME}/src" ]]; then + # ad-rss is built inside a colcon workspace, therefore we have to setup the workspace first + if [[ -d "${CARLA_BUILD_FOLDER}/${ADRSS_BASENAME}/build" ]]; then + rm -rf "${CARLA_BUILD_FOLDER}/${ADRSS_BASENAME}/build" + fi - mkdir -p "${CARLA_BUILD_FOLDER}/${ADRSS_BASENAME}/src" + mkdir -p "${CARLA_BUILD_FOLDER}/${ADRSS_BASENAME}/src" - log "Retrieving ${ADRSS_BASENAME}." + log "Retrieving ${ADRSS_BASENAME}." - pushd "${CARLA_BUILD_FOLDER}/${ADRSS_BASENAME}/src" >/dev/null - git clone --depth=1 -b v1.x https://github.com/gabime/spdlog.git - git clone --depth=1 -b v2.0.1 https://github.com/carla-simulator/map - git clone --depth=1 -b v3.0.0 https://github.com/intel/ad-rss-lib - popd + pushd "${CARLA_BUILD_FOLDER}/${ADRSS_BASENAME}/src" >/dev/null + git clone --depth=1 -b v1.x https://github.com/gabime/spdlog.git + git clone --depth=1 -b 4.9.3 https://github.com/OSGeo/PROJ.git + git clone --depth=1 -b v2.1.0 https://github.com/carla-simulator/map.git + git clone --depth=1 -b v4.0.0 https://github.com/intel/ad-rss-lib.git + popd - cat >"${CARLA_BUILD_FOLDER}/${ADRSS_BASENAME}/colcon.meta" <"${CARLA_BUILD_FOLDER}/${ADRSS_BASENAME}/colcon.meta" <${LIBSTDCPP_TOOLCHAIN_FILE}.gen <