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
GitHub
parent
4ed014a2fe
commit
151e38ce8d
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -38,11 +38,18 @@ void BatchControlStage::Action() {
|
|||
}
|
||||
const carla::ActorId actor_id = element.actor->GetId();
|
||||
|
||||
vehicle_control.throttle = element.throttle;
|
||||
vehicle_control.brake = element.brake;
|
||||
vehicle_control.steer = element.steer;
|
||||
// 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);
|
||||
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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -11,6 +11,7 @@
|
|||
#include <deque>
|
||||
#include <stdlib.h>
|
||||
#include <string>
|
||||
#include <tuple>
|
||||
#include <unordered_map>
|
||||
#include <vector>
|
||||
|
||||
|
|
@ -105,37 +106,43 @@ private:
|
|||
/// Snippet profiler for measuring execution time.
|
||||
SnippetProfiler snippet_profiler;
|
||||
|
||||
/// Returns the bounding box corners of the vehicle passed to the method.
|
||||
LocationList GetBoundary(const Actor &actor, const cg::Location &location);
|
||||
/// Returns the bounding box corners of the vehicle passed to the method.
|
||||
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);
|
||||
/// Returns the extrapolated bounding box of the vehicle along its
|
||||
/// trajectory.
|
||||
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);
|
||||
/// Method to construct a boost polygon object.
|
||||
Polygon GetPolygon(const LocationList &boundary);
|
||||
|
||||
/// The method returns true if ego_vehicle should stop and wait for
|
||||
/// other_vehicle to pass.
|
||||
/// The method returns true if ego_vehicle should stop and wait for
|
||||
/// other_vehicle to pass.
|
||||
std::pair<bool, float> NegotiateCollision(const Actor &ego_vehicle, const Actor &other_vehicle,
|
||||
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);
|
||||
/// Method to calculate the speed dependent bounding box extention for a 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);
|
||||
/// 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 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:
|
||||
|
||||
|
|
|
|||
|
|
@ -339,30 +339,14 @@ 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) {
|
||||
|
||||
const WaypointPtr left_waypoint = raw_waypoint->GetLeft();
|
||||
/// 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 &&
|
||||
(left_waypoint->GetLaneId() * raw_waypoint->GetLaneId() > 0)) {
|
||||
|
|
@ -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;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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(
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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.
|
||||
|
|
|
|||
|
|
@ -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,29 +141,120 @@ 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);
|
||||
|
||||
// State update for vehicle.
|
||||
StateEntry 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,
|
||||
longitudinal_parameters, lateral_parameters);
|
||||
|
||||
// In case of traffic light hazard.
|
||||
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.
|
||||
current_state = controller.StateUpdate(previous_state, current_velocity,
|
||||
dynamic_target_velocity, current_deviation,
|
||||
current_distance, current_time);
|
||||
|
||||
// Controller actuation.
|
||||
actuation_signal = controller.RunStep(current_state, previous_state,
|
||||
longitudinal_parameters, lateral_parameters);
|
||||
|
||||
if (emergency_stop) {
|
||||
|
||||
current_state.deviation_integral = 0.0f;
|
||||
current_state.velocity_integral = 0.0f;
|
||||
actuation_signal.throttle = 0.0f;
|
||||
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();
|
||||
|
||||
}
|
||||
|
||||
current_state.deviation_integral = 0.0f;
|
||||
current_state.velocity_integral = 0.0f;
|
||||
actuation_signal.throttle = 0.0f;
|
||||
actuation_signal.brake = 1.0f;
|
||||
}
|
||||
|
||||
// Updating PID 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() {
|
||||
|
|
|
|||
|
|
@ -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:
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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:
|
||||
|
||||
};
|
||||
|
|
|
|||
|
|
@ -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.
|
||||
|
|
|
|||
|
|
@ -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) {
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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();
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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();
|
||||
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
|
|
|
|||
|
|
@ -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))
|
||||
|
|
|
|||
Loading…
Reference in New Issue