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

Hybrid mode for Traffic Manager (#2674) 

* Improved braking, collision negotiation.

* Improved braking algorithm for smoother approach
to lead vehicle.

* Implemented smoother path boundary modification
to aid smoother braking.

* Re-worked collision negotiation algorithm.

* Improved collision candidate filtering.

* Added safe-guard in case of vehicle removal
in collision stage.

* Used local variable for heavily referenced object
in localization stage.

* Implemented vector relative velocities
for motion planner's collision consideration.

* Moved collision candidate sorting logic
from collision stage to localization stage.

* Sorting collision candidates using their ids
instead of shared pointers to avoid memory corruption.

* Improved conditions for collision consideration
for greater efficiency.

* Removed fps limit in async mode.

* Hybrid physics mode

* Introduced hybrid physics mode parameter
* Implemented physics independent velocity computation
* Modified localization stage to be physics agnostic

* Fixing velocity compute interval in sync and async mode.
Made motion planner stage work with internally computed velocities.

* Made collision stage agnostic to actor physics

* Sampling waypoint buffer for teleportation window

* WIP: Teleportation changes

* WIP2: Teleportation changes

* Fixes waypoint window and vehicle spawning

* hotfix to performance benchmark

* comment out debugs

* changelog

* fixes collision bug

* fixes package error and out_of_range bug

* changes after review

* Left & Right Transit of a lane: Waypoint mapping

Co-authored-by: Praveen Kumar <pravinblaze@hotmail.com>
Co-authored-by: Soumyadeep <soumyadeep.dhar@kpit.com>
This commit is contained in:
Jacopo Bartiromo 2020-03-30 22:53:29 +02:00 committed by GitHub
parent 4ed014a2fe
commit 151e38ce8d
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
26 changed files with 565 additions and 144 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
CHANGELOG.md
View File

@ -1,5 +1,6 @@
## Latest
* Introduced hybrid mode for Traffic Manager
* Upgraded to Unreal Engine 4.24
* Fixed autonomous agents' incorrect detection of red traffic lights affecting them
* Added walkable pedestrian crosswalks in OpenDRIVE standalone mode

7
LibCarla/source/carla/trafficmanager/BatchControlStage.cpp
View File

@ -38,12 +38,19 @@ void BatchControlStage::Action() {
}
const carla::ActorId actor_id = element.actor->GetId();
// Apply actuation from controller if physics enabled.
if (element.physics_enabled) {
vehicle_control.throttle = element.throttle;
vehicle_control.brake = element.brake;
vehicle_control.steer = element.steer;
commands->at(i) = carla::rpc::Command::ApplyVehicleControl(actor_id, vehicle_control);
}
// Apply target transform for physics-less vehicles.
else {
commands->at(i) = carla::rpc::Command::ApplyTransform(actor_id, element.transform);
}
}
}
void BatchControlStage::DataReceiver() {

98
LibCarla/source/carla/trafficmanager/CollisionStage.cpp
View File

@ -72,14 +72,16 @@ namespace CollisionStageConstants {
const Actor ego_actor = data.actor;
const ActorId ego_actor_id = ego_actor->GetId();
const cg::Location ego_location = ego_actor->GetLocation();
const cg::Vector3D ego_velocity = data.velocity;
const SimpleWaypointPtr& closest_point = data.closest_waypoint;
const SimpleWaypointPtr& junction_look_ahead = data.junction_look_ahead_waypoint;
const std::vector<std::pair<ActorId, Actor>>& collision_candidates = data.overlapping_actors;
using OverlappingActorInfo = std::vector<std::tuple<ActorId, Actor, cg::Vector3D>>;
const OverlappingActorInfo &collision_candidates = data.overlapping_actors;
bool collision_hazard = false;
float available_distance_margin = std::numeric_limits<float>::infinity();
cg::Vector3D other_vehicle_velocity;
cg::Vector3D obstacle_velocity;
const SimpleWaypointPtr safe_point_junction = data.safe_point_after_junction;
try {
@ -88,14 +90,17 @@ namespace CollisionStageConstants {
actor_info != collision_candidates.end() && !collision_hazard;
++actor_info) {
const ActorId other_actor_id = actor_info->first;
const Actor other_actor = actor_info->second;
ActorId other_actor_id;
Actor other_actor;
cg::Vector3D other_velocity;
std::tie(other_actor_id, other_actor, other_velocity) = *actor_info;
if (!other_actor->IsAlive()) continue;
const auto other_actor_type = other_actor->GetTypeId();
const cg::Location other_location = other_actor->GetLocation();
// Collision checks increase with speed
float collision_distance = std::pow(floor(ego_actor->GetVelocity().Length()*3.6f/10.0f),2.0f);
float collision_distance = std::pow(floor(ego_velocity.Length()*3.6f/10.0f),2.0f);
collision_distance = cg::Math::Clamp(collision_distance, MIN_COLLISION_RADIUS, MAX_COLLISION_RADIUS);
// Temporary fix to (0,0,0) bug
@ -109,19 +114,20 @@ namespace CollisionStageConstants {
if (parameters.GetCollisionDetection(ego_actor, other_actor)) {
std::pair<bool, float> negotiation_result = NegotiateCollision(ego_actor, other_actor, ego_location,
other_location, closest_point, junction_look_ahead);
other_location, closest_point, junction_look_ahead,
ego_velocity, other_velocity);
if ((safe_point_junction != nullptr
&& !IsLocationAfterJunctionSafe(ego_actor, other_actor, safe_point_junction, other_location))
&& !IsLocationAfterJunctionSafe(ego_actor, other_actor, safe_point_junction,
other_location, other_velocity))
|| negotiation_result.first)
{
if ((other_actor_type[0] == 'v' && parameters.GetPercentageIgnoreVehicles(ego_actor) <= (rand() % 101)) ||
(other_actor_type[0] == 'w' && parameters.GetPercentageIgnoreWalkers(ego_actor) <= (rand() % 101)))
{
collision_hazard = true;
available_distance_margin = negotiation_result.second;
other_vehicle_velocity = other_actor->GetVelocity();
obstacle_velocity = other_velocity;
}
}
}
@ -136,7 +142,7 @@ namespace CollisionStageConstants {
CollisionToPlannerData &message = current_planner_frame->at(i);
message.hazard = collision_hazard;
message.distance_to_other_vehicle = available_distance_margin;
message.other_vehicle_velocity = other_vehicle_velocity;
message.other_vehicle_velocity = obstacle_velocity;
}
}
@ -185,12 +191,19 @@ namespace CollisionStageConstants {
const cg::Location &reference_location,
const cg::Location &other_location,
const SimpleWaypointPtr &closest_point,
const SimpleWaypointPtr &junction_look_ahead) {
const SimpleWaypointPtr &junction_look_ahead,
const cg::Vector3D reference_velocity,
const cg::Vector3D other_velocity) {
// Vehicle IDs
const ActorId reference_vehicle_id = reference_vehicle->GetId();
const ActorId other_vehicle_id = other_vehicle->GetId();
// Output variables for the method.
bool hazard = false;
float available_distance_margin = std::numeric_limits<float>::infinity();
// Ego vehicle heading.
// Ego and other vehicle heading.
const cg::Vector3D reference_heading = reference_vehicle->GetTransform().GetForwardVector();
// Vector from ego position to position of the other vehicle.
cg::Vector3D reference_to_other = other_location - reference_location;
@ -209,8 +222,9 @@ namespace CollisionStageConstants {
float other_vehicle_length = other_vehicle_ptr->GetBoundingBox().extent.x * SQUARE_ROOT_OF_TWO;
float inter_vehicle_distance = cg::Math::DistanceSquared(reference_location, other_location);
float ego_bounding_box_extension = GetBoundingBoxExtention(reference_vehicle);
float other_bounding_box_extension = GetBoundingBoxExtention(other_vehicle);
float ego_bounding_box_extension = GetBoundingBoxExtention(reference_vehicle_id,
reference_velocity, reference_heading);
float other_bounding_box_extension = GetBoundingBoxExtention(other_vehicle_id, other_velocity, other_heading);
// Calculate minimum distance between vehicle to consider collision negotiation.
float inter_vehicle_length = reference_vehicle_length + other_vehicle_length;
float ego_detection_range = std::pow(ego_bounding_box_extension + inter_vehicle_length, 2.0f);
@ -226,14 +240,14 @@ namespace CollisionStageConstants {
bool ego_stopped_by_light = reference_vehicle_ptr->GetTrafficLightState() != carla::rpc::TrafficLightState::Green;
bool ego_at_junction_entrance = !closest_point->CheckJunction() && junction_look_ahead->CheckJunction();
const ActorId reference_vehicle_id = reference_vehicle->GetId();
// Conditions to consider collision negotiation.
if (!(ego_at_junction_entrance && ego_at_traffic_light && ego_stopped_by_light)
&& ((ego_inside_junction && other_vehicles_in_cross_detection_range)
|| (!ego_inside_junction && other_vehicle_in_front && other_vehicle_in_ego_range))) {
GeometryComparisonCache cache = GetGeometryBetweenActors(reference_vehicle, other_vehicle,
reference_location, other_location);
reference_location, other_location,
reference_velocity, other_velocity);
// Conditions for collision negotiation.
bool geodesic_path_bbox_touching = cache.inter_geodesic_distance < 0.1;
@ -256,7 +270,6 @@ namespace CollisionStageConstants {
available_distance_margin = static_cast<float>(MAX(cache.reference_vehicle_to_other_geodesic
- specific_distance_margin, 0.0));
ActorId other_vehicle_id = other_vehicle->GetId();
///////////////////////////////////// Collision locking mechanism /////////////////////////////////
// The idea is, when encountering a possible collision,
// we should ensure that the bounding box extension doesn't decrease too fast and loose collision tracking.
@ -306,23 +319,27 @@ namespace CollisionStageConstants {
return boundary_polygon;
}
LocationList CollisionStage::GetGeodesicBoundary(const Actor &actor, const cg::Location &vehicle_location) {
if (geodesic_boundaries.find(actor->GetId()) != geodesic_boundaries.end()) {
return geodesic_boundaries.at(actor->GetId());
LocationList CollisionStage::GetGeodesicBoundary(const Actor &actor, const cg::Location &vehicle_location,
const cg::Vector3D velocity) {
const ActorId actor_id = actor->GetId();
if (geodesic_boundaries.find(actor_id) != geodesic_boundaries.end()) {
return geodesic_boundaries.at(actor_id);
}
const LocationList bbox = GetBoundary(actor, vehicle_location);
const LocationList bbox = GetBoundary(actor, vehicle_location, velocity);
if (vehicle_id_to_index.find(actor->GetId()) != vehicle_id_to_index.end()) {
if (vehicle_id_to_index.find(actor_id) != vehicle_id_to_index.end()) {
float bbox_extension = GetBoundingBoxExtention(actor);
float bbox_extension = GetBoundingBoxExtention(actor_id, velocity, actor->GetTransform().GetForwardVector());
const float specific_distance_margin = parameters.GetDistanceToLeadingVehicle(actor);
if (specific_distance_margin > 0.0f) {
bbox_extension = MAX(specific_distance_margin, bbox_extension);
}
const auto &waypoint_buffer = localization_frame->at(vehicle_id_to_index.at(actor->GetId())).buffer;
const auto &waypoint_buffer = localization_frame->at(vehicle_id_to_index.at(actor_id)).buffer;
LocationList left_boundary;
LocationList right_boundary;
@ -393,17 +410,17 @@ namespace CollisionStageConstants {
}
float CollisionStage::GetBoundingBoxExtention(const Actor &actor) {
float CollisionStage::GetBoundingBoxExtention(const ActorId actor_id,
const cg::Vector3D velocity_vector,
const cg::Vector3D heading_vector) {
// Calculate velocity along vehicle's heading. This also avoids calculating square root.
const float velocity = cg::Math::Dot(actor->GetVelocity(), actor->GetTransform().GetForwardVector());
const float velocity = cg::Math::Dot(velocity_vector, heading_vector);
float bbox_extension;
// Using a linear function to calculate boundary length.
// min speed : 0kmph -> BOUNDARY_EXTENSION_MINIMUM
// max speed : 100kmph -> BOUNDARY_EXTENSION_MAXIMUM
float slope = (BOUNDARY_EXTENSION_MAXIMUM - BOUNDARY_EXTENSION_MINIMUM) / ARBITRARY_MAX_SPEED;
bbox_extension = slope * velocity + BOUNDARY_EXTENSION_MINIMUM;
const ActorId actor_id = actor->GetId();
// If a valid collision lock present, change boundary length to maintain lock.
if (collision_locks.find(actor_id) != collision_locks.end())
{
@ -421,7 +438,9 @@ namespace CollisionStageConstants {
return bbox_extension;
}
LocationList CollisionStage::GetBoundary(const Actor &actor, const cg::Location &location) {
LocationList CollisionStage::GetBoundary(const Actor &actor,
const cg::Location &location,
const cg::Vector3D velocity) {
const auto actor_type = actor->GetTypeId();
cg::Vector3D heading_vector = actor->GetTransform().GetForwardVector();
@ -437,7 +456,7 @@ namespace CollisionStageConstants {
const auto walker = boost::static_pointer_cast<cc::Walker>(actor);
bbox = walker->GetBoundingBox();
// Extend the pedestrians bbox to "predict" where they'll be and avoid collisions.
forward_extension = walker->GetVelocity().Length() * WALKER_TIME_EXTENSION;
forward_extension = velocity.Length() * WALKER_TIME_EXTENSION;
}
const cg::Vector3D extent = bbox.extent;
@ -461,11 +480,12 @@ namespace CollisionStageConstants {
bool CollisionStage::IsLocationAfterJunctionSafe(const Actor &ego_actor,
const Actor &other_actor,
const SimpleWaypointPtr safe_point,
const cg::Location &other_location) {
const cg::Location &other_location,
const cg::Vector3D other_velocity){
bool safe_junction = true;
if (other_actor->GetVelocity().Length() < EPSILON_VELOCITY){
if (other_velocity.Length() < EPSILON_VELOCITY){
cg::Location safe_location = safe_point->GetLocation();
cg::Vector3D heading_vector = safe_point->GetForwardVector();
@ -490,7 +510,7 @@ namespace CollisionStageConstants {
};
const Polygon reference_polygon = GetPolygon(ego_actor_boundary);
const Polygon other_polygon = GetPolygon(GetBoundary(other_actor, other_location));
const Polygon other_polygon = GetPolygon(GetBoundary(other_actor, other_location, other_velocity));
const auto inter_bbox_distance = bg::distance(reference_polygon, other_polygon);
if (inter_bbox_distance < INTER_BBOX_DISTANCE_THRESHOLD){
@ -511,7 +531,9 @@ namespace CollisionStageConstants {
}
GeometryComparisonCache CollisionStage:: GetGeometryBetweenActors(const Actor &reference_vehicle, const Actor &other_vehicle,
const cg::Location &reference_location, const cg::Location &other_location) {
const cg::Location &reference_location, const cg::Location &other_location,
const cg::Vector3D reference_velocity, const cg::Vector3D other_velocity) {
std::string actor_id_key = (reference_vehicle->GetId() < other_vehicle->GetId()) ?
std::to_string(reference_vehicle->GetId()) + "|" + std::to_string(other_vehicle->GetId())
: std::to_string(other_vehicle->GetId()) +"|"+ std::to_string(reference_vehicle->GetId());
@ -525,10 +547,10 @@ GeometryComparisonCache CollisionStage:: GetGeometryBetweenActors(const Actor &r
return mCache;
}
const Polygon reference_geodesic_polygon = GetPolygon(GetGeodesicBoundary(reference_vehicle, reference_location));
const Polygon other_geodesic_polygon = GetPolygon(GetGeodesicBoundary(other_vehicle, other_location));
const Polygon reference_polygon = GetPolygon(GetBoundary(reference_vehicle, reference_location));
const Polygon other_polygon = GetPolygon(GetBoundary(other_vehicle, other_location));
const Polygon reference_geodesic_polygon = GetPolygon(GetGeodesicBoundary(reference_vehicle, reference_location, reference_velocity));
const Polygon other_geodesic_polygon = GetPolygon(GetGeodesicBoundary(other_vehicle, other_location, other_velocity));
const Polygon reference_polygon = GetPolygon(GetBoundary(reference_vehicle, reference_location, reference_velocity));
const Polygon other_polygon = GetPolygon(GetBoundary(other_vehicle, other_location, other_velocity));
const double reference_vehicle_to_other_geodesic = bg::distance(reference_polygon, other_geodesic_polygon);
const double other_vehicle_to_reference_geodesic = bg::distance(other_polygon, reference_geodesic_polygon);

19
LibCarla/source/carla/trafficmanager/CollisionStage.h
View File

@ -11,6 +11,7 @@
#include <deque>
#include <stdlib.h>
#include <string>
#include <tuple>
#include <unordered_map>
#include <vector>
@ -106,11 +107,11 @@ private:
SnippetProfiler snippet_profiler;
/// Returns the bounding box corners of the vehicle passed to the method.
LocationList GetBoundary(const Actor &actor, const cg::Location &location);
LocationList GetBoundary(const Actor &actor, const cg::Location &location, const cg::Vector3D velocity);
/// Returns the extrapolated bounding box of the vehicle along its
/// trajectory.
LocationList GetGeodesicBoundary(const Actor &actor, const cg::Location &location);
LocationList GetGeodesicBoundary(const Actor &actor, const cg::Location &location, const cg::Vector3D velocity);
/// Method to construct a boost polygon object.
Polygon GetPolygon(const LocationList &boundary);
@ -121,21 +122,27 @@ private:
const cg::Location &reference_location,
const cg::Location &other_location,
const SimpleWaypointPtr& closest_point,
const SimpleWaypointPtr& junction_look_ahead);
const SimpleWaypointPtr& junction_look_ahead,
const cg::Vector3D reference_velocity,
const cg::Vector3D other_velocity);
/// Method to calculate the speed dependent bounding box extention for a vehicle.
float GetBoundingBoxExtention(const Actor &ego_vehicle);
float GetBoundingBoxExtention(const ActorId actor_id,
const cg::Vector3D velocity_vector,
const cg::Vector3D heading_vector);
/// At intersections, used to see if there is space after the junction
bool IsLocationAfterJunctionSafe(const Actor &ego_actor, const Actor &overlapped_actor,
const SimpleWaypointPtr safe_point, const cg::Location &other_location);
const SimpleWaypointPtr safe_point, const cg::Location &other_location,
const cg::Vector3D other_velocity);
/// A simple method used to draw bounding boxes around vehicles
void DrawBoundary(const LocationList &boundary);
/// Method to compute Geometry result between two vehicles
GeometryComparisonCache GetGeometryBetweenActors(const Actor &reference_vehicle, const Actor &other_vehicle,
const cg::Location &reference_location, const cg::Location &other_location);
const cg::Location &reference_location, const cg::Location &other_location,
const cg::Vector3D reference_velocity,const cg::Vector3D other_velocity);
public:

90
LibCarla/source/carla/trafficmanager/InMemoryMap.cpp
View File

@ -339,29 +339,13 @@ namespace MapConstants {
const WaypointPtr raw_waypoint = reference_waypoint->GetWaypoint();
const crd::element::LaneMarking::LaneChange lane_change = raw_waypoint->GetLaneChange();
const auto change_right = crd::element::LaneMarking::LaneChange::Right;
const auto change_left = crd::element::LaneMarking::LaneChange::Left;
const auto change_both = crd::element::LaneMarking::LaneChange::Both;
try {
if (lane_change == change_right || lane_change == change_both) {
const WaypointPtr right_waypoint = raw_waypoint->GetRight();
if (right_waypoint != nullptr &&
right_waypoint->GetType() == crd::Lane::LaneType::Driving &&
(right_waypoint->GetLaneId() * raw_waypoint->GetLaneId() > 0)) {
SimpleWaypointPtr closest_simple_waypoint =
GetWaypointInVicinity(right_waypoint->GetTransform().location);
if (closest_simple_waypoint == nullptr) {
closest_simple_waypoint = GetWaypoint(right_waypoint->GetTransform().location);
}
reference_waypoint->SetRightWaypoint(closest_simple_waypoint);
}
}
if (lane_change == change_left || lane_change == change_both) {
/// Cheack for transits
switch(lane_change)
{
/// Left transit way point present only
case crd::element::LaneMarking::LaneChange::Left:
{
const WaypointPtr left_waypoint = raw_waypoint->GetLeft();
if (left_waypoint != nullptr &&
left_waypoint->GetType() == crd::Lane::LaneType::Driving &&
@ -375,13 +359,61 @@ namespace MapConstants {
reference_waypoint->SetLeftWaypoint(closest_simple_waypoint);
}
}
} catch (const std::invalid_argument &e) {
cg::Location loc = reference_waypoint->GetLocation();
carla::log_info(
"Unable to link lane change connection at: " +
std::to_string(loc.x) + " " +
std::to_string(loc.y) + " " +
std::to_string(loc.z));
break;
/// Right transit way point present only
case crd::element::LaneMarking::LaneChange::Right:
{
const WaypointPtr right_waypoint = raw_waypoint->GetRight();
if(right_waypoint != nullptr &&
right_waypoint->GetType() == crd::Lane::LaneType::Driving &&
(right_waypoint->GetLaneId() * raw_waypoint->GetLaneId() > 0)) {
SimpleWaypointPtr closest_simple_waypoint =
GetWaypointInVicinity(right_waypoint->GetTransform().location);
if (closest_simple_waypoint == nullptr) {
closest_simple_waypoint = GetWaypoint(right_waypoint->GetTransform().location);
}
reference_waypoint->SetRightWaypoint(closest_simple_waypoint);
}
}
break;
/// Both left and right transit present
case crd::element::LaneMarking::LaneChange::Both:
{
/// Right transit way point
const WaypointPtr right_waypoint = raw_waypoint->GetRight();
if (right_waypoint != nullptr &&
right_waypoint->GetType() == crd::Lane::LaneType::Driving &&
(right_waypoint->GetLaneId() * raw_waypoint->GetLaneId() > 0)) {
SimpleWaypointPtr closest_simple_waypointR =
GetWaypointInVicinity(right_waypoint->GetTransform().location);
if (closest_simple_waypointR == nullptr) {
closest_simple_waypointR = GetWaypoint(right_waypoint->GetTransform().location);
}
reference_waypoint->SetRightWaypoint(closest_simple_waypointR);
}
/// Left transit way point
const WaypointPtr left_waypoint = raw_waypoint->GetLeft();
if (left_waypoint != nullptr &&
left_waypoint->GetType() == crd::Lane::LaneType::Driving &&
(left_waypoint->GetLaneId() * raw_waypoint->GetLaneId() > 0)) {
SimpleWaypointPtr closest_simple_waypointL =
GetWaypointInVicinity(left_waypoint->GetTransform().location);
if (closest_simple_waypointL == nullptr) {
closest_simple_waypointL = GetWaypoint(left_waypoint->GetTransform().location);
}
reference_waypoint->SetLeftWaypoint(closest_simple_waypointL);
}
}
break;
/// For no transit waypoint (left or right)
default: break;
}
}

172
LibCarla/source/carla/trafficmanager/LocalizationStage.cpp
View File

@ -27,7 +27,9 @@ namespace LocalizationConstants {
static const float STOPPED_VELOCITY_THRESHOLD = 0.8f; // meters per second.
static const float INTER_LANE_CHANGE_DISTANCE = 10.0f;
static const float MAX_COLLISION_RADIUS = 100.0f;
static const float PHYSICS_RADIUS = 50.0f;
static const float POSITION_WINDOW_SIZE = 2.1f;
static const float HYBRID_MODE_DT = 0.05f;
} // namespace LocalizationConstants
using namespace LocalizationConstants;
@ -66,7 +68,8 @@ namespace LocalizationConstants {
maximum_idle_time = std::make_pair(nullptr, 0.0);
// Initializing srand.
srand(static_cast<unsigned>(time(NULL)));
// Initializing kinematic update clock.
previous_update_instance = chr::system_clock::now();
}
LocalizationStage::~LocalizationStage() {}
@ -75,6 +78,8 @@ namespace LocalizationConstants {
ScanUnregisteredVehicles();
UpdateSwarmVelocities();
// Selecting output frames based on selector keys.
const auto current_planner_frame = planner_frame_selector ? planner_frame_a : planner_frame_b;
const auto current_collision_frame = collision_frame_selector ? collision_frame_a : collision_frame_b;
@ -90,7 +95,8 @@ namespace LocalizationConstants {
const Actor vehicle = actor_list.at(i);
const ActorId actor_id = vehicle->GetId();
const cg::Location vehicle_location = vehicle->GetLocation();
const float vehicle_velocity = vehicle->GetVelocity().Length();
const cg::Vector3D vehicle_velocity_vector = GetVelocity(actor_id);
const float vehicle_velocity = vehicle_velocity_vector.Length();
// Initializing idle times.
if (idle_time.find(actor_id) == idle_time.end() && current_timestamp.elapsed_seconds != 0) {
@ -303,18 +309,54 @@ namespace LocalizationConstants {
});
// Constructing output vector.
std::vector<std::pair<ActorId, Actor>> collision_candidates;
std::vector<std::tuple<ActorId, Actor, cg::Vector3D>> collision_candidates;
std::for_each(collision_candidate_ids.begin(), collision_candidate_ids.end(),
[&overlapping_actor_info, &collision_candidates] (const ActorId& a_id) {
collision_candidates.push_back({a_id, overlapping_actor_info.at(a_id).first});
[&overlapping_actor_info, &collision_candidates, this] (const ActorId& a_id) {
collision_candidates.push_back({a_id,
overlapping_actor_info.at(a_id).first,
this->GetVelocity(a_id)});
});
// Sampling waypoint window for teleportation in hybrid physics mode.
std::vector<SimpleWaypointPtr> position_window;
if (hybrid_physics_mode) {
cg::Vector3D heading = vehicle->GetTransform().GetForwardVector();
bool window_begin = false;
SimpleWaypointPtr begining_wp;
bool window_complete = false;
for (uint32_t j = 0u; j < waypoint_buffer.size() && !window_complete; ++j) {
SimpleWaypointPtr &swp = waypoint_buffer.at(j);
cg::Vector3D relative_position = swp->GetLocation() - vehicle_location;
// Sample waypoints in front of the vehicle;
if (!window_begin && cg::Math::Dot(relative_position, heading) > 0.0f) {
window_begin = true;
begining_wp = swp;
}
if (window_begin && !window_complete) {
if (swp->DistanceSquared(begining_wp) > std::pow(POSITION_WINDOW_SIZE, 2)) {
// Stop when maximum size is reached.
window_complete = true;
} else {
position_window.push_back(swp);
}
}
}
}
// Editing output frames.
LocalizationToPlannerData &planner_message = current_planner_frame->at(i);
planner_message.actor = vehicle;
planner_message.deviation = dot_product;
planner_message.distance = distance;
planner_message.approaching_true_junction = approaching_junction;
planner_message.velocity = vehicle_velocity_vector;
planner_message.position_window = std::move(position_window);
planner_message.physics_enabled = IsPhysicsEnabled(actor_id);
LocalizationToCollisionData &collision_message = current_collision_frame->at(i);
collision_message.actor = vehicle;
@ -323,6 +365,7 @@ namespace LocalizationConstants {
collision_message.closest_waypoint = updated_front_waypoint;
collision_message.junction_look_ahead_waypoint = waypoint_buffer.at(look_ahead_index);
collision_message.safe_point_after_junction = final_safe_points[actor_id];
collision_message.velocity = vehicle_velocity_vector;
LocalizationToTrafficLightData &traffic_light_message = current_traffic_light_frame->at(i);
traffic_light_message.actor = vehicle;
@ -342,6 +385,9 @@ namespace LocalizationConstants {
}
void LocalizationStage::DataReceiver() {
hybrid_physics_mode = parameters.GetHybridPhysicsMode();
bool is_deleted_actors_present = false;
std::set<uint32_t> world_actor_id;
std::vector<ActorPtr> actor_list_to_be_deleted;
@ -357,6 +403,7 @@ namespace LocalizationConstants {
return filtered;
};
// TODO: Combine this actor scanning logic with ScanUnregisteredActors()
// Get all the actors.
auto world_actors_list = episode_proxy_ls.Lock()->GetAllTheActorsInTheEpisode();
@ -366,14 +413,34 @@ namespace LocalizationConstants {
// Building a set of vehicle ids in the world.
for (const auto &actor : vehicles) {
world_actor_id.insert(actor.GetId());
// Identify hero vehicle if currently not present
// and system is in hybrid physics mode.
if (hybrid_physics_mode && hero_actor == nullptr) {
Actor actor_ptr = actor.Get(episode_proxy_ls);
for (auto&& attribute: actor_ptr->GetAttributes()) {
if (attribute.GetId() == "role_name" && attribute.GetValue() == "hero") {
hero_actor = actor_ptr;
break;
}
}
}
}
// Invalidate hero actor pointer if it is not alive anymore.
if (hybrid_physics_mode && hero_actor != nullptr
&& world_actor_id.find(hero_actor->GetId()) == world_actor_id.end()) {
hero_actor = nullptr;
}
// Search for invalid/destroyed vehicles.
for (auto &actor : actor_list) {
if (world_actor_id.find(actor->GetId()) == world_actor_id.end()) {
ActorId deletion_id = actor->GetId();
if (world_actor_id.find(deletion_id) == world_actor_id.end()) {
actor_list_to_be_deleted.emplace_back(actor);
track_traffic.DeleteActor(actor->GetId());
last_lane_change_location.erase(actor->GetId());
track_traffic.DeleteActor(deletion_id);
last_lane_change_location.erase(deletion_id);
kinematic_state_map.erase(deletion_id);
}
}
@ -531,7 +598,7 @@ namespace LocalizationConstants {
{
const ActorId actor_id = vehicle->GetId();
const float vehicle_velocity = vehicle->GetVelocity().Length();
const float vehicle_velocity = GetVelocity(actor_id).Length();
// Waypoint representing the new starting point for the waypoint buffer
// due to lane change. Remains nullptr if lane change not viable.
@ -760,7 +827,8 @@ namespace LocalizationConstants {
}
const auto vehicle = boost::static_pointer_cast<cc::Vehicle>(actor);
if (actor->GetVelocity().Length() > STOPPED_VELOCITY_THRESHOLD || (vehicle->IsAtTrafficLight() && vehicle->GetTrafficLightState() != TLS::Green)) {
if (GetVelocity(actor->GetId()).Length() > STOPPED_VELOCITY_THRESHOLD
|| (vehicle->IsAtTrafficLight() && vehicle->GetTrafficLightState() != TLS::Green)) {
idle_time[actor->GetId()] = current_timestamp.elapsed_seconds;
}
@ -814,5 +882,87 @@ namespace LocalizationConstants {
}
return false;
}
void LocalizationStage::UpdateSwarmVelocities() {
// Location of hero vehicle if present.
cg::Location hero_location;
if (hybrid_physics_mode && hero_actor != nullptr) {
hero_location = hero_actor->GetLocation();
}
// Using (1/20)s time delta for computing velocity.
float dt = HYBRID_MODE_DT;
// Skipping velocity update if elapsed time is less than 0.05s in asynchronous, hybrid mode.
if (!parameters.GetSynchronousMode()) {
TimePoint current_instance = chr::system_clock::now();
chr::duration<float> elapsed_time = current_instance - previous_update_instance;
if (elapsed_time.count() > dt) {
previous_update_instance = current_instance;
} else if (hybrid_physics_mode) {
return;
}
}
// Update velocity for all registered vehicles.
for (const Actor &actor: actor_list) {
ActorId actor_id = actor->GetId();
cg::Location vehicle_location = actor->GetLocation();
// Adding entry if not present.
if (kinematic_state_map.find(actor_id) == kinematic_state_map.end()) {
kinematic_state_map.insert({actor_id, KinematicState{true, vehicle_location, cg::Vector3D()}});
}
// Check if current actor is in range of hero actor and enable physics in hybrid mode.
bool in_range_of_hero_actor = false;
if (hybrid_physics_mode
&& hero_actor != nullptr
&& (cg::Math::DistanceSquared(vehicle_location, hero_location) < std::pow(PHYSICS_RADIUS, 2))) {
in_range_of_hero_actor = true;
}
bool enable_physics = hybrid_physics_mode? in_range_of_hero_actor: true;
kinematic_state_map.at(actor_id).physics_enabled = enable_physics;
actor->SetSimulatePhysics(enable_physics);
// When we say velocity, we usually mean velocity for a vehicle along it's heading.
// Velocity component due to rotation can be removed by taking dot product with heading vector.
cg::Vector3D heading = actor->GetTransform().GetForwardVector();
if (enable_physics) {
kinematic_state_map.at(actor_id).velocity = cg::Math::Dot(actor->GetVelocity(), heading) * heading;
} else {
cg::Vector3D displacement = (vehicle_location - kinematic_state_map.at(actor_id).location);
cg::Vector3D displacement_along_heading = cg::Math::Dot(displacement, heading) * heading;
cg::Vector3D velocity = displacement_along_heading/dt;
kinematic_state_map.at(actor_id).velocity = velocity;
}
// Updating location after velocity is computed.
kinematic_state_map.at(actor_id).location = vehicle_location;
}
}
cg::Vector3D LocalizationStage::GetVelocity(ActorId actor_id) {
cg::Vector3D vel;
if (kinematic_state_map.find(actor_id) != kinematic_state_map.end()) {
vel = kinematic_state_map.at(actor_id).velocity;
} else if (unregistered_actors.find(actor_id) != unregistered_actors.end()) {
vel = unregistered_actors.at(actor_id)->GetVelocity();
}
return vel;
}
bool LocalizationStage::IsPhysicsEnabled(ActorId actor_id) {
bool enabled = true;
if (kinematic_state_map.find(actor_id) != kinematic_state_map.end()) {
enabled = kinematic_state_map.at(actor_id).physics_enabled;
}
return enabled;
}
} // namespace traffic_manager
} // namespace carla

25
LibCarla/source/carla/trafficmanager/LocalizationStage.h
View File

@ -13,6 +13,7 @@
#include <deque>
#include <memory>
#include <mutex>
#include <tuple>
#include <unordered_map>
#include "carla/StringUtil.h"
@ -42,14 +43,23 @@
namespace carla {
namespace traffic_manager {
using namespace std::chrono;
namespace cc = carla::client;
namespace cg = carla::geom;
namespace chr = std::chrono;
using namespace chr;
using Actor = carla::SharedPtr<cc::Actor>;
using Vehicle = carla::SharedPtr<cc::Vehicle>;
using ActorId = carla::ActorId;
using ActorIdSet = std::unordered_set<ActorId>;
using TLS = carla::rpc::TrafficLightState;
/// Structure to hold kinematic state of actors.
struct KinematicState {
bool physics_enabled;
cg::Location location;
cg::Vector3D velocity;
};
/// This class is responsible for maintaining a horizon of waypoints ahead
/// of the vehicle for it to follow.
/// The class is also responsible for managing lane change decisions and
@ -122,6 +132,14 @@ namespace traffic_manager {
SnippetProfiler snippet_profiler;
/// Map to keep track of last lane change location.
std::unordered_map<ActorId, cg::Location> last_lane_change_location;
/// Reference of hero vehicle.
Actor hero_actor {nullptr};
/// Switch indicating hybrid physics mode.
bool hybrid_physics_mode {false};
/// Structure to hold previous state of physics-less vehicle.
std::unordered_map<ActorId, KinematicState> kinematic_state_map;
/// Time instance used to calculate dt in asynchronous mode.
TimePoint previous_update_instance;
/// A simple method used to draw waypoint buffer ahead of a vehicle.
void DrawBuffer(Buffer &buffer);
@ -146,6 +164,11 @@ namespace traffic_manager {
void CleanActor(const ActorId actor_id);
bool TryDestroyVehicle(const Actor& actor);
/// Methods for actor state management.
void UpdateSwarmVelocities();
cg::Vector3D GetVelocity(ActorId actor_id);
bool IsPhysicsEnabled(ActorId actor_id);
public:
LocalizationStage(

27
LibCarla/source/carla/trafficmanager/LocalizationUtils.cpp
View File

@ -234,7 +234,7 @@ namespace LocalizationConstants {
}
std::pair<SimpleWaypointPtr,uint64_t> TrackTraffic::GetTargetWaypoint(const Buffer& waypoint_buffer,const float& target_point_distance) {
std::pair<SimpleWaypointPtr,uint64_t> TrackTraffic::GetTargetWaypoint(const Buffer& waypoint_buffer,const float& target_point_distance) {
SimpleWaypointPtr target_waypoint = waypoint_buffer.front();
const SimpleWaypointPtr& buffer_front = waypoint_buffer.front();
@ -250,27 +250,28 @@ namespace LocalizationConstants {
}
if(mScanForward) {
for (index = startPosn ;
(index < waypoint_buffer.size()) &&
(buffer_front->DistanceSquared(target_waypoint)
< target_point_dist_power);
++index) {
target_waypoint = waypoint_buffer.at(index);
for (uint64_t i = startPosn;
(i < waypoint_buffer.size())
&& (buffer_front->DistanceSquared(target_waypoint) < target_point_dist_power);
++i) {
target_waypoint = waypoint_buffer.at(i);
index = i;
}
}
else {
for (index = startPosn ;
(buffer_front->DistanceSquared(target_waypoint)
> target_point_dist_power);
--index) {
target_waypoint = waypoint_buffer.at(index);
for (uint64_t i = startPosn;
(buffer_front->DistanceSquared(target_waypoint) > target_point_dist_power);
--i) {
target_waypoint = waypoint_buffer.at(i);
index = i;
}
}
}
else {
target_waypoint = waypoint_buffer.back();
index = waypoint_buffer.size() - 1;
}
return std::make_pair(target_waypoint,index);
return std::make_pair(target_waypoint, index);
}
} // namespace traffic_manager

11
LibCarla/source/carla/trafficmanager/MessengerAndDataTypes.h
View File

@ -7,11 +7,13 @@
#pragma once
#include <memory>
#include <tuple>
#include <unordered_map>
#include <vector>
#include "carla/client/Actor.h"
#include "carla/geom/Vector3D.h"
#include "carla/geom/Transform.h"
#include "carla/Memory.h"
#include "carla/rpc/ActorId.h"
@ -22,6 +24,7 @@ namespace carla {
namespace traffic_manager {
namespace cc = carla::client;
namespace cg = carla::geom;
/// Convenience typing.
@ -41,6 +44,9 @@ namespace traffic_manager {
float deviation;
float distance;
bool approaching_true_junction;
cg::Vector3D velocity;
bool physics_enabled;
std::vector<std::shared_ptr<SimpleWaypoint>> position_window;
};
/// Type of data sent by the motion planner stage to the batch control stage.
@ -49,16 +55,19 @@ namespace traffic_manager {
float throttle;
float brake;
float steer;
bool physics_enabled;
cg::Transform transform;
};
/// Type of data sent by the localization stage to the collision stage.
struct LocalizationToCollisionData {
Actor actor;
Buffer buffer;
std::vector<std::pair<ActorId, Actor>> overlapping_actors;
std::vector<std::tuple<ActorId, Actor, cg::Vector3D>> overlapping_actors;
std::shared_ptr<SimpleWaypoint> safe_point_after_junction;
std::shared_ptr<SimpleWaypoint> closest_waypoint;
std::shared_ptr<SimpleWaypoint> junction_look_ahead_waypoint;
cg::Vector3D velocity;
};
/// Type of data sent by the collision stage to the motion planner stage.

111
LibCarla/source/carla/trafficmanager/MotionPlannerStage.cpp
View File

@ -18,6 +18,8 @@ namespace PlannerConstants {
static const float ARBITRARY_MAX_SPEED = 100.0f / 3.6f;
static const float FOLLOW_DISTANCE_RATE = (MAX_FOLLOW_LEAD_DISTANCE-MIN_FOLLOW_LEAD_DISTANCE)/ARBITRARY_MAX_SPEED;
static const float CRITICAL_BRAKING_MARGIN = 0.25f;
static const float HYBRID_MODE_DT = 0.05f;
static const float EPSILON_VELOCITY = 0.001f;
} // namespace PlannerConstants
using namespace PlannerConstants;
@ -82,7 +84,7 @@ namespace PlannerConstants {
const ActorId actor_id = actor->GetId();
const auto vehicle = boost::static_pointer_cast<cc::Vehicle>(actor);
const cg::Vector3D current_velocity_vector = vehicle->GetVelocity();
const cg::Vector3D current_velocity_vector = localization_data.velocity;
const float current_velocity = current_velocity_vector.Length();
const auto current_time = chr::system_clock::now();
@ -120,7 +122,7 @@ namespace PlannerConstants {
// Consider collision avoidance decisions only if there is positive relative velocity
// of the ego vehicle (meaning, ego vehicle is closing the gap to the lead vehicle).
if (ego_relative_velocity > 0.0f) {
if (ego_relative_velocity > EPSILON_VELOCITY) {
// If other vehicle is approaching lead vehicle and lead vehicle is further
// than follow_lead_distance 0 kmph -> 5m, 100 kmph -> 10m.
float follow_lead_distance = ego_relative_velocity * FOLLOW_DISTANCE_RATE + MIN_FOLLOW_LEAD_DISTANCE;
@ -139,24 +141,41 @@ namespace PlannerConstants {
collision_emergency_stop = true;
}
}
if (collision_data.distance_to_other_vehicle < CRITICAL_BRAKING_MARGIN) {
collision_emergency_stop = true;
}
}
///////////////////////////////////////////////////////////////////////////////////
// Clip dynamic target velocity to maximum allowed speed for the vehicle.
dynamic_target_velocity = std::min(max_target_velocity, dynamic_target_velocity);
bool emergency_stop = false;
// In case of collision or traffic light hazard.
if (traffic_light_frame->at(i).traffic_light_hazard
|| collision_emergency_stop) {
emergency_stop = true;
}
// Message items to be sent to batch control stage.
ActuationSignal actuation_signal {0.0f, 0.0f, 0.0f};
bool physics_enabled = true;
cg::Transform teleportation_transform;
// If physics is enabled for the vehicle, use PID controller.
StateEntry current_state;
if (localization_data.physics_enabled) {
// State update for vehicle.
StateEntry current_state = controller.StateUpdate(previous_state, current_velocity,
current_state = controller.StateUpdate(previous_state, current_velocity,
dynamic_target_velocity, current_deviation,
current_distance, current_time);
// Controller actuation.
ActuationSignal actuation_signal = controller.RunStep(current_state, previous_state,
actuation_signal = controller.RunStep(current_state, previous_state,
longitudinal_parameters, lateral_parameters);
// In case of traffic light hazard.
if (traffic_light_frame->at(i).traffic_light_hazard
|| collision_emergency_stop) {
if (emergency_stop) {
current_state.deviation_integral = 0.0f;
current_state.velocity_integral = 0.0f;
@ -164,6 +183,80 @@ namespace PlannerConstants {
actuation_signal.brake = 1.0f;
}
}
// For physics-less vehicles, determine position and orientation for teleportation.
else if (hybrid_physics_mode) {
physics_enabled = false;
// Flushing controller state for vehicle.
current_state = {0.0f, 0.0f, 0.0f,
chr::system_clock::now(),
0.0f, 0.0f, 0.0f};
// Add entry to teleportation duration clock table if not present.
if (teleportation_instance.find(actor_id) == teleportation_instance.end()) {
teleportation_instance.insert({actor_id, chr::system_clock::now()});
}
// Measuring time elapsed since last teleportation for the vehicle.
chr::duration<float> elapsed_time = current_time - teleportation_instance.at(actor_id);
// Find a location ahead of the vehicle for teleportation to achieve intended velocity.
if (!emergency_stop && !(!parameters.GetSynchronousMode() && elapsed_time.count() < HYBRID_MODE_DT)) {
// Target displacement magnitude to achieve target velocity.
float target_displacement = dynamic_target_velocity * HYBRID_MODE_DT;
SimpleWaypointPtr target_interval_begin = nullptr;
SimpleWaypointPtr target_interval_end = nullptr;
bool teleportation_interval_found = false;
cg::Location vehicle_location = actor->GetLocation();
// Find the interval containing position to achieve target displacement.
for (uint32_t j = 0u;
j+1 < localization_data.position_window.size() && !teleportation_interval_found;
++j) {
target_interval_begin = localization_data.position_window.at(j);
target_interval_end = localization_data.position_window.at(j+1);
if (target_interval_begin->DistanceSquared(vehicle_location) > std::pow(target_displacement, 2)
|| (target_interval_begin->DistanceSquared(vehicle_location) < std::pow(target_displacement, 2)
&& target_interval_end->DistanceSquared(vehicle_location) > std::pow(target_displacement, 2))
) {
teleportation_interval_found = true;
}
}
if (target_interval_begin != nullptr && target_interval_end != nullptr) {
// Construct target transform to accurately achieve desired velocity.
float missing_displacement = target_displacement - (target_interval_begin->Distance(vehicle_location));
cg::Transform target_base_transform = target_interval_begin->GetTransform();
cg::Location target_base_location = target_base_transform.location;
cg::Vector3D target_heading = target_base_transform.GetForwardVector();
cg::Location teleportation_location = target_base_location
+ cg::Location(target_heading * missing_displacement);
teleportation_transform = cg::Transform(teleportation_location, target_base_transform.rotation);
} else {
teleportation_transform = actor->GetTransform();
}
}
// In case of an emergency stop, stay in the same location.
// Also, teleport only once every dt in asynchronous mode.
else {
teleportation_transform = actor->GetTransform();
}
}
// Updating PID state.
StateEntry &state = pid_state_map.at(actor_id);
state = current_state;
@ -174,6 +267,8 @@ namespace PlannerConstants {
message.throttle = actuation_signal.throttle;
message.brake = actuation_signal.brake;
message.steer = actuation_signal.steer;
message.physics_enabled = physics_enabled;
message.transform = teleportation_transform;
}
}
@ -192,6 +287,8 @@ namespace PlannerConstants {
control_frame_a = std::make_shared<PlannerToControlFrame>(number_of_vehicles);
control_frame_b = std::make_shared<PlannerToControlFrame>(number_of_vehicles);
}
hybrid_physics_mode = parameters.GetHybridPhysicsMode();
}
void MotionPlannerStage::DataSender() {

8
LibCarla/source/carla/trafficmanager/MotionPlannerStage.h
View File

@ -7,6 +7,7 @@
#pragma once
#include <chrono>
#include <memory>
#include <unordered_map>
#include <vector>
@ -18,6 +19,7 @@
#include "carla/trafficmanager/Parameters.h"
#include "carla/trafficmanager/PIDController.h"
#include "carla/trafficmanager/PipelineStage.h"
#include "carla/trafficmanager/SimpleWaypoint.h"
namespace carla {
namespace traffic_manager {
@ -27,6 +29,7 @@ namespace traffic_manager {
using Actor = carla::SharedPtr<cc::Actor>;
using ActorId = carla::rpc::ActorId;
using SimpleWaypointPtr = std::shared_ptr<SimpleWaypoint>;
/// The class is responsible for aggregating information from various stages
/// like the localization stage, traffic light stage, collision detection
@ -68,7 +71,10 @@ namespace traffic_manager {
uint64_t number_of_vehicles;
/// Reference to Carla's debug helper object.
cc::DebugHelper &debug_helper;
/// Switch indicating hybrid physics mode.
bool hybrid_physics_mode {false};
/// Teleportation duration clock;
std::unordered_map<ActorId, TimePoint> teleportation_instance;
public:

10
LibCarla/source/carla/trafficmanager/Parameters.cpp
View File

@ -19,6 +19,11 @@ Parameters::Parameters() {
Parameters::~Parameters() {}
void Parameters::SetHybridPhysicsMode(const bool mode_switch) {
hybrid_physics_mode.store(mode_switch);
}
void Parameters::SetPercentageSpeedDifference(const ActorPtr &actor, const float percentage) {
float new_percentage = std::min(100.0f, percentage);
@ -260,5 +265,10 @@ float Parameters::GetPercentageIgnoreVehicles(const ActorPtr &actor) {
return percentage;
}
bool Parameters::GetHybridPhysicsMode() {
return hybrid_physics_mode.load();
}
} // namespace traffic_manager
} // namespace carla

8
LibCarla/source/carla/trafficmanager/Parameters.h
View File

@ -60,6 +60,8 @@ namespace traffic_manager {
std::atomic<bool> synchronous_mode {false};
/// Distance Margin
std::atomic<float> distance_margin {2.0};
/// Hybrid physics mode switch.
std::atomic<bool> hybrid_physics_mode {false};
public:
Parameters();
@ -150,6 +152,12 @@ namespace traffic_manager {
/// Set Synchronous mode time out.
void SetSynchronousModeTimeOutInMiliSecond(const double time);
/// Method to set hybrid physics mode.
void SetHybridPhysicsMode(const bool mode_switch);
/// Method to retrieve hybrid physics mode.
bool GetHybridPhysicsMode();
/// Synchronous mode time out variable.
std::chrono::duration<double, std::milli> synchronous_time_out;

12
LibCarla/source/carla/trafficmanager/SimpleWaypoint.cpp
View File

@ -64,25 +64,21 @@ namespace traffic_manager {
return static_cast<uint64_t>(waypoints.size());
}
void SimpleWaypoint::SetLeftWaypoint(SimpleWaypointPtr _waypoint) {
void SimpleWaypoint::SetLeftWaypoint(SimpleWaypointPtr &_waypoint) {
const cg::Vector3D heading_vector = waypoint->GetTransform().GetForwardVector();
const cg::Vector3D relative_vector = GetLocation() - _waypoint->GetLocation();
if ((heading_vector.x * relative_vector.y - heading_vector.y * relative_vector.x) > 0.0f) {
next_left_waypoint = _waypoint;
} else {
throw std::invalid_argument("Argument not on the left side!");
}
}
void SimpleWaypoint::SetRightWaypoint(SimpleWaypointPtr _waypoint) {
void SimpleWaypoint::SetRightWaypoint(SimpleWaypointPtr &_waypoint) {
const cg::Vector3D heading_vector = waypoint->GetTransform().GetForwardVector();
const cg::Vector3D relative_vector = GetLocation() - _waypoint->GetLocation();
if ((heading_vector.x * relative_vector.y - heading_vector.y * relative_vector.x) < 0.0f) {
next_right_waypoint = _waypoint;
} else {
throw std::invalid_argument("Argument not on the right side!");
}
}
@ -132,5 +128,9 @@ namespace traffic_manager {
return waypoint->GetJunctionId();
}
cg::Transform SimpleWaypoint::GetTransform() const {
return waypoint->GetTransform();
}
} // namespace traffic_manager
} // namespace carla

7
LibCarla/source/carla/trafficmanager/SimpleWaypoint.h
View File

@ -12,6 +12,7 @@
#include "carla/client/Waypoint.h"
#include "carla/geom/Location.h"
#include "carla/geom/Math.h"
#include "carla/geom/Transform.h"
#include "carla/geom/Vector3D.h"
#include "carla/Memory.h"
#include "carla/road/RoadTypes.h"
@ -77,10 +78,10 @@ namespace traffic_manager {
uint64_t SetPreviousWaypoint(const std::vector<SimpleWaypointPtr> &next_waypoints);
/// This method is used to set the closest left waypoint for a lane change.
void SetLeftWaypoint(SimpleWaypointPtr waypoint);
void SetLeftWaypoint(SimpleWaypointPtr &waypoint);
/// This method is used to set the closest right waypoint for a lane change.
void SetRightWaypoint(SimpleWaypointPtr waypoint);
void SetRightWaypoint(SimpleWaypointPtr &waypoint);
/// This method is used to get the closest left waypoint for a lane change.
SimpleWaypointPtr GetLeftWaypoint();
@ -117,6 +118,8 @@ namespace traffic_manager {
/// Returns true if the object's waypoint belongs to an intersection (Doesn't use OpenDrive).
bool CheckIntersection() const;
/// Return transform object for the current waypoint.
cg::Transform GetTransform() const;
};
} // namespace traffic_manager

8
LibCarla/source/carla/trafficmanager/TrafficManager.h
View File

@ -71,6 +71,14 @@ public:
static void Tick();
/// This method sets the hybrid physics mode.
void SetHybridPhysicsMode(const bool mode_switch) {
TrafficManagerBase* tm_ptr = GetTM(_port);
if(tm_ptr != nullptr){
tm_ptr->SetHybridPhysicsMode(mode_switch);
}
}
/// This method registers a vehicle with the traffic manager.
void RegisterVehicles(const std::vector<ActorPtr> &actor_list) {
TrafficManagerBase* tm_ptr = GetTM(_port);

3
LibCarla/source/carla/trafficmanager/TrafficManagerBase.h
View File

@ -103,6 +103,9 @@ public:
/// Method to set probabilistic preference to keep on the right lane.
virtual void SetKeepRightPercentage(const ActorPtr &actor,const float percentage) = 0;
/// Method to set hybrid physics mode.
virtual void SetHybridPhysicsMode(const bool mode_switch) = 0;
protected:
};

6
LibCarla/source/carla/trafficmanager/TrafficManagerClient.h
View File

@ -179,6 +179,12 @@ public:
_client->call("set_percentage_keep_right_rule", actor, percentage);
}
/// Method to set hybrid physics mode.
void SetHybridPhysicsMode(const bool mode_switch) {
DEBUG_ASSERT(_client != nullptr);
_client->call("set_hybrid_physics_mode", mode_switch);
}
private:
/// RPC client.

4
LibCarla/source/carla/trafficmanager/TrafficManagerLocal.cpp
View File

@ -201,6 +201,10 @@ void TrafficManagerLocal::SetKeepRightPercentage(const ActorPtr &actor, const fl
parameters.SetKeepRightPercentage(actor, percentage);
}
void TrafficManagerLocal::SetHybridPhysicsMode(const bool mode_switch) {
parameters.SetHybridPhysicsMode(mode_switch);
}
bool TrafficManagerLocal::CheckAllFrozen(TLGroup tl_to_freeze) {
for (auto& elem : tl_to_freeze) {
if (!elem->IsFrozen() || elem->GetState() != TLS::Red) {

3
LibCarla/source/carla/trafficmanager/TrafficManagerLocal.h
View File

@ -184,6 +184,9 @@ namespace traffic_manager {
/// Method to set probabilistic preference to keep on the right lane.
void SetKeepRightPercentage(const ActorPtr &actor, const float percentage);
/// Method to set hybrid physics mode.
void SetHybridPhysicsMode(const bool mode_switch);
};
} // namespace traffic_manager

4
LibCarla/source/carla/trafficmanager/TrafficManagerRemote.cpp
View File

@ -176,6 +176,10 @@ void TrafficManagerRemote::SetKeepRightPercentage(const ActorPtr &_actor, const
client.SetKeepRightPercentage(actor, percentage);
}
void TrafficManagerRemote::SetHybridPhysicsMode(const bool mode_switch) {
client.SetHybridPhysicsMode(mode_switch);
}
void TrafficManagerRemote::ResetAllTrafficLights() {
client.ResetAllTrafficLights();
}

3
LibCarla/source/carla/trafficmanager/TrafficManagerRemote.h
View File

@ -100,6 +100,9 @@ public:
/// Method to set probabilistic preference to keep on the right lane.
void SetKeepRightPercentage(const ActorPtr &actor, const float percentage);
/// Method to set hybrid physics mode.
void SetHybridPhysicsMode(const bool mode_switch);
/// Method to provide synchronous tick
bool SynchronousTick();

5
LibCarla/source/carla/trafficmanager/TrafficManagerServer.h
View File

@ -150,6 +150,11 @@ public:
tm->SetKeepRightPercentage(carla::client::detail::ActorVariant(actor).Get(tm->GetEpisodeProxy()), percentage);
});
/// Method to set hybrid physics mode.
server->bind("set_hybrid_physics_mode", [=](const bool mode_switch) {
tm->SetHybridPhysicsMode(mode_switch);
});
/// Method to set synchronous mode.
server->bind("set_synchronous_mode", [=](const bool mode) {
tm->SetSynchronousMode(mode);

3
PythonAPI/carla/source/libcarla/TrafficManager.cpp
View File

@ -30,5 +30,6 @@ void export_trafficmanager() {
.def("ignore_signs_percentage", &carla::traffic_manager::TrafficManager::SetPercentageRunningSign)
.def("set_global_distance_to_leading_vehicle", &carla::traffic_manager::TrafficManager::SetGlobalDistanceToLeadingVehicle)
.def("set_percentage_keep_right_rule", &carla::traffic_manager::TrafficManager::SetKeepRightPercentage)
.def("set_synchronous_mode", &carla::traffic_manager::TrafficManager::SetSynchronousMode);
.def("set_synchronous_mode", &carla::traffic_manager::TrafficManager::SetSynchronousMode)
.def("set_hybrid_physics_mode", &carla::traffic_manager::TrafficManager::SetHybridPhysicsMode);
}

10
PythonAPI/examples/spawn_npc.py
View File

@ -78,6 +78,10 @@ def main():
'--sync',
action='store_true',
help='Synchronous mode execution')
argparser.add_argument(
'--hybrid',
action='store_true',
help='Enanble')
args = argparser.parse_args()
logging.basicConfig(format='%(levelname)s: %(message)s', level=logging.INFO)
@ -90,9 +94,13 @@ def main():
try:
world = client.get_world()
traffic_manager = client.get_trafficmanager(args.tm_port)
traffic_manager.set_global_distance_to_leading_vehicle(2.0)
world = client.get_world()
if args.hybrid:
traffic_manager.set_hybrid_physics_mode(True)
synchronous_master = False

2
PythonAPI/util/performance_benchmark.py
View File

@ -325,7 +325,7 @@ def run_benchmark(world, sensors, n_vehicles, n_walkers, client, debug=False):
sensor_list = sensors_ret
ticks = 0
while ticks < args.ticks:
while ticks < int(args.ticks):
_ = world.wait_for_tick()
if debug:
print("== Samples {} / {}".format(ticks + 1, args.ticks))