Fixed vehicle sorting + WIP physics deter check

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));

LibCarla/source/carla/trafficmanager/AtomicActorSet.h

@@ -7,7 +7,7 @@
 #pragma once
 
 #include <mutex>
-#include <unordered_map>
+#include <map>
 #include <vector>
 
 #include "carla/client/Actor.h"
@@ -25,7 +25,7 @@ namespace traffic_manager {
   private:
 
     std::mutex modification_mutex;
-    std::unordered_map<ActorId, ActorPtr> actor_set;
+    std::map<ActorId, ActorPtr> actor_set;
     int state_counter;
 
   public:

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;
     }
   }
 }

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<ActorPtr> &vehicle_list) {
   std::lock_guard<std::mutex> registration_lock(registration_mutex);
-  std::vector<ActorPtr> 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<ActorPtr> &actor_list) {
@@ -480,9 +476,7 @@ std::vector<ActorId> 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();
 }
 

LibCarla/source/carla/trafficmanager/TrafficManagerLocal.h

@@ -111,7 +111,6 @@ private:
   std::unique_ptr<std::thread> worker_thread;
   /// Randomization seed.
   uint64_t seed {static_cast<uint64_t>(time(NULL))};
-  bool is_custom_seed {false};
   /// Structure holding random devices per vehicle.
   RandomGenerator random_device = RandomGenerator(seed);
   std::vector<ActorId> marked_for_removal;

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<carla::traffic_manager::ActorPtr> 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<carla::traffic_manager::ActorPtr> 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;