From 83f0a0097c008b09003a10afef3aa3ea962e72b3 Mon Sep 17 00:00:00 2001 From: Jacopo Bartiromo Date: Thu, 13 May 2021 11:18:57 +0200 Subject: [PATCH] Removed all debug helper functions --- .../carla/trafficmanager/CollisionStage.cpp | 12 ------------ .../carla/trafficmanager/CollisionStage.h | 4 ---- .../carla/trafficmanager/InMemoryMap.cpp | 5 +---- .../source/carla/trafficmanager/InMemoryMap.h | 3 +-- .../carla/trafficmanager/LocalizationStage.cpp | 18 ------------------ .../carla/trafficmanager/LocalizationStage.h | 4 ---- .../carla/trafficmanager/MotionPlanStage.cpp | 7 ++----- .../carla/trafficmanager/MotionPlanStage.h | 6 +----- .../trafficmanager/TrafficManagerLocal.cpp | 12 ++++-------- .../carla/trafficmanager/TrafficManagerLocal.h | 5 +---- 10 files changed, 10 insertions(+), 66 deletions(-) diff --git a/LibCarla/source/carla/trafficmanager/CollisionStage.cpp b/LibCarla/source/carla/trafficmanager/CollisionStage.cpp index 7b070d1e7..600bf5914 100644 --- a/LibCarla/source/carla/trafficmanager/CollisionStage.cpp +++ b/LibCarla/source/carla/trafficmanager/CollisionStage.cpp @@ -22,7 +22,6 @@ CollisionStage::CollisionStage( const TrackTraffic &track_traffic, const Parameters ¶meters, CollisionFrame &output_array, - cc::DebugHelper &debug_helper, RandomGeneratorMap &random_devices) : vehicle_id_list(vehicle_id_list), simulation_state(simulation_state), @@ -30,7 +29,6 @@ CollisionStage::CollisionStage( track_traffic(track_traffic), parameters(parameters), output_array(output_array), - debug_helper(debug_helper), random_devices(random_devices) {} void CollisionStage::Update(const unsigned long index) { @@ -415,15 +413,5 @@ void CollisionStage::ClearCycleCache() { geometry_cache.clear(); } -void CollisionStage::DrawBoundary(const LocationVector &boundary) { - cg::Location one_meter_up(0.0f, 0.0f, 1.0f); - for (uint64_t i = 0u; i < boundary.size(); ++i) { - debug_helper.DrawLine( - boundary[i] + one_meter_up, - boundary[i+1 == boundary.size()? 0: i+1] + one_meter_up, - 0.1f, {255u, 255u, 0u}, 0.05f); - } -} - } // namespace traffic_manager } // namespace carla diff --git a/LibCarla/source/carla/trafficmanager/CollisionStage.h b/LibCarla/source/carla/trafficmanager/CollisionStage.h index 511803259..e86804867 100644 --- a/LibCarla/source/carla/trafficmanager/CollisionStage.h +++ b/LibCarla/source/carla/trafficmanager/CollisionStage.h @@ -8,8 +8,6 @@ #include "boost/geometry/geometries/point_xy.hpp" #include "boost/geometry/geometries/polygon.hpp" -#include "carla/client/DebugHelper.h" - #include "carla/trafficmanager/DataStructures.h" #include "carla/trafficmanager/Parameters.h" #include "carla/trafficmanager/RandomGenerator.h" @@ -52,7 +50,6 @@ private: const TrackTraffic &track_traffic; const Parameters ¶meters; CollisionFrame &output_array; - cc::DebugHelper &debug_helper; // Structure keeping track of blocking lead vehicles. CollisionLockMap collision_locks; // Structures to cache geodesic boundaries of vehicle and @@ -93,7 +90,6 @@ public: const TrackTraffic &track_traffic, const Parameters ¶meters, CollisionFrame &output_array, - cc::DebugHelper& debug_helper, RandomGeneratorMap &random_devices); void Update (const unsigned long index) override; diff --git a/LibCarla/source/carla/trafficmanager/InMemoryMap.cpp b/LibCarla/source/carla/trafficmanager/InMemoryMap.cpp index 184cec2a5..174b71ee2 100644 --- a/LibCarla/source/carla/trafficmanager/InMemoryMap.cpp +++ b/LibCarla/source/carla/trafficmanager/InMemoryMap.cpp @@ -64,7 +64,7 @@ namespace traffic_manager { return result; } - void InMemoryMap::SetUp(cc::DebugHelper &debug_helper) { + void InMemoryMap::SetUp() { // 1. Building segment topology (i.e., defining set of segment predecessors and successors) assert(_world_map != nullptr && "No map reference found."); @@ -162,14 +162,12 @@ namespace traffic_manager { float distance = distance_squared(segment_waypoints.at(i)->GetLocation(), segment_waypoints.at(i+1)->GetLocation()); if (angle(segment_waypoints.at(i)->GetLocation(), segment_waypoints.at(i+1)->GetLocation()) > 0.12f) { // 7 deg - debug_helper.DrawPoint(segment_waypoints.at(i)->GetLocation(), 0.15f, {0u, 0u, 255u}, 556.05f); auto new_waypoint = segment_waypoints.at(i)->GetWaypoint()->GetNext(std::sqrt(distance)/2.0f).front(); i++; segment_waypoints.insert(segment_waypoints.begin()+static_cast(i), std::make_shared(new_waypoint)); } else if (distance > MAX_WPT_DISTANCE) { auto new_waypoint = segment_waypoints.at(i)->GetWaypoint()->GetNext(std::sqrt(distance)/2.0f).front(); i++; - debug_helper.DrawPoint(segment_waypoints.at(i)->GetLocation(), 0.15f, {0u, 255u, 255u}, 556.05f); segment_waypoints.insert(segment_waypoints.begin()+static_cast(i), std::make_shared(new_waypoint)); } } @@ -209,7 +207,6 @@ namespace traffic_manager { for (auto &simple_waypoint: dense_topology) { if (simple_waypoint != nullptr) { const cg::Location loc = simple_waypoint->GetLocation(); - debug_helper.DrawPoint(loc, 0.1f, {255u, 0u, 0u}, 555.05f); Point3D point(loc.x, loc.y, loc.z); rtree.insert(std::make_pair(point, simple_waypoint)); } diff --git a/LibCarla/source/carla/trafficmanager/InMemoryMap.h b/LibCarla/source/carla/trafficmanager/InMemoryMap.h index fe028aebd..21037a933 100644 --- a/LibCarla/source/carla/trafficmanager/InMemoryMap.h +++ b/LibCarla/source/carla/trafficmanager/InMemoryMap.h @@ -13,7 +13,6 @@ #include "boost/geometry.hpp" #include "boost/geometry/geometries/point.hpp" #include "boost/geometry/index/rtree.hpp" -#include "carla/client/DebugHelper.h" #include "carla/client/Map.h" #include "carla/client/Waypoint.h" @@ -68,7 +67,7 @@ namespace bgi = boost::geometry::index; /// This method constructs the local map with a resolution of /// sampling_resolution. - void SetUp(cc::DebugHelper &debug_helper); + void SetUp(); /// This method returns the closest waypoint to a given location on the map. SimpleWaypointPtr GetWaypoint(const cg::Location loc) const; diff --git a/LibCarla/source/carla/trafficmanager/LocalizationStage.cpp b/LibCarla/source/carla/trafficmanager/LocalizationStage.cpp index 9e425a030..df9d1d137 100644 --- a/LibCarla/source/carla/trafficmanager/LocalizationStage.cpp +++ b/LibCarla/source/carla/trafficmanager/LocalizationStage.cpp @@ -19,7 +19,6 @@ LocalizationStage::LocalizationStage( Parameters ¶meters, std::vector& marked_for_removal, LocalizationFrame &output_array, - cc::DebugHelper &debug_helper, RandomGeneratorMap &random_devices) : vehicle_id_list(vehicle_id_list), buffer_map(buffer_map), @@ -29,7 +28,6 @@ LocalizationStage::LocalizationStage( parameters(parameters), marked_for_removal(marked_for_removal), output_array(output_array), - debug_helper(debug_helper), random_devices(random_devices) {} void LocalizationStage::Update(const unsigned long index) { @@ -404,21 +402,5 @@ SimpleWaypointPtr LocalizationStage::AssignLaneChange(const ActorId actor_id, return change_over_point; } -void LocalizationStage::DrawBuffer(Buffer &buffer) { - uint64_t buffer_size = buffer.size(); - uint64_t step_size = std::max(buffer_size/20u, static_cast(1)); - cc::DebugHelper::Color color {0u, 0u, 0u}; - cg::Location two_meters_up = cg::Location(0.0f, 0.0f, 2.0f); - for (uint64_t i = 0u; i + step_size < buffer_size; i += step_size) { - if (!buffer.at(i)->CheckJunction() && !buffer.at(i + step_size)->CheckJunction()) { - color.g = 255u; - } - debug_helper.DrawLine(buffer.at(i)->GetLocation() + two_meters_up, - buffer.at(i + step_size)->GetLocation() + two_meters_up, - 0.2f, color, 0.05f); - color = {0u, 0u, 0u}; - } -} - } // namespace traffic_manager } // namespace carla diff --git a/LibCarla/source/carla/trafficmanager/LocalizationStage.h b/LibCarla/source/carla/trafficmanager/LocalizationStage.h index 042f7218e..77696b51a 100644 --- a/LibCarla/source/carla/trafficmanager/LocalizationStage.h +++ b/LibCarla/source/carla/trafficmanager/LocalizationStage.h @@ -3,8 +3,6 @@ #include -#include "carla/client/DebugHelper.h" - #include "carla/trafficmanager/DataStructures.h" #include "carla/trafficmanager/InMemoryMap.h" #include "carla/trafficmanager/LocalizationUtils.h" @@ -37,7 +35,6 @@ private: // Array of vehicles marked by stages for removal. std::vector& marked_for_removal; LocalizationFrame &output_array; - cc::DebugHelper &debug_helper; LaneChangeLocationMap last_lane_change_location; ActorIdSet vehicles_at_junction; using SimpleWaypointPair = std::pair; @@ -64,7 +61,6 @@ public: Parameters ¶meters, std::vector& marked_for_removal, LocalizationFrame &output_array, - cc::DebugHelper &debug_helper, RandomGeneratorMap &random_devices); void Update(const unsigned long index) override; diff --git a/LibCarla/source/carla/trafficmanager/MotionPlanStage.cpp b/LibCarla/source/carla/trafficmanager/MotionPlanStage.cpp index f0aedb29d..cb8667a41 100644 --- a/LibCarla/source/carla/trafficmanager/MotionPlanStage.cpp +++ b/LibCarla/source/carla/trafficmanager/MotionPlanStage.cpp @@ -29,8 +29,7 @@ MotionPlanStage::MotionPlanStage( const CollisionFrame&collision_frame, const TLFrame &tl_frame, const cc::World &world, - ControlFrame &output_array, - cc::DebugHelper &debug_helper) + ControlFrame &output_array) : vehicle_id_list(vehicle_id_list), simulation_state(simulation_state), parameters(parameters), @@ -44,8 +43,7 @@ MotionPlanStage::MotionPlanStage( collision_frame(collision_frame), tl_frame(tl_frame), world(world), - output_array(output_array), - debug_helper(debug_helper) {} + output_array(output_array) {} void MotionPlanStage::Update(const unsigned long index) { const ActorId actor_id = vehicle_id_list.at(index); @@ -87,7 +85,6 @@ void MotionPlanStage::Update(const unsigned long index) { TARGET_WAYPOINT_HORIZON_LENGTH); const SimpleWaypointPtr &target_waypoint = GetTargetWaypoint(waypoint_buffer, target_point_distance).first; const cg::Location target_location = target_waypoint->GetLocation(); - debug_helper.DrawPoint(target_location + cg::Location(0,0,0.5), 0.2f, {0u, 255u, 0u}, .1f); float dot_product = DeviationDotProduct(ego_location, ego_heading, target_location); float cross_product = DeviationCrossProduct(ego_location, ego_heading, target_location); dot_product = 1.0f - dot_product; diff --git a/LibCarla/source/carla/trafficmanager/MotionPlanStage.h b/LibCarla/source/carla/trafficmanager/MotionPlanStage.h index df1d85022..9d6bb8fa1 100644 --- a/LibCarla/source/carla/trafficmanager/MotionPlanStage.h +++ b/LibCarla/source/carla/trafficmanager/MotionPlanStage.h @@ -11,8 +11,6 @@ #include "carla/trafficmanager/Stage.h" #include "carla/trafficmanager/TrackTraffic.h" -#include "carla/client/DebugHelper.h" - namespace carla { namespace traffic_manager { @@ -38,7 +36,6 @@ private: // in hybrid physics mode. std::unordered_map teleportation_instance; ControlFrame &output_array; - cc::DebugHelper debug_helper; cc::Timestamp current_timestamp; std::pair CollisionHandling(const CollisionHazardData &collision_hazard, @@ -65,8 +62,7 @@ public: const CollisionFrame &collision_frame, const TLFrame &tl_frame, const cc::World &world, - ControlFrame &output_array, - cc::DebugHelper &debug_helper); + ControlFrame &output_array); void Update(const unsigned long index); diff --git a/LibCarla/source/carla/trafficmanager/TrafficManagerLocal.cpp b/LibCarla/source/carla/trafficmanager/TrafficManagerLocal.cpp index e4613eaeb..81c0026f8 100644 --- a/LibCarla/source/carla/trafficmanager/TrafficManagerLocal.cpp +++ b/LibCarla/source/carla/trafficmanager/TrafficManagerLocal.cpp @@ -31,7 +31,6 @@ TrafficManagerLocal::TrafficManagerLocal( episode_proxy(episode_proxy), world(cc::World(episode_proxy)), - debug_helper(world.MakeDebugHelper()), localization_stage(LocalizationStage(vehicle_id_list, buffer_map, @@ -41,7 +40,6 @@ TrafficManagerLocal::TrafficManagerLocal( parameters, marked_for_removal, localization_frame, - debug_helper, random_devices)), collision_stage(CollisionStage(vehicle_id_list, @@ -50,7 +48,6 @@ TrafficManagerLocal::TrafficManagerLocal( track_traffic, parameters, collision_frame, - debug_helper, random_devices)), traffic_light_stage(TrafficLightStage(vehicle_id_list, @@ -74,8 +71,7 @@ TrafficManagerLocal::TrafficManagerLocal( collision_frame, tl_frame, world, - control_frame, - debug_helper)), + control_frame)), alsm(ALSM(registered_vehicles, buffer_map, @@ -97,7 +93,7 @@ TrafficManagerLocal::TrafficManagerLocal( registered_vehicles_state = -1; - SetupLocalMap(debug_helper); + SetupLocalMap(); Start(); } @@ -107,10 +103,10 @@ TrafficManagerLocal::~TrafficManagerLocal() { Release(); } -void TrafficManagerLocal::SetupLocalMap(cc::DebugHelper &debug_helper) { +void TrafficManagerLocal::SetupLocalMap() { const carla::SharedPtr world_map = world.GetMap(); local_map = std::make_shared(world_map); - local_map->SetUp(debug_helper); + local_map->SetUp(); } void TrafficManagerLocal::Start() { diff --git a/LibCarla/source/carla/trafficmanager/TrafficManagerLocal.h b/LibCarla/source/carla/trafficmanager/TrafficManagerLocal.h index d5c243e90..0f28e0b55 100644 --- a/LibCarla/source/carla/trafficmanager/TrafficManagerLocal.h +++ b/LibCarla/source/carla/trafficmanager/TrafficManagerLocal.h @@ -12,7 +12,6 @@ #include #include -#include "carla/client/DebugHelper.h" #include "carla/client/detail/EpisodeProxy.h" #include "carla/client/TrafficLight.h" #include "carla/client/World.h" @@ -71,8 +70,6 @@ private: LocalMapPtr local_map; /// Structures to hold waypoint buffers for all vehicles. BufferMap buffer_map; - /// Carla's debug helper object. - cc::DebugHelper debug_helper; /// Object for tracking paths of the traffic vehicles. TrackTraffic track_traffic; /// Type containing the current state of all actors involved in the simulation. @@ -136,7 +133,7 @@ public: virtual ~TrafficManagerLocal(); /// Method to setup InMemoryMap. - void SetupLocalMap(cc::DebugHelper &debug_helper); + void SetupLocalMap(); /// To start the TrafficManager. void Start();