Added failure states for vehicles (#5410)

* Added failure states for vehicles

* Added missing endline

CHANGELOG.md

@@ -1,5 +1,6 @@
 ## Latest
 
+  * Added failure state to vehicles, which can be retrieved by using `Vehicle.get_failure_state()`. Only Rollover failure state is currently supported.
   * Fixed bug causing the TM to block the simulation when another client teleported a vehicle with no physics.
   * Added check to avoid adding procedural trigger boxes inside intersections.
   * Python agents now accept a carla.Map and GlobalRoutePlanner instances as inputs, avoiding the need to recompute them.

Docs/python_api.md

@@ -2682,6 +2682,9 @@ Enables or disables the usage of CarSim vs terrain file specified in the `.simfi
 - <a name="carla.Vehicle.get_control"></a>**<font color="#7fb800">get_control</font>**(<font color="#00a6ed">**self**</font>)  
 The client returns the control applied in the last tick. The method does not call the simulator.  
     - **Return:** _[carla.VehicleControl](#carla.VehicleControl)_  
+- <a name="carla.Vehicle.get_failure_state"></a>**<font color="#7fb800">get_failure_state</font>**(<font color="#00a6ed">**self**</font>)  
+Vehicle have failure states, to  indicate that it is incapable of continuing its route. This function returns the vehicle's specific failure state, or in other words, the cause that resulted in it.  
+    - **Return:** _[carla.VehicleFailureState](#carla.VehicleFailureState)_  
 - <a name="carla.Vehicle.get_light_state"></a>**<font color="#7fb800">get_light_state</font>**(<font color="#00a6ed">**self**</font>)  
 Returns a flag representing the vehicle light state, this represents which lights are active or not.  
     - **Return:** _[carla.VehicleLightState](#carla.VehicleLightState)_  
@@ -2785,6 +2788,17 @@ Represents all doors.
 
 ---
 
+## carla.VehicleFailureState<a name="carla.VehicleFailureState"></a>
+Enum containing the different failure states of a vehicle, from which the it cannot recover. These are returned by __<font color="#7fb800">get_failure_state()</font>__ and only Rollover is currently implemented.  
+
+### Instance Variables
+- <a name="carla.VehicleFailureState.NONE"></a>**<font color="#f8805a">NONE</font>**  
+- <a name="carla.VehicleFailureState.Rollover"></a>**<font color="#f8805a">Rollover</font>**  
+- <a name="carla.VehicleFailureState.Engine"></a>**<font color="#f8805a">Engine</font>**  
+- <a name="carla.VehicleFailureState.TirePuncture"></a>**<font color="#f8805a">TirePuncture</font>**  
+
+---
+
 ## carla.VehicleLightState<a name="carla.VehicleLightState"></a>
 Class that recaps the state of the lights of a vehicle, these can be used as a flags. E.g: `VehicleLightState.HighBeam & VehicleLightState.Brake` will return `True` when both are active. Lights are off by default in any situation and should be managed by the user via script. The blinkers blink automatically. _Warning: Right now, not all vehicles have been prepared to work with this functionality, this will be added to all of them in later updates_.  
 

LibCarla/source/carla/client/Vehicle.cpp

@@ -144,5 +144,9 @@ namespace client {
         BaseJSONPath);
   }
 
+  rpc::VehicleFailureState Vehicle::GetFailureState() const {
+    return GetEpisode().Lock()->GetActorSnapshot(*this).state.vehicle_data.failure_state;
+  }
+
 } // namespace client
 } // namespace carla

LibCarla/source/carla/client/Vehicle.h

@@ -137,12 +137,15 @@ namespace client {
         std::string TireJSON = "",
         std::string BaseJSONPath = "");
 
+    /// Returns the failure state of the vehicle
+    rpc::VehicleFailureState GetFailureState() const;
+
   private:
 
     const bool _is_control_sticky;
 
     Control _control;
   };
-  
+
 } // namespace client
 } // namespace carla

LibCarla/source/carla/rpc/VehicleFailureState.h

@@ -0,0 +1,22 @@
+// Copyright (c) 2021 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 <cstdint>
+
+namespace carla {
+namespace rpc {
+
+  enum class VehicleFailureState : uint8_t {
+    None,
+    Rollover,
+    Engine,
+    TirePuncture,
+  };
+
+} // namespace rpc
+} // namespace carla

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

@@ -10,6 +10,7 @@
 #include "carla/geom/Vector3D.h"
 #include "carla/rpc/ActorId.h"
 #include "carla/rpc/ActorState.h"
+#include "carla/rpc/VehicleFailureState.h"
 #include "carla/rpc/TrafficLightState.h"
 #include "carla/rpc/VehicleControl.h"
 #include "carla/rpc/WalkerControl.h"
@@ -63,6 +64,7 @@ namespace detail {
     rpc::TrafficLightState traffic_light_state;
     bool has_traffic_light;
     rpc::ActorId traffic_light_id;
+    rpc::VehicleFailureState failure_state;
   };
 #pragma pack(pop)
 

PythonAPI/carla/source/libcarla/Actor.cpp

@@ -162,6 +162,13 @@ void export_actor() {
     .value("All", cr::VehicleDoor::All)
   ;
 
+  enum_<cr::VehicleFailureState>("VehicleFailureState")
+    .value("NONE", cr::VehicleFailureState::None)
+    .value("Rollover", cr::VehicleFailureState::Rollover)
+    .value("Engine", cr::VehicleFailureState::Engine)
+    .value("TirePuncture", cr::VehicleFailureState::TirePuncture)
+  ;
+
   class_<cc::Vehicle, bases<cc::Actor>, boost::noncopyable, boost::shared_ptr<cc::Vehicle>>("Vehicle",
       no_init)
       .def("apply_control", &cc::Vehicle::ApplyControl, (arg("control")))
@@ -186,6 +193,7 @@ void export_actor() {
       .def("enable_carsim", &cc::Vehicle::EnableCarSim, (arg("simfile_path") = ""))
       .def("use_carsim_road", &cc::Vehicle::UseCarSimRoad, (arg("enabled")))
       .def("enable_chrono_physics", &cc::Vehicle::EnableChronoPhysics, (arg("max_substeps")=30, arg("max_substep_delta_time")=0.002, arg("vehicle_json")="", arg("powetrain_json")="", arg("tire_json")="", arg("base_json_path")=""))
+      .def("get_failure_state", &cc::Vehicle::GetFailureState)
       .def(self_ns::str(self_ns::self))
   ;
 

PythonAPI/docs/actor.yml

@@ -403,6 +403,12 @@
       note: >
         Returns the angle based on the physics of the wheel, not the visual angle.
     # --------------------------------------
+    - def_name: get_failure_state
+      return: carla.VehicleFailureState
+      doc: >
+        Vehicle have failure states, to  indicate that it is incapable of continuing its route. This function returns the vehicle's specific failure state, or in other words, the cause that resulted in it.
+    # --------------------------------------
+
 
   - class_name: Walker
     parent: carla.Actor
@@ -710,4 +716,16 @@
       doc: >
         Represents all doors.
     # --------------------------------------
+
+  - class_name: VehicleFailureState
+    # - DESCRIPTION ------------------------
+    doc: >
+      Enum containing the different failure states of a vehicle, from which the it cannot recover. These are returned by __<font color="#7fb800">get_failure_state()</font>__ and only Rollover is currently implemented.
+
+    # - PROPERTIES -------------------------
+    instance_variables:
+    - var_name: NONE
+    - var_name: Rollover
+    - var_name: Engine
+    - var_name: TirePuncture
 ...

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

@@ -112,6 +112,7 @@ void FVehicleData::RecordActorData(FCarlaActor* CarlaActor, UCarlaEpisode* Carla
   {
     SpeedLimit = Controller->GetSpeedLimit();
   }
+  FailureState = Vehicle->GetFailureState();
 }
 
 void FVehicleData::RestoreActorData(FCarlaActor* CarlaActor, UCarlaEpisode* CarlaEpisode)
@@ -129,7 +130,7 @@ void FVehicleData::RestoreActorData(FCarlaActor* CarlaActor, UCarlaEpisode* Carl
   {
     Vehicle->ApplyVehicleControl(Control, EVehicleInputPriority::Client);
   }
-  else 
+  else
   {
     Vehicle->ApplyVehicleAckermannControl(AckermannControl, EVehicleInputPriority::Client);
   }
@@ -139,6 +140,7 @@ void FVehicleData::RestoreActorData(FCarlaActor* CarlaActor, UCarlaEpisode* Carl
   {
     Controller->SetSpeedLimit(SpeedLimit);
   }
+  Vehicle->SetFailureState(FailureState);
 }
 
 void FWalkerData::RecordActorData(FCarlaActor* CarlaActor, UCarlaEpisode* CarlaEpisode)

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

@@ -69,6 +69,8 @@ public:
 
   float SpeedLimit = 30;
 
+  carla::rpc::VehicleFailureState FailureState;
+
   virtual void RecordActorData(FCarlaActor* CarlaActor, UCarlaEpisode* CarlaEpisode) override;
 
   virtual void RestoreActorData(FCarlaActor* CarlaActor, UCarlaEpisode* CarlaEpisode) override;

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

@@ -650,6 +650,25 @@ ECarlaServerResponse FVehicleActor::GetPhysicsControl(FVehiclePhysicsControl& Ph
   return ECarlaServerResponse::Success;
 }
 
+ECarlaServerResponse FVehicleActor::GetFailureState(carla::rpc::VehicleFailureState& FailureState)
+{
+  if (IsDormant())
+  {
+    FVehicleData* ActorData = GetActorData<FVehicleData>();
+    FailureState = ActorData->FailureState;
+  }
+  else
+  {
+    auto Vehicle = Cast<ACarlaWheeledVehicle>(GetActor());
+    if (Vehicle == nullptr)
+    {
+      return ECarlaServerResponse::NotAVehicle;
+    }
+    FailureState = Vehicle->GetFailureState();
+  }
+  return ECarlaServerResponse::Success;
+}
+
 ECarlaServerResponse FVehicleActor::GetVehicleLightState(FVehicleLightState& LightState)
 {
   if (IsDormant())

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

@@ -243,6 +243,11 @@ public:
     return ECarlaServerResponse::ActorTypeMismatch;
   }
 
+  virtual ECarlaServerResponse GetFailureState(carla::rpc::VehicleFailureState&)
+  {
+    return ECarlaServerResponse::ActorTypeMismatch;
+  }
+
   virtual ECarlaServerResponse GetVehicleLightState(FVehicleLightState&)
   {
     return ECarlaServerResponse::ActorTypeMismatch;
@@ -472,6 +477,8 @@ public:
 
   virtual ECarlaServerResponse GetPhysicsControl(FVehiclePhysicsControl& PhysicsControl) final;
 
+  virtual ECarlaServerResponse GetFailureState(carla::rpc::VehicleFailureState&) final;
+
   virtual ECarlaServerResponse GetVehicleLightState(FVehicleLightState& LightState) final;
 
   virtual ECarlaServerResponse OpenVehicleDoor(const EVehicleDoor DoorIdx) final;
@@ -594,7 +601,7 @@ public:
   virtual ECarlaServerResponse GetWalkerControl(FWalkerControl&) final;
 
   virtual ECarlaServerResponse GetBonesTransform(FWalkerBoneControlOut&) final;
-  
+
   virtual ECarlaServerResponse SetBonesTransform(const FWalkerBoneControlIn&) final;
 
   virtual ECarlaServerResponse BlendPose(float Blend);

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

@@ -61,6 +61,9 @@ static auto FWorldObserver_GetActorState(const FCarlaActor &View, const FActorRe
           state.vehicle_data.has_traffic_light = false;
         }
       }
+      // Get the failure state by checking the rollover one as it is the only one currently implemented.
+      // This will have to be expanded once more states are added
+      state.vehicle_data.failure_state = Vehicle->GetFailureState();
     }
   }
 

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

@@ -775,6 +775,11 @@ void ACarlaWheeledVehicle::SetVehicleLightState(const FVehicleLightState &LightS
   RefreshLightState(LightState);
 }
 
+void ACarlaWheeledVehicle::SetFailureState(const carla::rpc::VehicleFailureState &InFailureState)
+{
+  FailureState = InFailureState;
+}
+
 void ACarlaWheeledVehicle::SetCarlaMovementComponent(UBaseCarlaMovementComponent* MovementComponent)
 {
   if (BaseMovementComponent)
@@ -962,20 +967,26 @@ void ACarlaWheeledVehicle::ApplyRolloverBehavior()
 {
   auto roll = GetVehicleTransform().Rotator().Roll;
 
-  // The angular velocity reduction is applied in 4 stages, to improve its smoothness
+  // The angular velocity reduction is applied in 4 stages, to improve its smoothness.
+  // Case 4 starts the timer to set the rollover flag, so users are notified.
   switch (RolloverBehaviorTracker) {
     case 0: CheckRollover(roll, std::make_pair(130.0, 230.0));      break;
     case 1: CheckRollover(roll, std::make_pair(140.0, 220.0));      break;
     case 2: CheckRollover(roll, std::make_pair(150.0, 210.0));      break;
     case 3: CheckRollover(roll, std::make_pair(160.0, 200.0));      break;
-    case 4: break;
+    case 4:
+      GetWorld()->GetTimerManager().SetTimer(TimerHandler, this, &ACarlaWheeledVehicle::SetRolloverFlag, RolloverFlagTime);
+      RolloverBehaviorTracker += 1;
+      break;
+    case 5: break;
     default:
-      RolloverBehaviorTracker = 4;
+      RolloverBehaviorTracker = 5;
   }
 
   // In case the vehicle recovers, reset the rollover tracker
   if (RolloverBehaviorTracker > 0 && -30 < roll && roll < 30){
     RolloverBehaviorTracker = 0;
+    FailureState = carla::rpc::VehicleFailureState::None;
   }
 }
 
@@ -987,3 +998,14 @@ void ACarlaWheeledVehicle::CheckRollover(const float roll, const std::pair<float
     RolloverBehaviorTracker += 1;
   }
 }
+
+void ACarlaWheeledVehicle::SetRolloverFlag(){
+  // Make sure the vehicle hasn't recovered since the timer started
+  if (RolloverBehaviorTracker >= 4) {
+    FailureState = carla::rpc::VehicleFailureState::Rollover;
+  }
+}
+
+carla::rpc::VehicleFailureState ACarlaWheeledVehicle::GetFailureState() const{
+  return FailureState;
+}

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

@@ -33,6 +33,7 @@
 
 #include <utility>
 
+#include "carla/rpc/VehicleFailureState.h"
 #include "CarlaWheeledVehicle.generated.h"
 
 class UBoxComponent;
@@ -186,6 +187,8 @@ public:
 
   void SetVehicleLightState(const FVehicleLightState &LightState);
 
+  void SetFailureState(const carla::rpc::VehicleFailureState &FailureState);
+
   UFUNCTION(BlueprintNativeEvent)
   bool IsTwoWheeledVehicle();
   virtual bool IsTwoWheeledVehicle_Implementation() {
@@ -346,6 +349,9 @@ private:
 
   float RolloverBehaviorForce = 0.35;
   int RolloverBehaviorTracker = 0;
+  float RolloverFlagTime = 5.0f;
+
+  carla::rpc::VehicleFailureState FailureState = carla::rpc::VehicleFailureState::None;
 
 public:
 
@@ -380,6 +386,10 @@ public:
   UPROPERTY(Category="CARLA Wheeled Vehicle", VisibleAnywhere)
   bool bIsNWVehicle = false;
 
+  void SetRolloverFlag();
+
+  carla::rpc::VehicleFailureState GetFailureState() const;
+
 private:
 
   UPROPERTY(Category="CARLA Wheeled Vehicle", VisibleAnywhere)
@@ -409,4 +419,5 @@ private:
 
   void CheckRollover(const float roll, const std::pair<float, float> threshold_roll);
 
+  FTimerHandle TimerHandler;
 };