From a649c67d7edbab81b1f85176c863dccf36a5a892 Mon Sep 17 00:00:00 2001 From: glopezdiest <58212725+glopezdiest@users.noreply.github.com> Date: Mon, 16 May 2022 21:55:19 +0200 Subject: [PATCH] Added failure states for vehicles (#5410) * Added failure states for vehicles * Added missing endline --- CHANGELOG.md | 1 + Docs/python_api.md | 14 ++++++++++ LibCarla/source/carla/client/Vehicle.cpp | 4 +++ LibCarla/source/carla/client/Vehicle.h | 5 +++- .../source/carla/rpc/VehicleFailureState.h | 22 +++++++++++++++ .../carla/sensor/data/ActorDynamicState.h | 2 ++ PythonAPI/carla/source/libcarla/Actor.cpp | 8 ++++++ PythonAPI/docs/actor.yml | 18 ++++++++++++ .../Carla/Source/Carla/Actor/ActorData.cpp | 4 ++- .../Carla/Source/Carla/Actor/ActorData.h | 2 ++ .../Carla/Source/Carla/Actor/CarlaActor.cpp | 19 +++++++++++++ .../Carla/Source/Carla/Actor/CarlaActor.h | 9 +++++- .../Source/Carla/Sensor/WorldObserver.cpp | 3 ++ .../Carla/Vehicle/CarlaWheeledVehicle.cpp | 28 +++++++++++++++++-- .../Carla/Vehicle/CarlaWheeledVehicle.h | 11 ++++++++ 15 files changed, 144 insertions(+), 6 deletions(-) create mode 100644 LibCarla/source/carla/rpc/VehicleFailureState.h diff --git a/CHANGELOG.md b/CHANGELOG.md index 24e241dd8..5d93858da 100644 --- a/CHANGELOG.md +++ b/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. diff --git a/Docs/python_api.md b/Docs/python_api.md index 24f47d555..98fe9ae9c 100644 --- a/Docs/python_api.md +++ b/Docs/python_api.md @@ -2682,6 +2682,9 @@ Enables or disables the usage of CarSim vs terrain file specified in the `.simfi - **get_control**(**self**) The client returns the control applied in the last tick. The method does not call the simulator. - **Return:** _[carla.VehicleControl](#carla.VehicleControl)_ +- **get_failure_state**(**self**) +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)_ - **get_light_state**(**self**) 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 +Enum containing the different failure states of a vehicle, from which the it cannot recover. These are returned by __get_failure_state()__ and only Rollover is currently implemented. + +### Instance Variables +- **NONE** +- **Rollover** +- **Engine** +- **TirePuncture** + +--- + ## carla.VehicleLightState 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_. diff --git a/LibCarla/source/carla/client/Vehicle.cpp b/LibCarla/source/carla/client/Vehicle.cpp index 70950fd2c..2e9348910 100644 --- a/LibCarla/source/carla/client/Vehicle.cpp +++ b/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 diff --git a/LibCarla/source/carla/client/Vehicle.h b/LibCarla/source/carla/client/Vehicle.h index ef7411e1d..cf5a37a17 100644 --- a/LibCarla/source/carla/client/Vehicle.h +++ b/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 diff --git a/LibCarla/source/carla/rpc/VehicleFailureState.h b/LibCarla/source/carla/rpc/VehicleFailureState.h new file mode 100644 index 000000000..64c617928 --- /dev/null +++ b/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 . + +#pragma once + +#include + +namespace carla { +namespace rpc { + + enum class VehicleFailureState : uint8_t { + None, + Rollover, + Engine, + TirePuncture, + }; + +} // namespace rpc +} // namespace carla diff --git a/LibCarla/source/carla/sensor/data/ActorDynamicState.h b/LibCarla/source/carla/sensor/data/ActorDynamicState.h index 804953dc9..41570b290 100644 --- a/LibCarla/source/carla/sensor/data/ActorDynamicState.h +++ b/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) diff --git a/PythonAPI/carla/source/libcarla/Actor.cpp b/PythonAPI/carla/source/libcarla/Actor.cpp index 155dbc748..1c6271057 100644 --- a/PythonAPI/carla/source/libcarla/Actor.cpp +++ b/PythonAPI/carla/source/libcarla/Actor.cpp @@ -162,6 +162,13 @@ void export_actor() { .value("All", cr::VehicleDoor::All) ; + enum_("VehicleFailureState") + .value("NONE", cr::VehicleFailureState::None) + .value("Rollover", cr::VehicleFailureState::Rollover) + .value("Engine", cr::VehicleFailureState::Engine) + .value("TirePuncture", cr::VehicleFailureState::TirePuncture) + ; + class_, boost::noncopyable, boost::shared_ptr>("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)) ; diff --git a/PythonAPI/docs/actor.yml b/PythonAPI/docs/actor.yml index 6a186fbd6..d4875c567 100644 --- a/PythonAPI/docs/actor.yml +++ b/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 __get_failure_state()__ and only Rollover is currently implemented. + + # - PROPERTIES ------------------------- + instance_variables: + - var_name: NONE + - var_name: Rollover + - var_name: Engine + - var_name: TirePuncture ... diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorData.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorData.cpp index c89ba1150..f62f5aea3 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorData.cpp +++ b/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) diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorData.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorData.h index 6f6a8af55..4a7186743 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorData.h +++ b/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; diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/CarlaActor.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/CarlaActor.cpp index 12de72a23..b2739f8ee 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/CarlaActor.cpp +++ b/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(); + FailureState = ActorData->FailureState; + } + else + { + auto Vehicle = Cast(GetActor()); + if (Vehicle == nullptr) + { + return ECarlaServerResponse::NotAVehicle; + } + FailureState = Vehicle->GetFailureState(); + } + return ECarlaServerResponse::Success; +} + ECarlaServerResponse FVehicleActor::GetVehicleLightState(FVehicleLightState& LightState) { if (IsDormant()) diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/CarlaActor.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/CarlaActor.h index 5f4c6d94c..b47969c87 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/CarlaActor.h +++ b/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); diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/WorldObserver.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/WorldObserver.cpp index 16791d2f0..c3c805330 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/WorldObserver.cpp +++ b/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(); } } diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/CarlaWheeledVehicle.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/CarlaWheeledVehicle.cpp index 5ba23131d..65fa13b9b 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/CarlaWheeledVehicle.cpp +++ b/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= 4) { + FailureState = carla::rpc::VehicleFailureState::Rollover; + } +} + +carla::rpc::VehicleFailureState ACarlaWheeledVehicle::GetFailureState() const{ + return FailureState; +} diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/CarlaWheeledVehicle.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/CarlaWheeledVehicle.h index 600627dc6..b07fb313d 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/CarlaWheeledVehicle.h +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/CarlaWheeledVehicle.h @@ -33,6 +33,7 @@ #include +#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 threshold_roll); + FTimerHandle TimerHandler; };