diff --git a/CHANGELOG.md b/CHANGELOG.md index 9ce5be9b8..1aa30e45f 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,4 +1,5 @@ ## Latest + * Add build variant with AD RSS library integration with RSS sensor and result visualisation * Added new sensor: Inertial measurement unit (IMU) * Moved GNSS sensor from client to server side * Now all the camera-based sensors are provided with an additional parametrized lens distortion shader diff --git a/Docs/img/rss_carla_integration.png b/Docs/img/rss_carla_integration.png new file mode 100644 index 000000000..7c8cab24a Binary files /dev/null and b/Docs/img/rss_carla_integration.png differ diff --git a/Docs/rss_lib_integration.md b/Docs/rss_lib_integration.md new file mode 100644 index 000000000..6b219a67b --- /dev/null +++ b/Docs/rss_lib_integration.md @@ -0,0 +1,42 @@ +

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. + +**As the _ad-rss-lib_ library is licensed under LGPL-2.1-only, building the variant which includes this feature and therefor the library might have some implications to the outgoing license of the resulting binary!** + +It provides basic implementations of both an **RssSensor**, the situation analysis and response generation by the **ad-rss-lib** and an basic **RssRestrictor** class which applies the restrictions to given vehicle commands. + +The **RssSensor** results can be visualized within CARLA. +[![RSS safety 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/documentation/Main.html) for further details. + + +

Compilation

+ +RSS integration is a Linux-only build variant. +Please see [Build System](dev/build_system.md) for general information. +*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 currently only considering vehicles within the same road segment, but on all lanes within that segment. Intersections are not yet supported! + +

RssRestrictor

+The current implementation of the RssRestrictor checks and potentially modifies a given *VehicleControl* generated by e.g. and Automated Driving stack or user imput 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. diff --git a/LibCarla/cmake/client/CMakeLists.txt b/LibCarla/cmake/client/CMakeLists.txt index 5c14a44d4..96a39803c 100644 --- a/LibCarla/cmake/client/CMakeLists.txt +++ b/LibCarla/cmake/client/CMakeLists.txt @@ -1,6 +1,19 @@ cmake_minimum_required(VERSION 3.5.1) project(libcarla-client) +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) + endforeach() +endif() + # Install Recast&Detour libraries install(DIRECTORY "${RECAST_INCLUDE_PATH}/recast" DESTINATION include/system) file(GLOB libcarla_carla_recastlib "${RECAST_LIB_PATH}/*.*") @@ -129,6 +142,14 @@ file(GLOB libcarla_carla_rpc_sources set(libcarla_sources "${libcarla_sources};${libcarla_carla_rpc_sources}") install(FILES ${libcarla_carla_rpc_sources} DESTINATION include/carla/rpc) +if (BUILD_RSS_VARIANT) + file(GLOB libcarla_carla_rss_sources + "${libcarla_source_path}/carla/rss/*.cpp" + "${libcarla_source_path}/carla/rss/*.h") + set(libcarla_sources "${libcarla_sources};${libcarla_carla_rss_sources}") + install(FILES ${libcarla_carla_rss_sources} DESTINATION include/carla/rss) +endif() + file(GLOB libcarla_carla_sensor_sources "${libcarla_source_path}/carla/sensor/*.cpp" "${libcarla_source_path}/carla/sensor/*.h") @@ -192,6 +213,7 @@ install(FILES ${libcarla_pugixml_sources} DESTINATION include/pugixml) # ============================================================================== # Create targets for debug and release in the same build type. # ============================================================================== + if (LIBCARLA_BUILD_RELEASE) add_library(carla_client STATIC ${libcarla_sources}) @@ -201,6 +223,11 @@ if (LIBCARLA_BUILD_RELEASE) "${RPCLIB_INCLUDE_PATH}" "${RECAST_INCLUDE_PATH}") + if (BUILD_RSS_VARIANT) + target_compile_definitions(carla_client PRIVATE RSS_ENABLED) + target_link_libraries(carla_client ad-rss-lib) + endif() + install(TARGETS carla_client DESTINATION lib) if (WIN32) # @todo Fix PythonAPI build on Windows. @@ -233,6 +260,11 @@ if (LIBCARLA_BUILD_DEBUG) "${RPCLIB_INCLUDE_PATH}" "${RECAST_INCLUDE_PATH}") + if (BUILD_RSS_VARIANT) + target_compile_definitions(carla_client_debug PRIVATE RSS_ENABLED) + target_link_libraries(carla_client_debug ad-rss-lib) + endif() + install(TARGETS carla_client_debug DESTINATION lib) if (WIN32) # @todo Fix PythonAPI build on Windows. diff --git a/LibCarla/source/carla/client/detail/ActorFactory.cpp b/LibCarla/source/carla/client/detail/ActorFactory.cpp index 4f71979f9..4a43e1851 100644 --- a/LibCarla/source/carla/client/detail/ActorFactory.cpp +++ b/LibCarla/source/carla/client/detail/ActorFactory.cpp @@ -11,6 +11,9 @@ #include "carla/client/Actor.h" #include "carla/client/LaneInvasionSensor.h" #include "carla/client/ServerSideSensor.h" +#ifdef RSS_ENABLED +#include "carla/rss/RssSensor.h" +#endif #include "carla/client/TrafficLight.h" #include "carla/client/TrafficSign.h" #include "carla/client/Vehicle.h" @@ -74,6 +77,10 @@ namespace detail { auto init = ActorInitializer{description, episode}; if (description.description.id == "sensor.other.lane_invasion") { return MakeActorImpl(std::move(init), gc); +#ifdef RSS_ENABLED + } else if (description.description.id == "sensor.other.rss") { + return MakeActorImpl(std::move(init), gc); +#endif } else if (description.HasAStream()) { return MakeActorImpl(std::move(init), gc); } else if (StringUtil::StartsWith(description.description.id, "vehicle.")) { diff --git a/LibCarla/source/carla/rss/RssCheck.cpp b/LibCarla/source/carla/rss/RssCheck.cpp new file mode 100644 index 000000000..ea881b2fd --- /dev/null +++ b/LibCarla/source/carla/rss/RssCheck.cpp @@ -0,0 +1,465 @@ +// Copyright (c) 2019 Intel Corporation +// +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#include "carla/rss/RssCheck.h" + +#include "carla/client/Map.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 + +namespace carla { +namespace rss { + + namespace csd = carla::sensor::data; + +// constants for deg-> rad conversion PI / 180 + 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; + } + + 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; + } + + RssCheck::~RssCheck() = default; + + const ::ad_rss::world::RssDynamics &RssCheck::getEgoVehicleDynamics() const { + return *_egoVehicleDynamics; + } + + void RssCheck::setEgoVehicleDynamics(const ::ad_rss::world::RssDynamics &dynamics) { + *_egoVehicleDynamics = dynamics; + } + + const ::ad_rss::world::RssDynamics &RssCheck::getOtherVehicleDynamics() const { + return *_otherVehicleDynamics; + } + + void RssCheck::setOtherVehicleDynamics(const ::ad_rss::world::RssDynamics &dynamics) { + *_otherVehicleDynamics = 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); + + ::ad_rss::world::WorldModel worldModel; + worldModel.timeIndex = timestamp.frame; + + ::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; + + cg::Location egoVehicleLocation = carlaEgoVehicle->GetLocation(); + auto egoClientWaypoint = clientMap->GetWaypoint(egoVehicleLocation, false); + + 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(); + + // calculate driving direction + auto yawDiff = calculateAngleDelta(egoWaypointLoc.rotation.yaw, egoVehicleTransform.rotation.yaw); + + 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; + } + 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.); + } + + void RssCheck::initEgoVehicleDynamics(::ad_rss::world::RssDynamics &egoDynamics) const { + egoDynamics = getEgoVehicleDynamics(); + } + + 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; + } + 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; + } + } + + 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}; + } + if (!state.longitudinalState.isSafe) { + lonColor.r = 255u; + if (dangerous) { + lonColor.g = 0u; + } else { + lonColor.g = 255u; + } + } + if (!state.lateralStateLeft.isSafe) { + latLColor.r = 255u; + if (dangerous) { + latLColor.g = 0u; + } else { + latLColor.g = 255u; + } + } + if (!state.lateralStateRight.isSafe) { + latRColor.r = 255u; + if (dangerous) { + latRColor.g = 0u; + } else { + latRColor.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); + } + } + } + +} // namespace rss +} // namespace carla diff --git a/LibCarla/source/carla/rss/RssCheck.h b/LibCarla/source/carla/rss/RssCheck.h new file mode 100644 index 000000000..046c2e0d7 --- /dev/null +++ b/LibCarla/source/carla/rss/RssCheck.h @@ -0,0 +1,97 @@ +// Copyright (c) 2019 Intel Corporation +// +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#pragma once + +#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; + + class RssCheck { + public: + + RssCheck(); + + ~RssCheck(); + + 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); + + 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); + + private: + + std::shared_ptr<::ad_rss::world::RssDynamics> _egoVehicleDynamics; + std::shared_ptr<::ad_rss::world::RssDynamics> _otherVehicleDynamics; + + std::shared_ptr<::ad_rss::core::RssSituationExtraction> _rssSituationExtraction; + std::shared_ptr<::ad_rss::core::RssSituationChecking> _rssSituationChecking; + std::shared_ptr<::ad_rss::core::RssResponseResolving> _rssResponseResolving; + + uint64_t calculateLaneSegmentId(const carla::road::Lane &lane) const; + + void calculateLatLonVelocities(const cg::Transform &laneLocation, + const cg::Vector3D &velocity, + ::ad_rss::world::Object &rssObject, bool inverseDirection) const; + + void calculateOccupiedRegions(const std::array &bounds, + ::ad_rss::world::OccupiedRegionVector &occupiedRegions, const carla::road::Map &map, + const bool drivingInRoadDirection, + const cg::Transform &vehicleTransform) const; + + void convertAndSetLaneSegmentId(const carla::road::Lane &lane, + ::ad_rss::world::LaneSegment &laneSegment) const; + + std::array getVehicleBounds(const cc::Vehicle &vehicle) 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; + + void visualizeRssResults(::ad_rss::state::RssStateSnapshot stateSnapshot, + const cg::Location &egoVehicleLocation, + const cg::Transform &egoVehicleTransform, cc::World &world) const; + }; + +} // namespace rss +} // namespace carla diff --git a/LibCarla/source/carla/rss/RssRestrictor.cpp b/LibCarla/source/carla/rss/RssRestrictor.cpp new file mode 100644 index 000000000..bf7b8a072 --- /dev/null +++ b/LibCarla/source/carla/rss/RssRestrictor.cpp @@ -0,0 +1,75 @@ +// Copyright (c) 2019 Intel Corporation +// +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#include "carla/rss/RssRestrictor.h" +#include "carla/rpc/VehicleControl.h" +#include "carla/rpc/VehiclePhysicsControl.h" + +#include "ad_rss/world/AccelerationRestriction.hpp" +#include "ad_rss/world/Velocity.hpp" + +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; + } + +} // namespace rss +} // namespace carla diff --git a/LibCarla/source/carla/rss/RssRestrictor.h b/LibCarla/source/carla/rss/RssRestrictor.h new file mode 100644 index 000000000..c7ed09a76 --- /dev/null +++ b/LibCarla/source/carla/rss/RssRestrictor.h @@ -0,0 +1,42 @@ +// Copyright (c) 2019 Intel Corporation +// +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#pragma once + +#include + +namespace ad_rss { +namespace world { + struct AccelerationRestriction; + struct Velocity; +} // namespace world +} // namespace ad_rss + +namespace carla { +namespace rpc { + class VehicleControl; + class VehiclePhysicsControl; +} // namespace rpc + +namespace rss { + + class RssRestrictor { + public: + + RssRestrictor(); + + ~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); + + private: + + }; + +} // namespace rss +} // namespace carla diff --git a/LibCarla/source/carla/rss/RssSensor.cpp b/LibCarla/source/carla/rss/RssSensor.cpp new file mode 100644 index 000000000..ae2cfb2ad --- /dev/null +++ b/LibCarla/source/carla/rss/RssSensor.cpp @@ -0,0 +1,146 @@ +// Copyright (c) 2019 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 "carla/client/ActorList.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() = 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; + } + + 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.*"); + + // 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(); + + csd::LongitudinalResponse longitudinalResponse; + csd::LateralResponse lateralResponseRight; + csd::LateralResponse lateralResponseLeft; + + 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; + } + 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; + } + } catch (const std::exception &e) { + std::cout << e.what() << std::endl; + _processing_lock.unlock(); + 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 new file mode 100644 index 000000000..a51746140 --- /dev/null +++ b/LibCarla/source/carla/rss/RssSensor.h @@ -0,0 +1,68 @@ +// Copyright (c) 2019 Intel Corporation +// +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#pragma once + +#include +#include "carla/client/Sensor.h" + +namespace ad_rss { +namespace world { + class RssDynamics; +} // namespace world +} // namespace ad_rss + +namespace carla { +namespace rss { + class RssCheck; +} // namespace rss +namespace client { + + class RssSensor final : public Sensor { + public: + + using Sensor::Sensor; + + ~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; + + /// 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; + } + + bool visualize_results = false; + + 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); + + private: + + SharedPtr TickRssSensor(const Timestamp ×tamp); + + std::mutex _processing_lock; + + bool _is_listening = false; + + SharedPtr _map; + + std::shared_ptr mRssCheck; + }; + +} // namespace client +} // namespace carla diff --git a/LibCarla/source/carla/sensor/SensorRegistry.h b/LibCarla/source/carla/sensor/SensorRegistry.h index d3ca49c57..f162c1064 100644 --- a/LibCarla/source/carla/sensor/SensorRegistry.h +++ b/LibCarla/source/carla/sensor/SensorRegistry.h @@ -33,6 +33,7 @@ class AObstacleDetectionSensor; class ARayCastLidar; class ASceneCaptureCamera; class ASemanticSegmentationCamera; +class ARssSensor; class FWorldObserver; namespace carla { @@ -55,6 +56,7 @@ namespace sensor { std::pair, std::pair, std::pair, + std::pair, std::pair >; @@ -75,6 +77,7 @@ namespace sensor { #include "Carla/Sensor/WorldObserver.h" #include "Carla/Sensor/GnssSensor.h" #include "Carla/Sensor/LaneInvasionSensor.h" +#include "Carla/Sensor/RssSensor.h" #include "Carla/Sensor/ObstacleDetectionSensor.h" #endif // LIBCARLA_SENSOR_REGISTRY_WITH_SENSOR_INCLUDES diff --git a/LibCarla/source/carla/sensor/data/RssResponse.h b/LibCarla/source/carla/sensor/data/RssResponse.h new file mode 100644 index 000000000..08ef16b46 --- /dev/null +++ b/LibCarla/source/carla/sensor/data/RssResponse.h @@ -0,0 +1,105 @@ +// Copyright (c) 2019 Intel Corporation +// +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#pragma once + +#include "carla/sensor/SensorData.h" +#include "ad_rss/world/AccelerationRestriction.hpp" +#include "ad_rss/world/Velocity.hpp" + +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: + + explicit RssResponse( + size_t frame_number, + 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) + : 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), + _acceleration_restriction(acceleration_restriction), + _ego_velocity(ego_velocity){} + + bool GetResponseValid() const { + return _response_valid; + } + + const LongitudinalResponse &GetLongitudinalResponse() const { + return _longitudinal_response; + } + + const LateralResponse &GetLateralResponseRight() const { + return _lateral_response_right; + } + + const LateralResponse &GetLateralResponseLeft() const { + return _lateral_response_left; + } + + const ad_rss::world::AccelerationRestriction &GetAccelerationRestriction() const { + return _acceleration_restriction; + } + + const ad_rss::world::Velocity &GetEgoVelocity() const { + return _ego_velocity; + } + + private: + + /// The validity of RSS calculation. + bool _response_valid; + + /// The current longitudinal rss response. + LongitudinalResponse _longitudinal_response; + + /// The current lateral rss response at right side in respect to ego-vehicle driving direction. + LateralResponse _lateral_response_right; + + /// The current lateral rss state at left side in respect to ego-vehicle driving direction. + LateralResponse _lateral_response_left; + + ad_rss::world::AccelerationRestriction _acceleration_restriction; + + ad_rss::world::Velocity _ego_velocity; + }; + +} // namespace data +} // namespace sensor +} // namespace carla diff --git a/PythonAPI/carla/setup.py b/PythonAPI/carla/setup.py index da5bbfb91..9f2c7faf3 100755 --- a/PythonAPI/carla/setup.py +++ b/PythonAPI/carla/setup.py @@ -16,8 +16,9 @@ import sys def get_libcarla_extensions(): include_dirs = ['dependencies/include'] + library_dirs = ['dependencies/lib'] - libraries = [] + libraries = ['jpeg', 'tiff'] sources = ['source/libcarla/libcarla.cpp'] @@ -50,12 +51,16 @@ def get_libcarla_extensions(): '-Wconversion', '-Wfloat-overflow-conversion', '-DBOOST_ERROR_CODE_HEADER_ONLY', '-DLIBCARLA_WITH_PYTHON_SUPPORT' ] + 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')] + 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 new file mode 100644 index 000000000..92be9589e --- /dev/null +++ b/PythonAPI/carla/source/libcarla/AdRss.cpp @@ -0,0 +1,230 @@ +// Copyright (c) 2019 Intel Corporation +// +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#include +#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 + +namespace carla { +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()) + << ", valid=" << resp.GetResponseValid() + << ')'; + return out; + } + +} // 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; +} + +static auto GetOtherVehicleDynamics(const carla::client::RssSensor &self) { + ad_rss::world::RssDynamics dynamics(self.GetOtherVehicleDynamics()); + return dynamics; +} + +void export_ad_rss() { + using namespace boost::python; + namespace cc = carla::client; + 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) + .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)) + ; + + 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)) + ; + + 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("acceleration_restriction", CALL_RETURNING_COPY(csd::RssResponse, GetAccelerationRestriction)) + .add_property("ego_velocity", CALL_RETURNING_COPY(csd::RssResponse, GetEgoVelocity)) + .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) + .def(self_ns::str(self_ns::self)) + ; + + class_> + ("RssRestrictor", no_init) + .def(init<>()) + .def("restrictVehicleControl", &carla::rss::RssRestrictor::restrictVehicleControl, (arg("restriction"), arg("vehicleControl"))) + .def(self_ns::str(self_ns::self)) + ; +} + + diff --git a/PythonAPI/carla/source/libcarla/libcarla.cpp b/PythonAPI/carla/source/libcarla/libcarla.cpp index 6867acdd5..45ee678b5 100644 --- a/PythonAPI/carla/source/libcarla/libcarla.cpp +++ b/PythonAPI/carla/source/libcarla/libcarla.cpp @@ -187,6 +187,10 @@ static auto MakeCallback(boost::python::object callback) { #include "World.cpp" #include "Commands.cpp" +#ifdef LIBCARLA_RSS_ENABLED +#include "AdRss.cpp" +#endif + BOOST_PYTHON_MODULE(libcarla) { using namespace boost::python; PyEval_InitThreads(); @@ -204,4 +208,8 @@ BOOST_PYTHON_MODULE(libcarla) { export_client(); export_exception(); export_commands(); + + #ifdef LIBCARLA_RSS_ENABLED + export_ad_rss(); + #endif } diff --git a/PythonAPI/examples/manual_control_rss.py b/PythonAPI/examples/manual_control_rss.py new file mode 100755 index 000000000..a051aed6e --- /dev/null +++ b/PythonAPI/examples/manual_control_rss.py @@ -0,0 +1,972 @@ +#!/usr/bin/env python + +# Copyright (c) 2019 Computer Vision Center (CVC) at the Universitat Autonoma de +# Barcelona (UAB). +# Copyright (c) 2019 Intel Corporation +# +# This work is licensed under the terms of the MIT license. +# For a copy, see . + +# Allows controlling a vehicle with a keyboard. For a simpler and more +# documented example, please take a look at tutorial.py. + +""" +Welcome to CARLA manual control. + +Use ARROWS or WASD keys for control. + + W : throttle + S : brake + AD : steer + Q : toggle reverse + Space : hand-brake + P : toggle autopilot + M : toggle manual transmission + ,/. : gear up/down + T : toggle RSS restrictor + + TAB : change sensor position + ` : next sensor + [1-9] : change to sensor [1-9] + C : change weather (Shift+C reverse) + Backspace : change vehicle + + R : toggle recording images to disk + + CTRL + R : toggle recording of simulation (replacing any previous) + CTRL + P : start replaying last recorded simulation + CTRL + + : increments the start time of the replay by 1 second (+SHIFT = 10 seconds) + CTRL + - : decrements the start time of the replay by 1 second (+SHIFT = 10 seconds) + + F1 : toggle HUD + H/? : toggle help + ESC : quit +""" + +from __future__ import print_function + + +# ============================================================================== +# -- find carla module --------------------------------------------------------- +# ============================================================================== + + +import glob +import os +import sys + +try: + sys.path.append(glob.glob('../carla/dist/carla-*%d.%d-%s.egg' % ( + sys.version_info.major, + sys.version_info.minor, + 'win-amd64' if os.name == 'nt' else 'linux-x86_64'))[0]) +except IndexError: + pass + + +# ============================================================================== +# -- imports ------------------------------------------------------------------- +# ============================================================================== + + +import carla + +from carla import ColorConverter as cc + +import argparse +import collections +import datetime +import inspect +import logging +import math +import random +import re +import weakref + +try: + import pygame + from pygame.locals import KMOD_CTRL + from pygame.locals import KMOD_SHIFT + from pygame.locals import K_0 + from pygame.locals import K_9 + from pygame.locals import K_BACKQUOTE + from pygame.locals import K_BACKSPACE + from pygame.locals import K_COMMA + from pygame.locals import K_DOWN + from pygame.locals import K_ESCAPE + from pygame.locals import K_F1 + from pygame.locals import K_LEFT + from pygame.locals import K_PERIOD + from pygame.locals import K_RIGHT + from pygame.locals import K_SLASH + from pygame.locals import K_SPACE + from pygame.locals import K_TAB + from pygame.locals import K_UP + from pygame.locals import K_a + from pygame.locals import K_c + from pygame.locals import K_d + from pygame.locals import K_h + from pygame.locals import K_m + from pygame.locals import K_p + from pygame.locals import K_q + from pygame.locals import K_r + from pygame.locals import K_s + from pygame.locals import K_t + from pygame.locals import K_w + from pygame.locals import K_MINUS + from pygame.locals import K_EQUALS +except ImportError: + raise RuntimeError('cannot import pygame, make sure pygame package is installed') + +try: + import numpy as np +except ImportError: + raise RuntimeError('cannot import numpy, make sure numpy package is installed') + + +# ============================================================================== +# -- Global functions ---------------------------------------------------------- +# ============================================================================== + + +def find_weather_presets(): + rgx = re.compile('.+?(?:(?<=[a-z])(?=[A-Z])|(?<=[A-Z])(?=[A-Z][a-z])|$)') + name = lambda x: ' '.join(m.group(0) for m in rgx.finditer(x)) + presets = [x for x in dir(carla.WeatherParameters) if re.match('[A-Z].+', x)] + return [(getattr(carla.WeatherParameters, x), name(x)) for x in presets] + + +def get_actor_display_name(actor, truncate=250): + name = ' '.join(actor.type_id.replace('_', '.').title().split('.')[1:]) + return (name[:truncate - 1] + u'\u2026') if len(name) > truncate else name + + +# ============================================================================== +# -- World --------------------------------------------------------------------- +# ============================================================================== + + +class World(object): + def __init__(self, carla_world, hud, actor_filter, actor_role_name='hero'): + self.world = carla_world + self.actor_role_name = actor_role_name + self.map = self.world.get_map() + self.hud = hud + self.player = None + self.collision_sensor = None + self.lane_invasion_sensor = None + self.gnss_sensor = None + self.rss_sensor = None + self.camera_manager = None + self._weather_presets = find_weather_presets() + self._weather_index = 0 + self._actor_filter = actor_filter + self.restart() + self.world.on_tick(hud.on_world_tick) + self.recording_enabled = False + self.recording_start = 0 + + def restart(self): + # Keep same camera config if the camera manager exists. + cam_index = self.camera_manager.index if self.camera_manager is not None else 0 + cam_pos_index = self.camera_manager.transform_index if self.camera_manager is not None else 0 + # 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) + # Set up the sensors. + self.collision_sensor = CollisionSensor(self.player, self.hud) + self.lane_invasion_sensor = LaneInvasionSensor(self.player, self.hud) + self.gnss_sensor = GnssSensor(self.player) + self.rss_sensor = RssSensor(self.player) + self.camera_manager = CameraManager(self.player, self.hud) + self.camera_manager.transform_index = cam_pos_index + self.camera_manager.set_sensor(cam_index, notify=False) + actor_type = get_actor_display_name(self.player) + self.hud.notification(actor_type) + + def next_weather(self, reverse=False): + self._weather_index += -1 if reverse else 1 + self._weather_index %= len(self._weather_presets) + preset = self._weather_presets[self._weather_index] + self.hud.notification('Weather: %s' % preset[1]) + self.player.get_world().set_weather(preset[0]) + + def tick(self, clock): + self.hud.tick(self, clock) + + def render(self, display): + self.camera_manager.render(display) + self.hud.render(display) + + def destroy_sensors(self): + self.camera_manager.sensor.destroy() + self.camera_manager.sensor = None + self.camera_manager.index = None + + def destroy(self): + actors = [ + self.camera_manager.sensor, + self.collision_sensor.sensor, + self.lane_invasion_sensor.sensor, + self.gnss_sensor.sensor] + if self.rss_sensor: + actors.append(self.rss_sensor.sensor) + actors.append(self.player) + for actor in actors: + if actor is not None: + actor.destroy() + +# ============================================================================== +# -- KeyboardControl ----------------------------------------------------------- +# ============================================================================== + + +class KeyboardControl(object): + def __init__(self, world, args): + self._autopilot_enabled = args.autopilot + self._world = world + self._restrictor = None + self._restrictorEnabled = True + if isinstance(world.player, carla.Vehicle): + self._control = carla.VehicleControl() + world.player.set_autopilot(self._autopilot_enabled) + self._restrictor = carla.RssRestrictor() + elif isinstance(world.player, carla.Walker): + self._control = carla.WalkerControl() + self._autopilot_enabled = False + self._rotation = world.player.get_transform().rotation + else: + raise NotImplementedError("Actor type not supported") + self._steer_cache = 0.0 + world.hud.notification("Press 'H' or '?' for help.", seconds=4.0) + + def parse_events(self, client, world, clock): + for event in pygame.event.get(): + if event.type == pygame.QUIT: + return True + elif event.type == pygame.KEYUP: + if self._is_quit_shortcut(event.key): + return True + elif event.key == K_BACKSPACE: + world.restart() + elif event.key == K_F1: + world.hud.toggle_info() + elif event.key == K_h or (event.key == K_SLASH and pygame.key.get_mods() & KMOD_SHIFT): + world.hud.help.toggle() + elif event.key == K_TAB: + world.camera_manager.toggle_camera() + elif event.key == K_c and pygame.key.get_mods() & KMOD_SHIFT: + world.next_weather(reverse=True) + elif event.key == K_c: + world.next_weather() + elif event.key == K_BACKQUOTE: + world.camera_manager.next_sensor() + elif event.key > K_0 and event.key <= K_9: + world.camera_manager.set_sensor(event.key - 1 - K_0) + elif event.key == K_r and not (pygame.key.get_mods() & KMOD_CTRL): + world.camera_manager.toggle_recording() + elif event.key == K_r and (pygame.key.get_mods() & KMOD_CTRL): + if (world.recording_enabled): + client.stop_recorder() + world.recording_enabled = False + world.hud.notification("Recorder is OFF") + else: + client.start_recorder("manual_recording.rec") + world.recording_enabled = True + world.hud.notification("Recorder is ON") + elif event.key == K_p and (pygame.key.get_mods() & KMOD_CTRL): + # stop recorder + client.stop_recorder() + world.recording_enabled = False + # work around to fix camera at start of replaying + currentIndex = world.camera_manager.index + world.destroy_sensors() + # disable autopilot + self._autopilot_enabled = False + world.player.set_autopilot(self._autopilot_enabled) + world.hud.notification("Replaying file 'manual_recording.rec'") + # replayer + client.replay_file("manual_recording.rec", world.recording_start, 0, 0) + world.camera_manager.set_sensor(currentIndex) + elif event.key == K_MINUS and (pygame.key.get_mods() & KMOD_CTRL): + if pygame.key.get_mods() & KMOD_SHIFT: + world.recording_start -= 10 + else: + world.recording_start -= 1 + world.hud.notification("Recording start time is %d" % (world.recording_start)) + elif event.key == K_EQUALS and (pygame.key.get_mods() & KMOD_CTRL): + if pygame.key.get_mods() & KMOD_SHIFT: + world.recording_start += 10 + else: + world.recording_start += 1 + world.hud.notification("Recording start time is %d" % (world.recording_start)) + if isinstance(self._control, carla.VehicleControl): + if event.key == K_q: + self._control.gear = 1 if self._control.reverse else -1 + elif event.key == K_m: + self._control.manual_gear_shift = not self._control.manual_gear_shift + self._control.gear = world.player.get_control().gear + world.hud.notification('%s Transmission' % + ('Manual' if self._control.manual_gear_shift else 'Automatic')) + elif self._control.manual_gear_shift and event.key == K_COMMA: + self._control.gear = max(-1, self._control.gear - 1) + elif self._control.manual_gear_shift and event.key == K_PERIOD: + self._control.gear = self._control.gear + 1 + elif event.key == K_p and not (pygame.key.get_mods() & KMOD_CTRL): + self._autopilot_enabled = not self._autopilot_enabled + world.player.set_autopilot(self._autopilot_enabled) + world.hud.notification('Autopilot %s' % ('On' if self._autopilot_enabled else 'Off')) + 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): + 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: + 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 + 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) + world.hud.restricted_vehicle_control = vehicle_control + + world.player.apply_control(vehicle_control) + + elif isinstance(self._control, carla.WalkerControl): + self._parse_walker_keys(pygame.key.get_pressed(), clock.get_time()) + world.player.apply_control(self._control) + + def _parse_vehicle_keys(self, keys, milliseconds): + self._control.throttle = 1.0 if keys[K_UP] or keys[K_w] else 0.0 + steer_increment = 5e-4 * milliseconds + if keys[K_LEFT] or keys[K_a]: + if self._steer_cache > 0: + self._steer_cache = 0 + else: + self._steer_cache -= steer_increment + elif keys[K_RIGHT] or keys[K_d]: + if self._steer_cache < 0: + self._steer_cache = 0 + else: + self._steer_cache += steer_increment + else: + self._steer_cache = 0.0 + self._steer_cache = min(0.7, max(-0.7, self._steer_cache)) + self._control.steer = round(self._steer_cache, 1) + self._control.brake = 1.0 if keys[K_DOWN] or keys[K_s] else 0.0 + self._control.hand_brake = keys[K_SPACE] + + def _parse_walker_keys(self, keys, milliseconds): + self._control.speed = 0.0 + if keys[K_DOWN] or keys[K_s]: + self._control.speed = 0.0 + if keys[K_LEFT] or keys[K_a]: + self._control.speed = .01 + self._rotation.yaw -= 0.08 * milliseconds + if keys[K_RIGHT] or keys[K_d]: + self._control.speed = .01 + self._rotation.yaw += 0.08 * milliseconds + if keys[K_UP] or keys[K_w]: + self._control.speed = 3.333 if pygame.key.get_mods() & KMOD_SHIFT else 2.778 + self._control.jump = keys[K_SPACE] + self._rotation.yaw = round(self._rotation.yaw, 1) + self._control.direction = self._rotation.get_forward_vector() + + @staticmethod + def _is_quit_shortcut(key): + return (key == K_ESCAPE) or (key == K_q and pygame.key.get_mods() & KMOD_CTRL) + + +# ============================================================================== +# -- HUD ----------------------------------------------------------------------- +# ============================================================================== + + +class HUD(object): + def __init__(self, width, height): + self.dim = (width, height) + font = pygame.font.Font(pygame.font.get_default_font(), 20) + fonts = [x for x in pygame.font.get_fonts() if 'mono' in x] + default_font = 'ubuntumono' + mono = default_font if default_font in fonts else fonts[0] + mono = pygame.font.match_font(mono) + self._font_mono = pygame.font.Font(mono, 14) + self._notifications = FadingText(font, (width, 40), (0, height - 40)) + self.help = HelpText(pygame.font.Font(mono, 24), width, height) + self.server_fps = 0 + self.frame = 0 + self.simulation_time = 0 + self.original_vehicle_control = None + self.restricted_vehicle_control = None + self._show_info = True + self._info_text = [] + self._server_clock = pygame.time.Clock() + + def on_world_tick(self, timestamp): + self._server_clock.tick() + self.server_fps = self._server_clock.get_fps() + self.frame = timestamp.frame + self.simulation_time = timestamp.elapsed_seconds + + def tick(self, world, clock): + self._notifications.tick(world, clock) + if not self._show_info: + return + t = world.player.get_transform() + v = world.player.get_velocity() + c = world.player.get_control() + heading = 'N' if abs(t.rotation.yaw) < 89.5 else '' + heading += 'S' if abs(t.rotation.yaw) > 90.5 else '' + heading += 'E' if 179.5 > t.rotation.yaw > 0.5 else '' + heading += 'W' if -0.5 > t.rotation.yaw > -179.5 else '' + colhist = world.collision_sensor.get_collision_history() + collision = [colhist[x + self.frame - 200] for x in range(0, 200)] + max_col = max(1.0, max(collision)) + collision = [x / max_col for x in collision] + vehicles = world.world.get_actors().filter('vehicle.*') + self._info_text = [ + 'Server: % 16.0f FPS' % self.server_fps, + 'Client: % 16.0f FPS' % clock.get_fps(), + '', + 'Vehicle: % 20s' % get_actor_display_name(world.player, truncate=20), + 'Map: % 20s' % world.map.name, + 'Simulation time: % 12s' % datetime.timedelta(seconds=int(self.simulation_time)), + '', + 'Speed: % 15.0f km/h' % (3.6 * math.sqrt(v.x**2 + v.y**2 + v.z**2)), + u'Heading:% 16.0f\N{DEGREE SIGN} % 2s' % (t.rotation.yaw, heading), + 'Location:% 20s' % ('(% 5.1f, % 5.1f)' % (t.location.x, t.location.y)), + 'GNSS:% 24s' % ('(% 2.6f, % 3.6f)' % (world.gnss_sensor.lat, world.gnss_sensor.lon)), + 'Height: % 18.0f m' % t.location.z, + ''] + if isinstance(c, carla.VehicleControl): + if isinstance(self.original_vehicle_control, carla.VehicleControl): + orig_control = self.original_vehicle_control + restricted_control = self.restricted_vehicle_control + self._info_text += [ + ('Throttle:', orig_control.throttle, 0.0, 1.0, restricted_control.throttle), + ('Steer:', orig_control.steer, -1.0, 1.0, restricted_control.steer), + ('Brake:', orig_control.brake, 0.0, 1.0, restricted_control.brake)] + else: + self._info_text += [ + ('Throttle:', c.throttle, 0.0, 1.0), + ('Steer:', c.steer, -1.0, 1.0), + ('Brake:', c.brake, 0.0, 1.0)] + self._info_text += [ + ('Reverse:', c.reverse), + ('Hand brake:', c.hand_brake), + ('Manual:', c.manual_gear_shift), + 'Gear: %s' % {-1: 'R', 0: 'N'}.get(c.gear, c.gear)] + elif isinstance(c, carla.WalkerControl): + self._info_text += [ + ('Speed:', c.speed, 0.0, 5.556), + ('Jump:', c.jump)] + self._info_text += [ + '', + 'Collision:', + collision, + '', + 'Number of vehicles: % 8d' % len(vehicles)] + if len(vehicles) > 1: + self._info_text += ['Nearby vehicles:'] + distance = lambda l: math.sqrt((l.x - t.location.x)**2 + (l.y - t.location.y)**2 + (l.z - t.location.z)**2) + vehicles = [(distance(x.get_location()), x) for x in vehicles if x.id != world.player.id] + for d, vehicle in sorted(vehicles): + if d > 200.0: + break + vehicle_type = get_actor_display_name(vehicle, truncate=22) + self._info_text.append('% 4dm %s' % (d, vehicle_type)) + + def toggle_info(self): + self._show_info = not self._show_info + + def notification(self, text, seconds=2.0): + self._notifications.set_text(text, seconds=seconds) + + def error(self, text): + self._notifications.set_text('Error: %s' % text, (255, 0, 0)) + + def render(self, display): + if self._show_info: + info_surface = pygame.Surface((220, self.dim[1])) + info_surface.set_alpha(100) + display.blit(info_surface, (0, 0)) + v_offset = 4 + bar_h_offset = 100 + bar_width = 106 + for item in self._info_text: + text_color = (255, 255, 255) + if v_offset + 18 > self.dim[1]: + break + if isinstance(item, list): + if len(item) > 1: + points = [(x + 8, v_offset + 8 + (1.0 - y) * 30) for x, y in enumerate(item)] + pygame.draw.lines(display, (255, 136, 0), False, points, 2) + item = None + v_offset += 18 + elif isinstance(item, tuple): + if isinstance(item[1], bool): + rect = pygame.Rect((bar_h_offset, v_offset + 8), (6, 6)) + pygame.draw.rect(display, (255, 255, 255), rect, 0 if item[1] else 1) + else: + rect_border = pygame.Rect((bar_h_offset, v_offset + 8), (bar_width, 6)) + pygame.draw.rect(display, (255, 255, 255), rect_border, 1) + f = (item[1] - item[2]) / (item[3] - item[2]) + if item[2] < 0.0: + rect = pygame.Rect((bar_h_offset + f * (bar_width - 6), v_offset + 8), (6, 6)) + else: + rect = pygame.Rect((bar_h_offset, v_offset + 8), (f * bar_width, 6)) + pygame.draw.rect(display, (255, 255, 255), rect) + if len(item) == 5: + if item[1] != item[4]: + pygame.draw.rect(display, (255, 0, 0), rect_border, 1) + f = (item[4] - item[2]) / (item[3] - item[2]) + if item[2] < 0.0: + rect = pygame.Rect((bar_h_offset + f * (bar_width - 6), v_offset + 8), (6, 6)) + else: + rect = pygame.Rect((bar_h_offset, v_offset + 8), (f * bar_width, 6)) + pygame.draw.rect(display, (255, 0, 0), rect) + text_color = (255, 0, 0) + item = item[0] + if item: # At this point has to be a str. + surface = self._font_mono.render(item, True, text_color) + display.blit(surface, (8, v_offset)) + v_offset += 18 + self._notifications.render(display) + self.help.render(display) + + +# ============================================================================== +# -- FadingText ---------------------------------------------------------------- +# ============================================================================== + + +class FadingText(object): + def __init__(self, font, dim, pos): + self.font = font + self.dim = dim + self.pos = pos + self.seconds_left = 0 + self.surface = pygame.Surface(self.dim) + + def set_text(self, text, color=(255, 255, 255), seconds=2.0): + text_texture = self.font.render(text, True, color) + self.surface = pygame.Surface(self.dim) + self.seconds_left = seconds + self.surface.fill((0, 0, 0, 0)) + self.surface.blit(text_texture, (10, 11)) + + def tick(self, _, clock): + delta_seconds = 1e-3 * clock.get_time() + self.seconds_left = max(0.0, self.seconds_left - delta_seconds) + self.surface.set_alpha(500.0 * self.seconds_left) + + def render(self, display): + display.blit(self.surface, self.pos) + + +# ============================================================================== +# -- HelpText ------------------------------------------------------------------ +# ============================================================================== + + +class HelpText(object): + def __init__(self, font, width, height): + lines = __doc__.split('\n') + self.font = font + self.dim = (680, len(lines) * 22 + 12) + self.pos = (0.5 * width - 0.5 * self.dim[0], 0.5 * height - 0.5 * self.dim[1]) + self.seconds_left = 0 + self.surface = pygame.Surface(self.dim) + self.surface.fill((0, 0, 0, 0)) + for n, line in enumerate(lines): + text_texture = self.font.render(line, True, (255, 255, 255)) + self.surface.blit(text_texture, (22, n * 22)) + self._render = False + self.surface.set_alpha(220) + + def toggle(self): + self._render = not self._render + + def render(self, display): + if self._render: + display.blit(self.surface, self.pos) + + +# ============================================================================== +# -- CollisionSensor ----------------------------------------------------------- +# ============================================================================== + + +class CollisionSensor(object): + def __init__(self, parent_actor, hud): + self.sensor = None + self.history = [] + self._parent = parent_actor + self.hud = hud + world = self._parent.get_world() + bp = world.get_blueprint_library().find('sensor.other.collision') + self.sensor = world.spawn_actor(bp, carla.Transform(), attach_to=self._parent) + # We need to pass the lambda a weak reference to self to avoid circular + # reference. + weak_self = weakref.ref(self) + self.sensor.listen(lambda event: CollisionSensor._on_collision(weak_self, event)) + + def get_collision_history(self): + history = collections.defaultdict(int) + for frame, intensity in self.history: + history[frame] += intensity + return history + + @staticmethod + def _on_collision(weak_self, event): + self = weak_self() + if not self: + return + actor_type = get_actor_display_name(event.other_actor) + self.hud.notification('Collision with %r' % actor_type) + impulse = event.normal_impulse + intensity = math.sqrt(impulse.x**2 + impulse.y**2 + impulse.z**2) + self.history.append((event.frame, intensity)) + if len(self.history) > 4000: + self.history.pop(0) + + +# ============================================================================== +# -- LaneInvasionSensor -------------------------------------------------------- +# ============================================================================== + + +class LaneInvasionSensor(object): + def __init__(self, parent_actor, hud): + self.sensor = None + self._parent = parent_actor + self.hud = hud + world = self._parent.get_world() + bp = world.get_blueprint_library().find('sensor.other.lane_invasion') + self.sensor = world.spawn_actor(bp, carla.Transform(), attach_to=self._parent) + # We need to pass the lambda a weak reference to self to avoid circular + # reference. + weak_self = weakref.ref(self) + self.sensor.listen(lambda event: LaneInvasionSensor._on_invasion(weak_self, event)) + + @staticmethod + def _on_invasion(weak_self, event): + self = weak_self() + if not self: + return + lane_types = set(x.type for x in event.crossed_lane_markings) + text = ['%r' % str(x).split()[-1] for x in lane_types] + self.hud.notification('Crossed line %s' % ' and '.join(text)) + +# ============================================================================== +# -- GnssSensor -------------------------------------------------------- +# ============================================================================== + + +class GnssSensor(object): + def __init__(self, parent_actor): + self.sensor = None + self._parent = parent_actor + self.lat = 0.0 + self.lon = 0.0 + world = self._parent.get_world() + bp = world.get_blueprint_library().find('sensor.other.gnss') + self.sensor = world.spawn_actor(bp, carla.Transform(carla.Location(x=1.0, z=2.8)), attach_to=self._parent) + # We need to pass the lambda a weak reference to self to avoid circular + # reference. + weak_self = weakref.ref(self) + self.sensor.listen(lambda event: GnssSensor._on_gnss_event(weak_self, event)) + + @staticmethod + def _on_gnss_event(weak_self, event): + self = weak_self() + if not self: + return + self.lat = event.latitude + self.lon = event.longitude + +# ============================================================================== +# -- RssSensor -------------------------------------------------------- +# ============================================================================== + + +class RssSensor(object): + def __init__(self, parent_actor): + 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.acceleration_restriction = None + self.ego_velocity = 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) + # We need to pass the lambda a weak reference to self to avoid circular + # reference. + + def check_rss_class(clazz): + return inspect.isclass(clazz) and "RssSensor" in clazz.__name__ + + if not inspect.getmembers(carla, check_rss_class): + raise RuntimeError('CARLA PythonAPI not compiled in RSS variant, please "make PythonAPI.rss"') + weak_self = weakref.ref(self) + self.sensor.visualize_results = True + self.sensor.listen(lambda event: RssSensor._on_rss_response(weak_self, event)) + + @staticmethod + def _on_rss_response(weak_self, response): + 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 + + +# ============================================================================== +# -- CameraManager ------------------------------------------------------------- +# ============================================================================== + + +class CameraManager(object): + def __init__(self, parent_actor, hud): + self.sensor = None + self.surface = None + self._parent = parent_actor + self.hud = hud + self.recording = False + bound_y = 0.5 + self._parent.bounding_box.extent.y + Attachment = carla.AttachmentType + self._camera_transforms = [ + (carla.Transform(carla.Location(x=-5.5, z=2.5), carla.Rotation(pitch=8.0)), Attachment.SpringArm), + (carla.Transform(carla.Location(x=1.6, z=1.7)), Attachment.Rigid), + (carla.Transform(carla.Location(x=5.5, y=1.5, z=1.5)), Attachment.SpringArm), + (carla.Transform(carla.Location(x=-8.0, z=6.0), carla.Rotation(pitch=6.0)), Attachment.SpringArm), + (carla.Transform(carla.Location(x=-1, y=-bound_y, z=0.5)), Attachment.Rigid)] + self.transform_index = 1 + self.sensors = [ + ['sensor.camera.rgb', cc.Raw, 'Camera RGB'], + ['sensor.camera.depth', cc.Raw, 'Camera Depth (Raw)'], + ['sensor.camera.depth', cc.Depth, 'Camera Depth (Gray Scale)'], + ['sensor.camera.depth', cc.LogarithmicDepth, 'Camera Depth (Logarithmic Gray Scale)'], + ['sensor.camera.semantic_segmentation', cc.Raw, 'Camera Semantic Segmentation (Raw)'], + ['sensor.camera.semantic_segmentation', cc.CityScapesPalette, + 'Camera Semantic Segmentation (CityScapes Palette)'], + ['sensor.lidar.ray_cast', None, 'Lidar (Ray-Cast)']] + world = self._parent.get_world() + bp_library = world.get_blueprint_library() + for item in self.sensors: + bp = bp_library.find(item[0]) + if item[0].startswith('sensor.camera'): + bp.set_attribute('image_size_x', str(hud.dim[0])) + bp.set_attribute('image_size_y', str(hud.dim[1])) + elif item[0].startswith('sensor.lidar'): + bp.set_attribute('range', '5000') + item.append(bp) + self.index = None + + def toggle_camera(self): + self.transform_index = (self.transform_index + 1) % len(self._camera_transforms) + self.set_sensor(self.index, notify=False, force_respawn=True) + + def set_sensor(self, index, notify=True, force_respawn=False): + index = index % len(self.sensors) + needs_respawn = True if self.index is None else \ + (force_respawn or (self.sensors[index][0] != self.sensors[self.index][0])) + if needs_respawn: + if self.sensor is not None: + self.sensor.destroy() + self.surface = None + self.sensor = self._parent.get_world().spawn_actor( + self.sensors[index][-1], + self._camera_transforms[self.transform_index][0], + attach_to=self._parent, + attachment_type=self._camera_transforms[self.transform_index][1]) + # We need to pass the lambda a weak reference to self to avoid + # circular reference. + weak_self = weakref.ref(self) + self.sensor.listen(lambda image: CameraManager._parse_image(weak_self, image)) + if notify: + self.hud.notification(self.sensors[index][2]) + self.index = index + + def next_sensor(self): + self.set_sensor(self.index + 1) + + def toggle_recording(self): + self.recording = not self.recording + self.hud.notification('Recording %s' % ('On' if self.recording else 'Off')) + + def render(self, display): + if self.surface is not None: + display.blit(self.surface, (0, 0)) + + @staticmethod + def _parse_image(weak_self, image): + self = weak_self() + if not self: + return + if self.sensors[self.index][0].startswith('sensor.lidar'): + points = np.frombuffer(image.raw_data, dtype=np.dtype('f4')) + points = np.reshape(points, (int(points.shape[0] / 3), 3)) + lidar_data = np.array(points[:, :2]) + lidar_data *= min(self.hud.dim) / 100.0 + lidar_data += (0.5 * self.hud.dim[0], 0.5 * self.hud.dim[1]) + lidar_data = np.fabs(lidar_data) # pylint: disable=E1111 + lidar_data = lidar_data.astype(np.int32) + lidar_data = np.reshape(lidar_data, (-1, 2)) + lidar_img_size = (self.hud.dim[0], self.hud.dim[1], 3) + lidar_img = np.zeros((lidar_img_size), dtype = int) + lidar_img[tuple(lidar_data.T)] = (255, 255, 255) + self.surface = pygame.surfarray.make_surface(lidar_img) + else: + image.convert(self.sensors[self.index][1]) + array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) + array = np.reshape(array, (image.height, image.width, 4)) + array = array[:, :, :3] + array = array[:, :, ::-1] + self.surface = pygame.surfarray.make_surface(array.swapaxes(0, 1)) + if self.recording: + image.save_to_disk('_out/%08d' % image.frame) + + +# ============================================================================== +# -- game_loop() --------------------------------------------------------------- +# ============================================================================== + + +def game_loop(args): + pygame.init() + pygame.font.init() + world = None + + try: + client = carla.Client(args.host, args.port) + client.set_timeout(2.0) + + display = pygame.display.set_mode( + (args.width, args.height), + pygame.HWSURFACE | pygame.DOUBLEBUF) + + hud = HUD(args.width, args.height) + world = World(client.get_world(), hud, args.filter, args.rolename) + controller = KeyboardControl(world, args) + + clock = pygame.time.Clock() + while True: + clock.tick_busy_loop(60) + if controller.parse_events(client, world, clock): + return + world.tick(clock) + world.render(display) + pygame.display.flip() + + finally: + + if (world and world.recording_enabled): + client.stop_recorder() + + if world is not None: + world.destroy() + + pygame.quit() + + +# ============================================================================== +# -- main() -------------------------------------------------------------------- +# ============================================================================== + + +def main(): + argparser = argparse.ArgumentParser( + description='CARLA Manual Control Client RSS') + argparser.add_argument( + '-v', '--verbose', + action='store_true', + dest='debug', + help='print debug information') + argparser.add_argument( + '--host', + metavar='H', + default='127.0.0.1', + help='IP of the host server (default: 127.0.0.1)') + argparser.add_argument( + '-p', '--port', + metavar='P', + default=2000, + type=int, + help='TCP port to listen to (default: 2000)') + argparser.add_argument( + '-a', '--autopilot', + action='store_true', + help='enable autopilot') + argparser.add_argument( + '--res', + metavar='WIDTHxHEIGHT', + default='1280x720', + help='window resolution (default: 1280x720)') + argparser.add_argument( + '--filter', + metavar='PATTERN', + default='vehicle.*', + help='actor filter (default: "vehicle.*")') + argparser.add_argument( + '--rolename', + metavar='NAME', + default='hero', + help='actor role name (default: "hero")') + args = argparser.parse_args() + + args.width, args.height = [int(x) for x in args.res.split('x')] + + log_level = logging.DEBUG if args.debug else logging.INFO + logging.basicConfig(format='%(levelname)s: %(message)s', level=log_level) + + logging.info('listening to server %s:%s', args.host, args.port) + + print(__doc__) + + try: + + game_loop(args) + + except KeyboardInterrupt: + print('\nCancelled by user. Bye!') + + +if __name__ == '__main__': + + main() diff --git a/README.md b/README.md index 2462e0b1b..124b6a3ea 100644 --- a/README.md +++ b/README.md @@ -97,4 +97,6 @@ CARLA specific code is distributed under MIT License. CARLA specific assets are distributed under CC-BY License. +The ad-rss-lib library compiled and linked by the [RSS Integration build variant](Docs/rss_lib_integration.md) introduces LGPL-2.1-only License. + Note that UE4 itself follows its own license terms. diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/RssSensor.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/RssSensor.cpp new file mode 100644 index 000000000..2c5d2856e --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/RssSensor.cpp @@ -0,0 +1,26 @@ +// Copyright (c) 2019 Intel Labs. +// +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#include "Carla.h" +#include "Carla/Sensor/RssSensor.h" +#include "StaticMeshResources.h" + +FActorDefinition ARssSensor::GetSensorDefinition() +{ + return UActorBlueprintFunctionLibrary::MakeGenericSensorDefinition(TEXT("other"), TEXT("rss")); +} + +ARssSensor::ARssSensor(const FObjectInitializer &ObjectInitializer) + : Super(ObjectInitializer) +{ + PrimaryActorTick.bCanEverTick = false; + + auto MeshComp = CreateDefaultSubobject(TEXT("RootComponent")); + MeshComp->SetCollisionProfileName(UCollisionProfile::NoCollision_ProfileName); + MeshComp->bHiddenInGame = true; + MeshComp->CastShadow = false; + MeshComp->PostPhysicsComponentTick.bCanEverTick = false; + RootComponent = MeshComp; +} diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/RssSensor.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/RssSensor.h new file mode 100644 index 000000000..65cdc5281 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/RssSensor.h @@ -0,0 +1,27 @@ +// Copyright (c) 2019 Intel Labs. +// +// This work is licensed under the terms of the MIT license. +// For a copy, see . + +#pragma once + +#include "Carla/Sensor/ShaderBasedSensor.h" + +#include "Carla/Actor/ActorDefinition.h" + +#include "RssSensor.generated.h" + +/// RSS sensor representation +/// The actual calculation is done one client side +UCLASS() +class CARLA_API ARssSensor : public ASensor +{ + GENERATED_BODY() + +public: + + static FActorDefinition GetSensorDefinition(); + + ARssSensor(const FObjectInitializer &ObjectInitializer); + +}; diff --git a/Util/BuildTools/Ad-rss.sh b/Util/BuildTools/Ad-rss.sh new file mode 100755 index 000000000..48c21cdd2 --- /dev/null +++ b/Util/BuildTools/Ad-rss.sh @@ -0,0 +1,56 @@ +#! /bin/bash + +# ============================================================================== +# -- Set up environment -------------------------------------------------------- +# ============================================================================== + +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 ---------------------------------------------------- +# ============================================================================== + +AD_RSS_BASENAME=ad-rss + +if [[ -d "${AD_RSS_BASENAME}-install" ]] ; then + log "${AD_RSS_BASENAME} already installed." +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 + +fi + +unset AD_RSS_BASENAME + +# ============================================================================== +# -- ...and we are done -------------------------------------------------------- +# ============================================================================== + +popd >/dev/null + +log "Success!" diff --git a/Util/BuildTools/BuildLibCarla.sh b/Util/BuildTools/BuildLibCarla.sh index a8060597d..0cbd2cfef 100755 --- a/Util/BuildTools/BuildLibCarla.sh +++ b/Util/BuildTools/BuildLibCarla.sh @@ -34,8 +34,9 @@ BUILD_CLIENT=false BUILD_OPTION_DEBUG=false BUILD_OPTION_RELEASE=false BUILD_OPTION_DUMMY=false +BUILD_RSS_VARIANT=false -OPTS=`getopt -o h --long help,rebuild,server,client,clean,debug,release -n 'parse-options' -- "$@"` +OPTS=`getopt -o h --long help,rebuild,server,client,clean,debug,release,rss -n 'parse-options' -- "$@"` if [ $? != 0 ] ; then echo "$USAGE_STRING" ; exit 2 ; fi @@ -66,6 +67,9 @@ while true; do --release ) BUILD_OPTION_RELEASE=true; shift ;; + --rss ) + BUILD_RSS_VARIANT=true; + shift ;; -h | --help ) echo "$DOC_STRING" echo "$USAGE_STRING" @@ -103,10 +107,12 @@ fi # Build LibCarla for the given configuration. # -# usage: build_libcarla {Server,Client} {Debug,Release} +# usage: build_libcarla {Server,Client,ClientRSS} {Debug,Release} # function build_libcarla { + CMAKE_EXTRA_OPTIONS='' + if [ $1 == Server ] ; then M_TOOLCHAIN=${LIBCPP_TOOLCHAIN_FILE} M_BUILD_FOLDER=${LIBCARLA_BUILD_SERVER_FOLDER}.$(echo "$2" | tr '[:upper:]' '[:lower:]') @@ -115,6 +121,12 @@ function build_libcarla { M_TOOLCHAIN=${LIBSTDCPP_TOOLCHAIN_FILE} M_BUILD_FOLDER=${LIBCARLA_BUILD_CLIENT_FOLDER}.$(echo "$2" | tr '[:upper:]' '[:lower:]') M_INSTALL_FOLDER=${LIBCARLA_INSTALL_CLIENT_FOLDER} + elif [ $1 == ClientRSS ] ; then + BUILD_TYPE='Client' + 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" else fatal_error "Invalid build configuration \"$1\"" fi @@ -136,14 +148,16 @@ function build_libcarla { if [ ! -f "build.ninja" ]; then +set -e -x cmake \ - -G "Ninja" \ - -DCMAKE_BUILD_TYPE=$1 \ + -G "Eclipse CDT4 - Ninja" \ + -DCMAKE_BUILD_TYPE=${BUILD_TYPE:-$1} \ -DLIBCARLA_BUILD_DEBUG=${M_DEBUG} \ -DLIBCARLA_BUILD_RELEASE=${M_RELEASE} \ -DCMAKE_TOOLCHAIN_FILE=${M_TOOLCHAIN} \ -DCMAKE_INSTALL_PREFIX=${M_INSTALL_FOLDER} \ -DCMAKE_EXPORT_COMPILE_COMMANDS=1 \ + ${CMAKE_EXTRA_OPTIONS} \ ${CARLA_ROOT_FOLDER} fi @@ -171,15 +185,20 @@ if { ${BUILD_SERVER} && ${BUILD_OPTION_RELEASE}; }; then fi +CLIENT_VARIANT='Client' +if [ $BUILD_RSS_VARIANT == true ] ; then + CLIENT_VARIANT='ClientRSS' +fi + if { ${BUILD_CLIENT} && ${BUILD_OPTION_DEBUG}; }; then - build_libcarla Client Debug + build_libcarla ${CLIENT_VARIANT} Debug fi if { ${BUILD_CLIENT} && ${BUILD_OPTION_RELEASE}; }; then - build_libcarla Client Release + build_libcarla ${CLIENT_VARIANT} Release fi diff --git a/Util/BuildTools/BuildPythonAPI.sh b/Util/BuildTools/BuildPythonAPI.sh index 551009897..6f74e43d1 100755 --- a/Util/BuildTools/BuildPythonAPI.sh +++ b/Util/BuildTools/BuildPythonAPI.sh @@ -16,8 +16,9 @@ USAGE_STRING="Usage: $0 [-h|--help] [--rebuild] [--py2] [--py3] [--clean]" REMOVE_INTERMEDIATE=false BUILD_FOR_PYTHON2=false BUILD_FOR_PYTHON3=false +BUILD_RSS_VARIANT=false -OPTS=`getopt -o h --long help,rebuild,py2,py3,clean -n 'parse-options' -- "$@"` +OPTS=`getopt -o h --long help,rebuild,py2,py3,clean,rss -n 'parse-options' -- "$@"` if [ $? != 0 ] ; then echo "$USAGE_STRING" ; exit 2 ; fi @@ -36,6 +37,9 @@ while true; do --py3 ) BUILD_FOR_PYTHON3=true; shift ;; + --rss ) + BUILD_RSS_VARIANT=true; + shift ;; --clean ) REMOVE_INTERMEDIATE=true; shift ;; @@ -74,6 +78,10 @@ fi # -- Build API ----------------------------------------------------------------- # ============================================================================== +if ${BUILD_RSS_VARIANT} ; then + export BUILD_RSS_VARIANT=${BUILD_RSS_VARIANT} +fi + if ${BUILD_FOR_PYTHON2} ; then log "Building Python API for Python 2." diff --git a/Util/BuildTools/Linux.mk b/Util/BuildTools/Linux.mk index e17fd5810..9dff05a15 100644 --- a/Util/BuildTools/Linux.mk +++ b/Util/BuildTools/Linux.mk @@ -91,6 +91,15 @@ PythonAPI.2: LibCarla.client.release PythonAPI.3: LibCarla.client.release @${CARLA_BUILD_TOOLS_FOLDER}/BuildPythonAPI.sh --py3 +PythonAPI.rss: LibCarla.client.rss.release + @${CARLA_BUILD_TOOLS_FOLDER}/BuildPythonAPI.sh --py2 --py3 --rss + +PythonAPI.rss.2: LibCarla.client.rss.release + @${CARLA_BUILD_TOOLS_FOLDER}/BuildPythonAPI.sh --py2 --rss + +PythonAPI.rss.3: LibCarla.client.rss.release + @${CARLA_BUILD_TOOLS_FOLDER}/BuildPythonAPI.sh --py3 --rss + PythonAPI.docs: @python PythonAPI/docs/doc_gen.py @cd PythonAPI/docs && python3 bp_doc_gen.py @@ -113,9 +122,18 @@ LibCarla.client.debug: setup LibCarla.client.release: setup @${CARLA_BUILD_TOOLS_FOLDER}/BuildLibCarla.sh --client --release +LibCarla.client.rss: LibCarla.client.rss.debug LibCarla.client.rss.release +LibCarla.client.rss.debug: setup ad-rss + @${CARLA_BUILD_TOOLS_FOLDER}/BuildLibCarla.sh --client --debug --rss +LibCarla.client.rss.release: setup ad-rss + @${CARLA_BUILD_TOOLS_FOLDER}/BuildLibCarla.sh --client --release --rss + setup: @${CARLA_BUILD_TOOLS_FOLDER}/Setup.sh +ad-rss: + @${CARLA_BUILD_TOOLS_FOLDER}/Ad-rss.sh + deploy: @${CARLA_BUILD_TOOLS_FOLDER}/Deploy.sh $(ARGS)