Send info of dynamic objects

Source/Carla/AI/VehicleSpawnerBase.h

@@ -33,15 +33,24 @@ protected:
 
 public:
 
-   void SetNumberOfVehicles(int32 Count);
+  void SetNumberOfVehicles(int32 Count);
 
-   APlayerStart* GetRandomSpawnPoint();
+  int32 GetNumberOfSpawnedVehicles() const
+  {
+    return Vehicles.Num();
+  }
 
-   void SpawnVehicleAtSpawnPoint(const APlayerStart &SpawnPoint);
+  const TArray<AWheeledVehicle *> &GetVehicles() const {
+    return Vehicles;
+  }
 
 protected:
 
-/** If false, no walker will be spawned. */
+  APlayerStart* GetRandomSpawnPoint();
+
+  void SpawnVehicleAtSpawnPoint(const APlayerStart &SpawnPoint);
+
+  /** If false, no walker will be spawned. */
   UPROPERTY(Category = "Vehicle Spawner", EditAnywhere)
   bool bSpawnVehicles = true;
 

Source/Carla/AI/WalkerSpawnerBase.h

@@ -64,6 +64,14 @@ public:
     return Walkers.Num() + WalkersBlackList.Num();
   }
 
+  const TArray<ACharacter *> &GetWalkersWhiteList() const {
+    return Walkers;
+  }
+
+  const TArray<ACharacter *> &GetWalkersBlackList() const {
+    return WalkersBlackList;
+  }
+
 private:
 
   const AWalkerSpawnPointBase &GetRandomSpawnPoint();

Source/Carla/Game/CarlaGameController.cpp

