Log vehicle control in world observer

Docs/python_api.md

@@ -101,8 +101,8 @@
 ## `carla.Vehicle(carla.Actor)`
 
 - `bounding_box`
-- `control`
 - `apply_control(vehicle_control)`
+- `get_vehicle_control()`
 - `set_autopilot(enabled=True)`
 
 ## `carla.TrafficLight(carla.Actor)`
@@ -258,7 +258,8 @@ Static presets
 
 ## `carla.TrafficLightState`
 
-- `Unknown`
+- `Off`
 - `Red`
 - `Yellow`
 - `Green`
+- `Unknown`

LibCarla/source/carla/client/TrafficLight.cpp

@@ -11,14 +11,8 @@
 namespace carla {
 namespace client {
 
-  TrafficLightState TrafficLight::GetState() {
-    auto state = GetEpisode().Lock()->GetActorDynamicState(*this).state;
-    switch (state) {
-      case 1u: return TrafficLightState::Red;
-      case 2u: return TrafficLightState::Yellow;
-      case 3u: return TrafficLightState::Green;
-      default: return TrafficLightState::Unknown;
-    }
+  rpc::TrafficLightState TrafficLight::GetState() {
+    return GetEpisode().Lock()->GetActorDynamicState(*this).state.traffic_light_state;
   }
 
 } // namespace client

LibCarla/source/carla/client/TrafficLight.h

@@ -7,17 +7,11 @@
 #pragma once
 
 #include "carla/client/Actor.h"
+#include "carla/rpc/TrafficLightState.h"
 
 namespace carla {
 namespace client {
 
-  enum class TrafficLightState {
-    Unknown,
-    Red,
-    Yellow,
-    Green
-  };
-
   class TrafficLight : public Actor {
   public:
 
@@ -27,7 +21,7 @@ namespace client {
     ///
     /// @note This function does not call the simulator, it returns the
     /// traffic light state received in the last tick.
-    TrafficLightState GetState();
+    rpc::TrafficLightState GetState();
   };
 
 } // namespace client

LibCarla/source/carla/client/Vehicle.cpp

@@ -11,6 +11,10 @@
 namespace carla {
 namespace client {
 
+  void Vehicle::SetAutopilot(bool enabled) {
+    GetEpisode().Lock()->SetVehicleAutopilot(*this, enabled);
+  }
+
   void Vehicle::ApplyControl(const Control &control) {
     if (control != _control) {
       GetEpisode().Lock()->ApplyControlToVehicle(*this, control);
@@ -18,8 +22,8 @@ namespace client {
     }
   }
 
-  void Vehicle::SetAutopilot(bool enabled) {
-    GetEpisode().Lock()->SetVehicleAutopilot(*this, enabled);
+  Vehicle::Control Vehicle::GetVehicleControl() const {
+    return GetEpisode().Lock()->GetActorDynamicState(*this).state.vehicle_control;
   }
 
 } // namespace client

LibCarla/source/carla/client/Vehicle.h

@@ -27,12 +27,9 @@ namespace client {
 
     /// Return the control last applied to this vehicle.
     ///
-    /// @warning This function only takes into account the control applied to
-    /// this instance of Vehicle. Note that several instances of Vehicle (even
-    /// in different processes) may point to the same vehicle in the simulator.
-    const Control &GetControl() const {
-      return _control;
-    }
+    /// @note This function does not call the simulator, it returns the Control
+    /// received in the last tick.
+    Control GetVehicleControl() const;
 
   private:
 

LibCarla/source/carla/client/detail/EpisodeState.h

@@ -30,7 +30,7 @@ namespace detail {
       geom::Transform transform;
       geom::Vector3D velocity;
       geom::Vector3D acceleration;
-      uint8_t state = 0u;
+      sensor::data::ActorDynamicState::TypeDependentState state;
     };
 
     const auto &GetTimestamp() const {

LibCarla/source/carla/rpc/TrafficLightState.h

@@ -0,0 +1,29 @@
+// Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma
+// de Barcelona (UAB).
+//
+// This work is licensed under the terms of the MIT license.
+// For a copy, see <https://opensource.org/licenses/MIT>.
+
+#pragma once
+
+#include "carla/MsgPack.h"
+
+#include <cstdint>
+
+namespace carla {
+namespace rpc {
+
+  enum class TrafficLightState : uint8_t {
+    Off,
+    Red,
+    Yellow,
+    Green,
+
+    Unknown,
+    SIZE
+  };
+
+} // namespace rpc
+} // namespace carla
+
+MSGPACK_ADD_ENUM(carla::rpc::TrafficLightState);

LibCarla/source/carla/sensor/data/ActorDynamicState.h

@@ -9,6 +9,8 @@
 #include "carla/geom/Transform.h"
 #include "carla/geom/Vector3D.h"
 #include "carla/rpc/ActorId.h"
+#include "carla/rpc/TrafficLightState.h"
+#include "carla/rpc/VehicleControl.h"
 
 #include <cstdint>
 
@@ -16,6 +18,37 @@ namespace carla {
 namespace sensor {
 namespace data {
 
+namespace detail {
+
+#pragma pack(push, 1)
+  class PackedVehicleControl {
+  public:
+
+    PackedVehicleControl() = default;
+
+    PackedVehicleControl(const rpc::VehicleControl &control)
+      : throttle(control.throttle),
+        steer(control.steer),
+        brake(control.brake),
+        hand_brake(control.hand_brake),
+        reverse(control.reverse) {}
+
+    operator rpc::VehicleControl() const {
+      return {throttle, steer, brake, hand_brake, reverse};
+    }
+
+  private:
+
+    float throttle;
+    float steer;
+    float brake;
+    bool hand_brake;
+    bool reverse;
+  };
+#pragma pack(pop)
+
+} // namespace detail
+
 #pragma pack(push, 1)
 
   /// Dynamic state of an actor at a certain frame.
@@ -27,12 +60,17 @@ namespace data {
 
     geom::Vector3D velocity;
 
-    uint8_t state;
+    union TypeDependentState {
+      rpc::TrafficLightState traffic_light_state;
+      detail::PackedVehicleControl vehicle_control;
+    } state;
   };
 
 #pragma pack(pop)
 
-  static_assert(sizeof(ActorDynamicState) == 1u + 10u * sizeof(uint32_t), "Invalid ActorDynamicState size!");
+  static_assert(
+      sizeof(ActorDynamicState) == 10u * sizeof(uint32_t) + sizeof(detail::PackedVehicleControl),
+      "Invalid ActorDynamicState size!");
 
 } // namespace data
 } // namespace sensor

PythonAPI/source/libcarla/Actor.cpp

@@ -7,6 +7,7 @@
 #include <carla/client/Actor.h>
 #include <carla/client/TrafficLight.h>
 #include <carla/client/Vehicle.h>
+#include <carla/rpc/TrafficLightState.h>
 
 #include <boost/python/suite/indexing/vector_indexing_suite.hpp>
 
@@ -59,17 +60,18 @@ void export_actor() {
 
   class_<cc::Vehicle, bases<cc::Actor>, boost::noncopyable, boost::shared_ptr<cc::Vehicle>>("Vehicle", no_init)
     .add_property("bounding_box", CALL_RETURNING_COPY(cc::Vehicle, GetBoundingBox))
-    .add_property("control", CALL_RETURNING_COPY(cc::Vehicle, GetControl))
     .def("apply_control", &cc::Vehicle::ApplyControl, (arg("control")))
+    .def("get_vehicle_control", &cc::Vehicle::GetVehicleControl)
     .def("set_autopilot", &cc::Vehicle::SetAutopilot, (arg("enabled")=true))
     .def(self_ns::str(self_ns::self))
   ;
 
-  enum_<cc::TrafficLightState>("TrafficLightState")
-    .value("Unknown", cc::TrafficLightState::Unknown)
-    .value("Red", cc::TrafficLightState::Red)
-    .value("Yellow", cc::TrafficLightState::Yellow)
-    .value("Green", cc::TrafficLightState::Green)
+  enum_<cr::TrafficLightState>("TrafficLightState")
+    .value("Off", cr::TrafficLightState::Off)
+    .value("Red", cr::TrafficLightState::Red)
+    .value("Yellow", cr::TrafficLightState::Yellow)
+    .value("Green", cr::TrafficLightState::Green)
+    .value("Unknown", cr::TrafficLightState::Unknown)
   ;
 
   class_<cc::TrafficLight, bases<cc::Actor>, boost::noncopyable, boost::shared_ptr<cc::TrafficLight>>("TrafficLight", no_init)

Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorRegistry.cpp

@@ -12,12 +12,20 @@
 #include "Carla/Game/Tagger.h"
 #include "Carla/Traffic/TrafficLightBase.h"
 
-static bool FActorRegistry_IsTrafficLight(const FActorView &View)
+static FActorView::ActorType FActorRegistry_GetActorType(const FActorView &View)
 {
-  return
-      View.IsValid() &&
-      View.GetSemanticTags().Contains(ECityObjectLabel::TrafficSigns) &&
-      (nullptr != Cast<ATrafficLightBase>(View.GetActor()));
+  if (View.IsValid())
+  {
+    if (nullptr != Cast<ACarlaWheeledVehicle>(View.GetActor()))
+    {
+      return FActorView::ActorType::Vehicle;
+    }
+    else if (nullptr != Cast<ATrafficLightBase>(View.GetActor()))
+    {
+      return FActorView::ActorType::TrafficLight;
+    }
+  }
+  return FActorView::ActorType::Other;
 }
 
 static FString GetRelevantTagAsString(const FActorView &View)
@@ -51,7 +59,7 @@ FActorView FActorRegistry::Register(AActor &Actor, FActorDescription Description
 
   auto View = FActorView(Id, Actor, std::move(Description));
   ATagger::GetTagsOfTaggedActor(Actor, View.SemanticTags);
-  View.bIsTrafficLight = FActorRegistry_IsTrafficLight(View);
+  View.Type = FActorRegistry_GetActorType(View);
 
   auto Result = ActorDatabase.emplace(Id, View);
   check(Result.second);

Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorView.h

@@ -18,6 +18,12 @@ public:
 
   using IdType = uint32;
 
+  enum class ActorType : uint8 {
+    Other,
+    Vehicle,
+    TrafficLight
+  };
+
   FActorView() = default;
   FActorView(const FActorView &) = default;
 
@@ -31,6 +37,11 @@ public:
     return Id;
   }
 
+  ActorType GetActorType() const
+  {
+    return Type;
+  }
+
   AActor *GetActor()
   {
     return TheActor;
@@ -51,11 +62,6 @@ public:
     return SemanticTags;
   }
 
-  bool IsTrafficLight() const
-  {
-    return bIsTrafficLight;
-  }
-
 private:
 
   friend class FActorRegistry;
@@ -67,12 +73,11 @@ private:
 
   IdType Id = 0u;
 
+  ActorType Type = ActorType::Other;
+
   AActor *TheActor = nullptr;
 
   TSharedPtr<const FActorDescription> Description = nullptr;
 
   TSet<ECityObjectLabel> SemanticTags;
-
-  /// @todo
-  bool bIsTrafficLight = false;
 };

Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/WorldObserver.cpp

@@ -13,19 +13,34 @@
 
 #include <compiler/disable-ue4-macros.h>
 #include <carla/sensor/SensorRegistry.h>
+#include <carla/sensor/data/ActorDynamicState.h>
 #include <compiler/enable-ue4-macros.h>
 
-static uint8 AWorldObserver_GetActorState(const FActorView &View)
+static auto AWorldObserver_GetActorState(const FActorView &View)
 {
-  if (View.IsTrafficLight())
+  using AType = FActorView::ActorType;
+
+  carla::sensor::data::ActorDynamicState::TypeDependentState state;
+
+  if (AType::Vehicle == View.GetActorType())
+  {
+    auto Vehicle = Cast<ACarlaWheeledVehicle>(View.GetActor());
+    if (Vehicle != nullptr)
+    {
+      state.vehicle_control = carla::rpc::VehicleControl{Vehicle->GetVehicleControl()};
+    }
+  }
+  else if (AType::TrafficLight == View.GetActorType())
   {
     auto TrafficLight = Cast<ATrafficLightBase>(View.GetActor());
     if (TrafficLight != nullptr)
     {
-      return static_cast<uint8>(TrafficLight->GetTrafficSignState());
+      using TLS = carla::rpc::TrafficLightState;
+      state.traffic_light_state = static_cast<TLS>(TrafficLight->GetTrafficSignState());
     }
   }
-  return 0u;
+
+  return state;
 }
 
 static carla::Buffer AWorldObserver_Serialize(

Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Traffic/TrafficSignBase.h

@@ -14,18 +14,18 @@
 
 UENUM(BlueprintType)
 enum class ETrafficSignState : uint8 {
-  UNKNOWN               UMETA(DisplayName = "UNKNOWN"),
-  TrafficLightRed       UMETA(DisplayName = "Traffic Light - Red"),
-  TrafficLightYellow    UMETA(DisplayName = "Traffic Light - Yellow"),
-  TrafficLightGreen     UMETA(DisplayName = "Traffic Light - Green"),
-  SpeedLimit_30         UMETA(DisplayName = "Speed Limit - 30"),
-  SpeedLimit_40         UMETA(DisplayName = "Speed Limit - 40"),
-  SpeedLimit_50         UMETA(DisplayName = "Speed Limit - 50"),
-  SpeedLimit_60         UMETA(DisplayName = "Speed Limit - 60"),
-  SpeedLimit_90         UMETA(DisplayName = "Speed Limit - 90"),
-  SpeedLimit_100        UMETA(DisplayName = "Speed Limit - 100"),
-  SpeedLimit_120        UMETA(DisplayName = "Speed Limit - 120"),
-  SpeedLimit_130        UMETA(DisplayName = "Speed Limit - 130")
+  UNKNOWN            = 0u   UMETA(DisplayName = "UNKNOWN"),
+  TrafficLightRed    = 1u   UMETA(DisplayName = "Traffic Light - Red"),
+  TrafficLightYellow = 2u   UMETA(DisplayName = "Traffic Light - Yellow"),
+  TrafficLightGreen  = 3u   UMETA(DisplayName = "Traffic Light - Green"),
+  SpeedLimit_30             UMETA(DisplayName = "Speed Limit - 30"),
+  SpeedLimit_40             UMETA(DisplayName = "Speed Limit - 40"),
+  SpeedLimit_50             UMETA(DisplayName = "Speed Limit - 50"),
+  SpeedLimit_60             UMETA(DisplayName = "Speed Limit - 60"),
+  SpeedLimit_90             UMETA(DisplayName = "Speed Limit - 90"),
+  SpeedLimit_100            UMETA(DisplayName = "Speed Limit - 100"),
+  SpeedLimit_120            UMETA(DisplayName = "Speed Limit - 120"),
+  SpeedLimit_130            UMETA(DisplayName = "Speed Limit - 130")
 };
 
 UCLASS()

Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/CarlaWheeledVehicle.cpp

@@ -86,29 +86,33 @@ void ACarlaWheeledVehicle::ApplyVehicleControl(const FVehicleControl &VehicleCon
 void ACarlaWheeledVehicle::SetThrottleInput(const float Value)
 {
   GetVehicleMovementComponent()->SetThrottleInput(Value);
+  Control.Throttle = Value;
 }
 
 void ACarlaWheeledVehicle::SetSteeringInput(const float Value)
 {
   GetVehicleMovementComponent()->SetSteeringInput(Value);
+  Control.Steer = Value;
 }
 
 void ACarlaWheeledVehicle::SetBrakeInput(const float Value)
 {
   GetVehicleMovementComponent()->SetBrakeInput(Value);
+  Control.Brake = Value;
 }
 
 void ACarlaWheeledVehicle::SetReverse(const bool Value)
 {
-  if (Value != bIsInReverse) {
-    bIsInReverse = Value;
+  if (Value != Control.bReverse) {
+    Control.bReverse = Value;
     auto MovementComponent = GetVehicleMovementComponent();
-    MovementComponent->SetUseAutoGears(!bIsInReverse);
-    MovementComponent->SetTargetGear(bIsInReverse ? -1 : 1, true);
+    MovementComponent->SetUseAutoGears(!Control.bReverse);
+    MovementComponent->SetTargetGear(Control.bReverse ? -1 : 1, true);
   }
 }
 
 void ACarlaWheeledVehicle::SetHandbrakeInput(const bool Value)
 {
   GetVehicleMovementComponent()->SetHandbrakeInput(Value);
+  Control.bHandBrake = Value;
 }

Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/CarlaWheeledVehicle.h

@@ -41,6 +41,13 @@ public:
   /// @{
 public:
 
+  /// Vehicle control currently applied to this vehicle.
+  UFUNCTION(Category = "CARLA Wheeled Vehicle", BlueprintCallable)
+  const FVehicleControl &GetVehicleControl() const
+  {
+    return Control;
+  }
+
   /// Transform of the vehicle. Location is shifted so it matches center of the
   /// vehicle bounds rather than the actor's location.
   UFUNCTION(Category = "CARLA Wheeled Vehicle", BlueprintCallable)
@@ -80,6 +87,12 @@ public:
   UFUNCTION(Category = "CARLA Wheeled Vehicle", BlueprintCallable)
   float GetMaximumSteerAngle() const;
 
+  UFUNCTION(Category = "CARLA Wheeled Vehicle", BlueprintCallable)
+  ECarlaWheeledVehicleState GetAIVehicleState() const
+  {
+    return State;
+  }
+
   /// @}
   // ===========================================================================
   /// @name Set functions
@@ -105,7 +118,7 @@ public:
   UFUNCTION(Category = "CARLA Wheeled Vehicle", BlueprintCallable)
   void ToggleReverse()
   {
-    SetReverse(!bIsInReverse);
+    SetReverse(!Control.bReverse);
   }
 
   UFUNCTION(Category = "CARLA Wheeled Vehicle", BlueprintCallable)
@@ -128,12 +141,6 @@ public:
     State = InState;
   }
 
-  UFUNCTION(Category = "AI Controller", BlueprintCallable)
-  ECarlaWheeledVehicleState GetAIVehicleState() const
-  {
-	  return State;
-  }
-
 private:
 
   /// Current state of the vehicle controller (for debugging purposes).
@@ -146,6 +153,5 @@ private:
   UPROPERTY(Category = "CARLA Wheeled Vehicle", VisibleAnywhere)
   UVehicleAgentComponent *VehicleAgentComponent;
 
-  UPROPERTY()
-  bool bIsInReverse = false;
+  FVehicleControl Control;
 };