From eea935952725e112743dc3e44b96d6fb698451fb Mon Sep 17 00:00:00 2001 From: Bernd Gassmann Date: Mon, 23 Mar 2020 17:20:31 +0100 Subject: [PATCH] Integrate AD RSS v3.0.0 Move carla::client::Timestamp ostream operator to Timestamp.h to enable reuse in log messages Use of generated python interfaces of ad::physics, ad::map and ad::rss types RssSensor (and therefore also the RssRestrictor) now work globally on the whole road network. RssCheck: object conversion done in parallel using tbb --- .gitmodules | 9 + CHANGELOG.md | 1 + Docs/rss_lib_integration.md | 61 + LibCarla/cmake/client/CMakeLists.txt | 45 +- LibCarla/source/carla/client/Timestamp.h | 20 + LibCarla/source/carla/rss/RssCheck.cpp | 1376 ++++++++++++----- LibCarla/source/carla/rss/RssCheck.h | 369 ++++- LibCarla/source/carla/rss/RssRestrictor.cpp | 164 +- LibCarla/source/carla/rss/RssRestrictor.h | 50 +- LibCarla/source/carla/rss/RssSensor.cpp | 271 ++-- LibCarla/source/carla/rss/RssSensor.h | 122 +- .../source/carla/sensor/data/RssResponse.h | 82 +- PythonAPI/carla/setup.py | 27 +- PythonAPI/carla/source/libcarla/AdRss.cpp | 242 +-- PythonAPI/carla/source/libcarla/Geom.cpp | 5 + PythonAPI/carla/source/libcarla/World.cpp | 8 - PythonAPI/carla/source/libcarla/libcarla.cpp | 15 +- PythonAPI/examples/manual_control_rss.py | 161 +- Util/BuildTools/Ad-rss.sh | 42 +- Util/BuildTools/BuildLibCarla.sh | 3 +- Util/BuildTools/Setup.sh | 18 +- dependencies/ad-rss/.gitignore | 3 + dependencies/ad-rss/colcon.meta | 22 + dependencies/ad-rss/src/ad-rss-lib | 1 + dependencies/ad-rss/src/map | 1 + dependencies/ad-rss/src/spdlog | 1 + 26 files changed, 2076 insertions(+), 1043 deletions(-) create mode 100644 .gitmodules create mode 100644 Docs/rss_lib_integration.md create mode 100644 dependencies/ad-rss/.gitignore create mode 100644 dependencies/ad-rss/colcon.meta create mode 160000 dependencies/ad-rss/src/ad-rss-lib create mode 160000 dependencies/ad-rss/src/map create mode 160000 dependencies/ad-rss/src/spdlog diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 000000000..fa6d31a01 --- /dev/null +++ b/.gitmodules @@ -0,0 +1,9 @@ +[submodule "dependencies/ad-rss/src/spdlog"] + path = dependencies/ad-rss/src/spdlog + url = https://github.com/gabime/spdlog.git +[submodule "dependencies/ad-rss/src/map"] + path = dependencies/ad-rss/src/map + url = https://github.com/carla-simulator/map +[submodule "dependencies/ad-rss/src/ad-rss-lib"] + path = dependencies/ad-rss/src/ad-rss-lib + url = https://github.com/intel/ad-rss-lib diff --git a/CHANGELOG.md b/CHANGELOG.md index 02e295d05..6e0633ad5 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -2,6 +2,7 @@ * Upgraded to Unreal Engine 4.24 * Fixed autonomous agents' incorrect detection of red traffic lights affecting them + * Upgraded to AD RSS v3.0.0 supporting complex road layouts and i.e. intersections ## CARLA 0.9.8 diff --git a/Docs/rss_lib_integration.md b/Docs/rss_lib_integration.md new file mode 100644 index 000000000..6aaffd712 --- /dev/null +++ b/Docs/rss_lib_integration.md @@ -0,0 +1,61 @@ +

AD Responsibility Sensitive Safety model (RSS) integration

