Add Responsibility Sensitive Safety (RSS) to CARLA Client library
- Add build variant with RSS sensor - Add RSS result visualization - Add RSS Restrictor implementation - Add RSS manual control example
This commit is contained in:
Johannes Quast
Marc Garcia Puig
parent
3e15ed3a2f
commit
fe83f1ab3b
|
|
@ -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
|
||||
|
|
|
|||
Binary file not shown.
|
After Width: | Height: | Size: 878 KiB |
|
|
@ -0,0 +1,42 @@
|
|||
<h1>AD Responsibility Sensitive Safety model (RSS) integration</h1>
|
||||
|
||||
> _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.
|
||||
[](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.
|
||||
|
||||
|
||||
<h2>Compilation</h2>
|
||||
|
||||
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
|
||||
```
|
||||
|
||||
|
||||
<h2>Current state</h2>
|
||||
<h3>RssSensor</h3>
|
||||
The RssSensor is currently only considering vehicles within the same road segment, but on all lanes within that segment. Intersections are not yet supported!
|
||||
|
||||
<h3>RssRestrictor</h3>
|
||||
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.
|
||||
|
|
@ -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.
|
||||
|
|
|
|||
|
|
@ -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<LaneInvasionSensor>(std::move(init), gc);
|
||||
#ifdef RSS_ENABLED
|
||||
} else if (description.description.id == "sensor.other.rss") {
|
||||
return MakeActorImpl<RssSensor>(std::move(init), gc);
|
||||
#endif
|
||||
} else if (description.HasAStream()) {
|
||||
return MakeActorImpl<ServerSideSensor>(std::move(init), gc);
|
||||
} else if (StringUtil::StartsWith(description.description.id, "vehicle.")) {
|
||||
|
|
|
|||
|
|
@ -0,0 +1,465 @@
|
|||
// Copyright (c) 2019 Intel Corporation
|
||||
//
|
||||
// This work is licensed under the terms of the MIT license.
|
||||
// For a copy, see <https://opensource.org/licenses/MIT>.
|
||||
|
||||
#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 <algorithm>
|
||||
#include <cmath>
|
||||
#include <vector>
|
||||
|
||||
namespace carla {
|
||||
namespace rss {
|
||||
|
||||
namespace csd = carla::sensor::data;
|
||||
|
||||
// constants for deg-> rad conversion PI / 180
|
||||
constexpr float toRadians = static_cast<float>(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<cc::ActorList> const & vehicles,
|
||||
carla::SharedPtr<cc::Actor> const & carlaEgoActor,
|
||||
carla::SharedPtr<cc::Map> 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<cc::Vehicle>(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<uint32_t>(lane.GetType()) &
|
||||
static_cast<uint32_t>(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<carla::road::element::RoadInfoLaneWidth>(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<cc::Vehicle>(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<cg::Location,
|
||||
4u> 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<int16_t>(lane.GetId());
|
||||
sectionAndLane |= (0xffff & (laneId + 256));
|
||||
laneSegmentId |= sectionAndLane;
|
||||
return laneSegmentId;
|
||||
}
|
||||
|
||||
void RssCheck::calculateOccupiedRegions(const std::array<cg::Location, 4u> &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<carla::rpc::ActorId>(state.objectId);
|
||||
carla::SharedPtr<cc::ActorList> vehicleList =
|
||||
world.GetActors(std::vector<carla::rpc::ActorId>{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<cc::Vehicle>(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
|
||||
|
|
@ -0,0 +1,97 @@
|
|||
// Copyright (c) 2019 Intel Corporation
|
||||
//
|
||||
// This work is licensed under the terms of the MIT license.
|
||||
// For a copy, see <https://opensource.org/licenses/MIT>.
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <memory>
|
||||
#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<cc::ActorList> const & vehicles,
|
||||
carla::SharedPtr<cc::Actor> const & carlaEgoVehicle,
|
||||
carla::SharedPtr<cc::Map> 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<cg::Location, 4u> &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<cg::Location, 4u> 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
|
||||
|
|
@ -0,0 +1,75 @@
|
|||
// Copyright (c) 2019 Intel Corporation
|
||||
//
|
||||
// This work is licensed under the terms of the MIT license.
|
||||
// For a copy, see <https://opensource.org/licenses/MIT>.
|
||||
|
||||
#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<double>(restriction.longitudinalRange.minimum)) *
|
||||
radius / 100.0;
|
||||
restrictedVehicleControl.brake =
|
||||
std::min(static_cast<float>(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<float>(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<float>(angle);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
return restrictedVehicleControl;
|
||||
}
|
||||
|
||||
} // namespace rss
|
||||
} // namespace carla
|
||||
|
|
@ -0,0 +1,42 @@
|
|||
// Copyright (c) 2019 Intel Corporation
|
||||
//
|
||||
// This work is licensed under the terms of the MIT license.
|
||||
// For a copy, see <https://opensource.org/licenses/MIT>.
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <memory>
|
||||
|
||||
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
|
||||
|
|
@ -0,0 +1,146 @@
|
|||
// Copyright (c) 2019 Intel Corporation
|
||||
//
|
||||
// This work is licensed under the terms of the MIT license.
|
||||
// For a copy, see <https://opensource.org/licenses/MIT>.
|
||||
|
||||
#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 <ad_rss/state/ProperResponse.hpp>
|
||||
#include <ad_rss/world/AccelerationRestriction.hpp>
|
||||
#include <ad_rss/world/Velocity.hpp>
|
||||
|
||||
#include <exception>
|
||||
|
||||
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<RssSensor>(shared_from_this());
|
||||
|
||||
log_debug(GetDisplayId(), ": subscribing to tick event");
|
||||
GetEpisode().Lock()->RegisterOnTickEvent(
|
||||
[cb = std::move(callback), weak_self = WeakPtr<RssSensor>(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<sensor::SensorData> 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<cc::ActorList> actors = world.GetActors();
|
||||
SharedPtr<cc::ActorList> 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<sensor::data::RssResponse>(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
|
||||
|
|
@ -0,0 +1,68 @@
|
|||
// Copyright (c) 2019 Intel Corporation
|
||||
//
|
||||
// This work is licensed under the terms of the MIT license.
|
||||
// For a copy, see <https://opensource.org/licenses/MIT>.
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <mutex>
|
||||
#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<sensor::SensorData> TickRssSensor(const Timestamp ×tamp);
|
||||
|
||||
std::mutex _processing_lock;
|
||||
|
||||
bool _is_listening = false;
|
||||
|
||||
SharedPtr<Map> _map;
|
||||
|
||||
std::shared_ptr<carla::rss::RssCheck> mRssCheck;
|
||||
};
|
||||
|
||||
} // namespace client
|
||||
} // namespace carla
|
||||
|
|
@ -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<ACollisionSensor *, s11n::CollisionEventSerializer>,
|
||||
std::pair<AGnssSensor *, s11n::GnssSerializer>,
|
||||
std::pair<ALaneInvasionSensor *, s11n::NoopSerializer>,
|
||||
std::pair<ARssSensor *, s11n::NoopSerializer>,
|
||||
std::pair<AObstacleDetectionSensor *, s11n::ObstacleDetectionEventSerializer>
|
||||
>;
|
||||
|
||||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -0,0 +1,105 @@
|
|||
// Copyright (c) 2019 Intel Corporation
|
||||
//
|
||||
// This work is licensed under the terms of the MIT license.
|
||||
// For a copy, see <https://opensource.org/licenses/MIT>.
|
||||
|
||||
#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
|
||||
|
|
@ -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']
|
||||
|
|
|
|||
|
|
@ -0,0 +1,230 @@
|
|||
// Copyright (c) 2019 Intel Corporation
|
||||
//
|
||||
// This work is licensed under the terms of the MIT license.
|
||||
// For a copy, see <https://opensource.org/licenses/MIT>.
|
||||
|
||||
#include <carla/rss/RssSensor.h>
|
||||
#include <carla/rss/RssRestrictor.h>
|
||||
#include <carla/sensor/data/RssResponse.h>
|
||||
|
||||
#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<double>(accel))
|
||||
<< ")";
|
||||
return out;
|
||||
}
|
||||
|
||||
std::ostream &operator<<(std::ostream &out, const Distance &d) {
|
||||
out << "Distance(value=" << std::to_string(static_cast<double>(d))
|
||||
<< ")";
|
||||
return out;
|
||||
}
|
||||
|
||||
std::ostream &operator<<(std::ostream &out, const Duration &d) {
|
||||
out << "Duration(value=" << std::to_string(static_cast<double>(d))
|
||||
<< ")";
|
||||
return out;
|
||||
}
|
||||
|
||||
std::ostream &operator<<(std::ostream &out, const Speed &speed) {
|
||||
out << "Speed(value=" << std::to_string(static_cast<double>(speed))
|
||||
<< ")";
|
||||
return out;
|
||||
}
|
||||
|
||||
std::ostream &operator<<(std::ostream &out, const AccelerationRange &range) {
|
||||
out << "AccelerationRestriction(min=" << std::to_string(static_cast<double>(range.minimum))
|
||||
<< ", max= " << std::to_string(static_cast<double>(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<double>(dyn.lateralFluctuationMargin))
|
||||
<< ", response_time=" << std::to_string(static_cast<double>(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_<csd::LateralResponse>("LateralResponse")
|
||||
.value("None", csd::LateralResponse::None)
|
||||
.value("BrakeMin", csd::LateralResponse::BrakeMin)
|
||||
;
|
||||
|
||||
enum_<csd::LongitudinalResponse>("LongitudinalResponse")
|
||||
.value("None", csd::LongitudinalResponse::None)
|
||||
.value("BrakeMinCorrect", csd::LongitudinalResponse::BrakeMinCorrect)
|
||||
.value("BrakeMin", csd::LongitudinalResponse::BrakeMin)
|
||||
;
|
||||
|
||||
class_<ad_rss::physics::Acceleration>("Acceleration", init<double>())
|
||||
.def_readonly("value", &ad_rss::physics::Acceleration::operator double)
|
||||
.def(self_ns::str(self_ns::self))
|
||||
;
|
||||
|
||||
class_<ad_rss::physics::Distance>("Distance", init<double>())
|
||||
.def_readonly("value", &ad_rss::physics::Distance::operator double)
|
||||
.def(self_ns::str(self_ns::self))
|
||||
;
|
||||
|
||||
class_<ad_rss::physics::Duration>("Duration", init<double>())
|
||||
.def_readonly("value", &ad_rss::physics::Duration::operator double)
|
||||
.def(self_ns::str(self_ns::self))
|
||||
;
|
||||
|
||||
class_<ad_rss::physics::AccelerationRange>("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_<ad_rss::world::AccelerationRestriction>("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_<ad_rss::world::LateralRssAccelerationValues>("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_<ad_rss::world::LongitudinalRssAccelerationValues>("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_<ad_rss::world::RssDynamics>("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_<ad_rss::physics::Speed>("Speed", init<double>())
|
||||
.def_readonly("value", &ad_rss::physics::Speed::operator double)
|
||||
.def(self_ns::str(self_ns::self))
|
||||
;
|
||||
|
||||
class_<ad_rss::world::Velocity>("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_<csd::RssResponse, bases<cs::SensorData>, boost::noncopyable, boost::shared_ptr<csd::RssResponse>>("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_<cc::RssSensor, bases<cc::Sensor>, boost::noncopyable, boost::shared_ptr<cc::RssSensor>>
|
||||
("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_<carla::rss::RssRestrictor, boost::noncopyable, boost::shared_ptr<carla::rss::RssRestrictor>>
|
||||
("RssRestrictor", no_init)
|
||||
.def(init<>())
|
||||
.def("restrictVehicleControl", &carla::rss::RssRestrictor::restrictVehicleControl, (arg("restriction"), arg("vehicleControl")))
|
||||
.def(self_ns::str(self_ns::self))
|
||||
;
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -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
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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 <https://opensource.org/licenses/MIT>.
|
||||
|
||||
# Allows controlling a vehicle with a keyboard. For a simpler and more
|
||||
# documented example, please take a look at tutorial.py.
|
||||
|
||||
"""
|
||||
Welcome to CARLA manual control.
|
||||
|
||||
Use ARROWS or WASD keys for control.
|
||||
|
||||
W : throttle
|
||||
S : brake
|
||||
AD : steer
|
||||
Q : toggle reverse
|
||||
Space : hand-brake
|
||||
P : toggle autopilot
|
||||
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()
|
||||
|
|
@ -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.
|
||||
|
|
|
|||
|
|
@ -0,0 +1,26 @@
|
|||
// Copyright (c) 2019 Intel Labs.
|
||||
//
|
||||
// This work is licensed under the terms of the MIT license.
|
||||
// For a copy, see <https://opensource.org/licenses/MIT>.
|
||||
|
||||
#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<UStaticMeshComponent>(TEXT("RootComponent"));
|
||||
MeshComp->SetCollisionProfileName(UCollisionProfile::NoCollision_ProfileName);
|
||||
MeshComp->bHiddenInGame = true;
|
||||
MeshComp->CastShadow = false;
|
||||
MeshComp->PostPhysicsComponentTick.bCanEverTick = false;
|
||||
RootComponent = MeshComp;
|
||||
}
|
||||
|
|
@ -0,0 +1,27 @@
|
|||
// Copyright (c) 2019 Intel Labs.
|
||||
//
|
||||
// This work is licensed under the terms of the MIT license.
|
||||
// For a copy, see <https://opensource.org/licenses/MIT>.
|
||||
|
||||
#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);
|
||||
|
||||
};
|
||||
|
|
@ -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!"
|
||||
|
|
@ -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
|
||||
|
||||
|
|
|
|||
|
|
@ -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."
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue