From c7d6aca07953df70fd169e0b9a9dfedf5de8b7c1 Mon Sep 17 00:00:00 2001 From: Jacopo Bartiromo Date: Fri, 25 Oct 2019 13:15:16 +0200 Subject: [PATCH] change from 2 speed to % less than speed limit. --- .../carla/trafficmanager/MotionPlannerStage.cpp | 17 +++++------------ .../carla/trafficmanager/MotionPlannerStage.h | 6 ++---- .../carla/trafficmanager/TrafficManager.cpp | 9 ++++----- .../carla/trafficmanager/TrafficManager.h | 3 +-- 4 files changed, 12 insertions(+), 23 deletions(-) diff --git a/LibCarla/source/carla/trafficmanager/MotionPlannerStage.cpp b/LibCarla/source/carla/trafficmanager/MotionPlannerStage.cpp index bccba5806..91a91f3f8 100644 --- a/LibCarla/source/carla/trafficmanager/MotionPlannerStage.cpp +++ b/LibCarla/source/carla/trafficmanager/MotionPlannerStage.cpp @@ -4,7 +4,6 @@ namespace traffic_manager { namespace PlannerConstants { static const float HIGHWAY_SPEED = 50 / 3.6f; - static const float INTERSECTION_APPROACH_SPEED = 15 / 3.6f; static const std::vector URBAN_LONGITUDINAL_DEFAULTS = {0.1f, 0.15f, 0.01f}; static const std::vector HIGHWAY_LONGITUDINAL_DEFAULTS = {5.0f, 0.1f, 0.01f}; static const std::vector LATERAL_DEFAULTS = {10.0f, 0.0f, 0.1f}; @@ -17,8 +16,7 @@ namespace PlannerConstants { std::shared_ptr traffic_light_messenger, std::shared_ptr control_messenger, AtomicMap &vehicle_target_velocity, - float urban_target_velocity = 25 / 3.6f, - float highway_target_velocity = 50 / 3.6f, + float perc_decrease_from_limit = 20.0f, std::vector longitudinal_parameters = URBAN_LONGITUDINAL_DEFAULTS, std::vector highway_longitudinal_parameters = HIGHWAY_LONGITUDINAL_DEFAULTS, std::vector lateral_parameters = LATERAL_DEFAULTS) @@ -27,8 +25,7 @@ namespace PlannerConstants { traffic_light_messenger(traffic_light_messenger), control_messenger(control_messenger), vehicle_target_velocity(vehicle_target_velocity), - urban_target_velocity(urban_target_velocity), - highway_target_velocity(highway_target_velocity), + perc_decrease_from_limit(perc_decrease_from_limit), longitudinal_parameters(longitudinal_parameters), highway_longitudinal_parameters(highway_longitudinal_parameters), lateral_parameters(lateral_parameters) { @@ -76,12 +73,12 @@ namespace PlannerConstants { traffic_manager::StateEntry previous_state; previous_state = pid_state_map.at(actor_id); - float dynamic_target_velocity = urban_target_velocity; - // Increase speed if on highway. float speed_limit = vehicle->GetSpeedLimit() / 3.6f; + + float dynamic_target_velocity = speed_limit - (speed_limit*perc_decrease_from_limit)/100.0f; + if (speed_limit > HIGHWAY_SPEED) { - dynamic_target_velocity = highway_target_velocity; longitudinal_parameters = highway_longitudinal_parameters; } @@ -93,10 +90,6 @@ namespace PlannerConstants { } } - // Decrease speed approaching an intersection. - if (localization_data.approaching_true_junction) { - dynamic_target_velocity = INTERSECTION_APPROACH_SPEED; - } // State update for vehicle. StateEntry current_state = controller.StateUpdate( diff --git a/LibCarla/source/carla/trafficmanager/MotionPlannerStage.h b/LibCarla/source/carla/trafficmanager/MotionPlannerStage.h index d7b96ea81..41375a2ec 100644 --- a/LibCarla/source/carla/trafficmanager/MotionPlannerStage.h +++ b/LibCarla/source/carla/trafficmanager/MotionPlannerStage.h @@ -54,8 +54,7 @@ namespace cc = carla::client; /// Target velocities for specific vehicles. AtomicMap &vehicle_target_velocity; /// Target velocities. - float urban_target_velocity; - float highway_target_velocity; + float perc_decrease_from_limit; /// Configuration parameters for the PID controller. std::vector longitudinal_parameters; std::vector highway_longitudinal_parameters; @@ -73,8 +72,7 @@ namespace cc = carla::client; std::shared_ptr traffic_light_messenger, std::shared_ptr control_messenger, AtomicMap &vehicle_target_velocity, - float urban_target_velocity, - float highway_target_velocity, + float perc_decrease_from_limit, std::vector longitudinal_parameters, std::vector highway_longitudinal_parameters, std::vector lateral_parameters); diff --git a/LibCarla/source/carla/trafficmanager/TrafficManager.cpp b/LibCarla/source/carla/trafficmanager/TrafficManager.cpp index a49d2d53e..f56afecc0 100644 --- a/LibCarla/source/carla/trafficmanager/TrafficManager.cpp +++ b/LibCarla/source/carla/trafficmanager/TrafficManager.cpp @@ -6,8 +6,7 @@ namespace traffic_manager { std::vector longitudinal_PID_parameters, std::vector longitudinal_highway_PID_parameters, std::vector lateral_PID_parameters, - float urban_target_velocity, - float highway_target_velocity, + float perc_decrease_from_limit, cc::Client &client_connection) : longitudinal_PID_parameters(longitudinal_PID_parameters), longitudinal_highway_PID_parameters(longitudinal_highway_PID_parameters), @@ -50,8 +49,7 @@ namespace traffic_manager { traffic_light_planner_messenger, planner_control_messenger, vehicle_target_velocity, - urban_target_velocity, - highway_target_velocity, + perc_decrease_from_limit, longitudinal_PID_parameters, longitudinal_highway_PID_parameters, lateral_PID_parameters); @@ -76,10 +74,11 @@ namespace traffic_manager { std::vector longitudinal_param = {0.1f, 0.15f, 0.01f}; std::vector longitudinal_highway_param = {5.0f, 0.09f, 0.01f}; std::vector lateral_param = {10.0f, 0.0f, 0.1f}; + float perc_decrease_from_limit = 20.0f; TrafficManager* tm_ptr = new TrafficManager( longitudinal_param, longitudinal_highway_param, lateral_param, - 25.0f/3.6f, 50.0f/3.6f, client_connection + perc_decrease_from_limit, client_connection ); singleton_pointer = std::unique_ptr(tm_ptr); diff --git a/LibCarla/source/carla/trafficmanager/TrafficManager.h b/LibCarla/source/carla/trafficmanager/TrafficManager.h index 7cab62bb7..b21fd35f2 100644 --- a/LibCarla/source/carla/trafficmanager/TrafficManager.h +++ b/LibCarla/source/carla/trafficmanager/TrafficManager.h @@ -83,8 +83,7 @@ namespace cc = carla::client; std::vector longitudinal_PID_parameters, std::vector longitudinal_highway_PID_parameters, std::vector lateral_PID_parameters, - float urban_target_velocity, - float highway_target_velocity, + float perc_decrease_from_limit, cc::Client &client_connection); /// To start the TrafficManager.