@@ -75,6 +75,9 @@ void CarlaGameController::RegisterPlayer(AController &NewPlayer)
 
 void CarlaGameController::BeginPlay()
 {
+  check(Player != nullptr);
+  GameState = Cast<ACarlaGameState>(Player->GetWorld()->GetGameState());
+  check(GameState != nullptr);
   if (Server != nullptr) {
     if (Errc::Success != Server->SendEpisodeReady(BLOCKING)) {
       UE_LOG(LogCarlaServer, Warning, TEXT("Failed to read episode start, server needs restart"));
@@ -108,7 +111,10 @@ void CarlaGameController::Tick(float DeltaSeconds)
 
   // Send measurements.
   if (Server != nullptr) {
-    if (Errc::Error == Server->SendMeasurements(Player->GetPlayerState())) {
+    check(GameState != nullptr);
+    if (Errc::Error == Server->SendMeasurements(
+            *GameState,
+            Player->GetPlayerState())) {
       Server = nullptr;
     }
   }

Source/Carla/Game/CarlaGameController.h

@@ -4,6 +4,7 @@
 
 #include "CarlaGameControllerBase.h"
 
+class ACarlaGameState;
 class ACarlaVehicleController;
 class CarlaServer;
 
@@ -34,5 +35,7 @@ private:
 
   ACarlaVehicleController *Player = nullptr;
 
+  const ACarlaGameState *GameState = nullptr;
+
   UCarlaSettings *CarlaSettings = nullptr;
 };

Source/Carla/Game/CarlaGameModeBase.cpp

@@ -124,6 +124,17 @@ void ACarlaGameModeBase::BeginPlay()
 {
   Super::BeginPlay();
 
+  auto CarlaGameState = Cast<ACarlaGameState>(GameState);
+  checkf(
+      CarlaGameState != nullptr,
+      TEXT("GameState is not a ACarlaGameState, did you forget to set it in the project settings?"));
+  if (WalkerSpawner != nullptr) {
+    CarlaGameState->WalkerSpawner = WalkerSpawner;
+  }
+  if (VehicleSpawner != nullptr) {
+    CarlaGameState->VehicleSpawner = VehicleSpawner;
+  }
+
   const auto &CarlaSettings = GameInstance->GetCarlaSettings();
 
   // Setup semantic segmentation if necessary.

Source/Carla/Game/CarlaGameState.h

@@ -13,7 +13,23 @@ class CARLA_API ACarlaGameState : public AGameStateBase
 {
 	GENERATED_BODY()
 
+public:
 
+  const AVehicleSpawnerBase *GetVehicleSpawner() const {
+    return VehicleSpawner;
+  }
 
+  const AWalkerSpawnerBase *GetWalkerSpawner() const {
+    return WalkerSpawner;
+  }
 
+private:
+
+  friend class ACarlaGameModeBase;
+
+  UPROPERTY()
+  AVehicleSpawnerBase *VehicleSpawner = nullptr;
+
+  UPROPERTY()
+  AWalkerSpawnerBase *WalkerSpawner = nullptr;
 };

Source/Carla/Game/CarlaServer.cpp

@@ -181,7 +181,68 @@ CarlaServer::ErrorCode CarlaServer::ReadControl(ACarlaVehicleController &Player,
   return ec;
 }
 
-CarlaServer::ErrorCode CarlaServer::SendMeasurements(const ACarlaPlayerState &PlayerState)
+static void SetBoxAndSpeed(carla_agent &values, const ACharacter *Walker)
+{
+  values.forward_speed = FVector::DotProduct(Walker->GetVelocity(), Walker->GetActorRotation().Vector()) * 0.036f;
+  /// @todo Perhaps the box it is not the same for every walker...
+  values.box_extent = {45.0f, 35.0f, 100.0f};
+}
+
+static void SetBoxAndSpeed(carla_agent &values, const AWheeledVehicle *Vehicle)
+{
+  /// @todo This is very slow and the code is duplicated at
+  /// ACarlaVehicleController.
+  values.forward_speed = Vehicle->GetVehicleMovementComponent()->GetForwardSpeed() * 0.036f;
+  TArray<UBoxComponent *> BoundingBoxes;
+  Vehicle->GetComponents<UBoxComponent>(BoundingBoxes);
+  if (BoundingBoxes.Num() > 0) {
+    Set(values.box_extent, BoundingBoxes[0]->GetScaledBoxExtent());
+  } else {
+    UE_LOG(LogCarla, Error, TEXT("Vehicle is missing the bounding box!"));
+  }
+}
+
+template <typename T>
+static void AddAgents(TArray<carla_agent> &Agents, const TArray<T> &Actors, uint32 type)
+{
+  for (auto &&Actor : Actors) {
+    Agents.Emplace();
+    auto &values = Agents.Last();
+    values.id = GetTypeHash(Actor);
+    values.type = type;
+    Set(values.transform, Actor->GetActorTransform());
+    SetBoxAndSpeed(values, Actor);
+  }
+}
+
+static void GetAgentInfo(
+    const ACarlaGameState &GameState,
+    TArray<carla_agent> &Agents)
+{
+  auto *WalkerSpawner = GameState.GetWalkerSpawner();
+  auto *VehicleSpawner = GameState.GetVehicleSpawner();
+
+  uint32 NumberOfAgents = 0u;
+  if (WalkerSpawner != nullptr) {
+    NumberOfAgents += WalkerSpawner->GetCurrentNumberOfWalkers();
+  }
+  if (VehicleSpawner != nullptr) {
+    NumberOfAgents += VehicleSpawner->GetNumberOfSpawnedVehicles();
+  }
+  Agents.Reserve(NumberOfAgents);
+
+  if (WalkerSpawner != nullptr) {
+    AddAgents(Agents, WalkerSpawner->GetWalkersWhiteList(), CARLA_SERVER_AGENT_PEDESTRIAN);
+    AddAgents(Agents, WalkerSpawner->GetWalkersBlackList(), CARLA_SERVER_AGENT_PEDESTRIAN);
+  }
+  if (VehicleSpawner != nullptr) {
+    AddAgents(Agents, VehicleSpawner->GetVehicles(), CARLA_SERVER_AGENT_VEHICLE);
+  }
+}
+
+CarlaServer::ErrorCode CarlaServer::SendMeasurements(
+    const ACarlaGameState &GameState,
+    const ACarlaPlayerState &PlayerState)
 {
   // Measurements.
   carla_measurements values;
@@ -197,6 +258,11 @@ CarlaServer::ErrorCode CarlaServer::SendMeasurements(const ACarlaPlayerState &Pl
   Set(player.intersection_otherlane, PlayerState.GetOtherLaneIntersectionFactor());
   Set(player.intersection_offroad, PlayerState.GetOffRoadIntersectionFactor());
 
+  TArray<carla_agent> Agents;
+  GetAgentInfo(GameState, Agents);
+  values.non_player_agents = (Agents.Num() > 0 ? Agents.GetData() : nullptr);
+  values.number_of_non_player_agents = Agents.Num();
+
   // Images.
   const auto NumberOfImages = PlayerState.GetNumberOfImages();
   TUniquePtr<carla_image[]> images;

Source/Carla/Game/CarlaServer.h

@@ -36,7 +36,9 @@ public:
 
   ErrorCode ReadControl(ACarlaVehicleController &Player, bool bBlocking);
 
-  ErrorCode SendMeasurements(const ACarlaPlayerState &PlayerState);
+  ErrorCode SendMeasurements(
+      const ACarlaGameState &GameState,
+      const ACarlaPlayerState &PlayerState);
 
 private:
 

Source/Carla/Game/MockGameController.cpp

@@ -6,7 +6,7 @@
 MockGameController::MockGameController(const FMockGameControllerSettings &InSettings) :
   Settings(InSettings) {}
 
-void MockGameController::Initialize(UCarlaSettings & CarlaSettings)
+void MockGameController::Initialize(UCarlaSettings &CarlaSettings)
 {
 #if WITH_EDITOR
   if (Settings.bOverrideCarlaSettings) {