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

New Traffic Manager features for 0.9.13 (#4786) 

* Changes in constant values
Added new constants

* Added Ambulance and Firetruck as unsafe vehicles in spawn_npc.py

* WIP: Rerouting algorithm

* Removing rerouting algorithm.
Fixed collisions at high speed.

* Changes to constant values and cleaning up.

* Users can now set their own path by giving a list of locations.
Implementation of RoadOption for SimpleWaypoints
Implementation of GetNextAction and GetActionBuffer for TM actions
Parameters for setting the path import and starting the import process.
Other minor changes

* Users now only need to use one parameter to import path

* added road option to the cache map

* Introduced RoadOption in InMemoryMap for each swp
Introduced getter in PythonAPI for high level path
Introduced getter in PythonAPI for next high level action
Introduced parameters for random lane changes
Fixed keep right parameter

* Introduced parameter SetImportedRoute to set a RoadOption path for TM controlled vehicles.
Method in Localization Stage to compute a path using RoadOptions.

* CHANGELOG.

* Changing names of methods
Cleanup

* Small changes to Collision Stage
Cleanup of SetRoadOption
Added debug_helper in Loc Stage (to be removed)

* Changes to get_next_action and get_all_actions
Fixed lane changes representation in those methods
Now get_next_action and get_all_actions need the Actor in Python

* Remove debug_helper
Set MAX_BRAKE to 0.7

* Constant values in Loc stage

* Code fixes

* Small changes in collision stage
Adding constants for VLStage
Changes in VLStage

* Choose randomly a right or left lane change if forcing both.

* Reviewable changes
Changes name of variables in Loc stage
Removed unnecessary import

* Minor final changes

* Change of method name in generate_traffic

* Change in Latest ContentVersion.txt

Co-authored-by: Joel Moriana <joel.moriana@gmail.com>
This commit is contained in:
Jacopo Bartiromo 2021-11-03 11:06:14 +01:00 committed by GitHub
parent 85da613f51
commit 5f07128176
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
27 changed files with 1152 additions and 132 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
CHANGELOG.md
View File

@ -1,4 +1,9 @@
## Latest
* Added the option for users to set a path using locations to a vehicle controlled by the Traffic Manager.
* Added a RoadOption element in each SimpleWaypoint to specify which action will the vehicle perform if it follows that route.
* Added the option for users to set a route using RoadOption elements to a vehicle controlled by the Traffic Manager.
* Fixed keep_right_rule parameter.
* Added set_percentage_random_left_lanechange and set_percentage_random_right_lanechange.
* Improved handling of collisions in Traffic Manager when driving at very high speeds.
* Added open/close doors feature for vehicles.
* Added API functions to 3D vectors: `squared_length`, `length`, `make_unit_vector`, `dot`, `dot_2d`, `distance`, `distance_2d`, `distance_squared`, `distance_squared_2d`, `get_vector_angle`

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

@ -35,6 +35,7 @@ namespace traffic_manager {
this->geodesic_grid_id = simple_waypoint->GetGeodesicGridId();
this->is_junction = simple_waypoint->CheckJunction();
this->road_option = static_cast<uint8_t>(simple_waypoint->GetRoadOption());
}
void CachedSimpleWaypoint::Write(std::ofstream &out_file) {
@ -70,6 +71,9 @@ namespace traffic_manager {
// is_junction
WriteValue<bool>(out_file, this->is_junction);
// road_option
WriteValue<uint8_t>(out_file, this->road_option);
}
void CachedSimpleWaypoint::Read(std::ifstream &in_file) {
@ -109,6 +113,9 @@ namespace traffic_manager {
// is_junction
ReadValue<bool>(in_file, this->is_junction);
// road_option
ReadValue<uint8_t>(in_file, this->road_option);
}
void CachedSimpleWaypoint::Read(const std::vector<uint8_t>& content, unsigned long& start) {
@ -147,6 +154,9 @@ namespace traffic_manager {
// is_junction
ReadValue<bool>(content, start, this->is_junction);
// road_option
ReadValue<uint8_t>(content, start, this->road_option);
}
} // namespace traffic_manager

1
LibCarla/source/carla/trafficmanager/CachedSimpleWaypoint.h
View File

@ -28,6 +28,7 @@ namespace traffic_manager {
uint64_t next_right_waypoint = 0;
int32_t geodesic_grid_id;
bool is_junction;
uint8_t road_option;
CachedSimpleWaypoint() = default;
CachedSimpleWaypoint(const SimpleWaypointPtr& simple_waypoint);

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

@ -47,15 +47,16 @@ void CollisionStage::Update(const unsigned long index) {
std::vector<ActorId> collision_candidate_ids;
// Run through vehicles with overlapping paths and filter them;
const float distance_to_leading = parameters.GetDistanceToLeadingVehicle(ego_actor_id);
float collision_radius_square = SQUARE(COLLISION_RADIUS_RATE * velocity + COLLISION_RADIUS_MIN) + distance_to_leading;;
float collision_radius_square = SQUARE(COLLISION_RADIUS_RATE * velocity + COLLISION_RADIUS_MIN);
if (velocity < 2.0f) {
const float length = simulation_state.GetDimensions(ego_actor_id).x;
const float collision_radius_stop = COLLISION_RADIUS_STOP + length;
collision_radius_square = SQUARE(collision_radius_stop);
if (distance_to_leading > collision_radius_stop) {
collision_radius_square = SQUARE(collision_radius_stop) + distance_to_leading;
}
}
if (distance_to_leading > collision_radius_square) {
collision_radius_square = SQUARE(distance_to_leading);
}
for (ActorId overlapping_actor_id : overlapping_actors) {
// If actor is within maximum collision avoidance and vertical overlap range.
const cg::Location &overlapping_actor_location = simulation_state.GetLocation(overlapping_actor_id);
@ -121,7 +122,7 @@ float CollisionStage::GetBoundingBoxExtention(const ActorId actor_id) {
const float velocity = cg::Math::Dot(simulation_state.GetVelocity(actor_id), simulation_state.GetHeading(actor_id));
float bbox_extension;
// Using a function to calculate boundary length.
float velocity_extension = VEL_EXT_FACTOR*velocity;
float velocity_extension = VEL_EXT_FACTOR * velocity;
bbox_extension = BOUNDARY_EXTENSION_MINIMUM + velocity_extension * velocity_extension;
// If a valid collision lock present, change boundary length to maintain lock.
if (collision_locks.find(actor_id) != collision_locks.end()) {

12
LibCarla/source/carla/trafficmanager/Constants.h
View File

@ -52,7 +52,7 @@ static const float HIGH_SPEED_HORIZON_RATE = 4.0f;
namespace WaypointSelection {
static const float TARGET_WAYPOINT_TIME_HORIZON = 0.5f;
static const float TARGET_WAYPOINT_HORIZON_LENGTH = 1.0f;
static const float MIN_TARGET_WAYPOINT_DISTANCE = 1.0f;
static const float JUNCTION_LOOK_AHEAD = 5.0f;
static const float SAFE_DISTANCE_AFTER_JUNCTION = 4.0f;
static const float MIN_JUNCTION_LENGTH = 8.0f;
@ -64,6 +64,10 @@ static const float MINIMUM_LANE_CHANGE_DISTANCE = 20.0f;
static const float MAXIMUM_LANE_OBSTACLE_DISTANCE = 50.0f;
static const float MAXIMUM_LANE_OBSTACLE_CURVATURE = 0.6f;
static const float INTER_LANE_CHANGE_DISTANCE = 10.0f;
static const float MIN_WPT_DISTANCE = 5.0f;
static const float MAX_WPT_DISTANCE = 20.0f;
static const float MIN_LANE_CHANGE_SPEED = 5.0f;
static const float FIFTYPERC = 50.0f;
} // namespace LaneChange
namespace Collision {
@ -80,7 +84,7 @@ static const float WALKER_TIME_EXTENSION = 1.5f;
static const float SQUARE_ROOT_OF_TWO = 1.414f;
static const float VERTICAL_OVERLAP_THRESHOLD = 4.0f;
static const float EPSILON = 2.0f * std::numeric_limits<float>::epsilon();
static const float MIN_REFERENCE_DISTANCE = 1.0f;
static const float MIN_REFERENCE_DISTANCE = 0.5f;
static const float MIN_VELOCITY_COLL_RADIUS = 2.0f;
static const float VEL_EXT_FACTOR = 0.36f;
} // namespace Collision
@ -100,6 +104,7 @@ static const double MAX_WPT_DISTANCE = MAP_RESOLUTION/2.0 + SQUARE(MAP_RESOLUTIO
static const float MAX_WPT_RADIANS = 0.1745f; // 10º
static float const DELTA = 25.0f;
static float const Z_DELTA = 500.0f;
static float const STRAIGHT_DEG = 19.0f;
} // namespace Map
namespace TrafficLight {
@ -135,11 +140,12 @@ static const float SUN_ALTITUDE_DEGREES_JUST_AFTER_DAWN = 35.0f;
static const float SUN_ALTITUDE_DEGREES_JUST_BEFORE_SUNSET = 145.0f;
static const float HEAVY_PRECIPITATION_THRESHOLD = 80.0f;
static const float FOG_DENSITY_THRESHOLD = 20.0f;
static const float MAX_DISTANCE_LIGHT_CHECK = 225.0f;
} // namespace VehicleLight
namespace PID {
static const float MAX_THROTTLE = 0.85f;
static const float MAX_BRAKE = 1.0f;
static const float MAX_BRAKE = 0.7f;
static const float MAX_STEERING = 0.8f;
static const float MAX_STEERING_DIFF = 0.15f;
static const float DT = 0.05f;

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

@ -130,6 +130,7 @@ namespace traffic_manager {
SimpleWaypointPtr wp = std::make_shared<SimpleWaypoint>(waypoint_ptr);
wp->SetGeodesicGridId(cached_wp.geodesic_grid_id);
wp->SetIsJunction(cached_wp.is_junction);
wp->SetRoadOption(static_cast<RoadOption>(cached_wp.road_option));
dense_topology.push_back(wp);
}
@ -229,8 +230,7 @@ namespace traffic_manager {
}
// 3. Processing waypoints.
auto distance_squared =
[](cg::Location l1, cg::Location l2) {
auto distance_squared = [](cg::Location l1, cg::Location l2) {
return cg::Math::DistanceSquared(l1, l2);
};
auto square = [](float input) {return std::pow(input, 2);};
@ -284,17 +284,19 @@ namespace traffic_manager {
// Placing intra-segment connections.
cg::Location grid_edge_location = segment_waypoints.front()->GetLocation();
for (std::size_t i = 0; i < segment_waypoints.size() - 1; ++i) {
SimpleWaypointPtr current_waypoint = segment_waypoints.at(i);
SimpleWaypointPtr next_waypoint = segment_waypoints.at(i+1);
// Assigning grid id.
if (distance_squared(grid_edge_location, segment_waypoints.at(i)->GetLocation()) >
if (distance_squared(grid_edge_location, current_waypoint->GetLocation()) >
square(MAX_GEODESIC_GRID_LENGTH)) {
++geodesic_grid_id_counter;
grid_edge_location = segment_waypoints.at(i)->GetLocation();
grid_edge_location = current_waypoint->GetLocation();
}
segment_waypoints.at(i)->SetGeodesicGridId(geodesic_grid_id_counter);
current_waypoint->SetGeodesicGridId(geodesic_grid_id_counter);
current_waypoint->SetNextWaypoint({next_waypoint});
next_waypoint->SetPreviousWaypoint({current_waypoint});
segment_waypoints.at(i)->SetNextWaypoint({segment_waypoints.at(i + 1)});
segment_waypoints.at(i + 1)->SetPreviousWaypoint({segment_waypoints.at(i)});
}
segment_waypoints.back()->SetGeodesicGridId(geodesic_grid_id_counter);
@ -328,14 +330,14 @@ namespace traffic_manager {
}
// Linking lane change connections.
for (auto &simple_waypoint:dense_topology) {
if (!simple_waypoint->CheckJunction()) {
FindAndLinkLaneChange(simple_waypoint);
for (auto &swp : dense_topology) {
if (!swp->CheckJunction()) {
FindAndLinkLaneChange(swp);
}
}
// Linking any unconnected segments.
for (auto &swp: dense_topology) {
for (auto &swp : dense_topology) {
if (swp->GetNextWaypoint().empty()) {
auto neighbour = swp->GetRightWaypoint();
if (!neighbour) {
@ -350,6 +352,9 @@ namespace traffic_manager {
}
}
}
// Specifying a RoadOption for each SimpleWaypoint
SetUpRoadOption();
}
void InMemoryMap::SetUpSpatialTree() {
@ -362,6 +367,95 @@ namespace traffic_manager {
}
}
void InMemoryMap::SetUpRoadOption() {
for (auto &swp : dense_topology) {
std::vector<SimpleWaypointPtr> next_waypoints = swp->GetNextWaypoint();
std::size_t next_swp_size = next_waypoints.size();
if (next_swp_size == 0) {
// No next waypoint means that this is an end of the road.
swp->SetRoadOption(RoadOption::RoadEnd);
}
else if (next_swp_size > 1 || (!swp->CheckJunction() && next_waypoints.front()->CheckJunction())) {
// To check if we are in an actual junction, and not on an highway, we try to see
// if there's a landmark nearby of type Traffic Light, Stop Sign or Yield Sign.
bool found_landmark= false;
if (next_swp_size <= 1) {
auto all_landmarks = swp->GetWaypoint()->GetAllLandmarksInDistance(15.0);
if (all_landmarks.empty()) {
// Landmark hasn't been found, this isn't a junction.
swp->SetRoadOption(RoadOption::LaneFollow);
} else {
for (auto &landmark : all_landmarks) {
auto landmark_type = landmark->GetType();
if (landmark_type == "1000001" || landmark_type == "206" || landmark_type == "205") {
// We found a landmark.
found_landmark= true;
break;
}
}
if (!found_landmark) {
swp->SetRoadOption(RoadOption::LaneFollow);
}
}
}
// If we did find a landmark, or we are in the other case, find all waypoints
// in the junction and assign the correct RoadOption.
if (found_landmark || next_swp_size > 1) {
swp->SetRoadOption(RoadOption::LaneFollow);
for (auto &next_swp : next_waypoints) {
std::vector<SimpleWaypointPtr> traversed_waypoints;
SimpleWaypointPtr junction_end_waypoint;
if (next_swp_size > 1) {
junction_end_waypoint = next_swp;
} else {
junction_end_waypoint = next_waypoints.front();
}
while (junction_end_waypoint->CheckJunction()){
traversed_waypoints.push_back(junction_end_waypoint);
std::vector<SimpleWaypointPtr> temp = junction_end_waypoint->GetNextWaypoint();
if (temp.empty()) {
break;
}
junction_end_waypoint = temp.front();
}
// Calculate the angle between the first and the last point of the junction.
int16_t current_angle = static_cast<int16_t>(traversed_waypoints.front()->GetTransform().rotation.yaw);
int16_t junction_end_angle = static_cast<int16_t>(traversed_waypoints.back()->GetTransform().rotation.yaw);
int16_t diff_angle = (junction_end_angle - current_angle) % 360;
bool straight = (diff_angle < STRAIGHT_DEG && diff_angle > -STRAIGHT_DEG) ||
(diff_angle > 360-STRAIGHT_DEG && diff_angle <= 360) ||
(diff_angle < -360+STRAIGHT_DEG && diff_angle >= -360);
bool right = (diff_angle >= STRAIGHT_DEG && diff_angle <= 180) ||
(diff_angle <= -180 && diff_angle >= -360+STRAIGHT_DEG);
auto assign_option = [](RoadOption ro, std::vector<SimpleWaypointPtr> traversed_waypoints) {
for (auto &twp : traversed_waypoints) {
twp->SetRoadOption(ro);
}
};
// Assign RoadOption according to the angle.
if (straight) assign_option(RoadOption::Straight, traversed_waypoints);
else if (right) assign_option(RoadOption::Right, traversed_waypoints);
else assign_option(RoadOption::Left, traversed_waypoints);
}
}
}
else if (next_swp_size == 1 && swp->GetRoadOption() == RoadOption::Void) {
swp->SetRoadOption(RoadOption::LaneFollow);
}
}
}
SimpleWaypointPtr InMemoryMap::GetWaypoint(const cg::Location loc) const {
Point3D query_point(loc.x, loc.y, loc.z);

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

@ -75,8 +75,7 @@ namespace bgi = boost::geometry::index;
//bool Load(const std::string& filename);
bool Load(const std::vector<uint8_t>& content);
/// This method constructs the local map with a resolution of
/// sampling_resolution.
/// This method constructs the local map with a resolution of sampling_resolution.
void SetUp();
/// This method returns the closest waypoint to a given location on the map.
@ -85,8 +84,7 @@ namespace bgi = boost::geometry::index;
/// This method returns n waypoints in an delta area with a certain distance from the ego vehicle.
NodeList GetWaypointsInDelta(const cg::Location loc, const uint16_t n_points, const float random_sample) const;
/// This method returns the full list of discrete samples of the map in the
/// local cache.
/// This method returns the full list of discrete samples of the map in the local cache.
NodeList GetDenseTopology() const;
std::string GetMapName();
@ -98,6 +96,7 @@ namespace bgi = boost::geometry::index;
void SetUpDenseTopology();
void SetUpSpatialTree();
void SetUpRoadOption();
/// This method is used to find and place lane change links.
void FindAndLinkLaneChange(SimpleWaypointPtr reference_waypoint);

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

@ -10,6 +10,8 @@ using namespace constants::PathBufferUpdate;
using namespace constants::LaneChange;
using namespace constants::WaypointSelection;
using namespace constants::SpeedThreshold;
using namespace constants::Collision;
using namespace constants::MotionPlan;
LocalizationStage::LocalizationStage(
const std::vector<ActorId> &vehicle_id_list,
@ -29,7 +31,7 @@ LocalizationStage::LocalizationStage(
parameters(parameters),
marked_for_removal(marked_for_removal),
output_array(output_array),
random_devices(random_devices) {}
random_devices(random_devices){}
void LocalizationStage::Update(const unsigned long index) {
@ -93,10 +95,11 @@ void LocalizationStage::Update(const unsigned long index) {
}
}
// Purge waypoints too far from the front of the buffer.
// Purge waypoints too far from the front of the buffer, but not if it has reached a junction.
while (!is_at_junction_entrance
&& !waypoint_buffer.empty()
&& waypoint_buffer.back()->DistanceSquared(waypoint_buffer.front()) > horizon_square) {
&& waypoint_buffer.back()->DistanceSquared(waypoint_buffer.front()) > horizon_square + horizon_square
&& !waypoint_buffer.back()->CheckJunction()) {
PopWaypoint(actor_id, track_traffic, waypoint_buffer, false);
}
}
@ -112,21 +115,38 @@ void LocalizationStage::Update(const unsigned long index) {
bool force_lane_change = lane_change_info.change_lane;
bool lane_change_direction = lane_change_info.direction;
if (!force_lane_change) {
float perc_keep_right = parameters.GetKeepRightPercentage(actor_id);
if (perc_keep_right >= 0.0f && perc_keep_right >= random_devices.at(actor_id).next()) {
// Apply parameters for keep right rule and random lane changes.
if (!force_lane_change && vehicle_speed > MIN_LANE_CHANGE_SPEED){
const float perc_keep_right = parameters.GetKeepRightPercentage(actor_id);
const float perc_random_leftlanechange = parameters.GetRandomLeftLaneChangePercentage(actor_id);
const float perc_random_rightlanechange = parameters.GetRandomRightLaneChangePercentage(actor_id);
const bool is_keep_right = perc_keep_right > random_devices.at(actor_id).next();
const bool is_random_left_change = perc_random_leftlanechange >= random_devices.at(actor_id).next();
const bool is_random_right_change = perc_random_rightlanechange >= random_devices.at(actor_id).next();
// Determine which of the parameters we should apply.
if (is_keep_right || is_random_right_change) {
force_lane_change = true;
lane_change_direction = true;
}
if (is_random_left_change) {
if (!force_lane_change) {
force_lane_change = true;
lane_change_direction = false;
} else {
// Both a left and right lane changes are forced. Choose between one of them.
lane_change_direction = FIFTYPERC > random_devices.at(actor_id).next();
}
}
}
const SimpleWaypointPtr front_waypoint = waypoint_buffer.front();
const float lane_change_distance = SQUARE(std::max(10.0f * vehicle_speed, INTER_LANE_CHANGE_DISTANCE));
bool recently_not_executed_lane_change = last_lane_change_location.find(actor_id) == last_lane_change_location.end();
bool recently_not_executed_lane_change = last_lane_change_swpt.find(actor_id) == last_lane_change_swpt.end();
bool done_with_previous_lane_change = true;
if (!recently_not_executed_lane_change) {
float distance_frm_previous = cg::Math::DistanceSquared(last_lane_change_location.at(actor_id), vehicle_location);
float distance_frm_previous = cg::Math::DistanceSquared(last_lane_change_swpt.at(actor_id)->GetLocation(), vehicle_location);
done_with_previous_lane_change = distance_frm_previous > lane_change_distance;
}
bool auto_or_force_lane_change = parameters.GetAutoLaneChange(actor_id) || force_lane_change;
@ -140,10 +160,10 @@ void LocalizationStage::Update(const unsigned long index) {
force_lane_change, lane_change_direction);
if (change_over_point != nullptr) {
if (last_lane_change_location.find(actor_id) != last_lane_change_location.end()) {
last_lane_change_location.at(actor_id) = vehicle_location;
if (last_lane_change_swpt.find(actor_id) != last_lane_change_swpt.end()) {
last_lane_change_swpt.at(actor_id) = change_over_point;
} else {
last_lane_change_location.insert({actor_id, vehicle_location});
last_lane_change_swpt.insert({actor_id, change_over_point});
}
auto number_of_pops = waypoint_buffer.size();
for (uint64_t j = 0u; j < number_of_pops; ++j) {
@ -153,27 +173,40 @@ void LocalizationStage::Update(const unsigned long index) {
}
}
// Populating the buffer.
while (waypoint_buffer.back()->DistanceSquared(waypoint_buffer.front()) <= horizon_square) {
Path imported_path = parameters.GetCustomPath(actor_id);
Route imported_actions = parameters.GetImportedRoute(actor_id);
// We are effectively importing a path.
if (!imported_path.empty()) {
ImportPath(imported_path, waypoint_buffer, actor_id, horizon_square);
} else if (!imported_actions.empty()) {
ImportRoute(imported_actions, waypoint_buffer, actor_id, horizon_square);
SimpleWaypointPtr furthest_waypoint = waypoint_buffer.back();
std::vector<SimpleWaypointPtr> next_waypoints = furthest_waypoint->GetNextWaypoint();
uint64_t selection_index = 0u;
// Pseudo-randomized path selection if found more than one choice.
if (next_waypoints.size() > 1) {
double r_sample = random_devices.at(actor_id).next();
selection_index = static_cast<uint64_t>(r_sample*next_waypoints.size()*0.01);
} else if (next_waypoints.size() == 0) {
if (!parameters.GetOSMMode()) {
std::cout << "This map has dead-end roads, please change the set_open_street_map parameter to true" << std::endl;
}
marked_for_removal.push_back(actor_id);
break;
}
SimpleWaypointPtr next_wp_selection = next_waypoints.at(selection_index);
PushWaypoint(actor_id, track_traffic, waypoint_buffer, next_wp_selection);
}
// Populating the buffer through randomly chosen waypoints.
else {
while (waypoint_buffer.back()->DistanceSquared(waypoint_buffer.front()) <= horizon_square) {
SimpleWaypointPtr furthest_waypoint = waypoint_buffer.back();
std::vector<SimpleWaypointPtr> next_waypoints = furthest_waypoint->GetNextWaypoint();
uint64_t selection_index = 0u;
// Pseudo-randomized path selection if found more than one choice.
if (next_waypoints.size() > 1) {
double r_sample = random_devices.at(actor_id).next();
selection_index = static_cast<uint64_t>(r_sample*next_waypoints.size()*0.01);
} else if (next_waypoints.size() == 0) {
if (!parameters.GetOSMMode()) {
std::cout << "This map has dead-end roads, please change the set_open_street_map parameter to true" << std::endl;
}
marked_for_removal.push_back(actor_id);
break;
}
SimpleWaypointPtr next_wp_selection = next_waypoints.at(selection_index);
PushWaypoint(actor_id, track_traffic, waypoint_buffer, next_wp_selection);
}
}
ExtendAndFindSafeSpace(actor_id, is_at_junction_entrance, waypoint_buffer);
// Editing output array
@ -282,12 +315,12 @@ void LocalizationStage::ExtendAndFindSafeSpace(const ActorId actor_id,
}
void LocalizationStage::RemoveActor(ActorId actor_id) {
last_lane_change_location.erase(actor_id);
last_lane_change_swpt.erase(actor_id);
vehicles_at_junction.erase(actor_id);
}
void LocalizationStage::Reset() {
last_lane_change_location.clear();
last_lane_change_swpt.clear();
vehicles_at_junction.clear();
}
@ -400,11 +433,11 @@ SimpleWaypointPtr LocalizationStage::AssignLaneChange(const ActorId actor_id,
}
if (change_over_point != nullptr) {
const float change_over_distance = cg::Math::Clamp(1.5f * vehicle_speed, 3.0f, 20.0f);
const auto starting_point = change_over_point;
const float change_over_distance = cg::Math::Clamp(1.5f * vehicle_speed, MIN_WPT_DISTANCE, MAX_WPT_DISTANCE);
const SimpleWaypointPtr starting_point = change_over_point;
while (change_over_point->DistanceSquared(starting_point) < SQUARE(change_over_distance) &&
!change_over_point->CheckJunction()) {
change_over_point = change_over_point->GetNextWaypoint()[0];
change_over_point = change_over_point->GetNextWaypoint().front();
}
}
}
@ -412,5 +445,215 @@ SimpleWaypointPtr LocalizationStage::AssignLaneChange(const ActorId actor_id,
return change_over_point;
}
void LocalizationStage::ImportPath(Path &imported_path, Buffer &waypoint_buffer, const ActorId actor_id, const float horizon_square) {
// Remove the waypoints already added to the path, except for the first.
if (parameters.GetUploadPath(actor_id)) {
auto number_of_pops = waypoint_buffer.size();
for (uint64_t j = 0u; j < number_of_pops - 1; ++j) {
PopWaypoint(actor_id, track_traffic, waypoint_buffer, false);
}
// We have successfully imported the path. Remove it from the list of paths to be imported.
parameters.RemoveUploadPath(actor_id, false);
}
// Get the latest imported waypoint. and find its closest waypoint in TM's InMemoryMap.
cg::Location latest_imported = imported_path.front();
SimpleWaypointPtr imported = local_map->GetWaypoint(latest_imported);
// We need to generate a path compatible with TM's waypoints.
while (!imported_path.empty() && waypoint_buffer.back()->DistanceSquared(waypoint_buffer.front()) <= horizon_square) {
// Get the latest point we added to the list. If starting, this will be the one referred to the vehicle's location.
SimpleWaypointPtr latest_waypoint = waypoint_buffer.back();
// Try to link the latest_waypoint to the imported waypoint.
std::vector<SimpleWaypointPtr> next_waypoints = latest_waypoint->GetNextWaypoint();
uint64_t selection_index = 0u;
// Choose correct path.
if (next_waypoints.size() > 1) {
const float imported_road_id = imported->GetWaypoint()->GetRoadId();
float min_distance = std::numeric_limits<float>::infinity();
for (uint64_t k = 0u; k < next_waypoints.size(); ++k) {
SimpleWaypointPtr junction_end_point = next_waypoints.at(k);
while (!junction_end_point->CheckJunction()) {
junction_end_point = junction_end_point->GetNextWaypoint().front();
}
while (junction_end_point->CheckJunction()) {
junction_end_point = junction_end_point->GetNextWaypoint().front();
}
while (next_waypoints.at(k)->DistanceSquared(junction_end_point) < 50.0f) {
junction_end_point = junction_end_point->GetNextWaypoint().front();
}
float jep_road_id = junction_end_point->GetWaypoint()->GetRoadId();
if (jep_road_id == imported_road_id) {
selection_index = k;
break;
}
float distance = junction_end_point->DistanceSquared(imported);
if (distance < min_distance) {
min_distance = distance;
selection_index = k;
}
}
} else if (next_waypoints.size() == 0) {
if (!parameters.GetOSMMode()) {
std::cout << "This map has dead-end roads, please change the set_open_street_map parameter to true" << std::endl;
}
marked_for_removal.push_back(actor_id);
break;
}
SimpleWaypointPtr next_wp_selection = next_waypoints.at(selection_index);
PushWaypoint(actor_id, track_traffic, waypoint_buffer, next_wp_selection);
// Remove the imported waypoint from the path if it's close to the last one.
if (next_wp_selection->DistanceSquared(imported) < 30.0f) {
imported_path.erase(imported_path.begin());
PushWaypoint(actor_id, track_traffic, waypoint_buffer, imported);
latest_imported = imported_path.front();
imported = local_map->GetWaypoint(latest_imported);
}
}
if (imported_path.empty()) {
// Once we are done, check if we can clear the structure.
parameters.RemoveUploadPath(actor_id, true);
} else {
// Otherwise, update the structure with the waypoints that we still need to import.
parameters.UpdateUploadPath(actor_id, imported_path);
}
}
void LocalizationStage::ImportRoute(Route &imported_actions, Buffer &waypoint_buffer, const ActorId actor_id, const float horizon_square) {
if (parameters.GetUploadRoute(actor_id)) {
auto number_of_pops = waypoint_buffer.size();
for (uint64_t j = 0u; j < number_of_pops - 1; ++j) {
PopWaypoint(actor_id, track_traffic, waypoint_buffer, false);
}
// We have successfully imported the route. Remove it from the list of routes to be imported.
parameters.RemoveImportedRoute(actor_id, false);
}
RoadOption next_road_option = static_cast<RoadOption>(imported_actions.front());
while (!imported_actions.empty() && waypoint_buffer.back()->DistanceSquared(waypoint_buffer.front()) <= horizon_square) {
// Get the latest point we added to the list. If starting, this will be the one referred to the vehicle's location.
SimpleWaypointPtr latest_waypoint = waypoint_buffer.back();
RoadOption latest_road_option = latest_waypoint->GetRoadOption();
// Try to link the latest_waypoint to the correct next RouteOption.
std::vector<SimpleWaypointPtr> next_waypoints = latest_waypoint->GetNextWaypoint();
uint16_t selection_index = 0u;
if (next_waypoints.size() > 1) {
for (uint16_t i=0; i<next_waypoints.size(); ++i) {
if (next_waypoints.at(i)->GetRoadOption() == next_road_option) {
selection_index = i;
break;
} else {
if (i == next_waypoints.size() - 1) {
std::cout << "We couldn't find the RoadOption you were looking for. This route might diverge from the one expected." << std::endl;
}
}
}
} else if (next_waypoints.size() == 0) {
if (!parameters.GetOSMMode()) {
std::cout << "This map has dead-end roads, please change the set_open_street_map parameter to true" << std::endl;
}
marked_for_removal.push_back(actor_id);
break;
}
SimpleWaypointPtr next_wp_selection = next_waypoints.at(selection_index);
PushWaypoint(actor_id, track_traffic, waypoint_buffer, next_wp_selection);
// If we are switching to a new RoadOption, it means the current one is already fully imported.
if (latest_road_option != next_wp_selection->GetRoadOption() && next_road_option == next_wp_selection->GetRoadOption()) {
imported_actions.erase(imported_actions.begin());
next_road_option = static_cast<RoadOption>(imported_actions.front());
}
}
if (imported_actions.empty()) {
// Once we are done, check if we can clear the structure.
parameters.RemoveImportedRoute(actor_id, true);
} else {
// Otherwise, update the structure with the waypoints that we still need to import.
parameters.UpdateImportedRoute(actor_id, imported_actions);
}
}
Action LocalizationStage::ComputeNextAction(const ActorId& actor_id) {
auto waypoint_buffer = buffer_map.at(actor_id);
auto next_action = std::make_pair(RoadOption::LaneFollow, waypoint_buffer.back()->GetWaypoint());
bool is_lane_change = false;
if (last_lane_change_swpt.find(actor_id) != last_lane_change_swpt.end()) {
// A lane change is happening.
is_lane_change = true;
const cg::Vector3D heading_vector = simulation_state.GetHeading(actor_id);
const cg::Vector3D relative_vector = simulation_state.GetLocation(actor_id) - last_lane_change_swpt.at(actor_id)->GetLocation();
bool left_heading = (heading_vector.x * relative_vector.y - heading_vector.y * relative_vector.x) > 0.0f;
if (left_heading) next_action = std::make_pair(RoadOption::ChangeLaneLeft, last_lane_change_swpt.at(actor_id)->GetWaypoint());
else next_action = std::make_pair(RoadOption::ChangeLaneRight, last_lane_change_swpt.at(actor_id)->GetWaypoint());
}
for (auto &swpt : waypoint_buffer) {
RoadOption road_opt = swpt->GetRoadOption();
if (road_opt != RoadOption::LaneFollow) {
if (!is_lane_change) {
// No lane change in sight, we can assume this will be the next action.
return std::make_pair(road_opt, swpt->GetWaypoint());
} else {
// A lane change will happen as well as another action, we need to figure out which one will happen first.
cg::Location lane_change = last_lane_change_swpt.at(actor_id)->GetLocation();
cg::Location actual_location = simulation_state.GetLocation(actor_id);
auto distance_lane_change = cg::Math::DistanceSquared(actual_location, lane_change);
auto distance_other_action = cg::Math::DistanceSquared(actual_location, swpt->GetLocation());
if (distance_lane_change < distance_other_action) return next_action;
else return std::make_pair(road_opt, swpt->GetWaypoint());
}
}
}
return next_action;
}
ActionBuffer LocalizationStage::ComputeActionBuffer(const ActorId& actor_id) {
auto waypoint_buffer = buffer_map.at(actor_id);
ActionBuffer action_buffer;
Action lane_change;
bool is_lane_change = false;
SimpleWaypointPtr buffer_front = waypoint_buffer.front();
RoadOption last_road_opt = buffer_front->GetRoadOption();
action_buffer.push_back(std::make_pair(last_road_opt, buffer_front->GetWaypoint()));
if (last_lane_change_swpt.find(actor_id) != last_lane_change_swpt.end()) {
// A lane change is happening.
is_lane_change = true;
const cg::Vector3D heading_vector = simulation_state.GetHeading(actor_id);
const cg::Vector3D relative_vector = simulation_state.GetLocation(actor_id) - last_lane_change_swpt.at(actor_id)->GetLocation();
bool left_heading = (heading_vector.x * relative_vector.y - heading_vector.y * relative_vector.x) > 0.0f;
if (left_heading) lane_change = std::make_pair(RoadOption::ChangeLaneLeft, last_lane_change_swpt.at(actor_id)->GetWaypoint());
else lane_change = std::make_pair(RoadOption::ChangeLaneRight, last_lane_change_swpt.at(actor_id)->GetWaypoint());
}
for (auto &wpt : waypoint_buffer) {
RoadOption current_road_opt = wpt->GetRoadOption();
if (current_road_opt != last_road_opt) {
action_buffer.push_back(std::make_pair(current_road_opt, wpt->GetWaypoint()));
last_road_opt = current_road_opt;
}
}
if (is_lane_change) {
// Insert the lane change action in the appropriate part of the action buffer.
auto distance_lane_change = cg::Math::DistanceSquared(waypoint_buffer.front()->GetLocation(), lane_change.second->GetTransform().location);
for (uint16_t i = 0; i < action_buffer.size(); ++i) {
auto distance_action = cg::Math::DistanceSquared(waypoint_buffer.front()->GetLocation(), waypoint_buffer.at(i)->GetLocation());
// If the waypoint related to the next action is further away from the one of the lane change, insert lane change action here.
// If we reached the end of the buffer, place the action at the end.
if (i == action_buffer.size()-1) {
action_buffer.push_back(lane_change);
break;
} else if (distance_action > distance_lane_change) {
action_buffer.insert(action_buffer.begin()+i, lane_change);
break;
}
}
}
return action_buffer;
}
} // namespace traffic_manager
} // namespace carla

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

@ -18,7 +18,12 @@ namespace traffic_manager {
namespace cc = carla::client;
using LocalMapPtr = std::shared_ptr<InMemoryMap>;
using LaneChangeLocationMap = std::unordered_map<ActorId, cg::Location>;
using LaneChangeSWptMap = std::unordered_map<ActorId, SimpleWaypointPtr>;
using WaypointPtr = carla::SharedPtr<cc::Waypoint>;
using Action = std::pair<RoadOption, WaypointPtr>;
using ActionBuffer = std::vector<Action>;
using Path = std::vector<cg::Location>;
using Route = std::vector<uint8_t>;
/// This class has functionality to maintain a horizon of waypoints ahead
/// of the vehicle for it to follow.
@ -35,7 +40,7 @@ private:
// Array of vehicles marked by stages for removal.
std::vector<ActorId>& marked_for_removal;
LocalizationFrame &output_array;
LaneChangeLocationMap last_lane_change_location;
LaneChangeSWptMap last_lane_change_swpt;
ActorIdSet vehicles_at_junction;
using SimpleWaypointPair = std::pair<SimpleWaypointPtr, SimpleWaypointPtr>;
std::unordered_map<ActorId, SimpleWaypointPair> vehicles_at_junction_entrance;
@ -46,12 +51,20 @@ private:
const float vehicle_speed,
bool force, bool direction);
void DrawBuffer(Buffer &buffer);
void ExtendAndFindSafeSpace(const ActorId actor_id,
const bool is_at_junction_entrance,
Buffer &waypoint_buffer);
void ImportPath(Path &imported_path,
Buffer &waypoint_buffer,
const ActorId actor_id,
const float horizon_square);
void ImportRoute(Route &imported_actions,
Buffer &waypoint_buffer,
const ActorId actor_id,
const float horizon_square);
public:
LocalizationStage(const std::vector<ActorId> &vehicle_id_list,
BufferMap &buffer_map,
@ -68,6 +81,11 @@ public:
void RemoveActor(const ActorId actor_id) override;
void Reset() override;
Action ComputeNextAction(const ActorId &actor_id);
ActionBuffer ComputeActionBuffer(const ActorId& actor_id);
};
} // namespace traffic_manager

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

@ -61,7 +61,7 @@ TargetWPInfo GetTargetWaypoint(const Buffer &waypoint_buffer, const float &targe
SimpleWaypointPtr target_waypoint = waypoint_buffer.front();
const SimpleWaypointPtr &buffer_front = waypoint_buffer.front();
uint64_t startPosn = static_cast<uint64_t>(std::fabs(target_point_distance * INV_MAP_RESOLUTION));
uint64_t index = 0;
uint64_t index = startPosn;
/// Condition to determine forward or backward scanning of waypoint buffer.
if (startPosn < waypoint_buffer.size()) {

2
LibCarla/source/carla/trafficmanager/MotionPlanStage.cpp
View File

@ -157,7 +157,7 @@ void MotionPlanStage::Update(const unsigned long index) {
ActuationSignal actuation_signal{0.0f, 0.0f, 0.0f};
const float target_point_distance = std::max(vehicle_speed * TARGET_WAYPOINT_TIME_HORIZON,
TARGET_WAYPOINT_HORIZON_LENGTH);
MIN_TARGET_WAYPOINT_DISTANCE);
const SimpleWaypointPtr &target_waypoint = GetTargetWaypoint(waypoint_buffer, target_point_distance).first;
const cg::Location target_location = target_waypoint->GetLocation();
float dot_product = DeviationDotProduct(vehicle_location, vehicle_heading, target_location);

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

@ -90,7 +90,21 @@ void Parameters::SetKeepRightPercentage(const ActorPtr &actor, const float perce
perc_keep_right.AddEntry(entry);
}
void Parameters::SetRandomLeftLaneChangePercentage(const ActorPtr &actor, const float percentage) {
const auto entry = std::make_pair(actor->GetId(), percentage);
perc_random_left.AddEntry(entry);
}
void Parameters::SetRandomRightLaneChangePercentage(const ActorPtr &actor, const float percentage) {
const auto entry = std::make_pair(actor->GetId(), percentage);
perc_random_right.AddEntry(entry);
}
void Parameters::SetUpdateVehicleLightState(const ActorPtr &actor, const bool do_update) {
const auto entry = std::make_pair(actor->GetId(), do_update);
auto_update_vehicle_lights.AddEntry(entry);
}
@ -158,6 +172,48 @@ void Parameters::SetOSMMode(const bool mode_switch) {
osm_mode.store(mode_switch);
}
void Parameters::SetCustomPath(const ActorPtr &actor, const Path path, const bool empty_buffer) {
const auto entry = std::make_pair(actor->GetId(), path);
custom_path.AddEntry(entry);
const auto entry2 = std::make_pair(actor->GetId(), empty_buffer);
upload_path.AddEntry(entry2);
}
void Parameters::RemoveUploadPath(const ActorId &actor_id, const bool remove_path) {
if (!remove_path) {
upload_path.RemoveEntry(actor_id);
} else {
custom_path.RemoveEntry(actor_id);
}
}
void Parameters::UpdateUploadPath(const ActorId &actor_id, const Path path) {
custom_path.RemoveEntry(actor_id);
const auto entry = std::make_pair(actor_id, path);
custom_path.AddEntry(entry);
}
void Parameters::SetImportedRoute(const ActorPtr &actor, const Route route, const bool empty_buffer) {
const auto entry = std::make_pair(actor->GetId(), route);
custom_route.AddEntry(entry);
const auto entry2 = std::make_pair(actor->GetId(), empty_buffer);
upload_route.AddEntry(entry2);
}
void Parameters::RemoveImportedRoute(const ActorId &actor_id, const bool remove_path) {
if (!remove_path) {
upload_route.RemoveEntry(actor_id);
} else {
custom_route.RemoveEntry(actor_id);
}
}
void Parameters::UpdateImportedRoute(const ActorId &actor_id, const Route route) {
custom_route.RemoveEntry(actor_id);
const auto entry = std::make_pair(actor_id, route);
custom_route.AddEntry(entry);
}
//////////////////////////////////// GETTERS //////////////////////////////////
float Parameters::GetHybridPhysicsRadius() const {
@ -217,7 +273,27 @@ float Parameters::GetKeepRightPercentage(const ActorId &actor_id) {
percentage = perc_keep_right.GetValue(actor_id);
}
perc_keep_right.RemoveEntry(actor_id);
return percentage;
}
float Parameters::GetRandomLeftLaneChangePercentage(const ActorId &actor_id) {
float percentage = -1.0f;
if (perc_random_left.Contains(actor_id)) {
percentage = perc_random_left.GetValue(actor_id);
}
return percentage;
}
float Parameters::GetRandomRightLaneChangePercentage(const ActorId &actor_id) {
float percentage = -1.0f;
if (perc_random_right.Contains(actor_id)) {
percentage = perc_random_right.GetValue(actor_id);
}
return percentage;
}
@ -325,5 +401,51 @@ bool Parameters::GetOSMMode() const {
return osm_mode.load();
}
bool Parameters::GetUploadPath(const ActorId &actor_id) const {
bool custom_path_bool = false;
if (upload_path.Contains(actor_id)) {
custom_path_bool = upload_path.GetValue(actor_id);
}
return custom_path_bool;
}
Path Parameters::GetCustomPath(const ActorId &actor_id) const {
Path custom_path_import;
if (custom_path.Contains(actor_id)) {
custom_path_import = custom_path.GetValue(actor_id);
}
return custom_path_import;
}
bool Parameters::GetUploadRoute(const ActorId &actor_id) const {
bool custom_route_bool = false;
if (upload_route.Contains(actor_id)) {
custom_route_bool = upload_route.GetValue(actor_id);
}
return custom_route_bool;
}
Route Parameters::GetImportedRoute(const ActorId &actor_id) const {
Route custom_route_import;
if (custom_route.Contains(actor_id)) {
custom_route_import = custom_route.GetValue(actor_id);
}
return custom_route_import;
}
} // namespace traffic_manager
} // namespace carla

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

@ -9,6 +9,7 @@
#include <atomic>
#include <chrono>
#include <random>
#include <unordered_map>
#include "carla/client/Actor.h"
#include "carla/client/Vehicle.h"
@ -25,6 +26,8 @@ namespace cc = carla::client;
namespace cg = carla::geom;
using ActorPtr = carla::SharedPtr<cc::Actor>;
using ActorId = carla::ActorId;
using Path = std::vector<cg::Location>;
using Route = std::vector<uint8_t>;
struct ChangeLaneInfo {
bool change_lane = false;
@ -56,6 +59,10 @@ private:
AtomicMap<ActorId, float> perc_ignore_vehicles;
/// Map containing % of keep right rule.
AtomicMap<ActorId, float> perc_keep_right;
/// Map containing % of random left lane change.
AtomicMap<ActorId, float> perc_random_left;
/// Map containing % of random right lane change.
AtomicMap<ActorId, float> perc_random_right;
/// Map containing the automatic vehicle lights update flag
AtomicMap<ActorId, bool> auto_update_vehicle_lights;
/// Synchronous mode switch.
@ -78,6 +85,14 @@ private:
std::atomic<float> hybrid_physics_radius {70.0};
/// Parameter specifying Open Street Map mode.
std::atomic<bool> osm_mode {true};
/// Parameter specifying if importing a custom path.
AtomicMap<ActorId, bool> upload_path;
/// Structure to hold all custom paths.
AtomicMap<ActorId, Path> custom_path;
/// Parameter specifying if importing a custom route.
AtomicMap<ActorId, bool> upload_route;
/// Structure to hold all custom routes.
AtomicMap<ActorId, Route> custom_route;
public:
Parameters();
@ -122,9 +137,15 @@ public:
/// Method to set % to ignore any vehicle.
void SetPercentageIgnoreWalkers(const ActorPtr &actor, const float perc);
/// Method to set probabilistic preference to keep on the right lane.
/// Method to set % to keep on the right lane.
void SetKeepRightPercentage(const ActorPtr &actor, const float percentage);
/// Method to set % to randomly do a left lane change.
void SetRandomLeftLaneChangePercentage(const ActorPtr &actor, const float percentage);
/// Method to set % to randomly do a right lane change.
void SetRandomRightLaneChangePercentage(const ActorPtr &actor, const float percentage);
/// Method to set the automatic vehicle light state update flag.
void SetUpdateVehicleLightState(const ActorPtr &actor, const bool do_update);
@ -155,6 +176,24 @@ public:
/// Method to set limits for boundaries when respawning vehicles.
void SetMaxBoundaries(const float lower, const float upper);
/// Method to set our own imported path.
void SetCustomPath(const ActorPtr &actor, const Path path, const bool empty_buffer);
/// Method to remove a list of points.
void RemoveUploadPath(const ActorId &actor_id, const bool remove_path);
/// Method to update an already set list of points.
void UpdateUploadPath(const ActorId &actor_id, const Path path);
/// Method to set our own imported route.
void SetImportedRoute(const ActorPtr &actor, const Route route, const bool empty_buffer);
/// Method to remove a route.
void RemoveImportedRoute(const ActorId &actor_id, const bool remove_path);
/// Method to update an already set route.
void UpdateImportedRoute(const ActorId &actor_id, const Route route);
///////////////////////////////// GETTERS /////////////////////////////////////
/// Method to retrieve hybrid physics radius.
@ -172,6 +211,12 @@ public:
/// Method to query percentage probability of keep right rule for a vehicle.
float GetKeepRightPercentage(const ActorId &actor_id);
/// Method to query percentage probability of a random right lane change for a vehicle.
float GetRandomLeftLaneChangePercentage(const ActorId &actor_id);
/// Method to query percentage probability of a random left lane change for a vehicle.
float GetRandomRightLaneChangePercentage(const ActorId &actor_id);
/// Method to query auto lane change rule for a vehicle.
bool GetAutoLaneChange(const ActorId &actor_id) const;
@ -214,6 +259,18 @@ public:
/// Method to get Open Street Map mode.
bool GetOSMMode() const;
/// Method to get if we are uploading a path.
bool GetUploadPath(const ActorId &actor_id) const;
/// Method to get a custom path.
Path GetCustomPath(const ActorId &actor_id) const;
/// Method to get if we are uploading a route.
bool GetUploadRoute(const ActorId &actor_id) const;
/// Method to get a custom route.
Route GetImportedRoute(const ActorId &actor_id) const;
/// Synchronous mode time out variable.
std::chrono::duration<double, std::milli> synchronous_time_out;
};

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

@ -134,5 +134,13 @@ namespace traffic_manager {
return waypoint->GetTransform();
}
void SimpleWaypoint::SetRoadOption(RoadOption _road_option) {
road_option = _road_option;
}
RoadOption SimpleWaypoint::GetRoadOption() {
return road_option;
}
} // namespace traffic_manager
} // namespace carla

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

@ -22,6 +22,16 @@ namespace traffic_manager {
namespace cg = carla::geom;
using WaypointPtr = carla::SharedPtr<cc::Waypoint>;
using GeoGridId = carla::road::JuncId;
enum class RoadOption : uint8_t {
Void = 0,
Left = 1,
Right = 2,
Straight = 3,
LaneFollow = 4,
ChangeLaneLeft = 5,
ChangeLaneRight = 6,
RoadEnd = 7
};
/// This is a simple wrapper class on Carla's waypoint object.
/// The class is used to represent discrete samples of the world map.
@ -41,6 +51,8 @@ namespace traffic_manager {
SimpleWaypointPtr next_left_waypoint;
/// Pointer to right lane change waypoint.
SimpleWaypointPtr next_right_waypoint;
/// RoadOption for the actual waypoint.
RoadOption road_option = RoadOption::Void;
/// Integer placing the waypoint into a geodesic grid.
GeoGridId geodesic_grid_id = 0;
// Boolean to hold if the waypoint belongs to a junction
@ -118,6 +130,10 @@ namespace traffic_manager {
/// Return transform object for the current waypoint.
cg::Transform GetTransform() const;
// Accessor methods for road option.
void SetRoadOption(RoadOption _road_option);
RoadOption GetRoadOption();
};
} // namespace traffic_manager

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

@ -65,6 +65,54 @@ public:
}
}
/// Method to set our own imported path.
void SetCustomPath(const ActorPtr &actor, const Path path, const bool empty_buffer) {
TrafficManagerBase* tm_ptr = GetTM(_port);
if (tm_ptr != nullptr) {
tm_ptr->SetCustomPath(actor, path, empty_buffer);
}
}
/// Method to remove a path.
void RemoveUploadPath(const ActorId &actor_id, const bool remove_path) {
TrafficManagerBase* tm_ptr = GetTM(_port);
if (tm_ptr != nullptr) {
tm_ptr->RemoveUploadPath(actor_id, remove_path);
}
}
/// Method to update an already set path.
void UpdateUploadPath(const ActorId &actor_id, const Path path) {
TrafficManagerBase* tm_ptr = GetTM(_port);
if (tm_ptr != nullptr) {
tm_ptr->UpdateUploadPath(actor_id, path);
}
}
/// Method to set our own imported route.
void SetImportedRoute(const ActorPtr &actor, const Route route, const bool empty_buffer) {
TrafficManagerBase* tm_ptr = GetTM(_port);
if (tm_ptr != nullptr) {
tm_ptr->SetImportedRoute(actor, route, empty_buffer);
}
}
/// Method to remove a route.
void RemoveImportedRoute(const ActorId &actor_id, const bool remove_path) {
TrafficManagerBase* tm_ptr = GetTM(_port);
if (tm_ptr != nullptr) {
tm_ptr->RemoveImportedRoute(actor_id, remove_path);
}
}
/// Method to update an already set route.
void UpdateImportedRoute(const ActorId &actor_id, const Route route) {
TrafficManagerBase* tm_ptr = GetTM(_port);
if (tm_ptr != nullptr) {
tm_ptr->UpdateImportedRoute(actor_id, route);
}
}
/// Method to set if we are automatically respawning vehicles.
void SetRespawnDormantVehicles(const bool mode_switch) {
TrafficManagerBase* tm_ptr = GetTM(_port);
@ -245,7 +293,7 @@ public:
}
}
/// Method to set probabilistic preference to keep on the right lane.
/// Method to set % to keep on the right lane.
void SetKeepRightPercentage(const ActorPtr &actor, const float percentage) {
TrafficManagerBase* tm_ptr = GetTM(_port);
if(tm_ptr != nullptr){
@ -253,6 +301,22 @@ public:
}
}
/// Method to set % to randomly do a left lane change.
void SetRandomLeftLaneChangePercentage(const ActorPtr &actor, const float percentage) {
TrafficManagerBase* tm_ptr = GetTM(_port);
if(tm_ptr != nullptr){
tm_ptr->SetRandomLeftLaneChangePercentage(actor, percentage);
}
}
/// Method to set % to randomly do a right lane change.
void SetRandomRightLaneChangePercentage(const ActorPtr &actor, const float percentage) {
TrafficManagerBase* tm_ptr = GetTM(_port);
if(tm_ptr != nullptr){
tm_ptr->SetRandomRightLaneChangePercentage(actor, percentage);
}
}
/// Method to set randomization seed.
void SetRandomDeviceSeed(const uint64_t seed) {
TrafficManagerBase* tm_ptr = GetTM(_port);
@ -263,6 +327,28 @@ public:
void ShutDown();
/// Method to get the next action.
Action GetNextAction(const ActorId &actor_id) {
Action next_action;
TrafficManagerBase* tm_ptr = GetTM(_port);
if (tm_ptr != nullptr) {
next_action = tm_ptr->GetNextAction(actor_id);
return next_action;
}
return next_action;
}
/// Method to get the action buffer.
ActionBuffer GetActionBuffer(const ActorId &actor_id) {
ActionBuffer action_buffer;
TrafficManagerBase* tm_ptr = GetTM(_port);
if (tm_ptr != nullptr) {
action_buffer = tm_ptr->GetActionBuffer(actor_id);
return action_buffer;
}
return action_buffer;
}
private:
void CreateTrafficManagerServer(

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

@ -8,11 +8,18 @@
#include <memory>
#include "carla/client/Actor.h"
#include "carla/trafficmanager/SimpleWaypoint.h"
namespace carla {
namespace traffic_manager {
using ActorPtr = carla::SharedPtr<carla::client::Actor>;
using Path = std::vector<cg::Location>;
using Route = std::vector<uint8_t>;
using WaypointPtr = carla::SharedPtr<carla::client::Waypoint>;
using Action = std::pair<RoadOption, WaypointPtr>;
using ActionBuffer = std::vector<Action>;
/// The function of this class is to integrate all the various stages of
/// the traffic manager appropriately using messengers.
@ -95,9 +102,15 @@ public:
/// Method to set Global Distance to Leading Vehicle.
virtual void SetGlobalDistanceToLeadingVehicle(const float dist) = 0;
/// Method to set probabilistic preference to keep on the right lane.
/// Method to set % to keep on the right lane.
virtual void SetKeepRightPercentage(const ActorPtr &actor,const float percentage) = 0;
/// Method to set % to randomly do a left lane change.
virtual void SetRandomLeftLaneChangePercentage(const ActorPtr &actor, const float percentage) = 0;
/// Method to set % to randomly do a right lane change.
virtual void SetRandomRightLaneChangePercentage(const ActorPtr &actor, const float percentage) = 0;
/// Method to set hybrid physics mode.
virtual void SetHybridPhysicsMode(const bool mode_switch) = 0;
@ -110,6 +123,24 @@ public:
/// Method to set Open Street Map mode.
virtual void SetOSMMode(const bool mode_switch) = 0;
/// Method to set our own imported path.
virtual void SetCustomPath(const ActorPtr &actor, const Path path, const bool empty_buffer) = 0;
/// Method to remove a path.
virtual void RemoveUploadPath(const ActorId &actor_id, const bool remove_path) = 0;
/// Method to update an already set path.
virtual void UpdateUploadPath(const ActorId &actor_id, const Path path) = 0;
/// Method to set our own imported route.
virtual void SetImportedRoute(const ActorPtr &actor, const Route route, const bool empty_buffer) = 0;
/// Method to remove a route.
virtual void RemoveImportedRoute(const ActorId &actor_id, const bool remove_path) = 0;
/// Method to update an already set route.
virtual void UpdateImportedRoute(const ActorId &actor_id, const Route route) = 0;
/// Method to set automatic respawn of dormant vehicles.
virtual void SetRespawnDormantVehicles(const bool mode_switch) = 0;
@ -119,6 +150,12 @@ public:
/// Method to set limits for boundaries when respawning vehicles.
virtual void SetMaxBoundaries(const float lower, const float upper) = 0;
/// Method to get the vehicle's next action.
virtual Action GetNextAction(const ActorId &actor_id) = 0;
/// Method to get the vehicle's action buffer.
virtual ActionBuffer GetActionBuffer(const ActorId &actor_id) = 0;
virtual void ShutDown() = 0;
protected:

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

@ -91,7 +91,7 @@ public:
/// Method to set the automatical management of the vehicle lights
void SetUpdateVehicleLights(const carla::rpc::Actor &_actor, const bool do_update) {
DEBUG_ASSERT(_client != nullptr);
_client->call("auto_update_lights", std::move(_actor), do_update);
_client->call("update_vehicle_lights", std::move(_actor), do_update);
}
/// Method to set collision detection rules between vehicles.
@ -175,10 +175,22 @@ public:
_client->call("set_global_distance_to_leading_vehicle",distance);
}
/// Method to set probabilistic preference to keep on the right lane.
/// Method to set % to keep on the right lane.
void SetKeepRightPercentage(const carla::rpc::Actor &actor, const float percentage) {
DEBUG_ASSERT(_client != nullptr);
_client->call("set_percentage_keep_right_rule", actor, percentage);
_client->call("keep_right_rule_percentage", actor, percentage);
}
/// Method to set % to randomly do a left lane change.
void SetRandomLeftLaneChangePercentage(const carla::rpc::Actor &actor, const float percentage) {
DEBUG_ASSERT(_client != nullptr);
_client->call("random_left_lanechange_percentage", actor, percentage);
}
/// Method to set % to randomly do a right lane change.
void SetRandomRightLaneChangePercentage(const carla::rpc::Actor &actor, const float percentage) {
DEBUG_ASSERT(_client != nullptr);
_client->call("random_right_lanechange_percentage", actor, percentage);
}
/// Method to set hybrid physics mode.
@ -205,6 +217,42 @@ public:
_client->call("set_osm_mode", mode_switch);
}
/// Method to set our own imported path.
void SetCustomPath(const carla::rpc::Actor &actor, const Path path, const bool empty_buffer) {
DEBUG_ASSERT(_client != nullptr);
_client->call("set_path", actor, path, empty_buffer);
}
/// Method to remove a list of points.
void RemoveUploadPath(const ActorId &actor_id, const bool remove_path) {
DEBUG_ASSERT(_client != nullptr);
_client->call("remove_custom_path", actor_id, remove_path);
}
/// Method to update an already set list of points.
void UpdateUploadPath(const ActorId &actor_id, const Path path) {
DEBUG_ASSERT(_client != nullptr);
_client->call("update_custom_path", actor_id, path);
}
/// Method to set our own imported route.
void SetImportedRoute(const carla::rpc::Actor &actor, const Route route, const bool empty_buffer) {
DEBUG_ASSERT(_client != nullptr);
_client->call("set_imported_route", actor, route, empty_buffer);
}
/// Method to remove a route.
void RemoveImportedRoute(const ActorId &actor_id, const bool remove_path) {
DEBUG_ASSERT(_client != nullptr);
_client->call("remove_imported_route", actor_id, remove_path);
}
/// Method to update an already set list of points.
void UpdateImportedRoute(const ActorId &actor_id, const Route route) {
DEBUG_ASSERT(_client != nullptr);
_client->call("update_imported_route", actor_id, route);
}
/// Method to set automatic respawn of dormant vehicles.
void SetRespawnDormantVehicles(const bool mode_switch) {
DEBUG_ASSERT(_client != nullptr);
@ -223,6 +271,20 @@ public:
_client->call("set_max_boundaries", lower, upper);
}
/// Method to get the vehicle's next action.
Action GetNextAction(const ActorId &actor_id) {
DEBUG_ASSERT(_client != nullptr);
_client->call("get_next_action", actor_id);
return Action();
}
/// Method to get the vehicle's action buffer.
ActionBuffer GetActionBuffer(const ActorId &actor_id) {
DEBUG_ASSERT(_client != nullptr);
_client->call("get_all_actions", actor_id);
return ActionBuffer();
}
void ShutDown() {
DEBUG_ASSERT(_client != nullptr);
_client->call("shut_down");

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

@ -233,7 +233,6 @@ void TrafficManagerLocal::Run() {
collision_stage.Update(index);
}
collision_stage.ClearCycleCache();
vehicle_light_stage.ClearCycleCache();
for (unsigned long index = 0u; index < vehicle_id_list.size(); ++index) {
traffic_light_stage.Update(index);
motion_plan_stage.Update(index);
@ -396,6 +395,14 @@ void TrafficManagerLocal::SetKeepRightPercentage(const ActorPtr &actor, const fl
parameters.SetKeepRightPercentage(actor, percentage);
}
void TrafficManagerLocal::SetRandomLeftLaneChangePercentage(const ActorPtr &actor, const float percentage) {
parameters.SetRandomLeftLaneChangePercentage(actor, percentage);
}
void TrafficManagerLocal::SetRandomRightLaneChangePercentage(const ActorPtr &actor, const float percentage) {
parameters.SetRandomRightLaneChangePercentage(actor, percentage);
}
void TrafficManagerLocal::SetHybridPhysicsMode(const bool mode_switch) {
parameters.SetHybridPhysicsMode(mode_switch);
}
@ -408,6 +415,30 @@ void TrafficManagerLocal::SetOSMMode(const bool mode_switch) {
parameters.SetOSMMode(mode_switch);
}
void TrafficManagerLocal::SetCustomPath(const ActorPtr &actor, const Path path, const bool empty_buffer) {
parameters.SetCustomPath(actor, path, empty_buffer);
}
void TrafficManagerLocal::RemoveUploadPath(const ActorId &actor_id, const bool remove_path) {
parameters.RemoveUploadPath(actor_id, remove_path);
}
void TrafficManagerLocal::UpdateUploadPath(const ActorId &actor_id, const Path path) {
parameters.UpdateUploadPath(actor_id, path);
}
void TrafficManagerLocal::SetImportedRoute(const ActorPtr &actor, const Route route, const bool empty_buffer) {
parameters.SetImportedRoute(actor, route, empty_buffer);
}
void TrafficManagerLocal::RemoveImportedRoute(const ActorId &actor_id, const bool remove_path) {
parameters.RemoveImportedRoute(actor_id, remove_path);
}
void TrafficManagerLocal::UpdateImportedRoute(const ActorId &actor_id, const Route route) {
parameters.UpdateImportedRoute(actor_id, route);
}
void TrafficManagerLocal::SetRespawnDormantVehicles(const bool mode_switch) {
parameters.SetRespawnDormantVehicles(mode_switch);
}
@ -420,6 +451,14 @@ void TrafficManagerLocal::SetMaxBoundaries(const float lower, const float upper)
parameters.SetMaxBoundaries(lower, upper);
}
Action TrafficManagerLocal::GetNextAction(const ActorId &actor_id) {
return localization_stage.ComputeNextAction(actor_id);
}
ActionBuffer TrafficManagerLocal::GetActionBuffer(const ActorId &actor_id) {
return localization_stage.ComputeActionBuffer(actor_id);
}
bool TrafficManagerLocal::CheckAllFrozen(TLGroup tl_to_freeze) {
for (auto &elem : tl_to_freeze) {
if (!elem->IsFrozen() || elem->GetState() != TLS::Red) {

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

@ -214,9 +214,15 @@ public:
/// the Global leading vehicle.
void SetGlobalDistanceToLeadingVehicle(const float distance);
/// Method to set probabilistic preference to keep on the right lane.
/// Method to set % to keep on the right lane.
void SetKeepRightPercentage(const ActorPtr &actor, const float percentage);
/// Method to set % to randomly do a left lane change.
void SetRandomLeftLaneChangePercentage(const ActorPtr &actor, const float percentage);
/// Method to set % to randomly do a right lane change.
void SetRandomRightLaneChangePercentage(const ActorPtr &actor, const float percentage);
/// Method to set hybrid physics mode.
void SetHybridPhysicsMode(const bool mode_switch);
@ -229,15 +235,39 @@ public:
/// Method to set Open Street Map mode.
void SetOSMMode(const bool mode_switch);
/// Method to set our own imported path.
void SetCustomPath(const ActorPtr &actor, const Path path, const bool empty_buffer);
/// Method to remove a list of points.
void RemoveUploadPath(const ActorId &actor_id, const bool remove_path);
/// Method to update an already set list of points.
void UpdateUploadPath(const ActorId &actor_id, const Path path);
/// Method to set our own imported route.
void SetImportedRoute(const ActorPtr &actor, const Route route, const bool empty_buffer);
/// Method to remove a route.
void RemoveImportedRoute(const ActorId &actor_id, const bool remove_path);
/// Method to update an already set route.
void UpdateImportedRoute(const ActorId &actor_id, const Route route);
/// Method to set automatic respawn of dormant vehicles.
void SetRespawnDormantVehicles(const bool mode_switch);
// Method to set boundaries to respawn of dormant vehicles.
/// Method to set boundaries to respawn of dormant vehicles.
void SetBoundariesRespawnDormantVehicles(const float lower_bound, const float upper_bound);
// Method to set limits for boundaries when respawning dormant vehicles.
/// Method to set limits for boundaries when respawning dormant vehicles.
void SetMaxBoundaries(const float lower, const float upper);
/// Method to get the vehicle's next action.
Action GetNextAction(const ActorId &actor_id);
/// Method to get the vehicle's action buffer.
ActionBuffer GetActionBuffer(const ActorId &actor_id);
void ShutDown() {};
};

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

@ -181,6 +181,18 @@ void TrafficManagerRemote::SetKeepRightPercentage(const ActorPtr &_actor, const
client.SetKeepRightPercentage(actor, percentage);
}
void TrafficManagerRemote::SetRandomLeftLaneChangePercentage(const ActorPtr &_actor, const float percentage) {
carla::rpc::Actor actor(_actor->Serialize());
client.SetRandomLeftLaneChangePercentage(actor, percentage);
}
void TrafficManagerRemote::SetRandomRightLaneChangePercentage(const ActorPtr &_actor, const float percentage) {
carla::rpc::Actor actor(_actor->Serialize());
client.SetRandomRightLaneChangePercentage(actor, percentage);
}
void TrafficManagerRemote::SetHybridPhysicsMode(const bool mode_switch) {
client.SetHybridPhysicsMode(mode_switch);
}
@ -193,6 +205,34 @@ void TrafficManagerRemote::SetOSMMode(const bool mode_switch) {
client.SetOSMMode(mode_switch);
}
void TrafficManagerRemote::SetCustomPath(const ActorPtr &_actor, const Path path, const bool empty_buffer) {
carla::rpc::Actor actor(_actor->Serialize());
client.SetCustomPath(actor, path, empty_buffer);
}
void TrafficManagerRemote::RemoveUploadPath(const ActorId &actor_id, const bool remove_path) {
client.RemoveUploadPath(actor_id, remove_path);
}
void TrafficManagerRemote::UpdateUploadPath(const ActorId &actor_id, const Path path) {
client.UpdateUploadPath(actor_id, path);
}
void TrafficManagerRemote::SetImportedRoute(const ActorPtr &_actor, const Route route, const bool empty_buffer) {
carla::rpc::Actor actor(_actor->Serialize());
client.SetImportedRoute(actor, route, empty_buffer);
}
void TrafficManagerRemote::RemoveImportedRoute(const ActorId &actor_id, const bool remove_path) {
client.RemoveImportedRoute(actor_id, remove_path);
}
void TrafficManagerRemote::UpdateImportedRoute(const ActorId &actor_id, const Route route) {
client.UpdateImportedRoute(actor_id, route);
}
void TrafficManagerRemote::SetRespawnDormantVehicles(const bool mode_switch) {
client.SetRespawnDormantVehicles(mode_switch);
}
@ -217,6 +257,14 @@ void TrafficManagerRemote::SetSynchronousModeTimeOutInMiliSecond(double time) {
client.SetSynchronousModeTimeOutInMiliSecond(time);
}
Action TrafficManagerRemote::GetNextAction(const ActorId &actor_id) {
return client.GetNextAction(actor_id);
}
ActionBuffer TrafficManagerRemote::GetActionBuffer(const ActorId &actor_id) {
return client.GetActionBuffer(actor_id);
}
bool TrafficManagerRemote::SynchronousTick() {
return false;
}

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

@ -19,6 +19,9 @@ namespace carla {
namespace traffic_manager {
using ActorPtr = carla::SharedPtr<carla::client::Actor>;
using Path = std::vector<cg::Location>;
using Route = std::vector<uint8_t>;
/// The function of this class is to integrate all the various stages of
/// the traffic manager appropriately using messengers.
@ -96,9 +99,15 @@ public:
/// Method to set Tick timeout for synchronous execution.
void SetSynchronousModeTimeOutInMiliSecond(double time);
/// Method to set probabilistic preference to keep on the right lane.
/// Method to set % to keep on the right lane.
void SetKeepRightPercentage(const ActorPtr &actor, const float percentage);
/// Method to set % to randomly do a left lane change.
void SetRandomLeftLaneChangePercentage(const ActorPtr &actor, const float percentage);
/// Method to set % to randomly do a right lane change.
void SetRandomRightLaneChangePercentage(const ActorPtr &actor, const float percentage);
/// Method to set hybrid physics mode.
void SetHybridPhysicsMode(const bool mode_switch);
@ -108,6 +117,24 @@ public:
/// Method to set Open Street Map mode.
void SetOSMMode(const bool mode_switch);
/// Method to set our own imported path.
void SetCustomPath(const ActorPtr &actor, const Path path, const bool empty_buffer);
/// Method to remove a path.
void RemoveUploadPath(const ActorId &actor_id, const bool remove_path);
/// Method to update an already set path.
void UpdateUploadPath(const ActorId &actor_id, const Path path);
/// Method to set our own imported route.
void SetImportedRoute(const ActorPtr &actor, const Route route, const bool empty_buffer);
/// Method to remove a route.
void RemoveImportedRoute(const ActorId &actor_id, const bool remove_path);
/// Method to update an already set route.
void UpdateImportedRoute(const ActorId &actor_id, const Route route);
/// Method to set automatic respawn of dormant vehicles.
void SetRespawnDormantVehicles(const bool mode_switch);
@ -119,6 +146,12 @@ public:
virtual void ShutDown();
/// Method to get the vehicle's next action.
Action GetNextAction(const ActorId &actor_id);
/// Method to get the vehicle's action buffer.
ActionBuffer GetActionBuffer(const ActorId &actor_id);
/// Method to provide synchronous tick
bool SynchronousTick();

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

@ -19,6 +19,8 @@ namespace carla {
namespace traffic_manager {
using ActorPtr = carla::SharedPtr<carla::client::Actor>;
using Path = std::vector<cg::Location>;
using Route = std::vector<uint8_t>;
using namespace constants::Networking;
@ -138,21 +140,31 @@ public:
tm->SetPercentageRunningSign(carla::client::detail::ActorVariant(actor).Get(tm->GetEpisodeProxy()), percentage);
});
/// Method to specify the % chance of ignoring collisions with any walker.
/// Method to specify the % chance of ignoring collisions with any walker.
server->bind("set_percentage_ignore_walkers", [=](carla::rpc::Actor actor, const float percentage) {
tm->SetPercentageIgnoreWalkers(carla::client::detail::ActorVariant(actor).Get(tm->GetEpisodeProxy()), percentage);
});
/// Method to specify the % chance of ignoring collisions with any vehicle.
/// Method to specify the % chance of ignoring collisions with any vehicle.
server->bind("set_percentage_ignore_vehicles", [=](carla::rpc::Actor actor, const float percentage) {
tm->SetPercentageIgnoreVehicles(carla::client::detail::ActorVariant(actor).Get(tm->GetEpisodeProxy()), percentage);
});
/// Method to specify the % chance of ignoring collisions with any vehicle.
/// Method to set % to keep on the right lane.
server->bind("set_percentage_keep_right_rule", [=](carla::rpc::Actor actor, const float percentage) {
tm->SetKeepRightPercentage(carla::client::detail::ActorVariant(actor).Get(tm->GetEpisodeProxy()), percentage);
});
/// Method to set % to randomly do a left lane change.
server->bind("set_percentage_random_left_lanechange", [=](carla::rpc::Actor actor, const float percentage) {
tm->SetRandomLeftLaneChangePercentage(carla::client::detail::ActorVariant(actor).Get(tm->GetEpisodeProxy()), percentage);
});
/// Method to set % to randomly do a right lane change.
server->bind("set_percentage_random_right_lanechange", [=](carla::rpc::Actor actor, const float percentage) {
tm->SetRandomRightLaneChangePercentage(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);
@ -165,7 +177,37 @@ public:
/// Method to set hybrid physics radius.
server->bind("set_osm_mode", [=](const bool mode_switch) {
tm->SetHybridPhysicsRadius(mode_switch);
tm->SetOSMMode(mode_switch);
});
/// Method to set our own imported path.
server->bind("set_path", [=](carla::rpc::Actor actor, const Path path, const bool empty_buffer) {
tm->SetCustomPath(carla::client::detail::ActorVariant(actor).Get(tm->GetEpisodeProxy()), path, empty_buffer);
});
/// Method to remove a list of points.
server->bind("remove_custom_path", [=](const ActorId actor_id, const bool remove_path) {
tm->RemoveUploadPath(actor_id, remove_path);
});
/// Method to update an already set list of points.
server->bind("update_custom_path", [=](const ActorId actor_id, const Path path) {
tm->UpdateUploadPath(actor_id, path);
});
/// Method to set our own imported route.
server->bind("set_imported_route", [=](carla::rpc::Actor actor, const Route route, const bool empty_buffer) {
tm->SetImportedRoute(carla::client::detail::ActorVariant(actor).Get(tm->GetEpisodeProxy()), route, empty_buffer);
});
/// Method to remove a route.
server->bind("remove_imported_route", [=](const ActorId actor_id, const bool remove_path) {
tm->RemoveImportedRoute(actor_id, remove_path);
});
/// Method to update an already set list of points.
server->bind("update_imported_route", [=](const ActorId actor_id, const Route route) {
tm->UpdateImportedRoute(actor_id, route);
});
/// Method to set respawn dormant vehicles mode.
@ -178,6 +220,16 @@ public:
tm->SetBoundariesRespawnDormantVehicles(lower_bound, upper_bound);
});
/// Method to get the vehicle's next action.
server->bind("get_next_action", [=](const ActorId actor_id) {
tm->GetNextAction(actor_id);
});
/// Method to get the vehicle's action buffer.
server->bind("get_all_actions", [=](const ActorId actor_id) {
tm->GetActionBuffer(actor_id);
});
server->bind("shut_down", [=]() {
tm->Release();
});

74
LibCarla/source/carla/trafficmanager/VehicleLightStage.cpp
View File

@ -7,6 +7,8 @@
namespace carla {
namespace traffic_manager {
using namespace constants::VehicleLight;
VehicleLightStage::VehicleLightStage(
const std::vector<ActorId> &vehicle_id_list,
const SimulationState &simulation_state,
@ -28,7 +30,11 @@ void VehicleLightStage::ClearCycleCache() {
}
void VehicleLightStage::Update(const unsigned long index) {
ActorId id = vehicle_id_list.at(index);
ActorId actor_id = vehicle_id_list.at(index);
if (!parameters.GetUpdateVehicleLightState(actor_id))
return; // this vehicle is not set to have automatic lights update
rpc::VehicleLightState::flag_type light_states = uint32_t(-1);
bool brake_lights = false;
bool left_turn_indicator = false;
@ -38,53 +44,27 @@ void VehicleLightStage::Update(const unsigned long index) {
bool high_beam = false;
bool fog_lights = false;
if (!parameters.GetUpdateVehicleLightState(id))
return; // this vehicle is not set to have automatic lights update
// search the current light state of the vehicle
for (auto&& vls : all_light_states) {
if (vls.first == id) {
if (vls.first == actor_id) {
light_states = vls.second;
break;
}
}
cg::Vector3D actor_vec = simulation_state.GetHeading(id);
// Determine if the vehicle is truning left or right by checking the close waypoints
// Recover the planned waypoints for this vehicle
if (buffer_map.count(id) == 1) {
const Buffer& waypoint_deque = buffer_map.at(id);
// Find the next intersection (if any) to decide to turn on the blinkers
for (const SimpleWaypointPtr& swpp : waypoint_deque) {
WaypointPtr wptr = swpp->GetWaypoint();
if (!wptr->IsJunction())
continue;
const Buffer& waypoint_buffer = buffer_map.at(actor_id);
cg::Location front_location = waypoint_buffer.front()->GetLocation();
// Get the end of the junction road segment
std::vector<WaypointPtr> next_wptrs = wptr -> GetNextUntilLaneEnd(2);
if(next_wptrs.empty())
break;
wptr = next_wptrs.back();
cg::Vector3D next_road_vec = wptr->GetTransform().GetForwardVector();
cg::Vector3D up_vec(0, 0, 1);
float dot_prod = actor_vec.x*next_road_vec.x +
actor_vec.y*next_road_vec.y +
actor_vec.z*next_road_vec.z;
cg::Vector3D cross_prod(actor_vec.y*up_vec.z - actor_vec.z*up_vec.y,
actor_vec.z*up_vec.x - actor_vec.x*up_vec.z,
actor_vec.x*up_vec.y - actor_vec.y*up_vec.x);
float dot_prod_left = cross_prod.x*next_road_vec.x +
cross_prod.y*next_road_vec.y +
cross_prod.z*next_road_vec.z;
// Determine if the vehicle is truning left or right
if(dot_prod < 0.5) {
if(dot_prod_left > 0.5)
left_turn_indicator = true;
if(dot_prod_left < -0.5)
right_turn_indicator = true;
}
for (const SimpleWaypointPtr& waypoint : waypoint_buffer) {
if (waypoint->CheckJunction()) {
RoadOption target_ro = waypoint->GetRoadOption();
if (target_ro == RoadOption::Left) left_turn_indicator = true;
else if (target_ro == RoadOption::Right) right_turn_indicator = true;
break;
}
if (cg::Math::DistanceSquared(front_location, waypoint->GetLocation()) > MAX_DISTANCE_LIGHT_CHECK) {
break;
}
}
@ -93,7 +73,7 @@ void VehicleLightStage::Update(const unsigned long index) {
for (size_t cc = 0; cc < control_frame.size(); cc++) {
if (control_frame[cc].command.type() == typeid(carla::rpc::Command::ApplyVehicleControl)) {
carla::rpc::Command::ApplyVehicleControl& ctrl = boost::get<carla::rpc::Command::ApplyVehicleControl>(control_frame[cc].command);
if (ctrl.actor == id) {
if (ctrl.actor == actor_id) {
brake_lights = (ctrl.control.brake > 0.5); // hard braking, avoid blinking for throttle control
break;
}
@ -103,24 +83,24 @@ void VehicleLightStage::Update(const unsigned long index) {
// Determine position, fog and beams
// Turn on beams & positions from sunset to dawn
if (weather.sun_altitude_angle < constants::VehicleLight::SUN_ALTITUDE_DEGREES_BEFORE_DAWN ||
weather.sun_altitude_angle > constants::VehicleLight::SUN_ALTITUDE_DEGREES_AFTER_SUNSET)
if (weather.sun_altitude_angle < SUN_ALTITUDE_DEGREES_BEFORE_DAWN ||
weather.sun_altitude_angle >SUN_ALTITUDE_DEGREES_AFTER_SUNSET)
{
position = true;
low_beam = true;
}
else if (weather.sun_altitude_angle < constants::VehicleLight::SUN_ALTITUDE_DEGREES_JUST_AFTER_DAWN ||
weather.sun_altitude_angle > constants::VehicleLight::SUN_ALTITUDE_DEGREES_JUST_BEFORE_SUNSET)
else if (weather.sun_altitude_angle < SUN_ALTITUDE_DEGREES_JUST_AFTER_DAWN ||
weather.sun_altitude_angle > SUN_ALTITUDE_DEGREES_JUST_BEFORE_SUNSET)
{
position = true;
}
// Turn on lights under heavy rain
if (weather.precipitation > constants::VehicleLight::HEAVY_PRECIPITATION_THRESHOLD) {
if (weather.precipitation > HEAVY_PRECIPITATION_THRESHOLD) {
position = true;
low_beam = true;
}
// Turn on fog lights
if (weather.fog_density > constants::VehicleLight::FOG_DENSITY_THRESHOLD) {
if (weather.fog_density > FOG_DENSITY_THRESHOLD) {
position = true;
low_beam = true;
fog_lights = true;
@ -165,7 +145,7 @@ void VehicleLightStage::Update(const unsigned long index) {
// Update the vehicle light state if it has changed
if (new_light_states != light_states)
control_frame.push_back(carla::rpc::Command::SetVehicleLightState(id, new_light_states));
control_frame.push_back(carla::rpc::Command::SetVehicleLightState(actor_id, new_light_states));
}
void VehicleLightStage::RemoveActor(const ActorId) {

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

@ -6,11 +6,78 @@
#include <chrono>
#include <memory>
#include <stdio.h>
#include "carla/PythonUtil.h"
#include "boost/python/suite/indexing/vector_indexing_suite.hpp"
#include "carla/trafficmanager/TrafficManager.h"
#include "carla/trafficmanager/SimpleWaypoint.h"
using ActorPtr = carla::SharedPtr<carla::client::Actor>;
using ActorId = carla::ActorId;
const char* RoadOptionToString(carla::client::RoadOption value) {
switch (value)
{
case carla::client::RoadOption::Void: return "Void";
case carla::client::RoadOption::Left: return "Left";
case carla::client::RoadOption::Right: return "Right";
case carla::client::RoadOption::Straight: return "Straight";
case carla::client::RoadOption::LaneFollow: return "LaneFollow";
case carla::client::RoadOption::ChangeLaneLeft: return "ChangeLaneLeft";
case carla::client::RoadOption::ChangeLaneRight: return "ChangeLaneRight";
case carla::client::RoadOption::RoadEnd: return "RoadEnd";
default: return "[Unknown RoadOption]";
}
}
std::vector<uint8_t> RoadOptionToUint(boost::python::list input) {
std::vector<uint8_t> route;
for (int i = 0; i < len(input); ++i) {
uint8_t val;
char* str = boost::python::extract<char*>(input[i]);
if (strcmp(str,"Void") == 0) val = 0u;
else if (strcmp(str,"Left") == 0) val = 1u;
else if (strcmp(str,"Right") == 0) val = 2u;
else if (strcmp(str,"Straight") == 0) val = 3u;
else if (strcmp(str,"LaneFollow") == 0) val = 4u;
else if (strcmp(str,"ChangeLaneLeft") == 0) val = 5u;
else if (strcmp(str,"ChangeLaneRight") == 0) val = 6u;
else if (strcmp(str,"RoadEnd") == 0) val = 7u;
else val = 10u;
route.push_back(val);
}
return route;
}
void InterSetCustomPath(carla::traffic_manager::TrafficManager& self, const ActorPtr &actor, boost::python::list input, bool empty_buffer) {
self.SetCustomPath(actor, PythonLitstToVector<carla::geom::Location>(input), empty_buffer);
}
void InterSetImportedRoute(carla::traffic_manager::TrafficManager& self, const ActorPtr &actor, boost::python::list input, bool empty_buffer) {
self.SetImportedRoute(actor, RoadOptionToUint(input), empty_buffer);
}
boost::python::list InterGetNextAction(carla::traffic_manager::TrafficManager& self, const ActorPtr &actor_ptr) {
boost::python::list l;
auto next_action = self.GetNextAction(actor_ptr->GetId());
l.append(RoadOptionToString(next_action.first));
l.append(next_action.second);
return l;
}
boost::python::list InterGetActionBuffer(carla::traffic_manager::TrafficManager& self, const ActorPtr &actor_ptr) {
boost::python::list l;
auto action_buffer = self.GetActionBuffer(actor_ptr->GetId());
for (auto &next_action : action_buffer) {
boost::python::list temp;
temp.append(RoadOptionToString(next_action.first));
temp.append(next_action.second);
l.append(temp);
}
return l;
}
void export_trafficmanager() {
namespace cc = carla::client;
@ -21,7 +88,7 @@ void export_trafficmanager() {
.def("get_port", &ctm::TrafficManager::Port)
.def("vehicle_percentage_speed_difference", &ctm::TrafficManager::SetPercentageSpeedDifference)
.def("global_percentage_speed_difference", &ctm::TrafficManager::SetGlobalPercentageSpeedDifference)
.def("auto_update_lights", &ctm::TrafficManager::SetUpdateVehicleLights)
.def("update_vehicle_lights", &ctm::TrafficManager::SetUpdateVehicleLights)
.def("collision_detection", &ctm::TrafficManager::SetCollisionDetection)
.def("force_lane_change", &ctm::TrafficManager::SetForceLaneChange)
.def("auto_lane_change", &ctm::TrafficManager::SetAutoLaneChange)
@ -31,13 +98,19 @@ void export_trafficmanager() {
.def("ignore_lights_percentage", &ctm::TrafficManager::SetPercentageRunningLight)
.def("ignore_signs_percentage", &ctm::TrafficManager::SetPercentageRunningSign)
.def("set_global_distance_to_leading_vehicle", &ctm::TrafficManager::SetGlobalDistanceToLeadingVehicle)
.def("set_percentage_keep_right_rule", &ctm::TrafficManager::SetKeepRightPercentage)
.def("keep_right_rule_percentage", &ctm::TrafficManager::SetKeepRightPercentage)
.def("random_left_lanechange_percentage", &ctm::TrafficManager::SetRandomLeftLaneChangePercentage)
.def("random_right_lanechange_percentage", &ctm::TrafficManager::SetRandomRightLaneChangePercentage)
.def("set_synchronous_mode", &ctm::TrafficManager::SetSynchronousMode)
.def("set_hybrid_physics_mode", &ctm::TrafficManager::SetHybridPhysicsMode)
.def("set_hybrid_physics_radius", &ctm::TrafficManager::SetHybridPhysicsRadius)
.def("set_random_device_seed", &ctm::TrafficManager::SetRandomDeviceSeed)
.def("set_osm_mode", &carla::traffic_manager::TrafficManager::SetOSMMode)
.def("set_path", &InterSetCustomPath, (arg("empty_buffer") = true))
.def("set_route", &InterSetImportedRoute, (arg("empty_buffer") = true))
.def("set_respawn_dormant_vehicles", &carla::traffic_manager::TrafficManager::SetRespawnDormantVehicles)
.def("set_boundaries_respawn_dormant_vehicles", &carla::traffic_manager::TrafficManager::SetBoundariesRespawnDormantVehicles)
.def("get_next_action", &InterGetNextAction)
.def("get_all_actions", &InterGetActionBuffer)
.def("shut_down", &ctm::TrafficManager::ShutDown);
}

2
PythonAPI/examples/generate_traffic.py
View File

@ -251,7 +251,7 @@ def main():
if args.car_lights_on:
all_vehicle_actors = world.get_actors(vehicles_list)
for actor in all_vehicle_actors:
traffic_manager.auto_update_lights(actor, True)
traffic_manager.update_vehicle_lights(actor, True)
# -------------
# Spawn Walkers

2
Util/ContentVersions.txt
View File

@ -35,4 +35,4 @@
0.9.10: 20200925_88f9ceb
0.9.11: 20201222_232b876
0.9.12: 20210730_564bcdc
Latest: 20211025_9055ff0
Latest: 20211102_29df0a8