From b75a4ebdeb8a82e38f9ebd60c408ff2676232037 Mon Sep 17 00:00:00 2001 From: Jacopo Bartiromo Date: Wed, 20 Nov 2019 16:18:07 +0100 Subject: [PATCH] visualize lane change options --- .../carla/road/element/RoadInfoMarkRecord.h | 2 +- .../trafficmanager/LocalizationStage.cpp | 3 +- .../carla/trafficmanager/LocalizationStage.h | 1 - .../trafficmanager/TrafficDistributor.cpp | 42 ++++++++++++++++--- .../carla/trafficmanager/TrafficDistributor.h | 4 +- 5 files changed, 41 insertions(+), 11 deletions(-) diff --git a/LibCarla/source/carla/road/element/RoadInfoMarkRecord.h b/LibCarla/source/carla/road/element/RoadInfoMarkRecord.h index 213acf2f6..0984820de 100644 --- a/LibCarla/source/carla/road/element/RoadInfoMarkRecord.h +++ b/LibCarla/source/carla/road/element/RoadInfoMarkRecord.h @@ -109,7 +109,7 @@ namespace element { /// Allow a lane change in the indicated direction taking into account that /// lanes are numbered in ascending order from right to left. If the - /// attributeis missing, “both” is assumed to be valid. + /// attribute is missing, “both” is assumed to be valid. LaneChange GetLaneChange() const { return _lane_change; } diff --git a/LibCarla/source/carla/trafficmanager/LocalizationStage.cpp b/LibCarla/source/carla/trafficmanager/LocalizationStage.cpp index 9aca04c7f..02c5e5303 100644 --- a/LibCarla/source/carla/trafficmanager/LocalizationStage.cpp +++ b/LibCarla/source/carla/trafficmanager/LocalizationStage.cpp @@ -100,7 +100,6 @@ namespace LocalizationConstants { }; traffic_distributor.UpdateVehicleRoadPosition(actor_id, current_road_ids); - ChangeLaneInfo lane_change_info = parameters.GetForceLaneChange(vehicle); bool force_lane_change = lane_change_info.change_lane; bool lane_change_direction = lane_change_info.direction; @@ -111,7 +110,7 @@ namespace LocalizationConstants { SimpleWaypointPtr change_over_point = traffic_distributor.AssignLaneChange( vehicle, front_waypoint, current_road_ids, buffer_list, vehicle_id_to_index, - actor_list, force_lane_change, lane_change_direction); + actor_list, debug_helper, force_lane_change, lane_change_direction); if (change_over_point != nullptr) { waypoint_buffer.clear(); diff --git a/LibCarla/source/carla/trafficmanager/LocalizationStage.h b/LibCarla/source/carla/trafficmanager/LocalizationStage.h index a5b2051fe..899c156a7 100644 --- a/LibCarla/source/carla/trafficmanager/LocalizationStage.h +++ b/LibCarla/source/carla/trafficmanager/LocalizationStage.h @@ -84,7 +84,6 @@ namespace cc = carla::client; std::unordered_map vehicle_id_to_index; /// Number of vehicles currently registered with the traffic manager. uint number_of_vehicles; - /// A simple method used to draw waypoint buffer ahead of a vehicle. void DrawBuffer(Buffer &buffer); diff --git a/LibCarla/source/carla/trafficmanager/TrafficDistributor.cpp b/LibCarla/source/carla/trafficmanager/TrafficDistributor.cpp index bf9efb72d..574b647a1 100644 --- a/LibCarla/source/carla/trafficmanager/TrafficDistributor.cpp +++ b/LibCarla/source/carla/trafficmanager/TrafficDistributor.cpp @@ -79,6 +79,36 @@ namespace TrafficDistributorConstants { } } + void TrafficDistributor::DrawLaneChange(carla::road::element::LaneMarking::LaneChange lane_change, const Actor &ego_actor, cc::DebugHelper debug_helper) { + std::string str; + if (lane_change == carla::road::element::LaneMarking::LaneChange::Right) { + str="Right"; + debug_helper.DrawString( + cg::Location(ego_actor->GetLocation().x, ego_actor->GetLocation().y, ego_actor->GetLocation().z+1), + str, + false, + {255u, 0u, 0u}, 0.1f, true); + } + + else if (lane_change == carla::road::element::LaneMarking::LaneChange::Left){ + str="Left"; + debug_helper.DrawString( + cg::Location(ego_actor->GetLocation().x, ego_actor->GetLocation().y, ego_actor->GetLocation().z+1), + str, + false, + {0u, 255u, 0u}, 0.1f, true); + } + + else if (lane_change == carla::road::element::LaneMarking::LaneChange::Both){ + str="Both"; + debug_helper.DrawString( + cg::Location(ego_actor->GetLocation().x, ego_actor->GetLocation().y, ego_actor->GetLocation().z+1), + str, + false, + {0u, 0u, 255u}, 0.1f, true); + } + } + std::shared_ptr TrafficDistributor::AssignLaneChange( Actor vehicle, std::shared_ptr current_waypoint, @@ -86,6 +116,7 @@ namespace TrafficDistributorConstants { std::shared_ptr buffer_list, std::unordered_map &vehicle_id_to_index, std::vector> &actor_list, + cc::DebugHelper &debug_helper, bool force, bool direction) { @@ -103,9 +134,11 @@ namespace TrafficDistributorConstants { auto lane_change = current_waypoint->GetWaypoint()->GetLaneChange(); + DrawLaneChange(lane_change, vehicle, debug_helper); + auto change_right = carla::road::element::LaneMarking::LaneChange::Right; auto change_left = carla::road::element::LaneMarking::LaneChange::Left; - //auto change_both = carla::road::element::LaneMarking::LaneChange::Both; + auto change_both = carla::road::element::LaneMarking::LaneChange::Both; // Don't try to change lane if the current lane has less than two vehicles. if (co_lane_vehicles.size() >= 2 && !force) { @@ -140,10 +173,7 @@ namespace TrafficDistributorConstants { // If lane change connections are available, // pick a direction (preferring left) and // announce the need for a lane change. - //if (lane_change == change_both) { - // Method to assign either left or right, based on road occupancy. - //} - if (left_waypoint != nullptr && lane_change == change_left) { + if (left_waypoint != nullptr && (lane_change == change_left || lane_change == change_both)) { traffic_manager::ActorIDSet left_lane_vehicles = GetVehicleIds({ current_road_ids.road_id, current_road_ids.section_id, @@ -153,7 +183,7 @@ namespace TrafficDistributorConstants { need_to_change_lane = true; lane_change_direction = true; } - } else if (right_waypoint != nullptr && lane_change == change_right) { + } else if (right_waypoint != nullptr && (lane_change == change_right || lane_change == change_both)) { traffic_manager::ActorIDSet right_lane_vehicles = GetVehicleIds({ current_road_ids.road_id, current_road_ids.section_id, diff --git a/LibCarla/source/carla/trafficmanager/TrafficDistributor.h b/LibCarla/source/carla/trafficmanager/TrafficDistributor.h index d4c701a0a..07f4ef004 100644 --- a/LibCarla/source/carla/trafficmanager/TrafficDistributor.h +++ b/LibCarla/source/carla/trafficmanager/TrafficDistributor.h @@ -92,12 +92,13 @@ namespace cg = carla::geom; GeoIds GetRoadIds(ActorId vehicle_id) const; public: - TrafficDistributor(); ~TrafficDistributor(); void UpdateVehicleRoadPosition(ActorId actor_id, GeoIds road_ids); + void DrawLaneChange(carla::road::element::LaneMarking::LaneChange lane_change, const Actor &ego_actor, cc::DebugHelper debug_helper); + /// Returns the shared pointer of SimpleWaypoint for Lane Change /// if Lane Change is required and possible, else returns nullptr. /// Lane change can be forced by setting the force flag and providing @@ -109,6 +110,7 @@ namespace cg = carla::geom; std::shared_ptr buffer_list, std::unordered_map &vehicle_id_to_index, std::vector &actor_list, + cc::DebugHelper &debug_helper, bool force = false, bool direction = false);