p71924506
/
carla
mirror of https://github.com/carla-simulator/carla.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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
This commit is contained in:
Pasch, Frederik 2020-07-03 15:54:04 +02:00 committed by Marc Garcia Puig
parent f324c73b10
commit 88ae9d04ae
32 changed files with 3860 additions and 1733 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
CHANGELOG.md
View File

@ -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`

10
Docs/adv_rss.md
View File

@ -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)
<div style="text-align: right"><i>Visualization of the RssSensor results.</i></div>
@ -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

4
Docs/core_concepts.md
View File

@ -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</a>
1st. World and client</a>
</p>
</div>
</div>
</div>

2
Docs/index.md
View File

@ -67,7 +67,7 @@ CARLA forum</a>
— 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.

72
Docs/python_api.md
View File

@ -1488,6 +1488,50 @@ Parses the axis' orientations to string.
---
## carla.RssActorConstellationData<a name="carla.RssActorConstellationData"></a>
Data structure that is provided within the callback registered by RssSensor.register_actor_constellation_callback().
<h3>Instance Variables</h3>
- <a name="carla.RssActorConstellationData.ego_match_object"></a>**<font color="#f8805a">ego_match_object</font>** (_<a href="https://ad-map-access.readthedocs.io/en/latest/ad_map_access/apidoc/html/structad_1_1map_1_1match_1_1Object.html">libad_map_access_python.Object</a>_)
The ego map matched information.
- <a name="carla.RssActorConstellationData.ego_route"></a>**<font color="#f8805a">ego_route</font>** (_<a href="https://ad-map-access.readthedocs.io/en/latest/ad_map_access/apidoc/html/structad_1_1map_1_1route_1_1FullRoute.html">libad_map_access_python.FullRoute</a>_)
The ego route.
- <a name="carla.RssActorConstellationData.ego_dynamics_on_route"></a>**<font color="#f8805a">ego_dynamics_on_route</font>** (_[carla.RssEgoDynamicsOnRoute](#carla.RssEgoDynamicsOnRoute)_)
Current ego vehicle dynamics regarding the route.
- <a name="carla.RssActorConstellationData.other_match_object"></a>**<font color="#f8805a">other_match_object</font>** (_<a href="https://ad-map-access.readthedocs.io/en/latest/ad_map_access/apidoc/html/structad_1_1map_1_1match_1_1Object.html">libad_map_access_python.Object</a>_)
The other object's map matched information.
- <a name="carla.RssActorConstellationData.other_actor"></a>**<font color="#f8805a">other_actor</font>** (_[carla.Actor](#carla.Actor)_)
The other actor.
<h3>Methods</h3>
<h5 style="margin-top: -20px">Dunder methods</h5>
<div style="padding-left:30px;margin-top:-25px"></div>- <a name="carla.RssActorConstellationData.__str__"></a>**<font color="#7fb800">\__str__</font>**(<font color="#00a6ed">**self**</font>)
---
## carla.RssActorConstellationResult<a name="carla.RssActorConstellationResult"></a>
Data structure that should be returned by the callback registered by RssSensor.register_actor_constellation_callback().
<h3>Instance Variables</h3>
- <a name="carla.RssActorConstellationResult.rss_calculation_mode"></a>**<font color="#f8805a">rss_calculation_mode</font>** (_<a href="https://TODO">libad_rss_map_integration_python.RssMode_TODO</a>_)
The calculation mode to be applied with the actor.
- <a name="carla.RssActorConstellationResult.restrict_speed_limit_mode"></a>**<font color="#f8805a">restrict_speed_limit_mode</font>** (_<a href="https://TODO">libad_rss_map_integration_python.RestrictSpeedLimitMode_TODO</a>_)
The mode for restricting speed limit.
- <a name="carla.RssActorConstellationResult.ego_vehicle_dynamics"></a>**<font color="#f8805a">ego_vehicle_dynamics</font>** (_<a href="https://intel.github.io/ad-rss-lib/doxygen/ad_rss/structad_1_1rss_1_1world_1_1RssDynamics.html">libad_rss_python.RssDynamics</a>_)
The RSS dynamics to be applied for the ego vehicle.
- <a name="carla.RssActorConstellationResult.actor_object_type"></a>**<font color="#f8805a">actor_object_type</font>** (_libad_rss_python.ObjectType_)
The RSS object type to be used for the actor.
- <a name="carla.RssActorConstellationResult.actor_dynamics"></a>**<font color="#f8805a">actor_dynamics</font>** (_<a href="https://intel.github.io/ad-rss-lib/doxygen/ad_rss/structad_1_1rss_1_1world_1_1RssDynamics.html">libad_rss_python.RssDynamics</a>_)
The RSS dynamics to be applied for the actor.
<h3>Methods</h3>
<h5 style="margin-top: -20px">Dunder methods</h5>
<div style="padding-left:30px;margin-top:-25px"></div>- <a name="carla.RssActorConstellationResult.__str__"></a>**<font color="#7fb800">\__str__</font>**(<font color="#00a6ed">**self**</font>)
---
## carla.RssEgoDynamicsOnRoute<a name="carla.RssEgoDynamicsOnRoute"></a>
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<a name="carla.RssLogLevel"></a>
Enum declaration used in [carla.RssSensor](#carla.RssSensor) to set the log level.
<h3>Instance Variables</h3>
- <a name="carla.RssLogLevel.trace"></a>**<font color="#f8805a">trace</font>**
- <a name="carla.RssLogLevel.debug"></a>**<font color="#f8805a">debug</font>**
- <a name="carla.RssLogLevel.info"></a>**<font color="#f8805a">info</font>**
- <a name="carla.RssLogLevel.warn"></a>**<font color="#f8805a">warn</font>**
- <a name="carla.RssLogLevel.err"></a>**<font color="#f8805a">err</font>**
- <a name="carla.RssLogLevel.critical"></a>**<font color="#f8805a">critical</font>**
- <a name="carla.RssLogLevel.off"></a>**<font color="#f8805a">off</font>**
---
## carla.RssResponse<a name="carla.RssResponse"></a>
<div style="padding-left:30px;margin-top:-20px"><small><b>Inherited from _[carla.SensorData](#carla.SensorData)_</b></small></div></p><p>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.
- <a name="carla.RssResponse.ego_dynamics_on_route"></a>**<font color="#f8805a">ego_dynamics_on_route</font>** (_[carla.RssEgoDynamicsOnRoute](#carla.RssEgoDynamicsOnRoute)_)
Current ego vehicle dynamics regarding the route.
- <a name="carla.RssResponse.world_model"></a>**<font color="#f8805a">world_model</font>** (_<a href="https://intel.github.io/ad-rss-lib/doxygen/ad_rss/structad_1_1rss_1_1world_1_1WorldModel.html">libad_rss_python.WorldModel</a>_)
World model used for calculations.
- <a name="carla.RssResponse.situation_snapshot"></a>**<font color="#f8805a">situation_snapshot</font>** (_<a href="https://intel.github.io/ad-rss-lib/doxygen/ad_rss/structad_1_1rss_1_1situation_1_1SituationSnapshot.html">libad_rss_python.SituationSnapshot</a>_)
Detailed RSS situations extracted from the world model.
<h3>Methods</h3>
@ -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.
- <a name="carla.RssSensor.drop_route"></a>**<font color="#7fb800">drop_route</font>**(<font color="#00a6ed">**self**</font>)
Discards the current route. If there are targets remaining in **<font color="#f8805a">routing_targets</font>**, creates a new route using those. Otherwise, a new route is created at random.
- <a name="carla.RssSensor.register_actor_constellation_callback"></a>**<font color="#7fb800">register_actor_constellation_callback</font>**(<font color="#00a6ed">**self**</font>, <font color="#00a6ed">**callback**</font>)
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.
<h5 style="margin-top: -20px">Setters</h5>
<div style="padding-left:30px;margin-top:-25px"></div>- <a name="carla.RssSensor.set_log_level"></a>**<font color="#7fb800">set_log_level</font>**(<font color="#00a6ed">**self**</font>, <font color="#00a6ed">**log_level**</font>)
Sets the log level.
- **Parameters:**
- `log_level` (_[carla.RssLogLevel](#carla.RssLogLevel)_) – New log level.
<h5 style="margin-top: -20px">Dunder methods</h5>
<div style="padding-left:30px;margin-top:-25px"></div>- <a name="carla.RssSensor.__str__"></a>**<font color="#7fb800">\__str__</font>**(<font color="#00a6ed">**self**</font>)

38
Docs/ref_sensors.md
View File

@ -1183,6 +1183,7 @@ The sensor allows to control the considered route by providing some key points,
<th>Description</th>
</thead>
<tbody>
<tr>
<td><code>routing_targets</code></td>
<td>Get the current list of routing targets used for route.</td>
<tr>
@ -1194,6 +1195,12 @@ The sensor allows to control the considered route by providing some key points,
<tr>
<td><code>drop_route</code></td>
<td>Discards the current route and creates a new one.</td>
<tr>
<td><code>register_actor_constellation_callback</code></td>
<td>Register a callback to customize the calculations.</td>
<tr>
<td><code>set_log_level</code></td>
<td>Sets the log level.</td>
</table>
<br>
@ -1236,9 +1243,40 @@ if routing_targets:
<td><code>ego_dynamics_on_route</code></td>
<td><a href="../python_api#carlarssegodynamicsonroute">carla.RssEgoDynamicsOnRoute</a></td>
<td>Current ego vehicle dynamics regarding the route.</td>
<tr>
<td><code>situation_snapshot</code></td>
<td><a href="../python_api#carlarssegodynamicsonroute">carla.RssEgoDynamicsOnRoute</a></td>
<td>Current situation snapshot extracted from the world model.</td>
</tbody>
</table>
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

2
LibCarla/cmake/client/CMakeLists.txt
View File

@ -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)

670
LibCarla/source/carla/rss/RssCheck.cpp
View File

@ -22,12 +22,14 @@
#include <ad/rss/map/RssObjectConversion.hpp>
#include <ad/rss/map/RssSceneCreator.hpp>
#include <ad/rss/state/RssStateOperation.hpp>
#include <ad/rss/map/RssObjectData.hpp>
#include <chrono>
#include <tuple>
#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<float>(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<spdlog::logger> 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<carla::client::Walker>(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<carla::client::Vehicle>(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<carla::client::Actor> 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 &timestamp,
carla::SharedPtr<carla::client::ActorList> const &actors,
carla::SharedPtr<carla::client::Actor> 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 &timestamp,
#endif
const auto carla_ego_vehicle = boost::dynamic_pointer_cast<carla::client::Vehicle>(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 &timestamp,
// 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 &timestamp,
_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 &timestamp,
#if DEBUG_TIMING
t_end = std::chrono::high_resolution_clock::now();
std::cout << "-> VI " << std::chrono::duration<double, std::milli>(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<double, std::milli>(t_end - t_start).count() << " end store viz"
std::cout << "-> AN " << std::chrono::duration<double, std::milli>(t_end - t_start).count() << " end analyze results"
<< std::endl;
#endif
@ -271,8 +388,9 @@ bool RssCheck::CheckObjects(carla::client::Timestamp const &timestamp,
// 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 &timestamp,
}
}
::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<carla::client::Actor> 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<carla::client::Vehicle>(actor);
auto const walker = boost::dynamic_pointer_cast<carla::client::Walker>(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 &current_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 &current_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<ActorConstellationData> 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<SharedPtr<carla::client::TrafficLight>> 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<carla::client::Vehicle> vehicle) const {
void RssCheck::RssObjectChecker::operator()(
const carla::SharedPtr<carla::client::Actor> 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<ActorConstellationData> 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 &timestamp, 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<SharedPtr<carla::client::TrafficLight>> traffic_lights;
std::vector<SharedPtr<carla::client::Vehicle>> other_vehicles;
std::vector<SharedPtr<carla::client::Actor>> other_traffic_participants;
for (const auto &actor : actors) {
const auto traffic_light = boost::dynamic_pointer_cast<carla::client::TrafficLight>(actor);
if (traffic_light != nullptr) {
@ -686,15 +878,15 @@ void RssCheck::CreateWorldModel(carla::client::Timestamp const &timestamp, carla
continue;
}
const auto vehicle = boost::dynamic_pointer_cast<carla::client::Vehicle>(actor);
if (vehicle != nullptr) {
if (vehicle->GetId() == carla_ego_vehicle.GetId()) {
if ((boost::dynamic_pointer_cast<carla::client::Vehicle>(actor) != nullptr) ||
(boost::dynamic_pointer_cast<carla::client::Walker>(actor) != nullptr)) {
if (actor->GetId() == carla_ego_vehicle.GetId()) {
continue;
}
auto const relevant_distance =
std::max(static_cast<double>(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 &timestamp, 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<carla::client::Actor> 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<carla::client::Actor> const &carla_ego_actor,
::ad::rss::state::RssStateSnapshot state_snapshot) const {
const auto carla_ego_vehicle = boost::dynamic_pointer_cast<carla::client::Vehicle>(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<carla::rpc::ActorId>(state.objectId);
carla::SharedPtr<carla::client::ActorList> vehicle_list =
world.GetActors(std::vector<carla::rpc::ActorId>{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<carla::client::Vehicle>(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<float>(static_cast<double>(point.x)),
static_cast<float>(static_cast<double>(-1. * point.y)),
static_cast<float>(static_cast<double>(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<carla::client::Actor> 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<carla::client::Actor> const &carla_ego_actor,
EgoDynamicsOnRoute const &ego_dynamics_on_route) const {
const auto carla_ego_vehicle = boost::dynamic_pointer_cast<carla::client::Vehicle>(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<float>(std::sin(static_cast<double>(ego_dynamics_on_route.route_heading)));
auto cos_heading = static_cast<float>(std::cos(static_cast<double>(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<float>(std::sin(static_cast<double>(ego_dynamics_on_route.route_heading) + M_PI_2));
auto cos_center = static_cast<float>(std::cos(static_cast<double>(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

243
LibCarla/source/carla/rss/RssCheck.h
View File

@ -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<carla::client::Actor> 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<ActorConstellationData>)>;
/// @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<carla::client::Actor> const &carla_ego_actor);
/// @brief destructor
~RssCheck();
@ -120,23 +152,28 @@ public:
bool CheckObjects(carla::client::Timestamp const &timestamp, carla::SharedPtr<carla::client::ActorList> const &actors,
carla::SharedPtr<carla::client::Actor> 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<carla::client::Actor> 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<spdlog::logger> _logger;
/// @brief logger for timing log messages
std::shared_ptr<spdlog::logger> _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<carla::client::Vehicle> vehicle) const;
void operator()(const carla::SharedPtr<carla::client::Actor> 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<carla::client::Actor> 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<SharedPtr<carla::client::TrafficLight>> 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<carla::client::Actor> 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<carla::client::Actor> 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<carla::client::Actor> 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

25
LibCarla/source/carla/rss/RssRestrictor.cpp
View File

@ -9,7 +9,8 @@
#include "carla/rss/RssCheck.h"
#include <spdlog/sinks/stdout_color_sinks.h>
#include <ad/rss/world/AccelerationRestriction.hpp>
#include <ad/rss/state/ProperResponse.hpp>
#include <ad/rss/unstructured/Geometry.hpp>
#include <ad/rss/world/Velocity.hpp>
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<double>(restriction.longitudinalRange.minimum));
double brake_acceleration =
std::fabs(static_cast<double>(proper_response.accelerationRestrictions.longitudinalRange.minimum));
double sum_brake_torque = mass * brake_acceleration * radius / 100.0;
restricted_vehicle_control.brake = std::min(static_cast<float>(sum_brake_torque / sum_max_brake_torque), 1.0f);
}

8
LibCarla/source/carla/rss/RssRestrictor.h
View File

@ -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);

195
LibCarla/source/carla/rss/RssSensor.cpp
View File

@ -7,7 +7,6 @@
#include <ad/map/access/Operation.hpp>
#include <ad/rss/state/ProperResponse.hpp>
#include <ad/rss/world/AccelerationRestriction.hpp>
#include <ad/rss/world/Velocity.hpp>
#include <exception>
#include <fstream>
@ -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<std::mutex> 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<carla::client::Vehicle>(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<float>(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<RssSensor>(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<std::mutex> 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<sensor::SensorData> RssSensor::TickRssSensor(const Timestamp &timestam
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<sensor::SensorData> RssSensor::TickRssSensor(const Timestamp &timestam
}
// 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<sensor::data::RssResponse>(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);

37
LibCarla/source/carla/rss/RssSensor.h
View File

@ -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;
};

24
LibCarla/source/carla/sensor/data/RssResponse.h
View File

@ -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;
};

5
PythonAPI/carla/setup.py
View File

@ -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)]

118
PythonAPI/carla/source/libcarla/AdRss.cpp
View File

@ -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<py::object>{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_<carla::rss::ActorConstellationResult>("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_<carla::rss::ActorConstellationData, boost::noncopyable, boost::shared_ptr<carla::rss::ActorConstellationData>>(
"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_<spdlog::level::level_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_<carla::rss::RoadBoundariesMode>("RssRoadBoundariesMode")
.value("Off", carla::rss::RoadBoundariesMode::Off)
.value("On", carla::rss::RoadBoundariesMode::On);
enum_<carla::rss::VisualizationMode>("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_<csd::RssResponse, bases<cs::SensorData>, boost::noncopyable, boost::shared_ptr<csd::RssResponse>>(
"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_<cc::RssSensor, bases<cc::Sensor>, boost::noncopyable, boost::shared_ptr<cc::RssSensor>>("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_<carla::rss::RssRestrictor, boost::noncopyable, boost::shared_ptr<carla::rss::RssRestrictor>>("RssRestrictor",

89
PythonAPI/docs/sensor.yml
View File

@ -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.
<b>Receive data on every tick.</b>
- [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).
<b>Only receive data when triggered.</b>
- [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.
<b>Receive data on every tick.</b>
- [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).
<b>Only receive data when triggered.</b>
- [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<carla.Transform>
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 **<font color="#f8805a">routing_targets</font>**, 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'
# --------------------------------------
...

80
PythonAPI/docs/sensor_data.yml
View File

@ -399,6 +399,16 @@
type: carla.RssEgoDynamicsOnRoute
doc: >
Current ego vehicle dynamics regarding the route.
# --------------------------------------
- var_name: world_model
type: <a href="https://intel.github.io/ad-rss-lib/doxygen/ad_rss/structad_1_1rss_1_1world_1_1WorldModel.html">libad_rss_python.WorldModel</a>
doc: >
World model used for calculations.
# --------------------------------------
- var_name: situation_snapshot
type: <a href="https://intel.github.io/ad-rss-lib/doxygen/ad_rss/structad_1_1rss_1_1situation_1_1SituationSnapshot.html">libad_rss_python.SituationSnapshot</a>
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: <a href="https://ad-map-access.readthedocs.io/en/latest/ad_map_access/apidoc/html/structad_1_1map_1_1match_1_1Object.html">libad_map_access_python.Object</a>
doc: >
The ego map matched information.
# --------------------------------------
- var_name: ego_route
type: <a href="https://ad-map-access.readthedocs.io/en/latest/ad_map_access/apidoc/html/structad_1_1map_1_1route_1_1FullRoute.html">libad_map_access_python.FullRoute</a>
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: <a href="https://ad-map-access.readthedocs.io/en/latest/ad_map_access/apidoc/html/structad_1_1map_1_1match_1_1Object.html">libad_map_access_python.Object</a>
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: <a href="https://TODO">libad_rss_map_integration_python.RssMode_TODO</a>
doc: >
The calculation mode to be applied with the actor.
# --------------------------------------
- var_name: restrict_speed_limit_mode
type: <a href="https://TODO">libad_rss_map_integration_python.RestrictSpeedLimitMode_TODO</a>
doc: >
The mode for restricting speed limit.
# --------------------------------------
- var_name: ego_vehicle_dynamics
type: <a href="https://intel.github.io/ad-rss-lib/doxygen/ad_rss/structad_1_1rss_1_1world_1_1RssDynamics.html">libad_rss_python.RssDynamics</a>
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: <a href="https://intel.github.io/ad-rss-lib/doxygen/ad_rss/structad_1_1rss_1_1world_1_1RssDynamics.html">libad_rss_python.RssDynamics</a>
doc: >
The RSS dynamics to be applied for the actor.
# - METHODS ----------------------------
methods:
- def_name: __str__
# --------------------------------------
- class_name: DVSEvent
# - DESCRIPTION ------------------------
doc: >

1096
PythonAPI/examples/manual_control_rss.py
View File

File diff suppressed because it is too large Load Diff

0
PythonAPI/examples/rss/__init__.py Normal file
View File

884
PythonAPI/examples/rss/manual_control_rss.py Executable file
View File

@ -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 <https://opensource.org/licenses/MIT>.
# 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()

402
PythonAPI/examples/rss/rss_sensor.py Normal file
View File

@ -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))

701
PythonAPI/examples/rss/rss_visualization.py Normal file
View File

@ -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)

218
PythonAPI/examples/rss/scenarios/FollowLeadingVehicle.xosc Normal file
View File

@ -0,0 +1,218 @@
<?xml version="1.0"?>
<OpenSCENARIO>
<FileHeader revMajor="1" revMinor="0" date="2019-06-25T00:00:00" description="CARLA:FollowLeadingVehicle" author=""/>
<ParameterDeclarations>
<ParameterDeclaration name="$leadingSpeed" parameterType="double" value="2.0"/>
</ParameterDeclarations>
<CatalogLocations>
</CatalogLocations>
<RoadNetwork>
<LogicFile filepath="Town01"/>
<SceneGraphFile filepath=""/>
</RoadNetwork>
<Entities>
<ScenarioObject name="ego_vehicle">
<Vehicle name="vehicle.lincoln.mkz2017" vehicleCategory="car">
<ParameterDeclarations/>
<Performance maxSpeed="69.444" maxAcceleration="10.0" maxDeceleration="10.0"/>
<BoundingBox>
<Center x="1.5" y="0.0" z="0.9"/>
<Dimensions width="2.1" length="4.5" height="1.8"/>
</BoundingBox>
<Axles>
<FrontAxle maxSteering="0.5" wheelDiameter="0.6" trackWidth="1.8" positionX="3.1" positionZ="0.3"/>
<RearAxle maxSteering="0.0" wheelDiameter="0.6" trackWidth="1.8" positionX="0.0" positionZ="0.3"/>
</Axles>
<Properties>
<Property name="type" value="ego_vehicle"/>
<Property name="color" value="0,0,255"/>
</Properties>
</Vehicle>
</ScenarioObject>
<ScenarioObject name="adversary">
<Vehicle name="vehicle.lincoln.mkz2017" vehicleCategory="car">
<ParameterDeclarations/>
<Performance maxSpeed="69.444" maxAcceleration="10.0" maxDeceleration="10.0"/>
<BoundingBox>
<Center x="1.5" y="0.0" z="0.9"/>
<Dimensions width="2.1" length="4.5" height="1.8"/>
</BoundingBox>
<Axles>
<FrontAxle maxSteering="0.5" wheelDiameter="0.6" trackWidth="1.8" positionX="3.1" positionZ="0.3"/>
<RearAxle maxSteering="0.0" wheelDiameter="0.6" trackWidth="1.8" positionX="0.0" positionZ="0.3"/>
</Axles>
<Properties>
<Property name="type" value="simulation"/>
<Property name="color" value="255,0,0"/>
</Properties>
</Vehicle>
</ScenarioObject>
</Entities>
<Storyboard>
<Init>
<Actions>
<GlobalAction>
<EnvironmentAction>
<Environment name="Environment1">
<TimeOfDay animation="false" dateTime="2019-06-25T12:00:00"/>
<Weather cloudState="free">
<Sun intensity="1.0" azimuth="0.0" elevation="1.31"/>
<Fog visualRange="100000.0"/>
<Precipitation precipitationType="dry" intensity="0.0"/>
</Weather>
<RoadCondition frictionScaleFactor="1.0"/>
</Environment>
</EnvironmentAction>
</GlobalAction>
<Private entityRef="ego_vehicle">
<PrivateAction>
<TeleportAction>
<Position>
<WorldPosition x="150" y="133" z="0" h="0"/>
</Position>
</TeleportAction>
</PrivateAction>
<PrivateAction>
<ControllerAction>
<AssignControllerAction>
<Controller name="EgoVehicleAgent">
<Properties>
<Property name="module" value="external_control" />
</Properties>
</Controller>
</AssignControllerAction>
<OverrideControllerValueAction>
<Throttle value="0" active="false" />
<Brake value="0" active="false" />
<Clutch value="0" active="false" />
<ParkingBrake value="0" active="false" />
<SteeringWheel value="0" active="false" />
<Gear number="0" active="false" />
</OverrideControllerValueAction>
</ControllerAction>
</PrivateAction>
</Private>
<Private entityRef="adversary">
<PrivateAction>
<TeleportAction>
<Position>
<WorldPosition x="190" y="133" z="0" h="0"/>
</Position>
</TeleportAction>
</PrivateAction>
</Private>
</Actions>
</Init>
<Story name="MyStory">
<Act name="Behavior">
<ManeuverGroup maximumExecutionCount="1" name="ManeuverSequence">
<Actors selectTriggeringEntities="false">
<EntityRef entityRef="adversary"/>
</Actors>
<Maneuver name="FollowLeadingVehicleManeuver">
<Event name="LeadingVehicleKeepsVelocity" priority="overwrite">
<Action name="LeadingVehicleKeepsVelocity">
<PrivateAction>
<LongitudinalAction>
<SpeedAction>
<SpeedActionDynamics dynamicsShape="step" value="100" dynamicsDimension="distance"/>
<SpeedActionTarget>
<AbsoluteTargetSpeed value="$leadingSpeed"/>
</SpeedActionTarget>
</SpeedAction>
</LongitudinalAction>
</PrivateAction>
</Action>
<StartTrigger>
<ConditionGroup>
<Condition name="StartCondition" delay="0" conditionEdge="rising">
<ByEntityCondition>
<TriggeringEntities triggeringEntitiesRule="any">
<EntityRef entityRef="ego_vehicle"/>
</TriggeringEntities>
<EntityCondition>
<RelativeDistanceCondition entityRef="adversary" relativeDistanceType="cartesianDistance" value="40.0" freespace="false" rule="lessThan"/>
</EntityCondition>
</ByEntityCondition>
</Condition>
</ConditionGroup>
</StartTrigger>
</Event>
</Maneuver>
</ManeuverGroup>
<StartTrigger>
<ConditionGroup>
<Condition name="OverallStartCondition" delay="0" conditionEdge="rising">
<ByEntityCondition>
<TriggeringEntities triggeringEntitiesRule="any">
<EntityRef entityRef="ego_vehicle"/>
</TriggeringEntities>
<EntityCondition>
<TraveledDistanceCondition value="1.0"/>
</EntityCondition>
</ByEntityCondition>
</Condition>
<Condition name="StartTime" delay="0" conditionEdge="rising">
<ByValueCondition>
<SimulationTimeCondition value="0" rule="equalTo"/>
</ByValueCondition>
</Condition>
</ConditionGroup>
</StartTrigger>
<StopTrigger>
<ConditionGroup>
<Condition name="EndCondition" delay="0" conditionEdge="rising">
<ByEntityCondition>
<TriggeringEntities triggeringEntitiesRule="any">
<EntityRef entityRef="ego_vehicle"/>
</TriggeringEntities>
<EntityCondition>
<TraveledDistanceCondition value="100.0"/>
</EntityCondition>
</ByEntityCondition>
</Condition>
</ConditionGroup>
</StopTrigger>
</Act>
</Story>
<StopTrigger>
<ConditionGroup>
<Condition name="criteria_RunningStopTest" delay="0" conditionEdge="rising">
<ByValueCondition>
<ParameterCondition parameterRef="" value="" rule="lessThan"/>
</ByValueCondition>
</Condition>
<Condition name="criteria_RunningRedLightTest" delay="0" conditionEdge="rising">
<ByValueCondition>
<ParameterCondition parameterRef="" value="" rule="lessThan"/>
</ByValueCondition>
</Condition>
<Condition name="criteria_WrongLaneTest" delay="0" conditionEdge="rising">
<ByValueCondition>
<ParameterCondition parameterRef="" value="" rule="lessThan"/>
</ByValueCondition>
</Condition>
<Condition name="criteria_OnSidewalkTest" delay="0" conditionEdge="rising">
<ByValueCondition>
<ParameterCondition parameterRef="" value="" rule="lessThan"/>
</ByValueCondition>
</Condition>
<Condition name="criteria_KeepLaneTest" delay="0" conditionEdge="rising">
<ByValueCondition>
<ParameterCondition parameterRef="" value="" rule="lessThan"/>
</ByValueCondition>
</Condition>
<Condition name="criteria_CollisionTest" delay="0" conditionEdge="rising">
<ByValueCondition>
<ParameterCondition parameterRef="" value="" rule="lessThan"/>
</ByValueCondition>
</Condition>
<Condition name="criteria_DrivenDistanceTest" delay="0" conditionEdge="rising">
<ByValueCondition>
<ParameterCondition parameterRef="distance_success" value="100" rule="lessThan"/>
</ByValueCondition>
</Condition>
</ConditionGroup>
</StopTrigger>
</Storyboard>
</OpenSCENARIO>

187
PythonAPI/examples/rss/scenarios/unstructured_blocked_road.xosc Normal file
View File

@ -0,0 +1,187 @@
<?xml version="1.0"?>
<OpenSCENARIO>
<FileHeader revMajor="1" revMinor="0" date="2019-06-25T00:00:00" description="CARLA:FollowLeadingVehicle" author=""/>
<ParameterDeclarations>
<ParameterDeclaration name="$leadingSpeed" parameterType="double" value="2.0"/>
</ParameterDeclarations>
<CatalogLocations>
</CatalogLocations>
<RoadNetwork>
<LogicFile filepath="Town01"/>
<SceneGraphFile filepath=""/>
</RoadNetwork>
<Entities>
<ScenarioObject name="hero">
<Vehicle name="vehicle.lincoln.mkz2017" vehicleCategory="car">
<ParameterDeclarations/>
<Performance maxSpeed="69.444" maxAcceleration="10.0" maxDeceleration="10.0"/>
<BoundingBox>
<Center x="1.5" y="0.0" z="0.9"/>
<Dimensions width="2.1" length="4.5" height="1.8"/>
</BoundingBox>
<Axles>
<FrontAxle maxSteering="0.5" wheelDiameter="0.6" trackWidth="1.8" positionX="3.1" positionZ="0.3"/>
<RearAxle maxSteering="0.0" wheelDiameter="0.6" trackWidth="1.8" positionX="0.0" positionZ="0.3"/>
</Axles>
<Properties>
<Property name="type" value="ego_vehicle"/>
<Property name="color" value="0,0,255"/>
</Properties>
</Vehicle>
</ScenarioObject>
<ScenarioObject name="blockingVehicle">
<Vehicle name="vehicle.lincoln.mkz2017" vehicleCategory="car">
<ParameterDeclarations/>
<Performance maxSpeed="69.444" maxAcceleration="10.0" maxDeceleration="10.0"/>
<BoundingBox>
<Center x="1.5" y="0.0" z="0.9"/>
<Dimensions width="2.1" length="4.5" height="1.8"/>
</BoundingBox>
<Axles>
<FrontAxle maxSteering="0.5" wheelDiameter="0.6" trackWidth="1.8" positionX="3.1" positionZ="0.3"/>
<RearAxle maxSteering="0.0" wheelDiameter="0.6" trackWidth="1.8" positionX="0.0" positionZ="0.3"/>
</Axles>
<Properties>
<Property name="type" value="simulation"/>
<Property name="color" value="255,0,0"/>
</Properties>
</Vehicle>
</ScenarioObject>
</Entities>
<Storyboard>
<Init>
<Actions>
<GlobalAction>
<EnvironmentAction>
<Environment name="Environment1">
<TimeOfDay animation="false" dateTime="2019-06-25T12:00:00"/>
<Weather cloudState="free">
<Sun intensity="1.0" azimuth="0.0" elevation="1.31"/>
<Fog visualRange="100000.0"/>
<Precipitation precipitationType="dry" intensity="0.0"/>
</Weather>
<RoadCondition frictionScaleFactor="1.0"/>
</Environment>
</EnvironmentAction>
</GlobalAction>
<Private entityRef="hero">
<PrivateAction>
<TeleportAction>
<Position>
<WorldPosition x="150" y="133" z="0" h="0"/>
</Position>
</TeleportAction>
</PrivateAction>
<PrivateAction>
<ControllerAction>
<AssignControllerAction>
<Controller name="EgoVehicleAgent">
<Properties>
<Property name="module" value="external_control" />
</Properties>
</Controller>
</AssignControllerAction>
<OverrideControllerValueAction>
<Throttle value="0" active="false" />
<Brake value="0" active="false" />
<Clutch value="0" active="false" />
<ParkingBrake value="0" active="false" />
<SteeringWheel value="0" active="false" />
<Gear number="0" active="false" />
</OverrideControllerValueAction>
</ControllerAction>
</PrivateAction>
</Private>
<Private entityRef="blockingVehicle">
<PrivateAction>
<TeleportAction>
<Position>
<WorldPosition x="165" y="133" z="0" h="2.8"/>
</Position>
</TeleportAction>
</PrivateAction>
</Private>
</Actions>
</Init>
<Story name="MyStory">
<Act name="Behavior">
<ManeuverGroup maximumExecutionCount="1" name="ManeuverSequence">
<Actors selectTriggeringEntities="false">
<EntityRef entityRef="hero"/>
</Actors>
<Maneuver name="EmptyManeuver">
<Event name="EmptyEvent" priority="overwrite">
<!-- never triggered -->
<Action name="EmptyAcion">
</Action>
<StartTrigger>
</StartTrigger>
</Event>
</Maneuver>
</ManeuverGroup>
<StartTrigger>
<ConditionGroup>
<Condition name="StartTime" delay="0" conditionEdge="rising">
<ByValueCondition>
<SimulationTimeCondition value="0" rule="equalTo"/>
</ByValueCondition>
</Condition>
</ConditionGroup>
</StartTrigger>
<StopTrigger>
<ConditionGroup>
<Condition name="EndCondition" delay="0" conditionEdge="rising">
<ByEntityCondition>
<TriggeringEntities triggeringEntitiesRule="any">
<EntityRef entityRef="hero"/>
</TriggeringEntities>
<EntityCondition>
<TraveledDistanceCondition value="100.0"/>
</EntityCondition>
</ByEntityCondition>
</Condition>
</ConditionGroup>
</StopTrigger>
</Act>
</Story>
<StopTrigger>
<ConditionGroup>
<Condition name="criteria_RunningStopTest" delay="0" conditionEdge="rising">
<ByValueCondition>
<ParameterCondition parameterRef="" value="" rule="lessThan"/>
</ByValueCondition>
</Condition>
<Condition name="criteria_RunningRedLightTest" delay="0" conditionEdge="rising">
<ByValueCondition>
<ParameterCondition parameterRef="" value="" rule="lessThan"/>
</ByValueCondition>
</Condition>
<Condition name="criteria_WrongLaneTest" delay="0" conditionEdge="rising">
<ByValueCondition>
<ParameterCondition parameterRef="" value="" rule="lessThan"/>
</ByValueCondition>
</Condition>
<Condition name="criteria_OnSidewalkTest" delay="0" conditionEdge="rising">
<ByValueCondition>
<ParameterCondition parameterRef="" value="" rule="lessThan"/>
</ByValueCondition>
</Condition>
<Condition name="criteria_KeepLaneTest" delay="0" conditionEdge="rising">
<ByValueCondition>
<ParameterCondition parameterRef="" value="" rule="lessThan"/>
</ByValueCondition>
</Condition>
<Condition name="criteria_CollisionTest" delay="0" conditionEdge="rising">
<ByValueCondition>
<ParameterCondition parameterRef="" value="" rule="lessThan"/>
</ByValueCondition>
</Condition>
<Condition name="criteria_DrivenDistanceTest" delay="0" conditionEdge="rising">
<ByValueCondition>
<ParameterCondition parameterRef="distance_success" value="100" rule="lessThan"/>
</ByValueCondition>
</Condition>
</ConditionGroup>
</StopTrigger>
</Storyboard>
</OpenSCENARIO>

196
PythonAPI/examples/rss/scenarios/unstructured_pedestrian_crossing.xosc Normal file
View File

@ -0,0 +1,196 @@
<?xml version="1.0" encoding="UTF-8"?>
<OpenSCENARIO>
<FileHeader revMajor="1" revMinor="0" date="2020-03-20T00:00:00" description="CARLA:AgentExample" author="" />
<ParameterDeclarations/>
<CatalogLocations/>
<RoadNetwork>
<LogicFile filepath="Town01" />
<SceneGraphFile filepath="" />
</RoadNetwork>
<Entities>
<ScenarioObject name="hero">
<Vehicle name="vehicle.lincoln.mkz2017" vehicleCategory="car">
<ParameterDeclarations/>
<Performance maxSpeed="69.444" maxAcceleration="200" maxDeceleration="10.0"/>
<BoundingBox>
<Center x="1.5" y="0.0" z="0.9" />
<Dimensions width="2.1" length="4.5" height="1.8"/>
</BoundingBox>
<Axles>
<FrontAxle maxSteering="0.5" wheelDiameter="0.6" trackWidth="1.8" positionX="3.1" positionZ="0.3" />
<RearAxle maxSteering="0.0" wheelDiameter="0.6" trackWidth="1.8" positionX="0.0" positionZ="0.3" />
</Axles>
<Properties>
<Property name="type" value="ego_vehicle" />
<Property name="color" value="0,0,255" />
</Properties>
</Vehicle>
</ScenarioObject>
<ScenarioObject name="pedestrian1">
<Pedestrian mass="100" model="walker.pedestrian.0001" name="Pedestrian1" pedestrianCategory="pedestrian">
<ParameterDeclarations/>
<BoundingBox>
<Center x="0.5" y="0.5" z="0.9"/>
<Dimensions width="1.0" length="1.0" height="1.8"/>
</BoundingBox>
<Properties>
<Property name="type" value="simulation"/>
</Properties>
</Pedestrian>
</ScenarioObject>
</Entities>
<Storyboard>
<Init>
<Actions>
<GlobalAction>
<EnvironmentAction>
<Environment name="Environment1">
<TimeOfDay animation="false" dateTime="2020-03-20T12:00:00"/>
<Weather cloudState="free">
<Sun intensity="0.85" azimuth="0" elevation="1.31" />
<Fog visualRange="100000.0" />
<Precipitation precipitationType="dry" intensity="0.0" />
</Weather>
<RoadCondition frictionScaleFactor="1.0" />
</Environment>
</EnvironmentAction>
</GlobalAction>
<Private entityRef="hero">
<PrivateAction>
<TeleportAction>
<Position>
<WorldPosition x="338.9" y="319.5" z="1" h="4.71" />
</Position>
</TeleportAction>
</PrivateAction>
<PrivateAction>
<ControllerAction>
<AssignControllerAction>
<Controller name="EgoVehicleAgent">
<Properties>
<Property name="module" value="external_control"/>
</Properties>
</Controller>
</AssignControllerAction>
<OverrideControllerValueAction>
<Throttle value="0" active="false" />
<Brake value="0" active="false" />
<Clutch value="0" active="false" />
<ParkingBrake value="0" active="false" />
<SteeringWheel value="0" active="false" />
<Gear number="0" active="false" />
</OverrideControllerValueAction>
</ControllerAction>
</PrivateAction>
<PrivateAction>
<LongitudinalAction>
<SpeedAction>
<SpeedActionDynamics dynamicsShape="step" value="20" dynamicsDimension="distance"/>
<SpeedActionTarget>
<AbsoluteTargetSpeed value="10" />
</SpeedActionTarget>
</SpeedAction>
</LongitudinalAction>
</PrivateAction>
</Private>
<Private entityRef="pedestrian1">
<PrivateAction>
<TeleportAction>
<Position>
<WorldPosition x="342" y="300" z="1" h="3.14" />
</Position>
</TeleportAction>
</PrivateAction>
</Private>
</Actions>
</Init>
<Story name="MyStory">
<Act name="Behavior">
<ManeuverGroup name="Pedestrian1ManeuverSequence" maximumExecutionCount="1">
<Actors selectTriggeringEntities="false">
<EntityRef entityRef="pedestrian1" />
</Actors>
<Maneuver name="Pedestrian1RouteManeuver">
<Event name="Pedestrian1RouteEvent" priority="overwrite">
<Action name="Pedestrian1RouteAction">
<PrivateAction>
<RoutingAction>
<AssignRouteAction>
<Route name="Pedestrian1Route" closed="false">
<Waypoint routeStrategy="fastest">
<Position>
<WorldPosition x="342" y="290" z="1" h="0"/>
</Position>
</Waypoint>
<Waypoint routeStrategy="fastest">
<Position>
<WorldPosition x="331.5" y="290" z="1" h="0"/>
</Position>
</Waypoint>
<Waypoint routeStrategy="fastest">
<Position>
<WorldPosition x="331.5" y="210" z="1" h="0"/>
</Position>
</Waypoint>
</Route>
</AssignRouteAction>
</RoutingAction>
</PrivateAction>
</Action>
<Action name="Pedestrian1RouteActionSpeed">
<PrivateAction>
<LongitudinalAction>
<SpeedAction>
<SpeedActionDynamics dynamicsShape="step" value="20" dynamicsDimension="distance"/>
<SpeedActionTarget>
<AbsoluteTargetSpeed value="3" />
</SpeedActionTarget>
</SpeedAction>
</LongitudinalAction>
</PrivateAction>
</Action>
<StartTrigger>
<ConditionGroup>
<Condition name="Pedestrian1RouteCondition" delay="0" conditionEdge="rising">
<ByEntityCondition>
<TriggeringEntities triggeringEntitiesRule="any">
<EntityRef entityRef="hero" />
</TriggeringEntities>
<EntityCondition>
<TraveledDistanceCondition value="1.0" />
</EntityCondition>
</ByEntityCondition>
</Condition>
</ConditionGroup>
</StartTrigger>
</Event>
</Maneuver>
</ManeuverGroup>
<StartTrigger>
<ConditionGroup>
<Condition name="StartTime" delay="0" conditionEdge="rising">
<ByValueCondition>
<SimulationTimeCondition value="0" rule="equalTo" />
</ByValueCondition>
</Condition>
</ConditionGroup>
</StartTrigger>
<StopTrigger>
<ConditionGroup>
<Condition name="EndCondition" delay="0" conditionEdge="rising">
<ByEntityCondition>
<TriggeringEntities triggeringEntitiesRule="any">
<EntityRef entityRef="hero" />
</TriggeringEntities>
<EntityCondition>
<TraveledDistanceCondition value="40.0" />
</EntityCondition>
</ByEntityCondition>
</Condition>
</ConditionGroup>
</StopTrigger>
</Act>
</Story>
<StopTrigger/>
</Storyboard>
</OpenSCENARIO>

223
PythonAPI/examples/rss/scenarios/unstructured_pedestrian_on_road.xosc Normal file
View File

@ -0,0 +1,223 @@
<?xml version="1.0" encoding="UTF-8"?>
<OpenSCENARIO>
<FileHeader revMajor="1" revMinor="0" date="2020-03-20T00:00:00" description="CARLA:AgentExample" author="" />
<ParameterDeclarations/>
<CatalogLocations/>
<RoadNetwork>
<LogicFile filepath="Town01" />
<SceneGraphFile filepath="" />
</RoadNetwork>
<Entities>
<ScenarioObject name="hero">
<Vehicle name="vehicle.lincoln.mkz2017" vehicleCategory="car">
<ParameterDeclarations/>
<Performance maxSpeed="69.444" maxAcceleration="200" maxDeceleration="10.0"/>
<BoundingBox>
<Center x="1.5" y="0.0" z="0.9" />
<Dimensions width="2.1" length="4.5" height="1.8"/>
</BoundingBox>
<Axles>
<FrontAxle maxSteering="0.5" wheelDiameter="0.6" trackWidth="1.8" positionX="3.1" positionZ="0.3" />
<RearAxle maxSteering="0.0" wheelDiameter="0.6" trackWidth="1.8" positionX="0.0" positionZ="0.3" />
</Axles>
<Properties>
<Property name="type" value="ego_vehicle" />
<Property name="color" value="0,0,255" />
</Properties>
</Vehicle>
</ScenarioObject>
<ScenarioObject name="pedestrian1">
<Pedestrian mass="100" model="walker.pedestrian.0001" name="Pedestrian1" pedestrianCategory="pedestrian">
<ParameterDeclarations/>
<BoundingBox>
<Center x="0.5" y="0.5" z="0.9"/>
<Dimensions width="1.0" length="1.0" height="1.8"/>
</BoundingBox>
<Properties>
<Property name="type" value="simulation"/>
</Properties>
</Pedestrian>
</ScenarioObject>
</Entities>
<Storyboard>
<Init>
<Actions>
<GlobalAction>
<EnvironmentAction>
<Environment name="Environment1">
<TimeOfDay animation="false" dateTime="2020-03-20T12:00:00"/>
<Weather cloudState="free">
<Sun intensity="0.85" azimuth="0" elevation="1.31" />
<Fog visualRange="100000.0" />
<Precipitation precipitationType="dry" intensity="0.0" />
</Weather>
<RoadCondition frictionScaleFactor="1.0" />
</Environment>
</EnvironmentAction>
</GlobalAction>
<Private entityRef="hero">
<PrivateAction>
<TeleportAction>
<Position>
<WorldPosition x="338.9" y="319.5" z="1" h="4.71" />
</Position>
</TeleportAction>
</PrivateAction>
<PrivateAction>
<ControllerAction>
<AssignControllerAction>
<Controller name="EgoVehicleAgent">
<Properties>
<Property name="module" value="external_control"/>
</Properties>
</Controller>
</AssignControllerAction>
<OverrideControllerValueAction>
<Throttle value="0" active="false" />
<Brake value="0" active="false" />
<Clutch value="0" active="false" />
<ParkingBrake value="0" active="false" />
<SteeringWheel value="0" active="false" />
<Gear number="0" active="false" />
</OverrideControllerValueAction>
</ControllerAction>
</PrivateAction>
<PrivateAction>
<LongitudinalAction>
<SpeedAction>
<SpeedActionDynamics dynamicsShape="step" value="20" dynamicsDimension="distance"/>
<SpeedActionTarget>
<AbsoluteTargetSpeed value="10" />
</SpeedActionTarget>
</SpeedAction>
</LongitudinalAction>
</PrivateAction>
</Private>
<Private entityRef="pedestrian1">
<PrivateAction>
<TeleportAction>
<Position>
<WorldPosition x="339.5" y="300" z="1" h="4.71" />
</Position>
</TeleportAction>
</PrivateAction>
</Private>
</Actions>
</Init>
<Story name="MyStory">
<Act name="Behavior">
<ManeuverGroup name="Pedestrian1ManeuverSequence" maximumExecutionCount="1">
<Actors selectTriggeringEntities="false">
<EntityRef entityRef="pedestrian1" />
</Actors>
<Maneuver name="Pedestrian1RouteManeuver">
<!--
1. walk 20m when the ego-vehicle starts moving
1. pause for some seconds
3. walk another 20m
-->
<Event name="Pedestrian1WalksEvent" priority="overwrite">
<Action name="Pedestrian1Walks">
<PrivateAction>
<LongitudinalAction>
<SpeedAction>
<SpeedActionDynamics dynamicsShape="step" value="20" dynamicsDimension="distance"/>
<SpeedActionTarget>
<AbsoluteTargetSpeed value="2" />
</SpeedActionTarget>
</SpeedAction>
</LongitudinalAction>
</PrivateAction>
</Action>
<StartTrigger>
<ConditionGroup>
<Condition name="Pedestrian1WalksCondition" delay="0" conditionEdge="rising">
<ByEntityCondition>
<TriggeringEntities triggeringEntitiesRule="any">
<EntityRef entityRef="hero" />
</TriggeringEntities>
<EntityCondition>
<TraveledDistanceCondition value="1.0" />
</EntityCondition>
</ByEntityCondition>
</Condition>
</ConditionGroup>
</StartTrigger>
</Event>
<Event name="Pedestrian1WaitsEvent" priority="overwrite">
<Action name="Pedestrian1Waits">
<PrivateAction>
<LongitudinalAction>
<SpeedAction>
<SpeedActionDynamics dynamicsShape="step" value="10" dynamicsDimension="time"/>
<SpeedActionTarget>
<AbsoluteTargetSpeed value="0.0"/>
</SpeedActionTarget>
</SpeedAction>
</LongitudinalAction>
</PrivateAction>
</Action>
<StartTrigger>
<ConditionGroup>
<Condition name="AfterPedestrian1Walks" delay="0" conditionEdge="rising">
<ByValueCondition>
<StoryboardElementStateCondition storyboardElementType="action" storyboardElementRef="Pedestrian1Walks" state="endTransition"/>
</ByValueCondition>
</Condition>
</ConditionGroup>
</StartTrigger>
</Event>
<Event name="Pedestrian1ContinuesWalkingEvent" priority="overwrite">
<!-- contine walking 20m -->
<Action name="Pedestrian1ContinuesWalking">
<PrivateAction>
<LongitudinalAction>
<SpeedAction>
<SpeedActionDynamics dynamicsShape="step" value="20" dynamicsDimension="distance"/>
<SpeedActionTarget>
<AbsoluteTargetSpeed value="2" />
</SpeedActionTarget>
</SpeedAction>
</LongitudinalAction>
</PrivateAction>
</Action>
<StartTrigger>
<ConditionGroup>
<Condition name="AfterPedestrian1Waits" delay="0" conditionEdge="rising">
<ByValueCondition>
<StoryboardElementStateCondition storyboardElementType="action" storyboardElementRef="Pedestrian1Waits" state="endTransition"/>
</ByValueCondition>
</Condition>
</ConditionGroup>
</StartTrigger>
</Event>
</Maneuver>
</ManeuverGroup>
<StartTrigger>
<ConditionGroup>
<Condition name="StartTime" delay="0" conditionEdge="rising">
<ByValueCondition>
<SimulationTimeCondition value="0" rule="equalTo" />
</ByValueCondition>
</Condition>
</ConditionGroup>
</StartTrigger>
<StopTrigger>
<ConditionGroup>
<Condition name="EndCondition" delay="0" conditionEdge="rising">
<ByEntityCondition>
<TriggeringEntities triggeringEntitiesRule="any">
<EntityRef entityRef="hero" />
</TriggeringEntities>
<EntityCondition>
<TraveledDistanceCondition value="40.0" />
</EntityCondition>
</ByEntityCondition>
</Condition>
</ConditionGroup>
</StopTrigger>
</Act>
</Story>
<StopTrigger/>
</Storyboard>
</OpenSCENARIO>

55
Util/BuildTools/Ad-rss.sh
View File

@ -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" <<EOL
cat >"${CARLA_BUILD_FOLDER}/${ADRSS_BASENAME}/colcon.meta" <<EOL
{
"names": {
"PROJ4": {
"cmake-args": ["-DCMAKE_POSITION_INDEPENDENT_CODE=ON", "-DBUILD_LIBPROJ_SHARED=OFF"]
},
"ad_physics": {
"cmake-args": ["-DBUILD_PYTHON_BINDING=ON", "-DCMAKE_POSITION_INDEPENDENT_CODE=ON", "-DBUILD_SHARED_LIBS=OFF"]
"cmake-args": ["-DBUILD_PYTHON_BINDING=ON", "-DCMAKE_POSITION_INDEPENDENT_CODE=ON", "-DBUILD_SHARED_LIBS=OFF", "-DDISABLE_WARNINGS_AS_ERRORS=ON"]
},
"ad_map_access": {
"cmake-args": ["-DBUILD_PYTHON_BINDING=ON", "-DCMAKE_POSITION_INDEPENDENT_CODE=ON", "-DBUILD_SHARED_LIBS=OFF"]
"cmake-args": ["-DBUILD_PYTHON_BINDING=ON", "-DCMAKE_POSITION_INDEPENDENT_CODE=ON", "-DBUILD_SHARED_LIBS=OFF", "-DDISABLE_WARNINGS_AS_ERRORS=ON"]
},
"ad_map_opendrive_reader": {
"cmake-args": ["-DCMAKE_POSITION_INDEPENDENT_CODE=ON", "-DBUILD_SHARED_LIBS=OFF"]
"cmake-args": ["-DCMAKE_POSITION_INDEPENDENT_CODE=ON", "-DBUILD_SHARED_LIBS=OFF", "-DDISABLE_WARNINGS_AS_ERRORS=ON"],
"dependencies": ["PROJ4"]
},
"ad_rss": {
"cmake-args": ["-DBUILD_PYTHON_BINDING=ON", "-DCMAKE_POSITION_INDEPENDENT_CODE=ON", "-DBUILD_SHARED_LIBS=OFF"]
"cmake-args": ["-DBUILD_PYTHON_BINDING=ON", "-DCMAKE_POSITION_INDEPENDENT_CODE=ON", "-DBUILD_SHARED_LIBS=OFF", "-DDISABLE_WARNINGS_AS_ERRORS=ON"]
},
"ad_rss_map_integration": {
"cmake-args": ["-DBUILD_PYTHON_BINDING=ON", "-DCMAKE_POSITION_INDEPENDENT_CODE=ON", "-DBUILD_SHARED_LIBS=OFF"]
"cmake-args": ["-DBUILD_PYTHON_BINDING=ON", "-DCMAKE_POSITION_INDEPENDENT_CODE=ON", "-DBUILD_SHARED_LIBS=OFF", "-DDISABLE_WARNINGS_AS_ERRORS=ON"]
},
"spdlog": {
"cmake-args": ["-DCMAKE_POSITION_INDEPENDENT_CODE=ON", "-DBUILD_SHARED_LIBS=OFF"]
@ -64,6 +63,7 @@ else
}
EOL
fi
log "Compiling ${ADRSS_BASENAME}."
@ -75,9 +75,8 @@ EOL
export CMAKE_PREFIX_PATH=${CMAKE_PREFIX_PATH}:${CARLA_BUILD_FOLDER}/boost-1.72.0-c8-install
fi
#after a fixing clang compile warnings and errors in components
# -DCMAKE_TOOLCHAIN_FILE=${CARLA_BUILD_FOLDER}/LibStdCppToolChain.cmake
colcon build --packages-up-to ad_rss_map_integration --cmake-args -DCMAKE_BUILD_TYPE=RelWithDebInfo
# enforce sequential executor to reduce the required memory for compilation
colcon build --executor sequential --packages-up-to ad_rss_map_integration --cmake-args -DCMAKE_BUILD_TYPE=RelWithDebInfo -DCMAKE_TOOLCHAIN_FILE=${CARLA_BUILD_FOLDER}/LibStdCppToolChain.cmake
COLCON_RESULT=$?
if (( COLCON_RESULT )); then

2
Util/BuildTools/BuildLibCarla.sh
View File

@ -131,7 +131,7 @@ function build_libcarla {
M_TOOLCHAIN=${LIBSTDCPP_TOOLCHAIN_FILE}
M_BUILD_FOLDER=${LIBCARLA_BUILD_CLIENT_FOLDER}.rss.$(echo "$2" | tr '[:upper:]' '[:lower:]')
M_INSTALL_FOLDER=${LIBCARLA_INSTALL_CLIENT_FOLDER}
CMAKE_EXTRA_OPTIONS="${CMAKE_EXTRA_OPTIONS:+${CMAKE_EXTRA_OPTIONS} }-DBUILD_RSS_VARIANT=ON -DADRSS_INSTALL_DIR=${CARLA_BUILD_FOLDER}/ad-rss-3.0.0/install"
CMAKE_EXTRA_OPTIONS="${CMAKE_EXTRA_OPTIONS:+${CMAKE_EXTRA_OPTIONS} }-DBUILD_RSS_VARIANT=ON -DADRSS_INSTALL_DIR=${CARLA_BUILD_FOLDER}/ad-rss-4.0.0/install"
else
fatal_error "Invalid build configuration \"$1\""
fi

1
Util/BuildTools/Package.sh
View File

@ -139,6 +139,7 @@ if ${DO_CARLA_RELEASE} ; then
copy_if_changed "./PythonAPI/carla/requirements.txt" "${DESTINATION}/PythonAPI/carla/"
copy_if_changed "./PythonAPI/examples/*.py" "${DESTINATION}/PythonAPI/examples/"
copy_if_changed "./PythonAPI/examples/rss/*.py" "${DESTINATION}/PythonAPI/examples/rss/"
copy_if_changed "./PythonAPI/examples/requirements.txt" "${DESTINATION}/PythonAPI/examples/"
copy_if_changed "./PythonAPI/util/*.py" "${DESTINATION}/PythonAPI/util/"

3
Util/BuildTools/Setup.sh
View File

@ -397,8 +397,9 @@ cat >${LIBSTDCPP_TOOLCHAIN_FILE}.gen <<EOL
set(CMAKE_C_COMPILER ${CC})
set(CMAKE_CXX_COMPILER ${CXX})
# disable -Werror since the boost 1.72 doesn't compile with ad_rss without warnings (i.e. the geometry headers)
set(CMAKE_CXX_FLAGS "\${CMAKE_CXX_FLAGS} -std=c++14 -pthread -fPIC" CACHE STRING "" FORCE)
set(CMAKE_CXX_FLAGS "\${CMAKE_CXX_FLAGS} -Werror -Wall -Wextra -Wpedantic" CACHE STRING "" FORCE)
set(CMAKE_CXX_FLAGS "\${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic" CACHE STRING "" FORCE)
set(CMAKE_CXX_FLAGS "\${CMAKE_CXX_FLAGS} -Wdeprecated -Wshadow -Wuninitialized -Wunreachable-code" CACHE STRING "" FORCE)
set(CMAKE_CXX_FLAGS "\${CMAKE_CXX_FLAGS} -Wpessimizing-move -Wold-style-cast -Wnull-dereference" CACHE STRING "" FORCE)
set(CMAKE_CXX_FLAGS "\${CMAKE_CXX_FLAGS} -Wduplicate-enum -Wnon-virtual-dtor -Wheader-hygiene" CACHE STRING "" FORCE)

2
mkdocs.yml
View File

@ -28,7 +28,7 @@ nav:
- 'PTV-Vissim co-simulation': 'adv_ptv.md'
- 'Recorder': 'adv_recorder.md'
- 'Rendering options': 'adv_rendering_options.md'
- 'RSS sensor': 'adv_rss.md'
- 'RSS': 'adv_rss.md'
- 'SUMO co-simulation': 'adv_sumo.md'
- 'Synchrony and time-step': 'adv_synchrony_timestep.md'
- 'Traffic Manager': 'adv_traffic_manager.md'