+ +> _This feature is a work in progress, only a Linux build variant is available._ + +This feature integrates the [C++ Library for Responsibility Sensitive Safety](https://github.com/intel/ad-rss-lib) into the CARLA Client library. + +It provides basic implementations of both a **RssSensor**, the situation analysis and response generation by the **ad-rss-lib** and a basic **RssRestrictor** class which applies the restrictions to given vehicle commands. + +The **RssSensor** results can be visualized within CARLA. +[![RSS sensor in CARLA](img/rss_carla_integration.png)](https://www.youtube.com/watch?v=UxKPXPT2T8Q) + + +Please see [C++ Library for Responsibility Sensitive Safety documentation](https://intel.github.io/ad-rss-lib/) and especially the [Background documentation](https://intel.github.io/ad-rss-lib/ad_rss/Overview/) for further details. + + +

Compilation

+ +RSS integration is a Linux-only build variant. +Please see [Build System](dev/build_system.md) for general information. +Furthermore, there are additional prerequisites required for building RSS and its dependencies (see also [**ad-rss-lib**](https://intel.github.io/ad-rss-lib/BUILDING)); briefly: + +Dependencies provided by Ubunutu (>= 16.04): +```bash +$> sudo apt-get install libgtest-dev libpython-dev +``` + +Additional dependencies for the python bindings: +```bash +$> sudo apt-get install castxml +$> pip install --user pygccxml +$> pip install --user https://bitbucket.org/ompl/pyplusplus/get/1.8.1.zip +``` + +Remaining dependencies are part of the __dependencies/ad-rss__ folder as GIT submodules. To properly fetch these, the submodules have to be updated and initialized. +```bash + ad_rss$> git submodule update --init +``` + +Once this is done, the full set of dependencies and RSS components can be built calling the following. + +*LibCarla* with RSS has the be explicitly compiled by + +```sh +make LibCarla.client.rss +``` + +The *PythonAPI* with RSS is built by + +```sh +make PythonAPI.rss +``` + + +

Current state

+

RssSensor

+The RssSensor is supporting the full spectrum of [**ad-rss-lib** v3.0.0 feature set](https://intel.github.io/ad-rss-lib/RELEASE_NOTES_AND_DISCLAIMERS), including intersections and [stay on road](https://intel.github.io/ad-rss-lib/ad_rss_map_integration/HandleRoadBoundaries/) support. + +

RssRestrictor

+The current implementation of the RssRestrictor checks and potentially modifies a given *VehicleControl* generated by e.g. and Automated Driving stack or user input via a *manual_control* client (see the *PythonAPI/examples/manual_control_rss.py*). + +Due to the structure of *VehicleControl* (just throttle, brake, streering values for the car under control), the Restrictor modifies and sets these values to best reach the desired accelerations or decelerations by a given restriction. Due to car physics and the simple control options these might not be met. While in an automated vehicle controller it might be possible to adapt the planned trajectory to meet the requirements of the restrictions and to use a fast control loop (e.g. > 1KHz) to ensure these are followed, the simple *RssRestrictor* intervenes in lateral direction simply by counter steering towards the parallel lane direction and activates the brake, if deceleration requested by RSS, depending on vehicle mass and brake torques provided by the CARLA vehicle. diff --git a/LibCarla/cmake/client/CMakeLists.txt b/LibCarla/cmake/client/CMakeLists.txt index 3d91f9581..b899d4396 100644 --- a/LibCarla/cmake/client/CMakeLists.txt +++ b/LibCarla/cmake/client/CMakeLists.txt @@ -1,16 +1,41 @@ cmake_minimum_required(VERSION 3.5.1) project(libcarla-client) +# Enable/Install RSS VARIANT option (BUILD_RSS_VARIANT "Enables ad-rss-lib based RSS components" OFF) - if (BUILD_RSS_VARIANT) - find_package(ad-rss-lib CONFIG REQUIRED) - get_target_property(ad-rss-lib_file ad-rss-lib LOCATION) - install(FILES ${ad-rss-lib_file} DESTINATION lib) - get_target_property(ad-rss-lib_include_dirs ad-rss-lib INTERFACE_INCLUDE_DIRECTORIES) - foreach(dir ${ad-rss-lib_include_dirs}) - file(GLOB ad-rss-lib_includes "${dir}/*") - install(DIRECTORY ${ad-rss-lib_includes} DESTINATION include/system) + find_package(spdlog CONFIG REQUIRED) + get_target_property(spdlog_include_dirs spdlog::spdlog INTERFACE_INCLUDE_DIRECTORIES) + get_target_property(spdlog_file spdlog::spdlog LOCATION) + foreach(dir ${spdlog_include_dirs}) + file(GLOB spdlog_includes "${dir}/*") + install(DIRECTORY ${spdlog_includes} DESTINATION include/system) + endforeach() + install(FILES ${spdlog_file} DESTINATION lib) + + foreach(ad_lib ad_physics ad_rss ad_map_access ad_map_opendrive_reader ad_rss_map_integration) + find_package(${ad_lib} CONFIG REQUIRED) + get_target_property(${ad_lib}_file ${ad_lib} LOCATION) + install(FILES ${${ad_lib}_file} DESTINATION lib) + + get_filename_component(ad_lib_dir ${${ad_lib}_file} DIRECTORY) + get_filename_component(ad_lib_name_we ${${ad_lib}_file} NAME_WE) + foreach(ad_lib_python ${ad_lib_dir}/${ad_lib_name_we}_python2.a ${ad_lib_dir}/${ad_lib_name_we}_python3.a) + if (EXISTS ${ad_lib_python}) + install(FILES ${ad_lib_python} DESTINATION lib) + endif() + message("Lib: ${ad_lib} : loc: ${${ad_lib}_file}, python: ${ad_lib_python}") + endforeach() + + get_target_property(${ad_lib}_include_dirs ${ad_lib} INTERFACE_INCLUDE_DIRECTORIES) + foreach(dir ${${ad_lib}_include_dirs}) + message(" Dirs: ${dir}") + string(FIND ${dir} "/usr/include" out) + if(NOT ${out} EQUAL 0) + file(GLOB ${ad_lib}_includes "${dir}/*") + install(DIRECTORY ${${ad_lib}_includes} DESTINATION include/system) + endif() + endforeach() endforeach() endif() @@ -231,7 +256,7 @@ if (LIBCARLA_BUILD_RELEASE) if (BUILD_RSS_VARIANT) target_compile_definitions(carla_client PRIVATE RSS_ENABLED) - target_link_libraries(carla_client ad-rss-lib) + target_link_libraries(carla_client ad_rss_map_integration tbb) endif() install(TARGETS carla_client DESTINATION lib) @@ -268,7 +293,7 @@ if (LIBCARLA_BUILD_DEBUG) if (BUILD_RSS_VARIANT) target_compile_definitions(carla_client_debug PRIVATE RSS_ENABLED) - target_link_libraries(carla_client_debug ad-rss-lib) + target_link_libraries(carla_client_debug ad_rss_map_integration) endif() install(TARGETS carla_client_debug DESTINATION lib) diff --git a/LibCarla/source/carla/client/Timestamp.h b/LibCarla/source/carla/client/Timestamp.h index 39ade2bfe..946936e56 100644 --- a/LibCarla/source/carla/client/Timestamp.h +++ b/LibCarla/source/carla/client/Timestamp.h @@ -50,3 +50,23 @@ namespace client { } // namespace client } // namespace carla + + +namespace std { +/** + * \brief standard ostream operator + * + * \param[in/out] out The output stream to write to + * \param[in] timestamp the timestamp to stream out + * + * \returns The stream object. + * + */ +inline std::ostream &operator<<(std::ostream &out, const ::carla::client::Timestamp ×tamp) { + out << "Timestamp(frame=" << std::to_string(timestamp.frame) + << ",elapsed_seconds=" << std::to_string(timestamp.elapsed_seconds) + << ",delta_seconds=" << std::to_string(timestamp.delta_seconds) + << ",platform_timestamp=" << std::to_string(timestamp.platform_timestamp) << ')'; + return out; +} +} // namespace std diff --git a/LibCarla/source/carla/rss/RssCheck.cpp b/LibCarla/source/carla/rss/RssCheck.cpp index ea881b2fd..17352a9c7 100644 --- a/LibCarla/source/carla/rss/RssCheck.cpp +++ b/LibCarla/source/carla/rss/RssCheck.cpp @@ -1,465 +1,1043 @@ -// Copyright (c) 2019 Intel Corporation +// Copyright (c) 2019-2020 Intel Corporation // // This work is licensed under the terms of the MIT license. // For a copy, see . #include "carla/rss/RssCheck.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + #include "carla/client/Map.h" +#include "carla/client/TrafficLight.h" #include "carla/client/Vehicle.h" #include "carla/client/Waypoint.h" -#include "carla/road/element/RoadInfoLaneWidth.h" -#include "ad_rss/core/RssResponseResolving.hpp" -#include "ad_rss/core/RssResponseTransformation.hpp" -#include "ad_rss/core/RssSituationChecking.hpp" -#include "ad_rss/core/RssSituationExtraction.hpp" -#include "ad_rss/state/RssStateOperation.hpp" - -#include -#include -#include +#define DEBUG_TIMING 0 namespace carla { namespace rss { - namespace csd = carla::sensor::data; +void printRoute(std::string const &routeDescr, ::ad::map::route::FullRoute const &route) { + std::cout << routeDescr << std::endl; + for ( auto roadSegment: route.roadSegments) { + for (auto laneSegment: roadSegment.drivableLaneSegments) { + std::cout << "(" << static_cast(laneSegment.laneInterval.laneId) << " | " << std::setprecision(2) << static_cast(laneSegment.laneInterval.start) << ":" << static_cast(laneSegment.laneInterval.end) << ") "; + } + std::cout << std::endl; + } +} // constants for deg-> rad conversion PI / 180 - constexpr float toRadians = static_cast(M_PI) / 180.0f; +constexpr float toRadians = static_cast(M_PI) / 180.0f; - inline float calculateAngleDelta(const float angle1, const float angle2) { - float delta = angle1 - angle2; - delta -= std::floor((delta + 180.f) / 360.f) * 360.f; - return delta; +EgoDynamicsOnRoute::EgoDynamicsOnRoute() + : ego_speed(0.) + , route_heading(0.) + , route_speed_lat(0.) + , route_speed_lon(0.) + , route_accel_lat(0.) + , route_accel_lon(0.) + , avg_route_accel_lat(0.) + , avg_route_accel_lon(0.) { + timestamp.elapsed_seconds = 0.; +} + +RssCheck::RssCheck(): + _ego_vehicle_dynamics(GetDefaultVehicleDynamics()), + _other_vehicle_dynamics(GetDefaultVehicleDynamics()), + _road_boundaries_mode(RoadBoundariesMode::Off), + _visualization_mode(VisualizationMode::All) { + + _logger = spdlog::get("RssCheck"); + if (!_logger) + { + _logger = spdlog::stdout_color_mt("RssCheck"); } + 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); - RssCheck::RssCheck() { - _rssSituationExtraction = std::make_shared<::ad_rss::core::RssSituationExtraction>(); - _rssSituationChecking = std::make_shared<::ad_rss::core::RssSituationChecking>(); - _rssResponseResolving = std::make_shared<::ad_rss::core::RssResponseResolving>(); - - _egoVehicleDynamics = std::make_shared<::ad_rss::world::RssDynamics>(); - _otherVehicleDynamics = std::make_shared<::ad_rss::world::RssDynamics>(); - - ::ad_rss::world::RssDynamics defaultDynamics; - - defaultDynamics.alphaLon.accelMax = ::ad_rss::physics::Acceleration(3.5); - defaultDynamics.alphaLon.brakeMax = ::ad_rss::physics::Acceleration(8.); - defaultDynamics.alphaLon.brakeMin = ::ad_rss::physics::Acceleration(4.); - defaultDynamics.alphaLon.brakeMinCorrect = ::ad_rss::physics::Acceleration(3); - defaultDynamics.alphaLat.accelMax = ::ad_rss::physics::Acceleration(0.2); - defaultDynamics.alphaLat.brakeMin = ::ad_rss::physics::Acceleration(0.8); - defaultDynamics.responseTime = ::ad_rss::physics::Duration(1.); - - *_egoVehicleDynamics = defaultDynamics; - *_otherVehicleDynamics = defaultDynamics; + _timing_logger = spdlog::get("RssCheckTiming"); + if (!_timing_logger) + { + _timing_logger = spdlog::stdout_color_mt("RssCheckTiming"); } + _timing_logger->set_level(spdlog::level::off); +} - RssCheck::~RssCheck() = default; +RssCheck::~RssCheck() +{ +} - const ::ad_rss::world::RssDynamics &RssCheck::getEgoVehicleDynamics() const { - return *_egoVehicleDynamics; - } +::ad::rss::world::RssDynamics RssCheck::GetDefaultVehicleDynamics() const { + ::ad::rss::world::RssDynamics default_dynamics; - void RssCheck::setEgoVehicleDynamics(const ::ad_rss::world::RssDynamics &dynamics) { - *_egoVehicleDynamics = 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.responseTime = ::ad::physics::Duration(.5); - const ::ad_rss::world::RssDynamics &RssCheck::getOtherVehicleDynamics() const { - return *_otherVehicleDynamics; - } + return default_dynamics; +} - void RssCheck::setOtherVehicleDynamics(const ::ad_rss::world::RssDynamics &dynamics) { - *_otherVehicleDynamics = dynamics; - } +const ::ad::rss::world::RssDynamics& RssCheck::GetEgoVehicleDynamics() const { + return _ego_vehicle_dynamics; +} - bool RssCheck::checkObjects(cc::Timestamp const & timestamp, cc::World &world, - carla::SharedPtr const & vehicles, - carla::SharedPtr const & carlaEgoActor, - carla::SharedPtr const & clientMap, - ::ad_rss::state::ProperResponse &response, - ::ad_rss::world::AccelerationRestriction &accelerationRestriction, - ::ad_rss::world::Velocity &rssEgoVelocity, bool visualizeResults) { - const auto carlaEgoVehicle = boost::dynamic_pointer_cast(carlaEgoActor); +void RssCheck::SetEgoVehicleDynamics(const ::ad::rss::world::RssDynamics &ego_vehicle_dynamics) { + _ego_vehicle_dynamics = ego_vehicle_dynamics; +} - ::ad_rss::world::WorldModel worldModel; - worldModel.timeIndex = timestamp.frame; +const ::ad::rss::world::RssDynamics& RssCheck::GetOtherVehicleDynamics() const { + return _other_vehicle_dynamics; +} - ::ad_rss::world::Object egoVehicle; - ::ad_rss::world::RssDynamics egoDynamics; - initVehicle(egoVehicle); - initEgoVehicleDynamics(egoDynamics); - egoVehicle.objectId = carlaEgoVehicle->GetId(); - egoVehicle.objectType = ::ad_rss::world::ObjectType::EgoVehicle; +void RssCheck::SetOtherVehicleDynamics(const ::ad::rss::world::RssDynamics &other_vehicle_dynamics) { + _other_vehicle_dynamics = other_vehicle_dynamics; +} - cg::Location egoVehicleLocation = carlaEgoVehicle->GetLocation(); - auto egoClientWaypoint = clientMap->GetWaypoint(egoVehicleLocation, false); +const ::carla::rss::RoadBoundariesMode& RssCheck::GetRoadBoundariesMode() const { + return _road_boundaries_mode; +} - if (egoClientWaypoint != nullptr) { - // ego vehicle is located on a lane marked for driving - auto egoWaypointLoc = egoClientWaypoint->GetTransform(); - auto egoLaneId = egoClientWaypoint->GetLaneId(); - auto egoVelocity = carlaEgoVehicle->GetVelocity(); - auto egoVehicleTransform = carlaEgoVehicle->GetTransform(); +void RssCheck::SetRoadBoundariesMode(const ::carla::rss::RoadBoundariesMode &road_boundaries_mode) { + _road_boundaries_mode = road_boundaries_mode; +} - // calculate driving direction - auto yawDiff = calculateAngleDelta(egoWaypointLoc.rotation.yaw, egoVehicleTransform.rotation.yaw); +void RssCheck::AppendRoutingTarget(::carla::geom::Transform const &routing_target) { + _routing_targets_to_append.push_back(::ad::map::point::createENUPoint(routing_target.location.x, -1. * routing_target.location.y, 0.)); +} - bool drivingInLaneDirection = true; - if (std::abs(yawDiff) > 45.f) { - drivingInLaneDirection = false; - } - - // calculate road direction - bool drivingInRoadDirection = (egoLaneId > 0) ^ drivingInLaneDirection; - - auto &carlaRoadMap = clientMap->GetMap(); - auto egoWaypoint = carlaRoadMap.GetWaypoint(egoVehicleLocation); - auto &egoLane = carlaRoadMap.GetLane(*egoWaypoint); - auto egoRoad = egoLane.GetRoad(); - - ::ad_rss::world::RoadArea roadArea; - ::ad_rss::world::RoadSegment roadSegment; - - // generate road area - for (auto &laneSection : egoRoad->GetLaneSections()) { - for (const auto &pair : laneSection.GetLanes()) { - const auto &lane = pair.second; - if ((static_cast(lane.GetType()) & - static_cast(carla::road::Lane::LaneType::Driving)) > 0) { - ::ad_rss::world::LaneSegment laneSegment; - // assumption: only one segment per road - - auto laneLength = lane.GetLength(); - // evaluate width at lane start - double pos = laneSection.GetDistance(); - const auto lane_width_info = lane.GetInfo(pos); - double laneWidth = 0.0; - if (lane_width_info != nullptr) { - laneWidth = lane_width_info->GetPolynomial().Evaluate(pos); - } - - convertAndSetLaneSegmentId(lane, laneSegment); - if ((lane.GetId() < 0) ^ drivingInRoadDirection) { - laneSegment.drivingDirection = ::ad_rss::world::LaneDrivingDirection::Negative; - } else { - laneSegment.drivingDirection = ::ad_rss::world::LaneDrivingDirection::Positive; - } - - // lane segment is assumed to be strait and evenly wide, so minimum - // and maximum length and width are set to the same values - laneSegment.length.minimum = ::ad_rss::physics::Distance(laneLength); - laneSegment.length.maximum = laneSegment.length.minimum; - laneSegment.width.minimum = ::ad_rss::physics::Distance(laneWidth); - laneSegment.width.maximum = laneSegment.width.minimum; - - roadSegment.push_back(laneSegment); - } - } - std::sort( - roadSegment.begin(), roadSegment.end(), - [&drivingInRoadDirection](::ad_rss::world::LaneSegment const &f, - ::ad_rss::world::LaneSegment const &s) { - return (f.id <= s.id) ^ drivingInRoadDirection; - }); - roadArea.push_back(roadSegment); - } - - calculateLatLonVelocities(egoWaypointLoc, egoVelocity, egoVehicle, !drivingInLaneDirection); - rssEgoVelocity = egoVehicle.velocity; - - auto egoBounds = getVehicleBounds(*carlaEgoVehicle); - - calculateOccupiedRegions(egoBounds, egoVehicle.occupiedRegions, carlaRoadMap, drivingInRoadDirection, - egoVehicleTransform); - - for (const auto &actor : *vehicles) { - const auto vehicle = boost::dynamic_pointer_cast(actor); - if (vehicle == nullptr) { - continue; - } - if (vehicle->GetId() == carlaEgoVehicle->GetId()) { - continue; - } - ::ad_rss::world::Object otherVehicle; - ::ad_rss::world::RssDynamics otherDynamics; - initVehicle(otherVehicle); - initOtherVehicleDynamics(otherDynamics); - otherVehicle.objectId = vehicle->GetId(); - otherVehicle.objectType = ::ad_rss::world::ObjectType::OtherVehicle; - - auto otherVehicleLoc = vehicle->GetLocation(); - auto otherVehicleClientWaypoint = clientMap->GetWaypoint(otherVehicleLoc); - - // same road - if (egoClientWaypoint->GetRoadId() == otherVehicleClientWaypoint->GetRoadId()) { - auto otherVehicleTransform = vehicle->GetTransform(); - auto otherBounds = getVehicleBounds(*vehicle); - auto otherWaypointLoc = otherVehicleClientWaypoint->GetTransform(); - auto otherVelocity = vehicle->GetVelocity(); - calculateLatLonVelocities(otherWaypointLoc, otherVelocity, otherVehicle, false); - - ::ad_rss::world::Scene scene; - - calculateOccupiedRegions(otherBounds, otherVehicle.occupiedRegions, carlaRoadMap, drivingInRoadDirection, - otherVehicleTransform); - - auto vehicleDirectionDiff = - calculateAngleDelta(otherVehicleTransform.rotation.yaw, egoVehicleTransform.rotation.yaw); - - bool drivingInSameDirection = true; - if (std::abs(vehicleDirectionDiff) > 90.f) { - drivingInSameDirection = false; - } - - if (drivingInSameDirection) { - scene.situationType = ad_rss::situation::SituationType::SameDirection; - } else { - scene.situationType = ad_rss::situation::SituationType::OppositeDirection; - } - - scene.object = otherVehicle; - scene.objectRssDynamics = otherDynamics; - scene.egoVehicle = egoVehicle; - scene.egoVehicleRoad = roadArea; - worldModel.scenes.push_back(scene); - } - } - worldModel.egoVehicleRssDynamics = egoDynamics; - - ::ad_rss::situation::SituationSnapshot situationSnapshot; - bool result = _rssSituationExtraction->extractSituations(worldModel, situationSnapshot); - - ::ad_rss::state::RssStateSnapshot stateSnapshot; - if (result) { - result = _rssSituationChecking->checkSituations(situationSnapshot, stateSnapshot); - } - if (result) { - result = _rssResponseResolving->provideProperResponse(stateSnapshot, response); - } - if (result) { - result = ::ad_rss::core::RssResponseTransformation::transformProperResponse(worldModel, response, - accelerationRestriction); - } - - if (result && visualizeResults) { - visualizeRssResults(stateSnapshot, egoVehicleLocation, egoVehicleTransform, world); - } - return result; +const std::vector<::carla::geom::Transform> RssCheck::GetRoutingTargets() const { + std::vector<::carla::geom::Transform> routing_targets; + if ( withinValidInputRange(_routing_targets) ) + { + for (auto const &target : _routing_targets) { + ::carla::geom::Transform routing_target; + routing_target.location.x = static_cast(target.x); + routing_target.location.y = static_cast(-target.y); + routing_target.location.z = 0.f; + routing_targets.push_back(routing_target); } + } + return routing_targets; +} + +void RssCheck::ResetRoutingTargets() { + _routing_targets.clear(); + _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(); +} + +bool RssCheck::CheckObjects(carla::client::Timestamp const ×tamp, + carla::SharedPtr const &actors, + carla::SharedPtr const &carla_ego_actor, + ::ad::rss::state::ProperResponse &output_response, + ::ad::rss::world::AccelerationRestriction &output_acceleration_restriction, + ::ad::rss::state::RssStateSnapshot &output_rss_state_snapshot, + EgoDynamicsOnRoute &output_rss_ego_dynamics_on_route) { + try { + double const time_since_epoch_check_start_ms = std::chrono::duration(std::chrono::system_clock::now().time_since_epoch()).count(); +#if DEBUG_TIMING + std::cout << "--- time: " << timestamp.frame << ", " << timestamp.elapsed_seconds << std::endl; + auto tStart = std::chrono::high_resolution_clock::now(); + auto tEnd = std::chrono::high_resolution_clock::now(); + std::cout << "-> SC " << std::chrono::duration(tEnd-tStart).count() << " start checkObjects" << std::endl; +#endif + + const auto carla_ego_vehicle = boost::dynamic_pointer_cast(carla_ego_actor); + +#if DEBUG_TIMING + tEnd = std::chrono::high_resolution_clock::now(); + std::cout << "-> ME " << std::chrono::duration(tEnd-tStart).count() << " before MapMatching" << std::endl; +#endif + + // 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)); + + if ( ::ad::map::point::isValid(_carla_rss_state.ego_match_object.enuPosition.centerPoint, false) ) + { + // check for bigger position jumps of the ego vehicle + auto const travelled_distance = ::ad::map::point::distance(_carla_rss_state.ego_match_object.enuPosition.centerPoint, ego_match_object.enuPosition.centerPoint); + if ( travelled_distance > ::ad::physics::Distance(10.) ) + { + _logger->warn("Jump in ego vehicle position detected {} -> {}! Force reroute!", _carla_rss_state.ego_match_object.enuPosition.centerPoint, ego_match_object.enuPosition.centerPoint); + DropRoute(); + } + } + + _carla_rss_state.ego_match_object = ego_match_object; + + _logger->debug("MapMatch:: {}", _carla_rss_state.ego_match_object); + +#if DEBUG_TIMING + tEnd = std::chrono::high_resolution_clock::now(); + std::cout << "-> ME " << std::chrono::duration(tEnd-tStart).count() << " after ego MapMatching" << std::endl; +#endif + + UpdateRoute(_carla_rss_state); + +#if DEBUG_TIMING + tEnd = std::chrono::high_resolution_clock::now(); + std::cout << "-> RU " << std::chrono::duration(tEnd-tStart).count() << " after route update " << std::endl; +#endif + + _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); + + CreateWorldModel(timestamp, *actors, *carla_ego_vehicle, _carla_rss_state); + +#if DEBUG_TIMING + tEnd = std::chrono::high_resolution_clock::now(); + std::cout << "-> WM " << std::chrono::duration(tEnd-tStart).count() << " after create world model " << std::endl; +#endif + + PerformCheck(_carla_rss_state); + +#if DEBUG_TIMING + tEnd = std::chrono::high_resolution_clock::now(); + std::cout << "-> CH " << std::chrono::duration(tEnd-tStart).count() << " end RSS check" << std::endl; +#endif + + AnalyseCheckResults(_carla_rss_state); + +#if DEBUG_TIMING + tEnd = std::chrono::high_resolution_clock::now(); + std::cout << "-> VI " << std::chrono::duration(tEnd-tStart).count() << " start store viz" << std::endl; +#endif + + StoreVisualizationResults(_carla_rss_state); + +#if DEBUG_TIMING + tEnd = std::chrono::high_resolution_clock::now(); + std::cout << "-> VI " << std::chrono::duration(tEnd-tStart).count() << " end store viz" << std::endl; +#endif + + _carla_rss_state.ego_dynamics_on_route.time_since_epoch_check_end_ms = std::chrono::duration(std::chrono::system_clock::now().time_since_epoch()).count(); + + //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_rss_ego_dynamics_on_route = _carla_rss_state.ego_dynamics_on_route; + if (_carla_rss_state.dangerous_state) { + _logger->debug("===== ROUTE NOT SAFE ====="); + } + else { + _logger->debug("===== ROUTE SAFE ====="); + } + +#if DEBUG_TIMING + tEnd = std::chrono::high_resolution_clock::now(); + std::cout << "-> EC " << std::chrono::duration(tEnd-tStart).count() << " end check objects" << std::endl; +#endif + + return true; + } + catch (...) { + _logger->error("Exception -> Check failed"); return false; } +} - void RssCheck::initVehicle(::ad_rss::world::Object &vehicle) const { - vehicle.velocity.speedLon = ::ad_rss::physics::Speed(0.); - vehicle.velocity.speedLat = ::ad_rss::physics::Speed(0.); +::ad::map::match::Object RssCheck::GetMatchObject(carla::client::Vehicle const &carla_vehicle, ::ad::physics::Distance const &match_distance) const { + + ::ad::map::match::Object match_object; + + auto const vehicle_transform = carla_vehicle.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.heading = ::ad::map::point::createENUHeading((-1 * vehicle_transform.rotation.yaw) * toRadians); + + 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); + 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.)); + + return match_object; +} + +::ad::physics::Speed RssCheck::GetSpeed(carla::client::Vehicle const &carla_vehicle) const { + auto const velocity = carla_vehicle.GetVelocity(); + ::ad::physics::Speed speed(std::sqrt(velocity.x * velocity.x + velocity.y * velocity.y)); + return speed; +} + +void RssCheck::UpdateRoute(CarlaRssState &carla_rss_state) { + + _logger->debug("Update route start: {}", carla_rss_state.ego_route); + + // remove the parts of the route already taken, try to prepend route sections (i.e. when driving backwards) + // try to ensure that the back of the vehicle is still within the route to support orientation calculation + ::ad::map::point::ParaPointList all_lane_matches; + for (auto reference_point: {::ad::map::match::ObjectReferencePoints::RearRight, ::ad::map::match::ObjectReferencePoints::RearLeft}) { + auto const &reference_position = carla_rss_state.ego_match_object.mapMatchedBoundingBox.referencePointPositions[size_t(reference_point)]; + auto const para_points = ::ad::map::match::getParaPoints(reference_position); + all_lane_matches.insert(all_lane_matches.end(), para_points.begin(), para_points.end()); } - void RssCheck::initEgoVehicleDynamics(::ad_rss::world::RssDynamics &egoDynamics) const { - egoDynamics = getEgoVehicleDynamics(); + auto shorten_route_result = ::ad::map::route::shortenRoute(all_lane_matches, carla_rss_state.ego_route, + ::ad::map::route::ShortenRouteMode::DontCutIntersectionAndPrependIfSucceededBeforeRoute); + if ( shorten_route_result == ::ad::map::route::ShortenRouteResult::SucceededIntersectionNotCut ) + { + shorten_route_result = ::ad::map::route::ShortenRouteResult::Succeeded; } - void RssCheck::initOtherVehicleDynamics(::ad_rss::world::RssDynamics &dynamics) const { - dynamics = getOtherVehicleDynamics(); - } - - void RssCheck::calculateLatLonVelocities(const cg::Transform &laneLocation, - const cg::Vector3D &velocity, ::ad_rss::world::Object &rssObject, - bool inverseDirection) const { - double roadDirSine = std::sin(laneLocation.rotation.yaw * toRadians); - double roadDirCosine = std::cos(laneLocation.rotation.yaw * toRadians); - double factor = 1.0; - if (inverseDirection) { - factor = -1.0; + bool routing_target_check_finished = false; + while ((!_routing_targets.empty()) && (!routing_target_check_finished)) { + auto const next_target = _routing_targets.front(); + auto const &distance_to_next_target = ::ad::map::point::distance(next_target, carla_rss_state.ego_match_object.enuPosition.centerPoint); + if ( distance_to_next_target < ::ad::physics::Distance(3.) ) { + _routing_targets.erase(_routing_targets.begin()); + _logger->debug("Next target reached: {}; remaining targets: {}", next_target, _routing_targets); } - rssObject.velocity.speedLon = - ::ad_rss::physics::Speed(std::abs(roadDirCosine * velocity.x + roadDirSine * velocity.y)); - rssObject.velocity.speedLat = - ::ad_rss::physics::Speed(factor * (-1.0 * roadDirSine * velocity.x + roadDirCosine * velocity.y)); - } - - void RssCheck::convertAndSetLaneSegmentId(const carla::road::Lane &lane, - ::ad_rss::world::LaneSegment &laneSegment) const { - uint64_t laneSegmentId = calculateLaneSegmentId(lane); - laneSegment.id = laneSegmentId; - } - -// return front right, back right, back left, front left bounds - std::array RssCheck::getVehicleBounds(const cc::Vehicle &vehicle) const { - const auto &box = vehicle.GetBoundingBox(); - const auto &transform = vehicle.GetTransform(); - const auto location = transform.location + box.location; - const auto yaw = transform.rotation.yaw * toRadians; - const float cosine = std::cos(yaw); - const float sine = std::sin(yaw); - cg::Location frontExtent{cosine *box.extent.x, sine *box.extent.x, 0.0f}; - cg::Location rightExtent{-sine * box.extent.y, cosine *box.extent.y, 0.0f}; - return {location + frontExtent + rightExtent, location - frontExtent + rightExtent, - location - frontExtent - rightExtent, location + frontExtent - rightExtent}; - } - - uint64_t RssCheck::calculateLaneSegmentId(const carla::road::Lane &lane) const { - uint64_t laneSegmentId = lane.GetRoad()->GetId(); - laneSegmentId = laneSegmentId << 32; - uint32_t sectionAndLane = lane.GetLaneSection()->GetId(); - sectionAndLane <<= 16; - int16_t laneId = static_cast(lane.GetId()); - sectionAndLane |= (0xffff & (laneId + 256)); - laneSegmentId |= sectionAndLane; - return laneSegmentId; - } - - void RssCheck::calculateOccupiedRegions(const std::array &bounds, - ::ad_rss::world::OccupiedRegionVector &occupiedRegions, - const carla::road::Map &carlaRoadMap, const bool drivingInRoadDirection, - const cg::Transform &vehicleTransform) const { - bool regionValid = true; - for (const auto &bound : bounds) { - auto boundWaypoint = carlaRoadMap.GetWaypoint(bound); - - if (!boundWaypoint.has_value()) { - // vehicle not entirely on road --> stop evaluation - regionValid = false; - break; - } - auto &lane = carlaRoadMap.GetLane(*boundWaypoint); - auto road = lane.GetRoad(); - uint64_t laneSegmentId = calculateLaneSegmentId(carlaRoadMap.GetLane(*boundWaypoint)); - - auto length = road->GetLength(); - auto lonPos = boundWaypoint->s; - if (!drivingInRoadDirection) { - lonPos = length - lonPos; - } - double lon = std::max(0.0, std::min(lonPos / length, 1.0)); - - auto waypointLocation = carlaRoadMap.ComputeTransform(*boundWaypoint); - double headingSine = std::sin(waypointLocation.rotation.yaw * toRadians); - double headingCosine = std::cos(waypointLocation.rotation.yaw * toRadians); - - auto yawDelta = calculateAngleDelta(waypointLocation.rotation.yaw, vehicleTransform.rotation.yaw); - bool inLaneDirection = true; - if (std::abs(yawDelta) > 45.f) { - inLaneDirection = false; - } - - auto width = carlaRoadMap.GetLaneWidth(*boundWaypoint); - double latOffset = (headingSine * (waypointLocation.location.x - bound.x) - - headingCosine * (waypointLocation.location.y - bound.y)); - if (!inLaneDirection) { - latOffset *= -1.0; - } - - double lat = std::max(0.0, std::min(0.5 + latOffset / width, 1.0)); - - // find or create occupied region - auto regionIt = std::find_if( - occupiedRegions.begin(), occupiedRegions.end(), - [laneSegmentId](const ::ad_rss::world::OccupiedRegion ®ion) { - return region.segmentId == laneSegmentId; - }); - if (regionIt == occupiedRegions.end()) { - ::ad_rss::world::OccupiedRegion newRegion; - newRegion.segmentId = laneSegmentId; - newRegion.lonRange.minimum = ::ad_rss::physics::ParametricValue(lon); - newRegion.lonRange.maximum = newRegion.lonRange.minimum; - newRegion.latRange.minimum = ::ad_rss::physics::ParametricValue(lat); - newRegion.latRange.maximum = newRegion.latRange.minimum; - occupiedRegions.push_back(newRegion); - } else { - ::ad_rss::physics::ParametricValue const lonParam(lon); - ::ad_rss::physics::ParametricValue const latParam(lat); - - regionIt->lonRange.minimum = std::min(regionIt->lonRange.minimum, lonParam); - regionIt->lonRange.maximum = std::max(regionIt->lonRange.maximum, lonParam); - regionIt->latRange.minimum = std::min(regionIt->latRange.minimum, latParam); - regionIt->latRange.maximum = std::max(regionIt->latRange.maximum, latParam); - } - } - // expand regions if more than one, ordered from right to left - ::ad_rss::physics::ParametricValue lonMinParam(1.0); - ::ad_rss::physics::ParametricValue lonMaxParam(0.0); - - for (auto ®ion : occupiedRegions) { - lonMinParam = std::min(lonMinParam, region.lonRange.minimum); - lonMaxParam = std::max(lonMaxParam, region.lonRange.maximum); - if (region != occupiedRegions.front()) { - // not the most right one, so extend lat maximum - region.latRange.maximum = ::ad_rss::physics::ParametricValue(1.0); - } - if (region != occupiedRegions.back()) { - // not the most left one, so extend lat minimum - region.latRange.minimum = ::ad_rss::physics::ParametricValue(0.0); - } - } - for (auto ®ion : occupiedRegions) { - region.lonRange.minimum = lonMinParam; - region.lonRange.maximum = lonMaxParam; + else { + routing_target_check_finished = true; } } - void RssCheck::visualizeRssResults(::ad_rss::state::RssStateSnapshot stateSnapshot, - const cg::Location &egoVehicleLocation, - const cg::Transform &egoVehicleTransform, - cc::World &world) const { - cc::DebugHelper dh = world.MakeDebugHelper(); - for (std::size_t i = 0; i < stateSnapshot.individualResponses.size(); ++i) { - ::ad_rss::state::RssState &state = stateSnapshot.individualResponses[i]; - carla::rpc::ActorId vehicleId = static_cast(state.objectId); - carla::SharedPtr vehicleList = - world.GetActors(std::vector{vehicleId}); - cg::Location egoPoint = egoVehicleLocation; - egoPoint.z += 0.05f; - const auto yaw = egoVehicleTransform.rotation.yaw; - const float cosine = std::cos(yaw * toRadians); - const float sine = std::sin(yaw * toRadians); - cg::Location lineOffset{-sine * 0.1f, cosine * 0.1f, 0.0f}; - for (const auto &actor : *vehicleList) { - const auto vehicle = boost::dynamic_pointer_cast(actor); - cg::Location point = vehicle->GetLocation(); - point.z += 0.05f; - csd::Color lonColor{0u, 255u, 0u}; - csd::Color latLColor = lonColor; - csd::Color latRColor = lonColor; - csd::Color indicatorColor = lonColor; - bool dangerous = ::ad_rss::state::isDangerous(state); - if (dangerous) { - indicatorColor = csd::Color{255u, 0u, 0u}; + bool reroute_required = false; + if ( !_routing_targets_to_append.empty() ) { + reroute_required = true; + _routing_targets.insert(_routing_targets.end(), _routing_targets_to_append.begin(), _routing_targets_to_append.end()); + _logger->debug("Appending new routing targets: {}; resulting targets: {}", _routing_targets_to_append, _routing_targets); + _routing_targets_to_append.clear(); + } + + ::ad::physics::Distance const route_target_length(50.); + + if ((!reroute_required) && (shorten_route_result == ::ad::map::route::ShortenRouteResult::Succeeded)) { + std::vector<::ad::map::route::FullRoute> additional_routes; + auto const route_valid = ::ad::map::route::extendRouteToDistance(carla_rss_state.ego_route, route_target_length, + additional_routes); + + if (route_valid) { + if (additional_routes.size() > 0u) { + // take a random extension to the route + std::size_t route_index = static_cast(std::rand()) % (additional_routes.size() + 1); + if ( route_index < additional_routes.size() ) { + // we decided for one of the additional routes + _logger->debug("Additional Routes: {}->{}", additional_routes.size(), route_index); + carla_rss_state.ego_route = additional_routes[route_index]; } + else { + // we decided for the extension within route, nothing to be done + _logger->debug("Additional Routes: expand current"); + } + } + } + else { + reroute_required = true; + } + } + else { + // on all other results we recreate the route + reroute_required = true; + } + + //create the route if required + if (reroute_required) { + //try to create routes + std::vector<::ad::map::route::FullRoute> all_new_routes; + for (const auto &position : carla_rss_state.ego_match_object.mapMatchedBoundingBox.referencePointPositions[int32_t(::ad::map::match::ObjectReferencePoints::Center)]) { + auto start_point = position.lanePoint.paraPoint; + auto projected_start_point = start_point; + if (!::ad::map::lane::isHeadingInLaneDirection(start_point, carla_rss_state.ego_match_object.enuPosition.heading)) { + _logger->debug("EgoVehicle heading in opposite lane direction"); + if (::ad::map::lane::projectPositionToLaneInHeadingDirection(start_point, carla_rss_state.ego_match_object.enuPosition.heading, projected_start_point)) { + _logger->debug("Projected to lane {}", projected_start_point.laneId); + } + } + _logger->debug("Route start_point: {}, projected_start_point: {}", start_point, projected_start_point); + auto routing_start_point = ::ad::map::route::planning::createRoutingPoint(projected_start_point, carla_rss_state.ego_match_object.enuPosition.heading); + if (!_routing_targets.empty() && ::ad::map::point::isValid(_routing_targets)) { + auto new_route = ::ad::map::route::planning::planRoute( + routing_start_point, _routing_targets, ::ad::map::route::RouteCreationMode::AllRoutableLanes); + all_new_routes.push_back(new_route); + } + else { + auto new_routes = ::ad::map::route::planning::predictRoutesOnDistance( + routing_start_point,route_target_length, ::ad::map::route::RouteCreationMode::AllRoutableLanes); + + for (const auto & new_route : new_routes) { + //extend route with all lanes + all_new_routes.push_back(new_route); + } + } + } + + _logger->debug("New routes: {}", all_new_routes.size()); + + if ( !all_new_routes.empty() ) { + // take a random route + std::size_t route_index = static_cast(std::rand()) % (all_new_routes.size()); + carla_rss_state.ego_route = all_new_routes[route_index]; + } + } + + _logger->debug("Update route result: {}", carla_rss_state.ego_route); +} + +EgoDynamicsOnRoute RssCheck::CalculateEgoDynamicsOnRoute(carla::client::Timestamp const ¤t_timestamp, + double const &time_since_epoch_check_start_ms, + carla::client::Vehicle const &carla_vehicle, + ::ad::map::match::Object match_object, + ::ad::map::route::FullRoute const &route, + EgoDynamicsOnRoute const &last_dynamics) const +{ + EgoDynamicsOnRoute 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; + + 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); + if (object_center.isValid()) { + auto lane_center_point = object_center.queryPosition; + auto lane_center_point_enu = ::ad::map::lane::getENULanePoint(lane_center_point); + if ( std::fabs(new_dynamics.route_heading) > ::ad::map::point::ENUHeading(M_PI) ) + { + // if the actual center point is already outside, try to use this extended object center for the route heading calculation + new_dynamics.route_heading = ::ad::map::lane::getENUHeading(border, lane_center_point_enu); + } + + if (object_center.laneSegmentIterator->laneInterval.wrongWay) { + // driving on the wrong lane, so we have to project to nominal route direction + ::ad::map::lane::projectPositionToLaneInHeadingDirection(lane_center_point, new_dynamics.route_heading, lane_center_point); + lane_center_point_enu = ::ad::map::lane::getENULanePoint(lane_center_point); + } + new_dynamics.route_nominal_center = lane_center_point_enu; + + } + else { + // the ego vehicle is completely outside the route, so we can't update the values + new_dynamics.route_nominal_center = last_dynamics.route_nominal_center; + new_dynamics.route_heading = last_dynamics.route_heading; + } + + + new_dynamics.heading_diff = ::ad::map::point::normalizeENUHeading(new_dynamics.route_heading - new_dynamics.ego_heading); + new_dynamics.route_speed_lon = std::fabs(std::cos(static_cast(new_dynamics.heading_diff))) * new_dynamics.ego_speed; + new_dynamics.route_speed_lat = std::sin(static_cast(new_dynamics.heading_diff)) * new_dynamics.ego_speed; + + bool keep_last_acceleration = true; + if ( last_dynamics.timestamp.elapsed_seconds > 0. ) { + ::ad::physics::Duration const delta_time(current_timestamp.elapsed_seconds - last_dynamics.timestamp.elapsed_seconds); + if ( delta_time > ::ad::physics::Duration(0.0001) ) { + try { + new_dynamics.route_accel_lat = (new_dynamics.route_speed_lat - last_dynamics.route_speed_lat)/delta_time; + new_dynamics.avg_route_accel_lat = ((last_dynamics.avg_route_accel_lat * 2.) + new_dynamics.route_accel_lat)/3.; + new_dynamics.route_accel_lon = (new_dynamics.route_speed_lon - last_dynamics.route_speed_lon)/delta_time; + new_dynamics.avg_route_accel_lon = ((last_dynamics.avg_route_accel_lon * 2.) + new_dynamics.route_accel_lon)/3.; + + if ( new_dynamics.avg_route_accel_lat == ::ad::physics::Acceleration(0.) ) + { + // prevent from underrun + new_dynamics.avg_route_accel_lat = ::ad::physics::Acceleration(0.); + } + if ( new_dynamics.avg_route_accel_lon == ::ad::physics::Acceleration(0.) ) + { + // prevent from underrun + new_dynamics.avg_route_accel_lon = ::ad::physics::Acceleration(0.); + } + keep_last_acceleration = false; + } + catch(...) { + } + } + } + + if (keep_last_acceleration) { + new_dynamics.route_accel_lat = last_dynamics.route_accel_lat; + new_dynamics.avg_route_accel_lat = last_dynamics.avg_route_accel_lat; + new_dynamics.route_accel_lon = last_dynamics.route_accel_lon; + new_dynamics.avg_route_accel_lon = last_dynamics.avg_route_accel_lon; + } + + // check if the center point (and only the center point) is still found on the route + ::ad::map::point::ParaPointList in_lane_matches; + for (auto &match_position: match_object.mapMatchedBoundingBox.referencePointPositions[int32_t(::ad::map::match::ObjectReferencePoints::Center)]) { + if ( match_position.type == ::ad::map::match::MapMatchedPositionType::LANE_IN ) { + in_lane_matches.push_back(match_position.lanePoint.paraPoint); + } + } + auto const object_in_lane_center = ::ad::map::route::findNearestWaypoint(in_lane_matches, route); + new_dynamics.ego_center_within_route = object_in_lane_center.isValid(); + // evaluated by AnalyseResults + 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()); + 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()); + new_dynamics.min_stopping_distance = ::ad::physics::Distance(100.); + } + + _logger->trace("CalculateEgoDynamicsOnRoute: route-section {} -> dynamics: {}", object_route, new_dynamics); + return new_dynamics; +} + + +::ad::map::landmark::LandmarkIdSet RssCheck::GetGreenTrafficLightsOnRoute(carla::client::ActorList const &actors, + ::ad::map::route::FullRoute const&route) const +{ + ::ad::map::landmark::LandmarkIdSet green_traffic_lights; + auto next_intersection = ::ad::map::intersection::Intersection::getNextIntersectionOnRoute(route); + if ( next_intersection && (next_intersection->intersectionType() == ::ad::map::intersection::IntersectionType::TrafficLight) ) { + // try to guess the the relevant traffic light with the rule: nearest traffic light in respect to the incoming lane. + // @todo: when OpenDrive maps have the traffic lights incorporated, we only have to fill all green traffic lights into the green_traffic_lights list + auto incoming_lanes = next_intersection->incomingLanesOnRoute(); + // since our route spans the whole street, we have to filter out the incoming lanes with wrong way flag + auto incoming_lanes_iter=incoming_lanes.begin(); + while (incoming_lanes_iter!=incoming_lanes.end()) { + auto find_waypoint = ::ad::map::route::findWaypoint(*incoming_lanes_iter, route); + if (find_waypoint.isValid() && find_waypoint.laneSegmentIterator->laneInterval.wrongWay) { + incoming_lanes_iter = incoming_lanes.erase(incoming_lanes_iter); + } + else { + incoming_lanes_iter++; + } + } + + ::ad::map::match::AdMapMatching traffic_light_map_matching; + bool found_relevant_traffic_light = false; + for (const auto &actor : actors) { + const auto traffic_light = boost::dynamic_pointer_cast(actor); + if (traffic_light == nullptr) { + continue; + } + + auto traffic_light_state = traffic_light->GetState(); + carla::geom::BoundingBox trigger_bounding_box = traffic_light->GetTriggerVolume(); + + auto traffic_light_transform = traffic_light->GetTransform(); + auto trigger_box_location = trigger_bounding_box.location; + traffic_light_transform.TransformPoint(trigger_box_location); + + ::ad::map::point::ENUPoint trigger_box_position; + trigger_box_position.x = ::ad::map::point::ENUCoordinate(trigger_box_location.x); + trigger_box_position.y = ::ad::map::point::ENUCoordinate(-1 * trigger_box_location.y); + trigger_box_position.z = ::ad::map::point::ENUCoordinate(0.); + + auto traffic_light_map_matched_positions = traffic_light_map_matching.getMapMatchedPositions(trigger_box_position, ::ad::physics::Distance(0.25), ::ad::physics::Probability(0.1)); + + _logger->trace("traffic light map matched positions: {}, {}", trigger_box_position, traffic_light_map_matched_positions); + + for (auto matched_position: traffic_light_map_matched_positions) + { + if ( incoming_lanes.find(matched_position.lanePoint.paraPoint.laneId) != incoming_lanes.end() ) + { + if ( found_relevant_traffic_light && (green_traffic_lights.empty() && (traffic_light_state==carla::rpc::TrafficLightState::Green)) ) + { + _logger->warn("found another relevant traffic light on lane {}; {} state {}", + matched_position.lanePoint.paraPoint.laneId, + traffic_light->GetId(), + (traffic_light_state == carla::rpc::TrafficLightState::Green)?"green":"not green"); + } + else + { + _logger->debug("found relevant traffic light on lane {}; {} state {}", + matched_position.lanePoint.paraPoint.laneId, + traffic_light->GetId(), + (traffic_light_state == carla::rpc::TrafficLightState::Green)?"green":"not green"); + + } + + found_relevant_traffic_light = true; + + // found matching traffic light + if ( traffic_light_state == carla::rpc::TrafficLightState::Green ) + { + // @todo: currently there is only this workaround because of missign OpenDrive map support for actual traffic light ids + green_traffic_lights.insert(::ad::map::landmark::LandmarkId::getMax()); + } + else + { + // if the light is not green, we don't have priority + green_traffic_lights.clear(); + } + break; + } + } + } + } + return green_traffic_lights; +} + + +RssCheck::RssObjectChecker::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) + : _rss_check(rss_check) + , _scene_creation(scene_creation) + , _carla_ego_vehicle(carla_ego_vehicle) + , _carla_rss_state(carla_rss_state) + , _green_traffic_lights(green_traffic_lights) { +} + +void RssCheck::RssObjectChecker::operator()( const carla::SharedPtr actor ) const { + const auto vehicle = boost::dynamic_pointer_cast(actor); + if (vehicle == nullptr) { + return; + } + if (vehicle->GetId() == _carla_ego_vehicle.GetId()) { + return; + } + + auto const relevant_distance = std::max(static_cast(_carla_rss_state.ego_dynamics_on_route.min_stopping_distance), 100.); + if (vehicle->GetTransform().location.Distance(_carla_ego_vehicle.GetTransform().location) > relevant_distance) { + return; + } + + try { + auto other_dynamics = _rss_check.GetOtherVehicleDynamics(); + auto other_speed = _rss_check.GetSpeed(*vehicle); + auto ego_dynamics = _rss_check.GetEgoVehicleDynamics(); + + 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.); + } + + auto other_match_object = _rss_check.GetMatchObject(*vehicle, ::ad::physics::Distance(2.0)); + + _rss_check._logger->debug("OtherVehicleMapMatching: {}", other_match_object.mapMatchedBoundingBox); + + _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); + } + catch(...) { + _rss_check._logger->error("Exception processing vehicle {} -> Ignoring it", vehicle->GetId()); + } +} + +void RssCheck::CreateWorldModel(carla::client::Timestamp const ×tamp, + carla::client::ActorList const &actors, + carla::client::Vehicle const &carla_ego_vehicle, + CarlaRssState &carla_rss_state) const { + + ::ad::map::landmark::LandmarkIdSet green_traffic_lights = GetGreenTrafficLightsOnRoute(actors, 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); + + tbb::parallel_for_each( actors.begin(), actors.end(), RssObjectChecker(*this, scene_creation, carla_ego_vehicle, carla_rss_state, green_traffic_lights) ); + + if ( _road_boundaries_mode != RoadBoundariesMode::Off ) { + // add artifical objects on the road boundaries for "stay-on-road" feature + // use 'smart' 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); + } + + carla_rss_state.world_model = scene_creation.getWorldModel(); +} + + +void RssCheck::PerformCheck(CarlaRssState &carla_rss_state) const +{ + bool result = carla_rss_state.rss_check.calculateAccelerationRestriction(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); + + if (!result) { + _logger->warn("calculateAccelerationRestriction 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); + } +} + + +void RssCheck::AnalyseCheckResults(CarlaRssState &carla_rss_state) const { + carla_rss_state.dangerous_state = false; + carla_rss_state.dangerous_vehicle = false; + carla_rss_state.dangerous_opposite_state = false; + bool left_border_is_dangerous = false; + bool right_border_is_dangerous = false; + bool vehicle_triggered_left_response = false; + bool vehicle_triggered_right_response = false; + bool vehicle_triggered_longitudinal_response = false; + for (auto const state: carla_rss_state.rss_state_snapshot.individualResponses) { + if (::ad::rss::state::isDangerous(state)) { + carla_rss_state.dangerous_state = true; + _logger->trace("DangerousState: {}", state); + auto dangerous_sitation_iter = std::find_if(carla_rss_state.situation_snapshot.situations.begin(), + carla_rss_state.situation_snapshot.situations.end(), + [&state](::ad::rss::situation::Situation const &situation) { + return situation.situationId == state.situationId; }); + if ( dangerous_sitation_iter != carla_rss_state.situation_snapshot.situations.end() ) { + _logger->trace("Situation: {}", *dangerous_sitation_iter); + if (dangerous_sitation_iter->objectId == ::ad::rss::map::RssSceneCreator::getRightBorderObjectId()) { + right_border_is_dangerous = true; + } + else if ( dangerous_sitation_iter->objectId == ::ad::rss::map::RssSceneCreator::getLeftBorderObjectId()) { + left_border_is_dangerous = true; + } + else { + carla_rss_state.dangerous_vehicle = true; + if (state.longitudinalState.response != ::ad::rss::state::LongitudinalResponse::None) { + vehicle_triggered_longitudinal_response = true; + } + if (state.lateralStateLeft.response != ::ad::rss::state::LateralResponse::None) { + vehicle_triggered_left_response = true; + } + if (state.lateralStateRight.response != ::ad::rss::state::LateralResponse::None) { + vehicle_triggered_right_response = true; + } + } + if (dangerous_sitation_iter->situationType == ::ad::rss::situation::SituationType::OppositeDirection) { + carla_rss_state.dangerous_opposite_state = true; + } + } + } + } + + // border are restricting potentially too much, fix this + if ( !vehicle_triggered_longitudinal_response && + (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; + } + 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.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.ego_dynamics_on_route.crossing_border = true; + } + + _logger->debug("RouteResponse: {}, AccelerationRestriction: {}", carla_rss_state.proper_response, carla_rss_state.acceleration_restriction); +} + + +/// +/// visualization +/// + +void RssCheck::VisualizeResults(carla::client::World &world, + carla::SharedPtr const &carla_ego_actor) const { + if (_visualization_mode==VisualizationMode::Off) { + return; + } + + if (_visualization_mutex.try_lock()) { + carla::client::DebugHelper dh = world.MakeDebugHelper(); + + if ((_visualization_mode==VisualizationMode::RouteOnly) + || (_visualization_mode==VisualizationMode::VehicleStateAndRoute) + || (_visualization_mode==VisualizationMode::All)) { + VisualizeRouteLocked(dh, _visualization_route.first, carla_ego_actor, _visualization_route.second); + } + + if ((_visualization_mode==VisualizationMode::VehicleStateOnly) + || (_visualization_mode==VisualizationMode::VehicleStateAndRoute) + || (_visualization_mode==VisualizationMode::All)) { + VisualizeRssResultsLocked(dh, world, carla_ego_actor, _visualization_state_snapshot); + } + + if ((_visualization_mode==VisualizationMode::All) + || (_visualization_mode==VisualizationMode::DebugRouteOrientationObjectRoute) + || (_visualization_mode==VisualizationMode::DebugRouteOrientationBorders) + || (_visualization_mode==VisualizationMode::DebugRouteOrientationBoth)) { + VisualizeEgoDynamics(dh, carla_ego_actor, _visualization_ego_dynamics); + } + + if ((_visualization_mode==VisualizationMode::DebugRouteOrientationObjectRoute) + || (_visualization_mode==VisualizationMode::DebugRouteOrientationBoth)) { + VisualizeRouteLocked(dh, _visualization_debug_route, carla_ego_actor, false); + } + + if ((_visualization_mode==VisualizationMode::DebugRouteOrientationBorders) + || (_visualization_mode==VisualizationMode::DebugRouteOrientationBoth)) { + carla::sensor::data::Color color_left{0u, 255u, 255u}; + carla::sensor::data::Color color_right{122u, 255u, 255u}; + for (auto const &border: _visualization_debug_enu_border) + { + VisualizeENUEdgeLocked(dh, border.left, color_left, carla_ego_actor->GetLocation().z); + VisualizeENUEdgeLocked(dh, border.right, color_right, carla_ego_actor->GetLocation().z); + } + } + + _visualization_mutex.unlock(); + } +} + +void RssCheck::StoreVisualizationResults(CarlaRssState const &carla_rss_state) { + if (_visualization_mutex.try_lock()) { + _visualization_state_snapshot = carla_rss_state.rss_state_snapshot; + _visualization_route = std::make_pair(carla_rss_state.ego_route, carla_rss_state.dangerous_state); + _visualization_ego_dynamics = carla_rss_state.ego_dynamics_on_route; + _visualization_mutex.unlock(); + } +} + +void RssCheck::StoreDebugVisualization(::ad::map::route::FullRoute const &debug_route, + std::vector<::ad::map::lane::ENUBorder> const &enu_border) const { + if (_visualization_mutex.try_lock()) { + _visualization_debug_route = debug_route; + _visualization_debug_enu_border = enu_border; + _visualization_mutex.unlock(); + } +} + +void RssCheck::VisualizeRssResultsLocked(carla::client::DebugHelper &dh, + carla::client::World &world, + carla::SharedPtr const &carla_ego_actor, + ::ad::rss::state::RssStateSnapshot state_snapshot) const { + + const auto carla_ego_vehicle = boost::dynamic_pointer_cast(carla_ego_actor); + carla::geom::Location ego_vehicle_location = carla_ego_vehicle->GetLocation(); + auto ego_vehicle_transform = carla_ego_vehicle->GetTransform(); + + for (auto const &state: state_snapshot.individualResponses) { + carla::rpc::ActorId vehicle_id = static_cast(state.objectId); + carla::SharedPtr vehicle_list = + world.GetActors(std::vector{vehicle_id}); + carla::geom::Location ego_point = ego_vehicle_location; + ego_point.z += 0.05f; + const auto yaw = ego_vehicle_transform.rotation.yaw; + const float cosine = std::cos(yaw * toRadians); + const float sine = std::sin(yaw * toRadians); + carla::geom::Location line_offset{-sine * 0.1f, cosine * 0.1f, 0.0f}; + for (const auto &actor : *vehicle_list) { + const auto vehicle = boost::dynamic_pointer_cast(actor); + carla::geom::Location point = vehicle->GetLocation(); + point.z += 0.05f; + carla::sensor::data::Color indicator_color{0u, 255u, 0u}; + bool dangerous = ::ad::rss::state::isDangerous(state); + if (dangerous) { + indicator_color = carla::sensor::data::Color{255u, 0u, 0u}; + } + if (_visualization_mode==VisualizationMode::All) { + // the car connections are only visualized if All is requested + carla::sensor::data::Color lon_color = indicator_color; + carla::sensor::data::Color lat_l_color = indicator_color; + carla::sensor::data::Color lat_r_color = indicator_color; if (!state.longitudinalState.isSafe) { - lonColor.r = 255u; + lon_color.r = 255u; if (dangerous) { - lonColor.g = 0u; + lon_color.g = 0u; } else { - lonColor.g = 255u; + lon_color.g = 255u; } } if (!state.lateralStateLeft.isSafe) { - latLColor.r = 255u; + lat_l_color.r = 255u; if (dangerous) { - latLColor.g = 0u; + lat_l_color.g = 0u; } else { - latLColor.g = 255u; + lat_l_color.g = 255u; } } if (!state.lateralStateRight.isSafe) { - latRColor.r = 255u; + lat_r_color.r = 255u; if (dangerous) { - latRColor.g = 0u; + lat_r_color.g = 0u; } else { - latRColor.g = 255u; + lat_r_color.g = 255u; } } - dh.DrawLine(egoPoint, point, 0.1f, lonColor, 0.02f, false); - dh.DrawLine(egoPoint - lineOffset, point - lineOffset, 0.1f, latLColor, 0.02f, false); - dh.DrawLine(egoPoint + lineOffset, point + lineOffset, 0.1f, latRColor, 0.02f, false); - point.z += 3.f; - dh.DrawPoint(point, 0.2f, indicatorColor, 0.02f, false); + dh.DrawLine(ego_point, point, 0.1f, lon_color, 0.02f, false); + dh.DrawLine(ego_point - line_offset, point - line_offset, 0.1f, lat_l_color, 0.02f, false); + dh.DrawLine(ego_point + line_offset, point + line_offset, 0.1f, lat_r_color, 0.02f, false); + } + point.z += 3.f; + dh.DrawPoint(point, 0.2f, indicator_color, 0.02f, false); + } + } +} + +void RssCheck::VisualizeENUEdgeLocked(carla::client::DebugHelper &dh, + ::ad::map::point::ENUEdge const &edge, + carla::sensor::data::Color const &color, + float const z_offset) const { + + for (auto const &point : edge) { + carla::geom::Location carla_point(static_cast(static_cast(point.x)), + static_cast(static_cast(-1. * point.y)), + static_cast(static_cast(point.z))); + carla_point.z += z_offset; + dh.DrawPoint(carla_point, 0.1f, color, 0.1f, false); + } +} + +void RssCheck::VisualizeRouteLocked(carla::client::DebugHelper &dh, + const ::ad::map::route::FullRoute &route, + carla::SharedPtr const &carla_ego_actor, + bool dangerous) const { + + std::map<::ad::map::lane::LaneId, ::ad::map::point::ENUEdge> right_lane_edges; + std::map<::ad::map::lane::LaneId, ::ad::map::point::ENUEdge> left_lane_edges; + + carla::geom::Location ego_vehicle_location = carla_ego_actor->GetLocation(); + for (auto const &road_segment : route.roadSegments) { + { + auto &right_most_lane = road_segment.drivableLaneSegments.front(); + if (right_lane_edges.find(right_most_lane.laneInterval.laneId) + == right_lane_edges.end()) { + ::ad::map::point::ENUEdge edge = ::ad::map::route::getRightProjectedENUEdge(right_most_lane.laneInterval); + right_lane_edges.insert( { right_most_lane.laneInterval.laneId, edge }); + bool intersection_lane = + ::ad::map::intersection::Intersection::isLanePartOfAnIntersection( + right_most_lane.laneInterval.laneId); + + carla::sensor::data::Color color((dangerous ? 128 : 255), 0, 0); + if (intersection_lane) { + color.b = (dangerous ? 128 : 255); + } + VisualizeENUEdgeLocked(dh, edge, color, ego_vehicle_location.z); + } + } + { + auto &left_most_lane = road_segment.drivableLaneSegments.back(); + if (left_lane_edges.find(left_most_lane.laneInterval.laneId) + == left_lane_edges.end()) { + ::ad::map::point::ENUEdge edge = ::ad::map::route::getLeftProjectedENUEdge(left_most_lane.laneInterval); + left_lane_edges.insert( { left_most_lane.laneInterval.laneId, edge }); + bool intersection_lane = + ::ad::map::intersection::Intersection::isLanePartOfAnIntersection( + left_most_lane.laneInterval.laneId); + carla::sensor::data::Color color(0, (dangerous ? 128 : 255), 0); + if (intersection_lane) { + color.b = (dangerous ? 128 : 255); + } + VisualizeENUEdgeLocked(dh, edge, color, ego_vehicle_location.z); } } } +} + +void RssCheck::VisualizeEgoDynamics(carla::client::DebugHelper &dh, + carla::SharedPtr const &carla_ego_actor, + EgoDynamicsOnRoute const &ego_dynamics_on_route) const +{ + const auto carla_ego_vehicle = boost::dynamic_pointer_cast(carla_ego_actor); + carla::geom::Location ego_vehicle_location = carla_ego_vehicle->GetLocation(); + + carla::sensor::data::Color color{0u, 0u, 255u}; + + auto sin_heading = static_cast(std::sin(static_cast(ego_dynamics_on_route.route_heading))); + auto cos_heading = static_cast(std::cos(static_cast(ego_dynamics_on_route.route_heading))); + + carla::geom::Location heading_location_start = ego_vehicle_location; + heading_location_start.x -= cos_heading * 10.f; + heading_location_start.y += sin_heading * 10.f; + heading_location_start.z += 0.5f; + carla::geom::Location heading_location_end = ego_vehicle_location; + heading_location_end.x += cos_heading * 10.f; + heading_location_end.y -= sin_heading * 10.f; + heading_location_end.z += 0.5f; + + dh.DrawArrow(heading_location_start, heading_location_end, 0.1f, 0.1f, color, 0.02f, false); + + auto sin_center = static_cast(std::sin(static_cast(ego_dynamics_on_route.route_heading)+M_PI_2)); + auto cos_center = static_cast(std::cos(static_cast(ego_dynamics_on_route.route_heading)+M_PI_2)); + carla::geom::Location center_location_start = ego_vehicle_location; + center_location_start.x -= cos_center * 2.f; + center_location_start.y += sin_center * 2.f; + center_location_start.z += 0.5f; + carla::geom::Location center_location_end = ego_vehicle_location; + center_location_end.x += cos_center * 2.f; + center_location_end.y -= sin_center * 2.f; + center_location_end.z += 0.5f; + + dh.DrawLine(center_location_start, center_location_end, 0.1f, color, 0.02f, false); +} + } // namespace rss } // namespace carla diff --git a/LibCarla/source/carla/rss/RssCheck.h b/LibCarla/source/carla/rss/RssCheck.h index 046c2e0d7..92d1df664 100644 --- a/LibCarla/source/carla/rss/RssCheck.h +++ b/LibCarla/source/carla/rss/RssCheck.h @@ -1,97 +1,338 @@ -// Copyright (c) 2019 Intel Corporation +// Copyright (c) 2019-2020 Intel Corporation // // This work is licensed under the terms of the MIT license. // For a copy, see . #pragma once +#include +#include +#include +#include +#include +#include +#include +#include #include +#include +#include +#include #include "carla/client/ActorList.h" #include "carla/client/Vehicle.h" #include "carla/road/Map.h" -namespace ad_rss { -namespace state { - struct ProperResponse; - struct RssStateSnapshot; -} // namespace state -namespace core { - class RssSituationChecking; - class RssSituationExtraction; - class RssResponseResolving; -} // namespace core -namespace lane { - struct VehicleState; -} -namespace world { - struct AccelerationRestriction; - class LaneSegment; - class Object; - class OccupiedRegion; - using OccupiedRegionVector = std::vector<::ad_rss::world::OccupiedRegion>; - class RssDynamics; - struct Velocity; -} // namespace world -} // namespace ad_rss - namespace carla { namespace rss { - namespace cc = carla::client; - namespace cg = carla::geom; +/// @brief struct defining the different supported handling of road boundaries +enum class RoadBoundariesMode { + Off, /// No road boundaries considered by RSS check + On /// The road boundaries of the current route are considered by RSS check +}; - class RssCheck { - public: +/// @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 +}; - RssCheck(); +/// @brief struct defining the ego vehicles current dynamics in respect to the current route +/// +/// Especially the velocity of the vehicle is split into lateral and longitudinal components +/// according to the current route +/// +struct EgoDynamicsOnRoute { - ~RssCheck(); + /// @brief constructor + EgoDynamicsOnRoute(); - bool checkObjects(cc::Timestamp const & timestamp, cc::World &world, - carla::SharedPtr const & vehicles, - carla::SharedPtr const & carlaEgoVehicle, - carla::SharedPtr const & map, ::ad_rss::state::ProperResponse &response, - ::ad_rss::world::AccelerationRestriction &restriction, - ::ad_rss::world::Velocity &egoVelocity, bool visualizeResults = false); + /// @brief the carla timestamp of the last calculation + carla::client::Timestamp timestamp; + /// @brief the time since epoch in ms at start of the checkObjects call + double time_since_epoch_check_start_ms; + /// @brief the time since epoch in ms at the end of the checkObjects call + double time_since_epoch_check_end_ms; + /// @brief the ego speed + ::ad::physics::Speed ego_speed; + /// @brief the current minimum stopping distance + ::ad::physics::Distance min_stopping_distance; + /// @brief the considered enu position of the ego vehicle + ::ad::map::point::ENUPoint ego_center; + /// @brief the considered heading of the ego vehicle + ::ad::map::point::ENUHeading ego_heading; + /// @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 borders + /// this is only evaluated if the border checks are active! + /// It is a hint to oversteer a bit on countersteering + bool crossing_border; + /// @brief the considered heading of the route + ::ad::map::point::ENUHeading route_heading; + /// @brief the considered nominal center of the current route + ::ad::map::point::ENUPoint route_nominal_center; + /// @brief the considered heading of the route + ::ad::map::point::ENUHeading heading_diff; + /// @brief the ego speed component lat in respect to a route + ::ad::physics::Speed route_speed_lat; + /// @brief the ego speed component lon in respect to a route + ::ad::physics::Speed route_speed_lon; + /// @brief the ego acceleration component lat in respect to a route + ::ad::physics::Acceleration route_accel_lat; + /// @brief the ego acceleration component lon in respect to a route + ::ad::physics::Acceleration route_accel_lon; + /// @brief the ego acceleration component lat in respect to a route + /// smoothened by an average filter + ::ad::physics::Acceleration avg_route_accel_lat; + /// @brief the ego acceleration component lat in respect to a route + /// smoothened by an average filter + ::ad::physics::Acceleration avg_route_accel_lon; +}; - const ::ad_rss::world::RssDynamics &getEgoVehicleDynamics() const; - void setEgoVehicleDynamics(const ::ad_rss::world::RssDynamics &egoDynamics); - const ::ad_rss::world::RssDynamics &getOtherVehicleDynamics() const; - void setOtherVehicleDynamics(const ::ad_rss::world::RssDynamics &otherVehicleDynamics); +/// @brief class implementing the actual RSS checks based on CARLA world description +class RssCheck { - private: +public: + /// @brief constructor + RssCheck(); - std::shared_ptr<::ad_rss::world::RssDynamics> _egoVehicleDynamics; - std::shared_ptr<::ad_rss::world::RssDynamics> _otherVehicleDynamics; + /// @brief destructor + ~RssCheck(); - std::shared_ptr<::ad_rss::core::RssSituationExtraction> _rssSituationExtraction; - std::shared_ptr<::ad_rss::core::RssSituationChecking> _rssSituationChecking; - std::shared_ptr<::ad_rss::core::RssResponseResolving> _rssResponseResolving; + /// @brief main function to trigger the RSS check at a certain point in time + /// + /// This function has to be called cyclic with increasing timestamps to ensure + /// proper RSS evaluation. + /// + bool CheckObjects(carla::client::Timestamp const ×tamp, + carla::SharedPtr const &actors, + carla::SharedPtr const &carla_ego_actor, + ::ad::rss::state::ProperResponse &output_response, + ::ad::rss::world::AccelerationRestriction &output_acceleration_restriction, + ::ad::rss::state::RssStateSnapshot &output_rss_state_snapshot, + EgoDynamicsOnRoute &output_rss_ego_dynamics_on_route); - uint64_t calculateLaneSegmentId(const carla::road::Lane &lane) const; + /// @brief function to visualize the RSS check results + /// + void VisualizeResults(carla::client::World &world, + carla::SharedPtr const &carla_ego_actor) const; - void calculateLatLonVelocities(const cg::Transform &laneLocation, - const cg::Vector3D &velocity, - ::ad_rss::world::Object &rssObject, bool inverseDirection) const; + /// @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); - void calculateOccupiedRegions(const std::array &bounds, - ::ad_rss::world::OccupiedRegionVector &occupiedRegions, const carla::road::Map &map, - const bool drivingInRoadDirection, - const cg::Transform &vehicleTransform) const; + /// @returns the current mode for respecting the road boundaries (@see also RssSensor::GetRoadBoundariesMode()) + const ::carla::rss::RoadBoundariesMode& GetRoadBoundariesMode() const; + /// @brief sets the current mode for respecting the road boundaries (@see also RssSensor::SetRoadBoundariesMode()) + void SetRoadBoundariesMode(const ::carla::rss::RoadBoundariesMode &road_boundaries_mode); - void convertAndSetLaneSegmentId(const carla::road::Lane &lane, - ::ad_rss::world::LaneSegment &laneSegment) const; + /// @returns the current routing targets (@see also RssSensor::GetRoutingTargets()) + const std::vector<::carla::geom::Transform> GetRoutingTargets() const; + /// @brief appends a routing target to the current routing target list (@see also RssSensor::AppendRoutingTargets()) + void AppendRoutingTarget(const ::carla::geom::Transform &routing_target); + /// @brief resets the current routing targets (@see also RssSensor::ResetRoutingTargets()) + void ResetRoutingTargets(); - std::array getVehicleBounds(const cc::Vehicle &vehicle) const; + /// @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; - void initVehicle(::ad_rss::world::Object &vehicle) const; - void initEgoVehicleDynamics(::ad_rss::world::RssDynamics &dynamics) const; - void initOtherVehicleDynamics(::ad_rss::world::RssDynamics &dynamics) const; + /// @brief drop the current route + /// + /// Afterwards a new route is selected randomly (if multiple routes are possible). + /// + void DropRoute(); - void visualizeRssResults(::ad_rss::state::RssStateSnapshot stateSnapshot, - const cg::Location &egoVehicleLocation, - const cg::Transform &egoVehicleTransform, cc::World &world) const; +private: + /// @brief standard logger + std::shared_ptr _logger; + /// @brief logger for timing log messages + std::shared_ptr _timing_logger; + /// @brief current used vehicle dynamics for ego vehcile + ::ad::rss::world::RssDynamics _ego_vehicle_dynamics; + /// @brief current used vehicle dynamics for other vehciles + ::ad::rss::world::RssDynamics _other_vehicle_dynamics; + /// @brief 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 { + /// @brief the actual RSS checker object + ::ad::rss::core::RssCheck rss_check; + + /// @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 check input: the RSS world model + ::ad::rss::world::WorldModel world_model; + + /// @brief check result: the situation snapshot + ::ad::rss::situation::SituationSnapshot situation_snapshot; + /// @brief check result: the rss state snapshot + ::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 vehicle + bool dangerous_vehicle; + /// @brief flag indicating if the current state is dangerous because of an opposite vehicle + bool dangerous_opposite_state; }; + + class RssObjectChecker { + public: + 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 actor ) const; + private: + 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; + }; + + friend class RssObjectChecker; + + /// @brief the current state of the ego vehicle + CarlaRssState _carla_rss_state; + + /// @returns the default vehicle dynamics + ::ad::rss::world::RssDynamics GetDefaultVehicleDynamics() const; + + /// @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 speed from the carla_vehicle + ::ad::physics::Speed GetSpeed(carla::client::Vehicle const &carla_vehicle) const; + + /// @brief update the desired ego vehicle route + void UpdateRoute(CarlaRssState &carla_rss_state); + + /// @brief calculate ego vehicle dynamics on the route + EgoDynamicsOnRoute CalculateEgoDynamicsOnRoute(carla::client::Timestamp const ¤t_timestamp, + double const &time_since_epoch_check_start_ms, + carla::client::Vehicle const &carla_vehicle, + ::ad::map::match::Object match_object, + ::ad::map::route::FullRoute const &route, + EgoDynamicsOnRoute const &last_dynamics) const; + + /// @brief collect the green traffic lights on the current route + ::ad::map::landmark::LandmarkIdSet GetGreenTrafficLightsOnRoute(carla::client::ActorList const &actors, + ::ad::map::route::FullRoute const&route) const; + + /// @brief Create the RSS world model + void CreateWorldModel(carla::client::Timestamp const ×tamp, + carla::client::ActorList const &actors, + carla::client::Vehicle const &carla_ego_vehicle, + CarlaRssState &carla_rss_state) const; + + /// @brief Perform the actual RSS check + void PerformCheck(CarlaRssState &carla_rss_state) const; + + /// @brief Analyse the RSS check results + void AnalyseCheckResults(CarlaRssState &carla_rss_state) const; + + /// + /// visualization + /// + void StoreVisualizationResults(CarlaRssState const &carla_rss_state); + + void StoreDebugVisualization(::ad::map::route::FullRoute const &debug_route, + std::vector<::ad::map::lane::ENUBorder> const &enu_border) const; + + /// mutex to protect the visualization content + mutable std::mutex _visualization_mutex; + /// the RssStateSnapshot to be visualized + ::ad::rss::state::RssStateSnapshot _visualization_state_snapshot; + void VisualizeRssResultsLocked(carla::client::DebugHelper &dh, + carla::client::World &world, + carla::SharedPtr const &carla_ego_actor, + ::ad::rss::state::RssStateSnapshot state_snapshot) const; + + /// the FullRoute to be visualized + std::pair<::ad::map::route::FullRoute, bool> _visualization_route; + void VisualizeRouteLocked(carla::client::DebugHelper &dh, + ::ad::map::route::FullRoute const &route, + carla::SharedPtr const &carla_ego_actor, + bool dangerous) const; + + void VisualizeENUEdgeLocked(carla::client::DebugHelper &dh, + ::ad::map::point::ENUEdge const &edge, + carla::sensor::data::Color const &color, + float const z_offset) const; + + /// the EgoDynamicsOnRoute to be visualized + EgoDynamicsOnRoute _visualization_ego_dynamics; + void VisualizeEgoDynamics(carla::client::DebugHelper &dh, + carla::SharedPtr const &carla_ego_actor, + EgoDynamicsOnRoute const &ego_dynamics_on_route) const; + + mutable ::ad::map::route::FullRoute _visualization_debug_route; + mutable std::vector<::ad::map::lane::ENUBorder> _visualization_debug_enu_border; +}; + } // namespace rss } // namespace carla + +namespace std { +/** + * \brief standard ostream operator + * + * \param[in/out] os The output stream to write to + * \param[in] ego_dynamics_on_route the ego dynamics on route to stream out + * + * \returns The stream object. + * + */ +inline std::ostream &operator<<(std::ostream &out, const ::carla::rss::EgoDynamicsOnRoute &ego_dynamics_on_route) { + out << "EgoDynamicsOnRoute(timestamp=" << ego_dynamics_on_route.timestamp + << ", time_since_epoch_check_start_ms=" << ego_dynamics_on_route.time_since_epoch_check_start_ms + << ", time_since_epoch_check_end_ms=" << ego_dynamics_on_route.time_since_epoch_check_end_ms + << ", 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_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 + << ", route_nominal_center=" << ego_dynamics_on_route.route_nominal_center + << ", heading_diff=" << ego_dynamics_on_route.heading_diff + << ", route_speed_lat=" << ego_dynamics_on_route.route_speed_lat + << ", route_speed_lon=" << ego_dynamics_on_route.route_speed_lon + << ", 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 + << ')'; + return out; +} +} // namespace std diff --git a/LibCarla/source/carla/rss/RssRestrictor.cpp b/LibCarla/source/carla/rss/RssRestrictor.cpp index bf7b8a072..91f84af0c 100644 --- a/LibCarla/source/carla/rss/RssRestrictor.cpp +++ b/LibCarla/source/carla/rss/RssRestrictor.cpp @@ -1,75 +1,121 @@ -// Copyright (c) 2019 Intel Corporation +// Copyright (c) 2019-2020 Intel Corporation // // This work is licensed under the terms of the MIT license. // For a copy, see . #include "carla/rss/RssRestrictor.h" +#include "carla/rss/RssCheck.h" #include "carla/rpc/VehicleControl.h" #include "carla/rpc/VehiclePhysicsControl.h" -#include "ad_rss/world/AccelerationRestriction.hpp" -#include "ad_rss/world/Velocity.hpp" +#include +#include +#include namespace carla { namespace rss { - RssRestrictor::RssRestrictor() {} - - RssRestrictor::~RssRestrictor() = default; - - carla::rpc::VehicleControl RssRestrictor::restrictVehicleControl( - const carla::rpc::VehicleControl &vehicleControl, const ad_rss::world::AccelerationRestriction &restriction, - const ad_rss::world::Velocity &egoVelocity, const carla::rpc::VehiclePhysicsControl &vehiclePhysics) { - carla::rpc::VehicleControl restrictedVehicleControl(vehicleControl); - - // Pretty basic implementation of a RSS restrictor modifying the - // VehicleControl according to the given - // restrictions. Basic braking and countersteering actions are applied. - - ad_rss::physics::Acceleration zeroAccel(0.0); - - float mass = vehiclePhysics.mass; - float sumMaxBrakeTorque = 0.f; - float radius = 1.f; - for (auto const &wheel : vehiclePhysics.wheels) { - sumMaxBrakeTorque += wheel.max_brake_torque; - radius = wheel.radius; - } - - // do not apply any restrictions when in reverse gear - if (!vehicleControl.reverse) { - - // decelerate - if (restriction.longitudinalRange.maximum < zeroAccel) { - restrictedVehicleControl.throttle = 0.0f; - - double sumBrakeTorque = mass * std::fabs(static_cast(restriction.longitudinalRange.minimum)) * - radius / 100.0; - restrictedVehicleControl.brake = - std::min(static_cast(sumBrakeTorque / sumMaxBrakeTorque), 1.0f); - } - - if (restriction.lateralLeftRange.maximum <= ad_rss::physics::Acceleration(0.0)) { - if (egoVelocity.speedLat < ad_rss::physics::Speed(0.0)) { - // driving to the left - if (egoVelocity.speedLon != ad_rss::physics::Speed(0.0)) { - double angle = std::atan(egoVelocity.speedLat / egoVelocity.speedLon); - restrictedVehicleControl.steer = -1.f * static_cast(angle); - } - } - } - if (restriction.lateralRightRange.maximum <= ad_rss::physics::Acceleration(0.0)) { - if (egoVelocity.speedLat > ad_rss::physics::Speed(0.0)) { - // driving to the right - if (egoVelocity.speedLon != ad_rss::physics::Speed(0.0)) { - double angle = std::atan(egoVelocity.speedLat / egoVelocity.speedLon); - restrictedVehicleControl.steer = -1.f * static_cast(angle); - } - } - } - } - return restrictedVehicleControl; +RssRestrictor::RssRestrictor() +{ + std::string loggerName = "RssRestrictor"; + _logger = spdlog::get(loggerName); + if (!_logger) + { + _logger = spdlog::create(loggerName); } + //_logger->set_level(spdlog::level::debug); +} + +RssRestrictor::~RssRestrictor() = default; + +carla::rpc::VehicleControl RssRestrictor::RestrictVehicleControl( + const carla::rpc::VehicleControl &vehicle_control, + const ::ad::rss::world::AccelerationRestriction &restriction, + const carla::rss::EgoDynamicsOnRoute &ego_dynamics_on_route, + const carla::rpc::VehiclePhysicsControl &vehicle_physics) { + carla::rpc::VehicleControl restricted_vehicle_control(vehicle_control); + + // Pretty basic implementation of a RSS restrictor modifying the + // VehicleControl according to the given + // restrictions. Basic braking and countersteering actions are applied. + // In case countersteering is not possible anymore (i.e. the lateral velocity reached zero), + // as a fallback longitudinal braking is applied instead (escalation strategy). + + ::ad::physics::Acceleration zero_accel(0.0); + + float mass = vehicle_physics.mass; + float max_steer_angle_deg = 0.f; + float sum_max_brake_torque = 0.f; + float radius = 1.f; + for (auto const &wheel : vehicle_physics.wheels) { + sum_max_brake_torque += wheel.max_brake_torque; + radius = wheel.radius; + max_steer_angle_deg = std::max(max_steer_angle_deg, wheel.max_steer_angle); + } + + // 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)) { + 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)) { + double angle_rad = std::atan(ego_dynamics_on_route.route_speed_lat / ego_dynamics_on_route.route_speed_lon); + float desired_steer_ratio = -180.f * static_cast(angle_rad / M_PI) / max_steer_angle_deg; + if ( ego_dynamics_on_route.crossing_border ) + { + desired_steer_ratio += 0.1f; + } + float orig_steer = restricted_vehicle_control.steer; + restricted_vehicle_control.steer = std::max(restricted_vehicle_control.steer, desired_steer_ratio); + restricted_vehicle_control.steer = std::min(restricted_vehicle_control.steer, 1.0f); + _logger->debug("EgoVelocity: {}", ego_dynamics_on_route); + _logger->debug("Countersteer left to right: {} -> {}", orig_steer, restricted_vehicle_control.steer); + } + } + } + + if (restriction.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)) { + double angle_rad = std::atan(ego_dynamics_on_route.route_speed_lat / ego_dynamics_on_route.route_speed_lon); + float desired_steer_ratio = -180.f * static_cast(angle_rad / M_PI) / max_steer_angle_deg; + if ( ego_dynamics_on_route.crossing_border ) + { + desired_steer_ratio -= 0.1f; + } + float orig_steer = restricted_vehicle_control.steer; + restricted_vehicle_control.steer = std::min(restricted_vehicle_control.steer, desired_steer_ratio); + restricted_vehicle_control.steer = std::max(restricted_vehicle_control.steer, -1.0f); + _logger->debug("EgoVelocity: {}", ego_dynamics_on_route); + _logger->debug("Countersteer right to left: {} -> {}", orig_steer, restricted_vehicle_control.steer); + } + } + } + + // restrict acceleration + if (restriction.longitudinalRange.maximum > zero_accel) { + // TODO: determine acceleration and limit throttle + } + + // decelerate + if (restriction.longitudinalRange.maximum < zero_accel) { + restricted_vehicle_control.throttle = 0.0f; + + double brake_acceleration = std::fabs(static_cast(restriction.longitudinalRange.minimum)); + double sum_brake_torque = mass * brake_acceleration * radius / 100.0; + restricted_vehicle_control.brake = std::min(static_cast(sum_brake_torque / sum_max_brake_torque), 1.0f); + } + } + if(restricted_vehicle_control != vehicle_control) { + _logger->debug("Restrictor active: throttle({} -> {}), brake ({} -> {}). steer ({} -> {})", + vehicle_control.throttle, restricted_vehicle_control.throttle, + vehicle_control.brake, restricted_vehicle_control.brake, + vehicle_control.steer, restricted_vehicle_control.steer); + } + return restricted_vehicle_control; +} } // namespace rss } // namespace carla diff --git a/LibCarla/source/carla/rss/RssRestrictor.h b/LibCarla/source/carla/rss/RssRestrictor.h index c7ed09a76..8d7512bd5 100644 --- a/LibCarla/source/carla/rss/RssRestrictor.h +++ b/LibCarla/source/carla/rss/RssRestrictor.h @@ -1,4 +1,4 @@ -// Copyright (c) 2019 Intel Corporation +// Copyright (c) 2019-2020 Intel Corporation // // This work is licensed under the terms of the MIT license. // For a copy, see . @@ -6,37 +6,51 @@ #pragma once #include +#include -namespace ad_rss { +namespace ad{ +namespace rss { namespace world { - struct AccelerationRestriction; - struct Velocity; + +/// @brief forward declararion for the RSS Acceleration restrictions +struct AccelerationRestriction; + } // namespace world -} // namespace ad_rss +} // namespace rss +} // namespace ad namespace carla { namespace rpc { - class VehicleControl; - class VehiclePhysicsControl; +class VehicleControl; +class VehiclePhysicsControl; } // namespace rpc namespace rss { - class RssRestrictor { - public: +/// @brief forward declararion for ego vehicles current dynamics in respect to the current route +struct EgoDynamicsOnRoute; - RssRestrictor(); +/// @brief class implementing the RSS restrictions within CARLA +class RssRestrictor { + public: + /// @brief constructor + RssRestrictor(); - ~RssRestrictor(); + /// @brief destructor + ~RssRestrictor(); - carla::rpc::VehicleControl restrictVehicleControl(const carla::rpc::VehicleControl &vehicleControl, - const ad_rss::world::AccelerationRestriction &restriction, - const ad_rss::world::Velocity &egoVelocity, - const carla::rpc::VehiclePhysicsControl &vehiclePhysics); + /// @brief the actual function to restrict the given vehicle control input to mimick + /// RSS conform behavior by braking + /// 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 carla::rss::EgoDynamicsOnRoute &ego_dynamics_on_route, + const carla::rpc::VehiclePhysicsControl &vehicle_physics); - private: - - }; + private: + /// @brief the logger instance + std::shared_ptr _logger; +}; } // namespace rss } // namespace carla diff --git a/LibCarla/source/carla/rss/RssSensor.cpp b/LibCarla/source/carla/rss/RssSensor.cpp index ae2cfb2ad..949ace6d4 100644 --- a/LibCarla/source/carla/rss/RssSensor.cpp +++ b/LibCarla/source/carla/rss/RssSensor.cpp @@ -1,146 +1,185 @@ -// Copyright (c) 2019 Intel Corporation +// Copyright (c) 2019-2020 Intel Corporation // // This work is licensed under the terms of the MIT license. // For a copy, see . #include "carla/rss/RssSensor.h" -#include "carla/Logging.h" -#include "carla/client/detail/Simulator.h" +#include +#include +#include +#include +#include +#include -#include "carla/client/ActorList.h" +#include "carla/Logging.h" +#include "carla/client/Sensor.h" +#include "carla/client/Map.h" +#include "carla/client/detail/Simulator.h" #include "carla/rss/RssCheck.h" #include "carla/sensor/data/RssResponse.h" -#include -#include -#include - -#include namespace carla { namespace client { - namespace cc = carla::client; - namespace csd = carla::sensor::data; +RssSensor::RssSensor(ActorInitializer init) + : Sensor(std::move(init)) + , _on_tick_register_id(0u) + , _rss_check(std::make_shared<::carla::rss::RssCheck>()) + , _drop_route(false) { +} - RssSensor::~RssSensor() = default; +RssSensor::~RssSensor() = default; - void RssSensor::Listen(CallbackFunctionType callback) { - if (_is_listening) { - log_error(GetDisplayId(), ": already listening"); - return; - } - - if (GetParent() == nullptr) { - throw_exception(std::runtime_error(GetDisplayId() + ": not attached to vehicle")); - return; - } - - _map = GetWorld().GetMap(); - DEBUG_ASSERT(_map != nullptr); - - mRssCheck = std::make_shared<::carla::rss::RssCheck>(); - - auto self = boost::static_pointer_cast(shared_from_this()); - - log_debug(GetDisplayId(), ": subscribing to tick event"); - GetEpisode().Lock()->RegisterOnTickEvent( - [cb = std::move(callback), weak_self = WeakPtr(self)](const auto &snapshot) { - auto self = weak_self.lock(); - if (self != nullptr) { - auto data = self->TickRssSensor(snapshot.GetTimestamp()); - if (data != nullptr) { - cb(std::move(data)); - } - } - }); - - _is_listening = true; +void RssSensor::Listen(CallbackFunctionType callback) { + if (IsListening()) { + log_error(GetDisplayId(), ": already listening"); + return; } - SharedPtr RssSensor::TickRssSensor(const Timestamp ×tamp) { - try { - bool result = false; - ::ad_rss::state::ProperResponse response; - ::ad_rss::world::AccelerationRestriction accelerationRestriction; - ::ad_rss::world::Velocity egoVelocity; - if (_processing_lock.try_lock()) { - cc::World world = GetWorld(); - SharedPtr actors = world.GetActors(); - SharedPtr vehicles = actors->Filter("vehicle.*"); + if (GetParent() == nullptr) { + throw_exception(std::runtime_error(GetDisplayId() + ": not attached to vehicle")); + return; + } - // check all object<->ego pairs with RSS and calculate proper response - result = - mRssCheck->checkObjects(timestamp, world, vehicles, GetParent(), _map, response, - accelerationRestriction, - egoVelocity, visualize_results); - _processing_lock.unlock(); + auto map = GetWorld().GetMap(); + DEBUG_ASSERT(map != nullptr); + std::string const open_drive_content = map->GetOpenDrive(); - csd::LongitudinalResponse longitudinalResponse; - csd::LateralResponse lateralResponseRight; - csd::LateralResponse lateralResponseLeft; + ::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); - switch (response.longitudinalResponse) { - case ::ad_rss::state::LongitudinalResponse::None: - longitudinalResponse = csd::LongitudinalResponse::None; - break; - case ::ad_rss::state::LongitudinalResponse::BrakeMinCorrect: - longitudinalResponse = csd::LongitudinalResponse::BrakeMinCorrect; - break; - case ::ad_rss::state::LongitudinalResponse::BrakeMin: - longitudinalResponse = csd::LongitudinalResponse::BrakeMin; - break; + auto self = boost::static_pointer_cast(shared_from_this()); + + log_debug(GetDisplayId(), ": subscribing to tick event"); + _on_tick_register_id = GetEpisode().Lock()->RegisterOnTickEvent( + [cb = std::move(callback), weak_self = WeakPtr(self)](const auto &snapshot) { + auto self = weak_self.lock(); + if (self != nullptr) { + auto data = self->TickRssSensor(snapshot.GetTimestamp()); + if (data != nullptr) { + cb(std::move(data)); + } } - switch (response.lateralResponseRight) { - case ::ad_rss::state::LateralResponse::None: - lateralResponseRight = csd::LateralResponse::None; - break; - case ::ad_rss::state::LateralResponse::BrakeMin: - lateralResponseRight = csd::LateralResponse::BrakeMin; - break; - } - switch (response.lateralResponseLeft) { - case ::ad_rss::state::LateralResponse::None: - lateralResponseLeft = csd::LateralResponse::None; - break; - case ::ad_rss::state::LateralResponse::BrakeMin: - lateralResponseLeft = csd::LateralResponse::BrakeMin; - break; - } - return MakeShared(timestamp.frame, timestamp.elapsed_seconds, - GetTransform(), - result, longitudinalResponse, lateralResponseRight, - lateralResponseLeft, accelerationRestriction, egoVelocity); - } else { - return nullptr; + }); +} + +void RssSensor::Stop() { + if (!IsListening()) { + log_error(GetDisplayId(), ": not listening at all"); + return; + } + + // ensure there is no processing anymore + const std::lock_guard lock(_processing_lock); + + log_debug(GetDisplayId(), ": unsubscribing from tick event"); + GetEpisode().Lock()->RemoveOnTickEvent(_on_tick_register_id); + _on_tick_register_id = 0u; + + ::ad::map::access::cleanup(); +} + +const ::ad::rss::world::RssDynamics& RssSensor::GetEgoVehicleDynamics() const { + return _rss_check->GetEgoVehicleDynamics(); +} + +void RssSensor::SetEgoVehicleDynamics(const ::ad::rss::world::RssDynamics &ego_dynamics) { + _rss_check->SetEgoVehicleDynamics(ego_dynamics); +} + +const ::ad::rss::world::RssDynamics& RssSensor::GetOtherVehicleDynamics() const { + return _rss_check->GetOtherVehicleDynamics(); +} + +void RssSensor::SetOtherVehicleDynamics(const ::ad::rss::world::RssDynamics &other_vehicle_dynamics) { + _rss_check->SetOtherVehicleDynamics(other_vehicle_dynamics); +} + +const ::carla::rss::RoadBoundariesMode& RssSensor::GetRoadBoundariesMode() const { + return _rss_check->GetRoadBoundariesMode(); +} + +void RssSensor::SetRoadBoundariesMode(const ::carla::rss::RoadBoundariesMode &road_boundaries_mode) { + _rss_check->SetRoadBoundariesMode(road_boundaries_mode); +} + +void RssSensor::AppendRoutingTarget(const ::carla::geom::Transform &routing_target) { + _rss_check->AppendRoutingTarget(routing_target); +} + +const std::vector<::carla::geom::Transform> RssSensor::GetRoutingTargets() const { + return _rss_check->GetRoutingTargets(); +} + +void RssSensor::ResetRoutingTargets() { + _rss_check->ResetRoutingTargets(); +} + +void RssSensor::SetVisualizationMode(const ::carla::rss::VisualizationMode &visualization_mode) { + _rss_check->SetVisualizationMode(visualization_mode); +} + +const ::carla::rss::VisualizationMode& RssSensor::GetVisualizationMode() const { + return _rss_check->GetVisualizationMode(); +} + +void RssSensor::DropRoute() { + // don't execute this immediately as it might break calculations completely + // postpone to next sensor tick + _drop_route=true; +} + +SharedPtr RssSensor::TickRssSensor(const Timestamp ×tamp) { + try { + bool result = false; + ::ad::rss::state::ProperResponse response; + ::ad::rss::world::AccelerationRestriction acceleration_restriction; + ::ad::rss::state::RssStateSnapshot rss_state_snapshot; + carla::rss::EgoDynamicsOnRoute ego_dynamics_on_route; + if (_processing_lock.try_lock()) { + spdlog::debug("RssSensor tick: T={}", timestamp.frame); + carla::client::World world = GetWorld(); + SharedPtr actors = world.GetActors(); + + if ( _drop_route ) + { + _drop_route=false; + _rss_check->DropRoute(); } - } catch (const std::exception &e) { - std::cout << e.what() << std::endl; + + // 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); _processing_lock.unlock(); + + _rss_check->VisualizeResults(world, GetParent()); + + spdlog::debug("RssSensor response: T={} S:{}->E:{} DeltaT:{}", timestamp.frame, ego_dynamics_on_route.time_since_epoch_check_start_ms, + ego_dynamics_on_route.time_since_epoch_check_end_ms, + ego_dynamics_on_route.time_since_epoch_check_end_ms-ego_dynamics_on_route.time_since_epoch_check_start_ms); + return MakeShared(timestamp.frame, timestamp.elapsed_seconds, GetTransform(), + result, response, acceleration_restriction, rss_state_snapshot, ego_dynamics_on_route); + } else { + spdlog::debug("RssSensor tick dropped: T={}", timestamp.frame); return nullptr; } + } catch (const std::exception &e) { + /// @todo do we need to unsubscribe the sensor here? + std::cout << e.what() << std::endl; + _processing_lock.unlock(); + spdlog::error("RssSensor tick exception"); + return nullptr; + } catch (...) { + _processing_lock.unlock(); + spdlog::error("RssSensor tick exception"); + return nullptr; } - - void RssSensor::Stop() { - _is_listening = false; - } - - const ::ad_rss::world::RssDynamics &RssSensor::GetEgoVehicleDynamics() const { - return mRssCheck->getEgoVehicleDynamics(); - } - - void RssSensor::SetEgoVehicleDynamics(const ::ad_rss::world::RssDynamics &dynamics) { - mRssCheck->setEgoVehicleDynamics(dynamics); - } - - const ::ad_rss::world::RssDynamics &RssSensor::GetOtherVehicleDynamics() const { - return mRssCheck->getOtherVehicleDynamics(); - } - - void RssSensor::SetOtherVehicleDynamics(const ::ad_rss::world::RssDynamics &dynamics) { - mRssCheck->setOtherVehicleDynamics(dynamics); - } +} } // namespace client } // namespace carla diff --git a/LibCarla/source/carla/rss/RssSensor.h b/LibCarla/source/carla/rss/RssSensor.h index a51746140..bcb200b63 100644 --- a/LibCarla/source/carla/rss/RssSensor.h +++ b/LibCarla/source/carla/rss/RssSensor.h @@ -1,68 +1,116 @@ -// Copyright (c) 2019 Intel Corporation +// Copyright (c) 2019-2020 Intel Corporation // // This work is licensed under the terms of the MIT license. // For a copy, see . #pragma once +#include #include +#include #include "carla/client/Sensor.h" -namespace ad_rss { +namespace ad { +namespace rss { namespace world { - class RssDynamics; -} // namespace world -} // namespace ad_rss + +/// forward declaration of the RssDynamics struct +struct RssDynamics; + +} //namespace world +} //namespace rss +} // namespace ad namespace carla { + namespace rss { - class RssCheck; -} // 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; + +} //namespace rss + namespace client { - class RssSensor final : public Sensor { - public: +/// The RSS Sensor class implementing the carla::client::Sensor interface +/// This class is a proxy to the RssCheck class +class RssSensor : public Sensor { + public: + using Sensor::Sensor; - using Sensor::Sensor; + /// @brief constructor + explicit RssSensor(ActorInitializer init); - ~RssSensor(); + /// @brief destructor + ~RssSensor(); - /// Register a @a callback to be executed each time a new measurement is - /// received. - /// - /// @warning Calling this function on a sensor that is already listening - /// steals the data stream from the previously set callback. Note that - /// several instances of Sensor (even in different processes) may point to - /// the same sensor in the simulator. - void Listen(CallbackFunctionType callback) override; + /// Register a @a callback to be executed each time a new measurement is + /// received. + /// + /// @warning Calling this function on a sensor that is already listening + /// steals the data stream from the previously set callback. Note that + /// several instances of Sensor (even in different processes) may point to + /// the same sensor in the simulator. + void Listen(CallbackFunctionType callback) override; - /// Stop listening for new measurements. - void Stop() override; + /// Stop listening for new measurements. + void Stop() override; - /// Return whether this Sensor instance is currently listening to the - /// associated sensor in the simulator. - bool IsListening() const override { return _is_listening; - } + /// Return whether this Sensor instance is currently listening to the + /// associated sensor in the simulator. + bool IsListening() const override { return _on_tick_register_id != 0u; } - bool visualize_results = false; + /// @returns the currently used dynamics of the ego vehicle (@see also RssCheck::GetEgoVehicleDynamics()) + const ::ad::rss::world::RssDynamics& GetEgoVehicleDynamics() const; + /// @brief sets the ego vehicle dynamics to be used by the ego vehicle (@see also RssCheck::SetEgoVehicleDynamics()) + void SetEgoVehicleDynamics(const ::ad::rss::world::RssDynamics &ego_dynamics); - const ::ad_rss::world::RssDynamics &GetEgoVehicleDynamics() const; - void SetEgoVehicleDynamics(const ::ad_rss::world::RssDynamics &egoDynamics); - const ::ad_rss::world::RssDynamics &GetOtherVehicleDynamics() const; - void SetOtherVehicleDynamics(const ::ad_rss::world::RssDynamics &otherVehicleDynamics); + /// @returns the currently used dynamics of other vehicles (@see also RssCheck::GetOtherVehicleDynamics()) + const ::ad::rss::world::RssDynamics& GetOtherVehicleDynamics() const; + /// @brief sets the ego vehicle dynamics to be used by other vehicles (@see also RssCheck::SetOtherVehicleDynamics()) + void SetOtherVehicleDynamics(const ::ad::rss::world::RssDynamics &other_vehicle_dynamics); - private: + /// @returns the current mode for respecting the road boundaries (@see also RssCheck::GetRoadBoundariesMode()) + const ::carla::rss::RoadBoundariesMode& GetRoadBoundariesMode() const; + /// @brief sets the current mode for respecting the road boundaries (@see also RssCheck::SetRoadBoundariesMode()) + void SetRoadBoundariesMode(const ::carla::rss::RoadBoundariesMode &road_boundaries_mode); - SharedPtr TickRssSensor(const Timestamp ×tamp); + /// @returns the current routing targets (@see also RssCheck::GetRoutingTargets()) + const std::vector<::carla::geom::Transform> GetRoutingTargets() const; + /// @brief appends a routing target to the current routing target list (@see also RssCheck::AppendRoutingTarget()) + void AppendRoutingTarget(const ::carla::geom::Transform &routing_target); + /// @brief resets the current routing target (@see also RssCheck::ResetRoutingTargets()) + void ResetRoutingTargets(); - std::mutex _processing_lock; + /// @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; - bool _is_listening = false; + /// @brief drop the current route (@see also RssCheck::DropRoute()) + void DropRoute(); - SharedPtr _map; +private: - std::shared_ptr mRssCheck; - }; + /// the acutal sensor tick callback function + SharedPtr TickRssSensor(const Timestamp ×tamp); + + /// 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 + std::mutex _processing_lock; + + //// the object actually performing the RSS processing + std::shared_ptr<::carla::rss::RssCheck> _rss_check; + + /// reqired to store DropRoute() requests until next sensor tick + bool _drop_route; +}; } // namespace client } // namespace carla diff --git a/LibCarla/source/carla/sensor/data/RssResponse.h b/LibCarla/source/carla/sensor/data/RssResponse.h index 08ef16b46..6a393e625 100644 --- a/LibCarla/source/carla/sensor/data/RssResponse.h +++ b/LibCarla/source/carla/sensor/data/RssResponse.h @@ -1,4 +1,4 @@ -// Copyright (c) 2019 Intel Corporation +// Copyright (c) 2019-2020 Intel Corporation // // This work is licensed under the terms of the MIT license. // For a copy, see . @@ -6,35 +6,12 @@ #pragma once #include "carla/sensor/SensorData.h" -#include "ad_rss/world/AccelerationRestriction.hpp" -#include "ad_rss/world/Velocity.hpp" +#include "carla/rss/RssCheck.h" namespace carla { namespace sensor { namespace data { - enum class LateralResponse : int32_t - { - /// No action required. - None = 0, - - /// Vehicle has to decerate at least with brake min laterally - BrakeMin = 1 - }; - - enum class LongitudinalResponse : int32_t - { - /// No action required. - None = 0, - - /// Vehicle has to decerate at least with brake min correct longitudinally - BrakeMinCorrect = 1, - - /// Vehicle has to decerate at least with brake min longitudinally - BrakeMin = 2 - }; - - /// A RSS Response class RssResponse : public SensorData { public: @@ -44,60 +21,51 @@ namespace data { double timestamp, const rpc::Transform &sensor_transform, const bool &response_valid, - const LongitudinalResponse &longitudinal_response, - const LateralResponse &lateral_response_right, - const LateralResponse &lateral_response_left, - const ad_rss::world::AccelerationRestriction &acceleration_restriction, - const ad_rss::world::Velocity &ego_velocity) + const ::ad::rss::state::ProperResponse &response, + const ::ad::rss::world::AccelerationRestriction &acceleration_restriction, + const ::ad::rss::state::RssStateSnapshot &rss_state_snapshot, + const carla::rss::EgoDynamicsOnRoute &ego_dynamics_on_route) : SensorData(frame_number, timestamp, sensor_transform), _response_valid(response_valid), - _longitudinal_response(longitudinal_response), - _lateral_response_right(lateral_response_right), - _lateral_response_left(lateral_response_left), + _response(response), _acceleration_restriction(acceleration_restriction), - _ego_velocity(ego_velocity){} + _rss_state_snapshot(rss_state_snapshot), + _ego_dynamics_on_route(ego_dynamics_on_route){} bool GetResponseValid() const { return _response_valid; } - const LongitudinalResponse &GetLongitudinalResponse() const { - return _longitudinal_response; + const ::ad::rss::state::ProperResponse &GetProperResponse() const { + return _response; } - const LateralResponse &GetLateralResponseRight() const { - return _lateral_response_right; - } - - const LateralResponse &GetLateralResponseLeft() const { - return _lateral_response_left; - } - - const ad_rss::world::AccelerationRestriction &GetAccelerationRestriction() const { + const ::ad::rss::world::AccelerationRestriction &GetAccelerationRestriction() const { return _acceleration_restriction; } - const ad_rss::world::Velocity &GetEgoVelocity() const { - return _ego_velocity; + const ::ad::rss::state::RssStateSnapshot &GetRssStateSnapshot() const { + return _rss_state_snapshot; + } + + const carla::rss::EgoDynamicsOnRoute &GetEgoDynamicsOnRoute() const { + return _ego_dynamics_on_route; } private: - /// The validity of RSS calculation. + /*! + * The validity of RSS calculation. + */ bool _response_valid; - /// The current longitudinal rss response. - LongitudinalResponse _longitudinal_response; + ::ad::rss::state::ProperResponse _response; - /// The current lateral rss response at right side in respect to ego-vehicle driving direction. - LateralResponse _lateral_response_right; + ::ad::rss::world::AccelerationRestriction _acceleration_restriction; - /// The current lateral rss state at left side in respect to ego-vehicle driving direction. - LateralResponse _lateral_response_left; + ::ad::rss::state::RssStateSnapshot _rss_state_snapshot; - ad_rss::world::AccelerationRestriction _acceleration_restriction; - - ad_rss::world::Velocity _ego_velocity; + carla::rss::EgoDynamicsOnRoute _ego_dynamics_on_route; }; } // namespace data diff --git a/PythonAPI/carla/setup.py b/PythonAPI/carla/setup.py index 7f4987c92..eb22b83d4 100755 --- a/PythonAPI/carla/setup.py +++ b/PythonAPI/carla/setup.py @@ -13,12 +13,11 @@ import os import platform import sys - def get_libcarla_extensions(): include_dirs = ['dependencies/include'] library_dirs = ['dependencies/lib'] - libraries = [] + libraries = ['jpeg', 'tiff'] sources = ['source/libcarla/libcarla.cpp'] @@ -41,8 +40,7 @@ def get_libcarla_extensions(): os.path.join(pwd, 'dependencies/lib/libboost_filesystem.a'), os.path.join(pwd, 'dependencies/lib/libRecast.a'), os.path.join(pwd, 'dependencies/lib/libDetour.a'), - os.path.join(pwd, 'dependencies/lib/libDetourCrowd.a'), - os.path.join(pwd, 'dependencies/lib', pylib)] + os.path.join(pwd, 'dependencies/lib/libDetourCrowd.a')] extra_compile_args = [ '-isystem', 'dependencies/include/system', '-fPIC', '-std=c++14', '-Werror', '-Wall', '-Wextra', '-Wpedantic', '-Wno-self-assign-overloaded', @@ -55,14 +53,29 @@ def get_libcarla_extensions(): if 'BUILD_RSS_VARIANT' in os.environ and os.environ['BUILD_RSS_VARIANT'] == 'true': print('Building AD RSS variant.') extra_compile_args += ['-DLIBCARLA_RSS_ENABLED'] - extra_link_args += [os.path.join(pwd, 'dependencies/lib/libad-rss.a')] + extra_link_args += [os.path.join(pwd, 'dependencies/lib/libad_rss_map_integration_python' + str(sys.version_info[0]) + '.a')] + extra_link_args += [os.path.join(pwd, 'dependencies/lib/libad_rss_map_integration.a')] + extra_link_args += [os.path.join(pwd, 'dependencies/lib/libad_map_access_python' + str(sys.version_info[0]) + '.a')] + extra_link_args += [os.path.join(pwd, 'dependencies/lib/libad_map_access.a')] + extra_link_args += [os.path.join(pwd, 'dependencies/lib/libad_rss_python' + str(sys.version_info[0]) + '.a')] + extra_link_args += [os.path.join(pwd, 'dependencies/lib/libad_rss.a')] + extra_link_args += [os.path.join(pwd, 'dependencies/lib/libad_physics_python' + str(sys.version_info[0]) + '.a')] + extra_link_args += [os.path.join(pwd, 'dependencies/lib/libad_physics.a')] + 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 += ['/usr/lib/x86_64-linux-gnu/libboost_system.so'] + extra_link_args += ['-ltbb'] + extra_link_args += ['-lrt'] + extra_link_args += ['-lproj'] + + extra_link_args += [os.path.join(pwd, 'dependencies/lib', pylib)] if 'TRAVIS' in os.environ and os.environ['TRAVIS'] == 'true': print('Travis CI build detected: disabling PNG support.') - extra_link_args += ['-ljpeg', '-ltiff'] extra_compile_args += ['-DLIBCARLA_IMAGE_WITH_PNG_SUPPORT=false'] else: - extra_link_args += ['-lpng', '-ljpeg', '-ltiff'] + libraries += ['png'] extra_compile_args += ['-DLIBCARLA_IMAGE_WITH_PNG_SUPPORT=true'] # @todo Why would we need this? include_dirs += ['/usr/lib/gcc/x86_64-linux-gnu/7/include'] diff --git a/PythonAPI/carla/source/libcarla/AdRss.cpp b/PythonAPI/carla/source/libcarla/AdRss.cpp index 92be9589e..a5de47912 100644 --- a/PythonAPI/carla/source/libcarla/AdRss.cpp +++ b/PythonAPI/carla/source/libcarla/AdRss.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2019 Intel Corporation +// Copyright (c) 2019-2020 Intel Corporation // // This work is licensed under the terms of the MIT license. // For a copy, see . @@ -7,91 +7,25 @@ #include #include -#include "ad_rss/world/RssDynamics.hpp" - -namespace ad_rss { -namespace physics { - std::ostream &operator<<(std::ostream &out, const Acceleration &accel) { - out << "Acceleration(value=" << std::to_string(static_cast(accel)) - << ")"; - return out; - } - - std::ostream &operator<<(std::ostream &out, const Distance &d) { - out << "Distance(value=" << std::to_string(static_cast(d)) - << ")"; - return out; - } - - std::ostream &operator<<(std::ostream &out, const Duration &d) { - out << "Duration(value=" << std::to_string(static_cast(d)) - << ")"; - return out; - } - - std::ostream &operator<<(std::ostream &out, const Speed &speed) { - out << "Speed(value=" << std::to_string(static_cast(speed)) - << ")"; - return out; - } - - std::ostream &operator<<(std::ostream &out, const AccelerationRange &range) { - out << "AccelerationRestriction(min=" << std::to_string(static_cast(range.minimum)) - << ", max= " << std::to_string(static_cast(range.maximum)) - << ")"; - return out; - } -} // namespace physics - -namespace world { - std::ostream &operator<<(std::ostream &out, const AccelerationRestriction &restr) { - out << "AccelerationRestriction(longitudinal_range=" << restr.longitudinalRange - << ", lateral_left_range=" << restr.lateralLeftRange - << ", lateral_right_range=" << restr.lateralRightRange - << ")"; - return out; - } - - std::ostream &operator<<(std::ostream &out, const LateralRssAccelerationValues &values) { - out << "LateralRssAccelerationValues(accel_max=" << values.accelMax - << ", brake_min=" << values.brakeMin - << ")"; - return out; - } - - std::ostream &operator<<(std::ostream &out, const LongitudinalRssAccelerationValues &values) { - out << "LongitudinalRssAccelerationValues(accel_max=" << values.accelMax - << ", brake_max=" << values.brakeMax - << ", brake_min=" << values.brakeMin - << ", brake_min_correct=" << values.brakeMinCorrect - << ")"; - return out; - } - - std::ostream &operator<<(std::ostream &out, const RssDynamics &dyn) { - out << "RssDynamics(alpha_lon=" << dyn.alphaLon - << ", alpha_lat=" << dyn.alphaLat - << ", lateral_fluctuation_margin=" << std::to_string(static_cast(dyn.lateralFluctuationMargin)) - << ", response_time=" << std::to_string(static_cast(dyn.responseTime)) - << ")"; - return out; - } - - std::ostream &operator<<(std::ostream &out, const Velocity &vel) { - out << "Velocity(speed_lon=" << vel.speedLon - << ", speed_lat=" << vel.speedLat - << ")"; - return out; - } -} // namespace world -} // namespace ad_rss +#include +#include namespace carla { +namespace rss { + + std::ostream &operator<<(std::ostream &out, const RssRestrictor &) { + out << "RssRestrictor()"; + return out; + } + +} // namespace rss + namespace sensor { namespace data { + std::ostream &operator<<(std::ostream &out, const RssResponse &resp) { - out << "RssResponse(frame=" << std::to_string(resp.GetFrame()) - << ", timestamp=" << std::to_string(resp.GetTimestamp()) + out << "RssResponse(frame=" << resp.GetFrame() + << ", timestamp=" << resp.GetTimestamp() << ", valid=" << resp.GetResponseValid() << ')'; return out; @@ -99,28 +33,31 @@ namespace data { } // namespace data } // namespace sensor - -namespace rss { -std::ostream &operator<<(std::ostream &out, const RssRestrictor &) { - out << "RssRestrictor()"; - return out; -} // namespace rss } // namespace carla - - -namespace carla { -} -} - static auto GetEgoVehicleDynamics(const carla::client::RssSensor &self) { - ad_rss::world::RssDynamics dynamics(self.GetEgoVehicleDynamics()); - return dynamics; + ad::rss::world::RssDynamics ego_dynamics(self.GetEgoVehicleDynamics()); + return ego_dynamics; } static auto GetOtherVehicleDynamics(const carla::client::RssSensor &self) { - ad_rss::world::RssDynamics dynamics(self.GetOtherVehicleDynamics()); - return dynamics; + ad::rss::world::RssDynamics other_dynamics(self.GetOtherVehicleDynamics()); + return other_dynamics; +} + +static auto GetRoadBoundariesMode(const carla::client::RssSensor &self) { + carla::rss::RoadBoundariesMode road_boundaries_mode(self.GetRoadBoundariesMode()); + return road_boundaries_mode; +} + +static auto GetRoutingTargets(const carla::client::RssSensor &self) { + std::vector routing_targets(self.GetRoutingTargets()); + return routing_targets; +} + +static auto GetVisualizationMode(const carla::client::RssSensor &self) { + carla::rss::VisualizationMode visualization_mode(self.GetVisualizationMode()); + return visualization_mode; } void export_ad_rss() { @@ -129,102 +66,67 @@ void export_ad_rss() { namespace cs = carla::sensor; namespace csd = carla::sensor::data; - enum_("LateralResponse") - .value("None", csd::LateralResponse::None) - .value("BrakeMin", csd::LateralResponse::BrakeMin) - ; - - enum_("LongitudinalResponse") - .value("None", csd::LongitudinalResponse::None) - .value("BrakeMinCorrect", csd::LongitudinalResponse::BrakeMinCorrect) - .value("BrakeMin", csd::LongitudinalResponse::BrakeMin) - ; - - class_("Acceleration", init()) - .def_readonly("value", &ad_rss::physics::Acceleration::operator double) + class_("EgoDynamicsOnRoute") + .def_readwrite("timestamp", &carla::rss::EgoDynamicsOnRoute::timestamp) + .def_readwrite("time_since_epoch_check_start_ms", &carla::rss::EgoDynamicsOnRoute::time_since_epoch_check_start_ms) + .def_readwrite("time_since_epoch_check_end_ms", &carla::rss::EgoDynamicsOnRoute::time_since_epoch_check_end_ms) + .def_readwrite("ego_speed", &carla::rss::EgoDynamicsOnRoute::ego_speed) + .def_readwrite("min_stopping_distance", &carla::rss::EgoDynamicsOnRoute::min_stopping_distance) + .def_readwrite("ego_center", &carla::rss::EgoDynamicsOnRoute::ego_center) + .def_readwrite("ego_heading", &carla::rss::EgoDynamicsOnRoute::ego_heading) + .def_readwrite("ego_center_within_route", &carla::rss::EgoDynamicsOnRoute::ego_center_within_route) + .def_readwrite("crossing_border", &carla::rss::EgoDynamicsOnRoute::crossing_border) + .def_readwrite("route_heading", &carla::rss::EgoDynamicsOnRoute::route_heading) + .def_readwrite("route_nominal_center", &carla::rss::EgoDynamicsOnRoute::route_nominal_center) + .def_readwrite("heading_diff", &carla::rss::EgoDynamicsOnRoute::heading_diff) + .def_readwrite("route_speed_lat", &carla::rss::EgoDynamicsOnRoute::route_speed_lat) + .def_readwrite("route_speed_lon", &carla::rss::EgoDynamicsOnRoute::route_speed_lon) + .def_readwrite("route_accel_lat", &carla::rss::EgoDynamicsOnRoute::route_accel_lat) + .def_readwrite("route_accel_lon", &carla::rss::EgoDynamicsOnRoute::route_accel_lon) + .def_readwrite("avg_route_accel_lat", &carla::rss::EgoDynamicsOnRoute::avg_route_accel_lat) + .def_readwrite("avg_route_accel_lon", &carla::rss::EgoDynamicsOnRoute::avg_route_accel_lon) .def(self_ns::str(self_ns::self)) ; - class_("Distance", init()) - .def_readonly("value", &ad_rss::physics::Distance::operator double) - .def(self_ns::str(self_ns::self)) + enum_("RoadBoundariesMode") + .value("Off", carla::rss::RoadBoundariesMode::Off) + .value("On", carla::rss::RoadBoundariesMode::On) ; - class_("Duration", init()) - .def_readonly("value", &ad_rss::physics::Duration::operator double) - .def(self_ns::str(self_ns::self)) - ; - - class_("AccelerationRange") - .def_readwrite("minimum", &ad_rss::physics::AccelerationRange::minimum) - .def_readwrite("maximum", &ad_rss::physics::AccelerationRange::maximum) - .def(self_ns::str(self_ns::self)) - ; - - class_("AccelerationRestriction") - .def_readwrite("longitudinal_range", &ad_rss::world::AccelerationRestriction::longitudinalRange) - .def_readwrite("lateral_left_range", &ad_rss::world::AccelerationRestriction::lateralLeftRange) - .def_readwrite("lateral_right_range", &ad_rss::world::AccelerationRestriction::lateralRightRange) - .def(self_ns::str(self_ns::self)) - ; - - class_("LateralRssAccelerationValues") - .def_readwrite("accel_max", &ad_rss::world::LateralRssAccelerationValues::accelMax) - .def_readwrite("brake_min", &ad_rss::world::LateralRssAccelerationValues::brakeMin) - .def(self_ns::str(self_ns::self)) - ; - - class_("LongitudinalRssAccelerationValues") - .def_readwrite("accel_max", &ad_rss::world::LongitudinalRssAccelerationValues::accelMax) - .def_readwrite("brake_max", &ad_rss::world::LongitudinalRssAccelerationValues::brakeMax) - .def_readwrite("brake_min", &ad_rss::world::LongitudinalRssAccelerationValues::brakeMin) - .def_readwrite("brake_min_correct", &ad_rss::world::LongitudinalRssAccelerationValues::brakeMinCorrect) - .def(self_ns::str(self_ns::self)) - ; - - class_("RssDynamics") - .def_readwrite("alpha_lon", &ad_rss::world::RssDynamics::alphaLon) - .def_readwrite("alpha_lat", &ad_rss::world::RssDynamics::alphaLat) - .def_readwrite("lateral_fluctuation_margin", &ad_rss::world::RssDynamics::lateralFluctuationMargin) - .def_readwrite("response_time", &ad_rss::world::RssDynamics::responseTime) - .def(self_ns::str(self_ns::self)) - ; - - class_("Speed", init()) - .def_readonly("value", &ad_rss::physics::Speed::operator double) - .def(self_ns::str(self_ns::self)) - ; - - class_("Velocity") - .def_readwrite("speed_lon", &ad_rss::world::Velocity::speedLon) - .def_readwrite("speed_lat", &ad_rss::world::Velocity::speedLat) - .def(self_ns::str(self_ns::self)) + enum_("VisualizationMode") + .value("Off", carla::rss::VisualizationMode::Off) + .value("RouteOnly", carla::rss::VisualizationMode::RouteOnly) + .value("VehicleStateOnly", carla::rss::VisualizationMode::VehicleStateOnly) + .value("VehicleStateAndRoute", carla::rss::VisualizationMode::VehicleStateAndRoute) + .value("All", carla::rss::VisualizationMode::All) ; class_, boost::noncopyable, boost::shared_ptr>("RssResponse", no_init) .add_property("response_valid", &csd::RssResponse::GetResponseValid) - .add_property("longitudinal_response", CALL_RETURNING_COPY(csd::RssResponse, GetLongitudinalResponse)) - .add_property("lateral_response_right", CALL_RETURNING_COPY(csd::RssResponse, GetLateralResponseRight)) - .add_property("lateral_response_left", CALL_RETURNING_COPY(csd::RssResponse, GetLateralResponseLeft)) + .add_property("proper_response", CALL_RETURNING_COPY(csd::RssResponse, GetProperResponse)) .add_property("acceleration_restriction", CALL_RETURNING_COPY(csd::RssResponse, GetAccelerationRestriction)) - .add_property("ego_velocity", CALL_RETURNING_COPY(csd::RssResponse, GetEgoVelocity)) + .add_property("rss_state_snapshot", CALL_RETURNING_COPY(csd::RssResponse, GetRssStateSnapshot)) + .add_property("ego_dynamics_on_route", CALL_RETURNING_COPY(csd::RssResponse, GetEgoDynamicsOnRoute)) .def(self_ns::str(self_ns::self)) ; class_, boost::noncopyable, boost::shared_ptr> ("RssSensor", no_init) - .def_readwrite("visualize_results", &cc::RssSensor::visualize_results) .add_property("ego_vehicle_dynamics", &GetEgoVehicleDynamics, &cc::RssSensor::SetEgoVehicleDynamics) .add_property("other_vehicle_dynamics", &GetOtherVehicleDynamics, &cc::RssSensor::SetOtherVehicleDynamics) + .add_property("road_boundaries_mode", &GetRoadBoundariesMode, &cc::RssSensor::SetRoadBoundariesMode) + .add_property("visualization_mode", &GetVisualizationMode, &cc::RssSensor::SetVisualizationMode) + .add_property("routing_targets", &GetRoutingTargets) + .def("append_routing_target", &cc::RssSensor::AppendRoutingTarget) + .def("reset_routing_targets", &cc::RssSensor::ResetRoutingTargets) + .def("drop_route", &cc::RssSensor::DropRoute) .def(self_ns::str(self_ns::self)) ; class_> ("RssRestrictor", no_init) .def(init<>()) - .def("restrictVehicleControl", &carla::rss::RssRestrictor::restrictVehicleControl, (arg("restriction"), arg("vehicleControl"))) + .def("restrict_vehicle_control", &carla::rss::RssRestrictor::RestrictVehicleControl, (arg("restriction"), arg("vehicleControl"))) .def(self_ns::str(self_ns::self)) ; } - - diff --git a/PythonAPI/carla/source/libcarla/Geom.cpp b/PythonAPI/carla/source/libcarla/Geom.cpp index 3c88beef2..64cba8e3a 100644 --- a/PythonAPI/carla/source/libcarla/Geom.cpp +++ b/PythonAPI/carla/source/libcarla/Geom.cpp @@ -176,6 +176,11 @@ void export_geom() { .def(self_ns::str(self_ns::self)) ; + class_>("vector_of_Transform") + .def(boost::python::vector_indexing_suite>()) + .def(self_ns::str(self_ns::self)) + ; + class_("BoundingBox") .def(init( (arg("location")=cg::Location(), arg("extent")=cg::Vector3D()))) diff --git a/PythonAPI/carla/source/libcarla/World.cpp b/PythonAPI/carla/source/libcarla/World.cpp index 2d57ff535..665e86d4a 100644 --- a/PythonAPI/carla/source/libcarla/World.cpp +++ b/PythonAPI/carla/source/libcarla/World.cpp @@ -18,14 +18,6 @@ namespace client { return PrintList(out, actors); } - std::ostream &operator<<(std::ostream &out, const Timestamp ×tamp) { - out << "Timestamp(frame=" << std::to_string(timestamp.frame) - << ",elapsed_seconds=" << std::to_string(timestamp.elapsed_seconds) - << ",delta_seconds=" << std::to_string(timestamp.delta_seconds) - << ",platform_timestamp=" << std::to_string(timestamp.platform_timestamp) << ')'; - return out; - } - std::ostream &operator<<(std::ostream &out, const World &world) { out << "World(id=" << world.GetId() << ')'; return out; diff --git a/PythonAPI/carla/source/libcarla/libcarla.cpp b/PythonAPI/carla/source/libcarla/libcarla.cpp index 03e781153..5088e6a30 100644 --- a/PythonAPI/carla/source/libcarla/libcarla.cpp +++ b/PythonAPI/carla/source/libcarla/libcarla.cpp @@ -198,8 +198,15 @@ static auto MakeCallback(boost::python::object callback) { #ifdef LIBCARLA_RSS_ENABLED #include "AdRss.cpp" +extern "C" { + void initlibad_physics_python(); + void initlibad_rss_python(); + void initlibad_map_access_python(); + void initlibad_rss_map_integration_python(); +} #endif + BOOST_PYTHON_MODULE(libcarla) { using namespace boost::python; PyEval_InitThreads(); @@ -218,7 +225,11 @@ BOOST_PYTHON_MODULE(libcarla) { export_exception(); export_commands(); export_trafficmanager(); - #ifdef LIBCARLA_RSS_ENABLED +# ifdef LIBCARLA_RSS_ENABLED + initlibad_physics_python(); + initlibad_rss_python(); + initlibad_map_access_python(); + initlibad_rss_map_integration_python(); export_ad_rss(); - #endif +# endif } diff --git a/PythonAPI/examples/manual_control_rss.py b/PythonAPI/examples/manual_control_rss.py index a051aed6e..ea18342e6 100755 --- a/PythonAPI/examples/manual_control_rss.py +++ b/PythonAPI/examples/manual_control_rss.py @@ -23,7 +23,7 @@ Use ARROWS or WASD keys for control. P : toggle autopilot M : toggle manual transmission ,/. : gear up/down - T : toggle RSS restrictor + B : Toggle RSS Road Boundaries Mode TAB : change sensor position ` : next sensor @@ -33,6 +33,8 @@ Use ARROWS or WASD keys for control. R : toggle recording images to disk + D : RSS check drop current route + CTRL + R : toggle recording of simulation (replacing any previous) CTRL + P : start replaying last recorded simulation CTRL + + : increments the start time of the replay by 1 second (+SHIFT = 10 seconds) @@ -95,6 +97,7 @@ try: from pygame.locals import K_DOWN from pygame.locals import K_ESCAPE from pygame.locals import K_F1 + from pygame.locals import K_F2 from pygame.locals import K_LEFT from pygame.locals import K_PERIOD from pygame.locals import K_RIGHT @@ -103,6 +106,7 @@ try: from pygame.locals import K_TAB from pygame.locals import K_UP from pygame.locals import K_a + from pygame.locals import K_b from pygame.locals import K_c from pygame.locals import K_d from pygame.locals import K_h @@ -111,7 +115,6 @@ try: from pygame.locals import K_q from pygame.locals import K_r from pygame.locals import K_s - from pygame.locals import K_t from pygame.locals import K_w from pygame.locals import K_MINUS from pygame.locals import K_EQUALS @@ -147,9 +150,10 @@ def get_actor_display_name(actor, truncate=250): class World(object): - def __init__(self, carla_world, hud, actor_filter, actor_role_name='hero'): + def __init__(self, carla_world, hud, actor_filter, actor_role_name='hero', external_actor=False): self.world = carla_world self.actor_role_name = actor_role_name + self.external_actor = external_actor self.map = self.world.get_map() self.hud = hud self.player = None @@ -170,29 +174,48 @@ class World(object): # Keep same camera config if the camera manager exists. cam_index = self.camera_manager.index if self.camera_manager is not None else 0 cam_pos_index = self.camera_manager.transform_index if self.camera_manager is not None else 0 - # Get a random blueprint. - blueprint = random.choice(self.world.get_blueprint_library().filter(self._actor_filter)) - blueprint.set_attribute('role_name', self.actor_role_name) - if blueprint.has_attribute('color'): - color = random.choice(blueprint.get_attribute('color').recommended_values) - blueprint.set_attribute('color', color) - if blueprint.has_attribute('driver_id'): - driver_id = random.choice(blueprint.get_attribute('driver_id').recommended_values) - blueprint.set_attribute('driver_id', driver_id) - if blueprint.has_attribute('is_invincible'): - blueprint.set_attribute('is_invincible', 'true') - # Spawn the player. - if self.player is not None: - spawn_point = self.player.get_transform() - spawn_point.location.z += 2.0 - spawn_point.rotation.roll = 0.0 - spawn_point.rotation.pitch = 0.0 - self.destroy() - self.player = self.world.try_spawn_actor(blueprint, spawn_point) - while self.player is None: - spawn_points = self.map.get_spawn_points() - spawn_point = random.choice(spawn_points) if spawn_points else carla.Transform() - self.player = self.world.try_spawn_actor(blueprint, spawn_point) + + if self.external_actor: + # Check whether there is already an actor with defined role name + for actor in self.world.get_actors(): + if actor.attributes.get('role_name') == self.actor_role_name: + self.player = actor; + break; + else: + # Get a random blueprint. + blueprint = random.choice(self.world.get_blueprint_library().filter(self._actor_filter)) + blueprint.set_attribute('role_name', self.actor_role_name) + if blueprint.has_attribute('color'): + color = random.choice(blueprint.get_attribute('color').recommended_values) + blueprint.set_attribute('color', color) + if blueprint.has_attribute('driver_id'): + driver_id = random.choice(blueprint.get_attribute('driver_id').recommended_values) + blueprint.set_attribute('driver_id', driver_id) + if blueprint.has_attribute('is_invincible'): + blueprint.set_attribute('is_invincible', 'true') + # Spawn the player. + if self.player is not None: + spawn_point = self.player.get_transform() + spawn_point.location.z += 2.0 + spawn_point.rotation.roll = 0.0 + spawn_point.rotation.pitch = 0.0 + self.destroy() + self.player = self.world.try_spawn_actor(blueprint, spawn_point) + while self.player is None: + spawn_points = self.map.get_spawn_points() + spawn_point = random.choice(spawn_points) if spawn_points else carla.Transform() + self.player = self.world.try_spawn_actor(blueprint, spawn_point) + + if self.external_actor: + ego_sensors = [] + for actor in self.world.get_actors(): + if actor.parent == self.player: + ego_sensors.append(actor) + + for ego_sensor in ego_sensors: + if ego_sensor is not None: + ego_sensor.destroy() + # Set up the sensors. self.collision_sensor = CollisionSensor(self.player, self.hud) self.lane_invasion_sensor = LaneInvasionSensor(self.player, self.hud) @@ -246,7 +269,6 @@ class KeyboardControl(object): self._autopilot_enabled = args.autopilot self._world = world self._restrictor = None - self._restrictorEnabled = True if isinstance(world.player, carla.Vehicle): self._control = carla.VehicleControl() world.player.set_autopilot(self._autopilot_enabled) @@ -285,6 +307,9 @@ class KeyboardControl(object): world.camera_manager.set_sensor(event.key - 1 - K_0) elif event.key == K_r and not (pygame.key.get_mods() & KMOD_CTRL): world.camera_manager.toggle_recording() + elif event.key == K_d and not (pygame.key.get_mods() & KMOD_CTRL): + if self._restrictor: + self._world.rss_sensor.drop_route() elif event.key == K_r and (pygame.key.get_mods() & KMOD_CTRL): if (world.recording_enabled): client.stop_recorder() @@ -320,6 +345,21 @@ class KeyboardControl(object): else: world.recording_start += 1 world.hud.notification("Recording start time is %d" % (world.recording_start)) + elif event.key == K_F2: + if self._world and self._world.rss_sensor: + visualization_mode = self._world.rss_sensor.sensor.visualization_mode + visualization_mode = visualization_mode + 1 + if visualization_mode > carla.VisualizationMode.All: + visualization_mode = carla.VisualizationMode.Off + self._world.rss_sensor.sensor.visualization_mode = carla.VisualizationMode(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.RoadBoundariesMode.Off: + self._world.rss_sensor.sensor.road_boundaries_mode = carla.RoadBoundariesMode.On; + print("carla.RoadBoundariesMode.On") + else: + self._world.rss_sensor.sensor.road_boundaries_mode = carla.RoadBoundariesMode.Off; + print("carla.RoadBoundariesMode.Off") if isinstance(self._control, carla.VehicleControl): if event.key == K_q: self._control.gear = 1 if self._control.reverse else -1 @@ -336,25 +376,33 @@ class KeyboardControl(object): 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_t: - self._restrictorEnabled = not self._restrictorEnabled - world.hud.notification('RSS Restrictor %s' % ('On' if self._restrictorEnabled else 'Off')) if not self._autopilot_enabled: if isinstance(self._control, carla.VehicleControl): + prev_steer_cache = self._steer_cache; self._parse_vehicle_keys(pygame.key.get_pressed(), clock.get_time()) self._control.reverse = self._control.gear < 0 vehicle_control = self._control world.hud.original_vehicle_control = vehicle_control world.hud.restricted_vehicle_control = vehicle_control - if self._restrictor and self._restrictorEnabled: + + #limit speed to 30kmh + v = self._world.player.get_velocity() + if (3.6 * math.sqrt(v.x**2 + v.y**2 + v.z**2)) > 30.0: + self._control.throttle = 0 + + #if self._world.rss_sensor and self._world.rss_sensor.ego_dynamics_on_route and not self._world.rss_sensor.ego_dynamics_on_route.ego_center_within_route: + # print ("Not on route!" + str(self._world.rss_sensor.ego_dynamics_on_route)) + if self._restrictor: rss_restriction = self._world.rss_sensor.acceleration_restriction if self._world.rss_sensor and self._world.rss_sensor.response_valid else None if rss_restriction: - rss_ego_velocity = self._world.rss_sensor.ego_velocity + rss_ego_dynamics_on_route = self._world.rss_sensor.ego_dynamics_on_route vehicle_physics = world.player.get_physics_control() if not (pygame.key.get_mods() & KMOD_CTRL): - vehicle_control = self._restrictor.restrictVehicleControl(vehicle_control, rss_restriction, rss_ego_velocity, vehicle_physics) + vehicle_control = self._restrictor.restrict_vehicle_control(vehicle_control, rss_restriction, rss_ego_dynamics_on_route, vehicle_physics) world.hud.restricted_vehicle_control = vehicle_control + if world.hud.original_vehicle_control.steer != world.hud.restricted_vehicle_control.steer: + self._steer_cache = prev_steer_cache world.player.apply_control(vehicle_control) @@ -366,15 +414,9 @@ class KeyboardControl(object): self._control.throttle = 1.0 if keys[K_UP] or keys[K_w] else 0.0 steer_increment = 5e-4 * milliseconds if keys[K_LEFT] or keys[K_a]: - if self._steer_cache > 0: - self._steer_cache = 0 - else: - self._steer_cache -= steer_increment + 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 + self._steer_cache += steer_increment else: self._steer_cache = 0.0 self._steer_cache = min(0.7, max(-0.7, self._steer_cache)) @@ -553,7 +595,7 @@ class HUD(object): pygame.draw.rect(display, (255, 0, 0), rect) text_color = (255, 0, 0) item = item[0] - if item: # At this point has to be a str. + if isinstance(item, basestring) and len(item) > 0: # At this point has to be a str. surface = self._font_mono.render(item, True, text_color) display.blit(surface, (8, v_offset)) v_offset += 18 @@ -715,18 +757,15 @@ class GnssSensor(object): # -- RssSensor -------------------------------------------------------- # ============================================================================== - class RssSensor(object): def __init__(self, parent_actor): self.sensor = None self._parent = parent_actor self.timestamp = None self.response_valid = False - self.lon_response = None - self.lat_response_right = None - self.lat_response_left = None + self.proper_response = None self.acceleration_restriction = None - self.ego_velocity = None + self.ego_dynamics_on_route = None 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) @@ -740,6 +779,7 @@ class RssSensor(object): raise RuntimeError('CARLA PythonAPI not compiled in RSS variant, please "make PythonAPI.rss"') weak_self = weakref.ref(self) self.sensor.visualize_results = True + self.sensor.road_boundaries_mode = carla.RoadBoundariesMode.On; self.sensor.listen(lambda event: RssSensor._on_rss_response(weak_self, event)) @staticmethod @@ -747,15 +787,22 @@ class RssSensor(object): self = weak_self() if not self or not response: return - self.timestamp = response.timestamp - self.response_valid = response.response_valid - self.lon_response = response.longitudinal_response - self.lat_response_right = response.lateral_response_right - self.lat_response_left = response.lateral_response_left - self.acceleration_restriction = response.acceleration_restriction - self.ego_velocity = response.ego_velocity + 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.acceleration_restriction = response.acceleration_restriction + self.ego_dynamics_on_route = response.ego_dynamics_on_route + #else: + # print("ignore outdated response {}".format(delta_time)) + def drop_route(self): + self.sensor.drop_route() + # ============================================================================== # -- CameraManager ------------------------------------------------------------- # ============================================================================== @@ -882,7 +929,7 @@ def game_loop(args): pygame.HWSURFACE | pygame.DOUBLEBUF) hud = HUD(args.width, args.height) - world = World(client.get_world(), hud, args.filter, args.rolename) + world = World(client.get_world(), hud, args.filter, args.rolename, args.externalActor) controller = KeyboardControl(world, args) clock = pygame.time.Clock() @@ -948,6 +995,10 @@ def main(): 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')] diff --git a/Util/BuildTools/Ad-rss.sh b/Util/BuildTools/Ad-rss.sh index 48c21cdd2..5be3d3e44 100755 --- a/Util/BuildTools/Ad-rss.sh +++ b/Util/BuildTools/Ad-rss.sh @@ -4,48 +4,26 @@ # -- Set up environment -------------------------------------------------------- # ============================================================================== -export CC=/usr/bin/clang-7 -export CXX=/usr/bin/clang++-7 +#export CC=/usr/bin/clang-7 +#export CXX=/usr/bin/clang++-7 source $(dirname "$0")/Environment.sh -mkdir -p ${CARLA_BUILD_FOLDER} -pushd ${CARLA_BUILD_FOLDER} >/dev/null - # ============================================================================== -# -- Get and compile ad-rss ---------------------------------------------------- +# -- Get and compile ad-rss ------------------------------------------- # ============================================================================== -AD_RSS_BASENAME=ad-rss +pushd ${CARLA_ROOT_FOLDER}/dependencies/ad-rss >/dev/null -if [[ -d "${AD_RSS_BASENAME}-install" ]] ; then - log "${AD_RSS_BASENAME} already installed." +if [ "${CMAKE_PREFIX_PATH}" == "" ]; then + export CMAKE_PREFIX_PATH=${CARLA_BUILD_FOLDER}/boost-1.72.0-c7-install else - rm -Rf ${AD_RSS_BASENAME}-source ${AD_RSS_BASENAME}-build - - log "Retrieving ad-rss." - - git clone --depth=1 -b v1.5.0 https://github.com/intel/ad-rss-lib.git ${AD_RSS_BASENAME}-source - - log "Compiling ad-rss." - - mkdir -p ${AD_RSS_BASENAME}-build - - pushd ${AD_RSS_BASENAME}-build >/dev/null - - cmake -G "Ninja" \ - -DCMAKE_BUILD_TYPE=RelWithDebInfo -DBUILD_SHARED_LIBS=OFF -DCMAKE_POSITION_INDEPENDENT_CODE=ON -DBUILD_TESTING=OFF -DCMAKE_INSTALL_PREFIX="../${AD_RSS_BASENAME}-install" \ - ../${AD_RSS_BASENAME}-source - - ninja install - - popd >/dev/null - - rm -Rf ${AD_RSS_BASENAME}-source ${AD_RSS_BASENAME}-build - + export CMAKE_PREFIX_PATH=${CMAKE_PREFIX_PATH}:${CARLA_BUILD_FOLDER}/boost-1.72.0-c7-install fi -unset AD_RSS_BASENAME +#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 # ============================================================================== # -- ...and we are done -------------------------------------------------------- diff --git a/Util/BuildTools/BuildLibCarla.sh b/Util/BuildTools/BuildLibCarla.sh index e8eeb4155..3ba4bf403 100755 --- a/Util/BuildTools/BuildLibCarla.sh +++ b/Util/BuildTools/BuildLibCarla.sh @@ -131,7 +131,8 @@ 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 -DCMAKE_PREFIX_PATH=$CARLA_BUILD_FOLDER/ad-rss-install" + . ${CARLA_ROOT_FOLDER}/dependencies/ad-rss/install/setup.bash + CMAKE_EXTRA_OPTIONS="${CMAKE_EXTRA_OPTIONS:+${CMAKE_EXTRA_OPTIONS} }-DBUILD_RSS_VARIANT=ON" else fatal_error "Invalid build configuration \"$1\"" fi diff --git a/Util/BuildTools/Setup.sh b/Util/BuildTools/Setup.sh index e3bb17a1e..a1d86148c 100755 --- a/Util/BuildTools/Setup.sh +++ b/Util/BuildTools/Setup.sh @@ -106,18 +106,20 @@ else ./bootstrap.sh \ --with-toolset=clang \ --prefix=../boost-install \ - --with-libraries=python,filesystem \ + --with-libraries=python,filesystem,system,program_options \ --with-python=${py2} --with-python-root=${py2_root} - if ${TRAVIS} ; then + EXTRA_B2_ARGS="" + if [ "${TRAVIS}" == "true" ] ; then echo "using python : ${pyv} : ${py2_root}/bin/python2 ;" > ${HOME}/user-config.jam else echo "using python : ${pyv} : ${py2_root}/bin/python2 ;" > project-config.jam + EXTRA_B2_ARGS="--ignore-user-config" fi - ./b2 toolset="${BOOST_TOOLSET}" cxxflags="${BOOST_CFLAGS}" --prefix="../${BOOST_BASENAME}-install" -j ${CARLA_BUILD_CONCURRENCY} stage release - ./b2 toolset="${BOOST_TOOLSET}" cxxflags="${BOOST_CFLAGS}" --prefix="../${BOOST_BASENAME}-install" -j ${CARLA_BUILD_CONCURRENCY} install - ./b2 toolset="${BOOST_TOOLSET}" cxxflags="${BOOST_CFLAGS}" --prefix="../${BOOST_BASENAME}-install" -j ${CARLA_BUILD_CONCURRENCY} --clean-all + ./b2 ${EXTRA_B2_ARGS} toolset="${BOOST_TOOLSET}" cxxflags="${BOOST_CFLAGS}" --prefix="../${BOOST_BASENAME}-install" -j ${CARLA_BUILD_CONCURRENCY} stage release + ./b2 ${EXTRA_B2_ARGS} toolset="${BOOST_TOOLSET}" cxxflags="${BOOST_CFLAGS}" --prefix="../${BOOST_BASENAME}-install" -j ${CARLA_BUILD_CONCURRENCY} install + ./b2 ${EXTRA_B2_ARGS} toolset="${BOOST_TOOLSET}" cxxflags="${BOOST_CFLAGS}" --prefix="../${BOOST_BASENAME}-install" -j ${CARLA_BUILD_CONCURRENCY} --clean-all # Get rid of python2 build artifacts completely & do a clean build for python3 popd >/dev/null @@ -143,14 +145,14 @@ else --with-libraries=python \ --with-python=${py3} --with-python-root=${py3_root} - if ${TRAVIS} ; then + if [ "${TRAVIS}" == "true" ] ; then echo "using python : ${pyv} : ${py3_root}/bin/python3 ;" > ${HOME}/user-config.jam else echo "using python : ${pyv} : ${py3_root}/bin/python3 ;" > project-config.jam fi - ./b2 toolset="${BOOST_TOOLSET}" cxxflags="${BOOST_CFLAGS}" --prefix="../${BOOST_BASENAME}-install" -j ${CARLA_BUILD_CONCURRENCY} stage release - ./b2 toolset="${BOOST_TOOLSET}" cxxflags="${BOOST_CFLAGS}" --prefix="../${BOOST_BASENAME}-install" -j ${CARLA_BUILD_CONCURRENCY} install + ./b2 ${EXTRA_B2_ARGS} toolset="${BOOST_TOOLSET}" cxxflags="${BOOST_CFLAGS}" --prefix="../${BOOST_BASENAME}-install" -j ${CARLA_BUILD_CONCURRENCY} stage release + ./b2 ${EXTRA_B2_ARGS} toolset="${BOOST_TOOLSET}" cxxflags="${BOOST_CFLAGS}" --prefix="../${BOOST_BASENAME}-install" -j ${CARLA_BUILD_CONCURRENCY} install popd >/dev/null diff --git a/dependencies/ad-rss/.gitignore b/dependencies/ad-rss/.gitignore new file mode 100644 index 000000000..134565718 --- /dev/null +++ b/dependencies/ad-rss/.gitignore @@ -0,0 +1,3 @@ +build/ +install/ +log/ diff --git a/dependencies/ad-rss/colcon.meta b/dependencies/ad-rss/colcon.meta new file mode 100644 index 000000000..b7ef22440 --- /dev/null +++ b/dependencies/ad-rss/colcon.meta @@ -0,0 +1,22 @@ +{ + "names": { + "ad_physics": { + "cmake-args": ["-DBUILD_PYTHON_BINDING=ON", "-DCMAKE_POSITION_INDEPENDENT_CODE=ON", "-DBUILD_SHARED_LIBS=OFF"] + }, + "ad_map_access": { + "cmake-args": ["-DBUILD_PYTHON_BINDING=ON", "-DCMAKE_POSITION_INDEPENDENT_CODE=ON", "-DBUILD_SHARED_LIBS=OFF"] + }, + "ad_map_opendrive_reader": { + "cmake-args": ["-DCMAKE_POSITION_INDEPENDENT_CODE=ON", "-DBUILD_SHARED_LIBS=OFF"] + }, + "ad_rss": { + "cmake-args": ["-DBUILD_PYTHON_BINDING=ON", "-DCMAKE_POSITION_INDEPENDENT_CODE=ON", "-DBUILD_SHARED_LIBS=OFF"] + }, + "ad_rss_map_integration": { + "cmake-args": ["-DBUILD_PYTHON_BINDING=ON", "-DCMAKE_POSITION_INDEPENDENT_CODE=ON", "-DBUILD_SHARED_LIBS=OFF"] + }, + "spdlog": { + "cmake-args": ["-DCMAKE_POSITION_INDEPENDENT_CODE=ON", "-DBUILD_SHARED_LIBS=OFF"] + } + } +} diff --git a/dependencies/ad-rss/src/ad-rss-lib b/dependencies/ad-rss/src/ad-rss-lib new file mode 160000 index 000000000..eabf53da4 --- /dev/null +++ b/dependencies/ad-rss/src/ad-rss-lib @@ -0,0 +1 @@ +Subproject commit eabf53da4c68549e079c4ab152f4c40ba6c98f65 diff --git a/dependencies/ad-rss/src/map b/dependencies/ad-rss/src/map new file mode 160000 index 000000000..dc9ee91d2 --- /dev/null +++ b/dependencies/ad-rss/src/map @@ -0,0 +1 @@ +Subproject commit dc9ee91d2546b3f676882c82de63dfbddc8aff76 diff --git a/dependencies/ad-rss/src/spdlog b/dependencies/ad-rss/src/spdlog new file mode 160000 index 000000000..cca004efe --- /dev/null +++ b/dependencies/ad-rss/src/spdlog @@ -0,0 +1 @@ +Subproject commit cca004efe4e66136a5f9f37e007d28a23bb729e0