From e0719a82704670704a62a49701462e56fa4f4039 Mon Sep 17 00:00:00 2001 From: Guillermo Date: Wed, 15 Dec 2021 17:35:06 +0100 Subject: [PATCH] Fixed vehicle sorting + WIP physics deter check --- LibCarla/source/carla/trafficmanager/ALSM.cpp | 2 +- .../source/carla/trafficmanager/AtomicActorSet.h | 4 ++-- .../source/carla/trafficmanager/MotionPlanStage.cpp | 12 ++++++++++++ .../carla/trafficmanager/TrafficManagerLocal.cpp | 10 ++-------- .../carla/trafficmanager/TrafficManagerLocal.h | 1 - PythonAPI/carla/source/libcarla/Client.cpp | 13 ++++++++++--- 6 files changed, 27 insertions(+), 15 deletions(-) diff --git a/LibCarla/source/carla/trafficmanager/ALSM.cpp b/LibCarla/source/carla/trafficmanager/ALSM.cpp index c9066f39a..10f068047 100644 --- a/LibCarla/source/carla/trafficmanager/ALSM.cpp +++ b/LibCarla/source/carla/trafficmanager/ALSM.cpp @@ -225,7 +225,7 @@ void ALSM::UpdateData(const bool hybrid_physics_mode, bool enable_physics = hybrid_physics_mode ? in_range_of_hero_actor : true; if (!has_physics_enabled.count(actor_id) || has_physics_enabled[actor_id] != enable_physics) { if (hero_actors.find(actor_id) == hero_actors.end()) { - vehicle->SetSimulatePhysics(enable_physics); + // vehicle->SetSimulatePhysics(enable_physics); has_physics_enabled[actor_id] = enable_physics; if (enable_physics == true && simulation_state.ContainsActor(actor_id)) { vehicle->SetTargetVelocity(simulation_state.GetVelocity(actor_id)); diff --git a/LibCarla/source/carla/trafficmanager/AtomicActorSet.h b/LibCarla/source/carla/trafficmanager/AtomicActorSet.h index d7d4a1892..f9d1adcb5 100644 --- a/LibCarla/source/carla/trafficmanager/AtomicActorSet.h +++ b/LibCarla/source/carla/trafficmanager/AtomicActorSet.h @@ -7,7 +7,7 @@ #pragma once #include -#include +#include #include #include "carla/client/Actor.h" @@ -25,7 +25,7 @@ namespace traffic_manager { private: std::mutex modification_mutex; - std::unordered_map actor_set; + std::map actor_set; int state_counter; public: diff --git a/LibCarla/source/carla/trafficmanager/MotionPlanStage.cpp b/LibCarla/source/carla/trafficmanager/MotionPlanStage.cpp index 386f5c7d4..7f826cf2f 100644 --- a/LibCarla/source/carla/trafficmanager/MotionPlanStage.cpp +++ b/LibCarla/source/carla/trafficmanager/MotionPlanStage.cpp @@ -84,6 +84,15 @@ void MotionPlanStage::Update(const unsigned long index) { current_timestamp = world.GetSnapshot().GetTimestamp(); StateEntry current_state; + // std::cout << index << std::endl; + // std::cout << " Location: " << vehicle_location.x << " " << vehicle_location.y << " " << vehicle_location.z << std::endl; + // std::cout << " Rotation: " << vehicle_rotation.roll << " " << vehicle_rotation.pitch << " " << vehicle_rotation.yaw << std::endl; + // std::cout << " Velocity: " << vehicle_velocity.x << " " << vehicle_velocity.y << " " << vehicle_velocity.z << std::endl; + // std::cout << " Speed: " << vehicle_speed << std::endl; + // std::cout << " Heading: " << vehicle_heading.x << " " << vehicle_heading.y << " " << vehicle_heading.z << std::endl; + // std::cout << " Physics: " << vehicle_physics_enabled << std::endl; + + // Instanciating teleportation transform as current vehicle transform. cg::Transform teleportation_transform = cg::Transform(vehicle_location, vehicle_rotation); @@ -216,6 +225,7 @@ void MotionPlanStage::Update(const unsigned long index) { StateEntry &state = pid_state_map.at(actor_id); state = current_state; + // std::cout << " Control: " << vehicle_control.throttle << " " << vehicle_control.brake << " " << vehicle_control.steer << " " << std::endl; } // For physics-less vehicles, determine position and orientation for teleportation. else { @@ -258,6 +268,8 @@ void MotionPlanStage::Update(const unsigned long index) { } // Constructing the actuation signal. output_array.at(index) = carla::rpc::Command::ApplyTransform(actor_id, teleportation_transform); + + // std::cout << " Teleport transform: " << teleportation_transform.location.x << " " << teleportation_transform.location.y << " " << teleportation_transform.location.z << " " << teleportation_transform.rotation.roll << " " << teleportation_transform.rotation.pitch << " " << teleportation_transform.rotation.yaw << std::endl; } } } diff --git a/LibCarla/source/carla/trafficmanager/TrafficManagerLocal.cpp b/LibCarla/source/carla/trafficmanager/TrafficManagerLocal.cpp index 22c173a52..1d4f38e96 100644 --- a/LibCarla/source/carla/trafficmanager/TrafficManagerLocal.cpp +++ b/LibCarla/source/carla/trafficmanager/TrafficManagerLocal.cpp @@ -183,13 +183,11 @@ void TrafficManagerLocal::Run() { // Updating simulation state, actor life cycle and performing necessary cleanup. alsm.Update(); - // Re-allocating inter-stage communication frames based on changed number of registered vehicles. int current_registered_vehicles_state = registered_vehicles.GetState(); unsigned long number_of_vehicles = vehicle_id_list.size(); if (registered_vehicles_state != current_registered_vehicles_state || number_of_vehicles != registered_vehicles.Size()) { vehicle_id_list = registered_vehicles.GetIDList(); - std::sort(vehicle_id_list.begin(), vehicle_id_list.end()); number_of_vehicles = vehicle_id_list.size(); // Reserve more space if needed. @@ -229,6 +227,7 @@ void TrafficManagerLocal::Run() { } collision_stage.ClearCycleCache(); vehicle_light_stage.UpdateWorldInfo(); + // std::cout << "New Frame" << std::endl; for (unsigned long index = 0u; index < vehicle_id_list.size(); ++index) { traffic_light_stage.Update(index); motion_plan_stage.Update(index); @@ -308,7 +307,6 @@ void TrafficManagerLocal::Release() { } void TrafficManagerLocal::Reset() { - Release(); episode_proxy = episode_proxy.Lock()->GetCurrentEpisode(); world = cc::World(episode_proxy); @@ -318,9 +316,7 @@ void TrafficManagerLocal::Reset() { void TrafficManagerLocal::RegisterVehicles(const std::vector &vehicle_list) { std::lock_guard registration_lock(registration_mutex); - std::vector sorted_vehicle_list = vehicle_list; - std::sort(sorted_vehicle_list.begin(), sorted_vehicle_list.end(), [](ActorPtr &a, ActorPtr &b) {return a->GetId() > b->GetId(); }); - registered_vehicles.Insert(sorted_vehicle_list); + registered_vehicles.Insert(vehicle_list); } void TrafficManagerLocal::UnregisterVehicles(const std::vector &actor_list) { @@ -480,9 +476,7 @@ std::vector TrafficManagerLocal::GetRegisteredVehiclesIDs() { void TrafficManagerLocal::SetRandomDeviceSeed(const uint64_t _seed) { seed = _seed; - is_custom_seed = true; random_device = RandomGenerator(seed); - std::cout << random_device.next() << std::endl; world.ResetAllTrafficLights(); } diff --git a/LibCarla/source/carla/trafficmanager/TrafficManagerLocal.h b/LibCarla/source/carla/trafficmanager/TrafficManagerLocal.h index 190ad5762..007ab1f16 100644 --- a/LibCarla/source/carla/trafficmanager/TrafficManagerLocal.h +++ b/LibCarla/source/carla/trafficmanager/TrafficManagerLocal.h @@ -111,7 +111,6 @@ private: std::unique_ptr worker_thread; /// Randomization seed. uint64_t seed {static_cast(time(NULL))}; - bool is_custom_seed {false}; /// Structure holding random devices per vehicle. RandomGenerator random_device = RandomGenerator(seed); std::vector marked_for_removal; diff --git a/PythonAPI/carla/source/libcarla/Client.cpp b/PythonAPI/carla/source/libcarla/Client.cpp index 5b38acd9d..caf7be07d 100644 --- a/PythonAPI/carla/source/libcarla/Client.cpp +++ b/PythonAPI/carla/source/libcarla/Client.cpp @@ -156,10 +156,17 @@ static auto ApplyBatchCommandsSync( vehicles_to_enable.shrink_to_fit(); vehicles_to_disable.shrink_to_fit(); + // Ensure the TM always receives the same vector by sorting the elements + std::vector sorted_vehicle_to_enable = vehicles_to_enable; + std::sort(sorted_vehicle_to_enable.begin(), sorted_vehicle_to_enable.end(), [](carla::traffic_manager::ActorPtr &a, carla::traffic_manager::ActorPtr &b) {return a->GetId() < b->GetId(); }); + + std::vector sorted_vehicle_to_disable = vehicles_to_disable; + std::sort(sorted_vehicle_to_disable.begin(), sorted_vehicle_to_disable.end(), [](carla::traffic_manager::ActorPtr &a, carla::traffic_manager::ActorPtr &b) {return a->GetId() < b->GetId(); }); + // check if any autopilot command was sent - if (vehicles_to_enable.size() || vehicles_to_disable.size()) { - self.GetInstanceTM(tm_port).RegisterVehicles(vehicles_to_enable); - self.GetInstanceTM(tm_port).UnregisterVehicles(vehicles_to_disable); + if (sorted_vehicle_to_enable.size() || sorted_vehicle_to_disable.size()) { + self.GetInstanceTM(tm_port).RegisterVehicles(sorted_vehicle_to_enable); + self.GetInstanceTM(tm_port).UnregisterVehicles(sorted_vehicle_to_disable); } return result;