From 1a1ed0ba9a72844385b40d7ca8800b5c8de17df9 Mon Sep 17 00:00:00 2001 From: Charles Khoury Date: Mon, 20 Jan 2025 16:45:48 -0500 Subject: [PATCH] Port Telemetry Data (see #5153) - WIP --- CHANGELOG.md | 3 + LibCarla/source/carla/client/Vehicle.cpp | 4 + LibCarla/source/carla/client/Vehicle.h | 7 + .../source/carla/client/detail/Client.cpp | 5 + LibCarla/source/carla/client/detail/Client.h | 3 + .../source/carla/client/detail/Simulator.h | 4 + .../source/carla/rpc/VehicleTelemetryData.h | 115 +++++++++++++++++ .../source/carla/rpc/WheelTelemetryData.h | 120 ++++++++++++++++++ PythonAPI/carla/include/PythonAPI.h | 29 +++++ PythonAPI/carla/src/Actor.cpp | 1 + PythonAPI/carla/src/Control.cpp | 88 +++++++++++++ .../Carla/Source/Carla/Actor/CarlaActor.cpp | 19 +++ .../Carla/Source/Carla/Actor/CarlaActor.h | 8 ++ .../Carla/Source/Carla/Server/CarlaServer.cpp | 26 ++++ .../Carla/Vehicle/CarlaWheeledVehicle.cpp | 50 ++++++++ .../Carla/Vehicle/CarlaWheeledVehicle.h | 4 + .../Carla/Vehicle/VehicleTelemetryData.h | 78 ++++++++++++ .../WheeledVehicleMovementComponentNW.cpp | 2 +- 18 files changed, 565 insertions(+), 1 deletion(-) create mode 100644 LibCarla/source/carla/rpc/VehicleTelemetryData.h create mode 100644 LibCarla/source/carla/rpc/WheelTelemetryData.h create mode 100644 Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Vehicle/VehicleTelemetryData.h diff --git a/CHANGELOG.md b/CHANGELOG.md index 76490f4fb9b..6f0b2a739d3 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,3 +1,6 @@ +## Latest Changes +* Ported API function `get_telemetry_data` to the vehicle actor from UE4 version. + ## CARLA 0.10.0 * Unreal Engine migration from version 4.26 to version 5.5 diff --git a/LibCarla/source/carla/client/Vehicle.cpp b/LibCarla/source/carla/client/Vehicle.cpp index 201afa239e4..6a7313ccc19 100644 --- a/LibCarla/source/carla/client/Vehicle.cpp +++ b/LibCarla/source/carla/client/Vehicle.cpp @@ -44,6 +44,10 @@ namespace client { } } + Vehicle::TelemetryData Vehicle::GetTelemetryData() const { + return GetEpisode().Lock()->GetVehicleTelemetryData(*this); + } + void Vehicle::ShowDebugTelemetry(bool enabled) { GetEpisode().Lock()->ShowVehicleDebugTelemetry(*this, enabled); } diff --git a/LibCarla/source/carla/client/Vehicle.h b/LibCarla/source/carla/client/Vehicle.h index a086e775f51..d8b10c7b942 100644 --- a/LibCarla/source/carla/client/Vehicle.h +++ b/LibCarla/source/carla/client/Vehicle.h @@ -14,6 +14,7 @@ #include "carla/rpc/VehicleDoor.h" #include "carla/rpc/VehicleLightState.h" #include "carla/rpc/VehiclePhysicsControl.h" +#include "carla/rpc/VehicleTelemetryData.h" #include "carla/rpc/VehicleWheels.h" #include "carla/trafficmanager/TrafficManager.h" @@ -35,6 +36,7 @@ namespace client { using Control = rpc::VehicleControl; using AckermannControl = rpc::VehicleAckermannControl; using PhysicsControl = rpc::VehiclePhysicsControl; + using TelemetryData = rpc::VehicleTelemetryData; using LightState = rpc::VehicleLightState::LightState; using TM = traffic_manager::TrafficManager; using VehicleDoor = rpc::VehicleDoor; @@ -46,6 +48,11 @@ namespace client { /// Switch on/off this vehicle's autopilot. void SetAutopilot(bool enabled = true, uint16_t tm_port = TM_DEFAULT_PORT); + /// Return the telemetry data for this vehicle. + /// + /// @warning This function does call the simulator. + TelemetryData GetTelemetryData() const; + /// Switch on/off this vehicle's autopilot. void ShowDebugTelemetry(bool enabled = true); diff --git a/LibCarla/source/carla/client/detail/Client.cpp b/LibCarla/source/carla/client/detail/Client.cpp index 76143d24d97..29ec6b54ffd 100644 --- a/LibCarla/source/carla/client/detail/Client.cpp +++ b/LibCarla/source/carla/client/detail/Client.cpp @@ -448,6 +448,11 @@ namespace detail { _pimpl->AsyncCall("set_actor_autopilot", vehicle, enabled); } + rpc::VehicleTelemetryData Client::GetVehicleTelemetryData( + rpc::ActorId vehicle) const { + return _pimpl->CallAndWait("get_telemetry_data", vehicle); + } + void Client::ShowVehicleDebugTelemetry(rpc::ActorId vehicle, const bool enabled) { _pimpl->AsyncCall("show_vehicle_debug_telemetry", vehicle, enabled); } diff --git a/LibCarla/source/carla/client/detail/Client.h b/LibCarla/source/carla/client/detail/Client.h index 1761636cab7..009071335a4 100644 --- a/LibCarla/source/carla/client/detail/Client.h +++ b/LibCarla/source/carla/client/detail/Client.h @@ -29,6 +29,7 @@ #include "carla/rpc/VehicleLightStateList.h" #include "carla/rpc/VehicleLightState.h" #include "carla/rpc/VehiclePhysicsControl.h" +#include "carla/rpc/VehicleTelemetryData.h" #include "carla/rpc/VehicleWheels.h" #include "carla/rpc/WeatherParameters.h" #include "carla/rpc/Texture.h" @@ -263,6 +264,8 @@ namespace detail { rpc::ActorId vehicle, bool enabled); + rpc::VehicleTelemetryData GetVehicleTelemetryData(rpc::ActorId vehicle) const; + void ShowVehicleDebugTelemetry( rpc::ActorId vehicle, bool enabled); diff --git a/LibCarla/source/carla/client/detail/Simulator.h b/LibCarla/source/carla/client/detail/Simulator.h index 8c0225b1a6d..26a22b83aa4 100644 --- a/LibCarla/source/carla/client/detail/Simulator.h +++ b/LibCarla/source/carla/client/detail/Simulator.h @@ -510,6 +510,10 @@ namespace detail { _client.SetActorAutopilot(vehicle.GetId(), enabled); } + rpc::VehicleTelemetryData GetVehicleTelemetryData(const Vehicle &vehicle) const { + return _client.GetVehicleTelemetryData(vehicle.GetId()); + } + void ShowVehicleDebugTelemetry(Vehicle &vehicle, bool enabled = true) { _client.ShowVehicleDebugTelemetry(vehicle.GetId(), enabled); } diff --git a/LibCarla/source/carla/rpc/VehicleTelemetryData.h b/LibCarla/source/carla/rpc/VehicleTelemetryData.h new file mode 100644 index 00000000000..443ed43bc7d --- /dev/null +++ b/LibCarla/source/carla/rpc/VehicleTelemetryData.h @@ -0,0 +1,115 @@ +// Copyright (c) 2022 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 "carla/MsgPack.h" +#include "carla/rpc/WheelTelemetryData.h" + +#include + +namespace carla { + namespace rpc { + class VehicleTelemetryData { + public: + + VehicleTelemetryData() = default; + + VehicleTelemetryData( + float speed, + float steer, + float throttle, + float brake, + float engine_rpm, + int32_t gear, + float drag, + std::vector wheels) + : speed(speed), + steer(steer), + throttle(throttle), + brake(brake), + engine_rpm(engine_rpm), + gear(gear), + drag(drag), + wheels(wheels) {} + + float speed = 0.0f; + float steer = 0.0f; + float throttle = 0.0f; + float brake = 0.0f; + float engine_rpm = 0.0f; + int32_t gear = 0.0f; + float drag = 0.0f; + std::vector wheels; + + bool operator!=(const VehicleTelemetryData &rhs) const { + return + speed != rhs.speed || + steer != rhs.steer || + throttle != rhs.throttle || + brake != rhs.brake || + engine_rpm != rhs.engine_rpm || + gear != rhs.gear || + drag != rhs.drag || + wheels != rhs.wheels; + } + + bool operator==(const VehicleTelemetryData &rhs) const { + return !(*this != rhs); + } + + #ifdef LIBCARLA_INCLUDED_FROM_UE4 + + VehicleTelemetryData(const FVehicleTelemetryData &TelemetryData) { + speed = TelemetryData.Speed; + steer = TelemetryData.Steer; + throttle = TelemetryData.Throttle; + brake = TelemetryData.Brake; + engine_rpm = TelemetryData.EngineRPM; + gear = TelemetryData.Gear; + drag = TelemetryData.Drag; + + // Wheels Setup + wheels = std::vector(); + for (const auto &Wheel : TelemetryData.Wheels) { + wheels.push_back(WheelTelemetryData(Wheel)); + } + } + + operator FVehicleTelemetryData() const { + FVehicleTelemetryData TelemetryData; + + TelemetryData.Speed = speed; + TelemetryData.Steer = steer; + TelemetryData.Throttle = throttle; + TelemetryData.Brake = brake; + TelemetryData.EngineRPM = engine_rpm; + TelemetryData.Gear = gear; + TelemetryData.Drag = drag; + + TArray Wheels; + for (const auto &wheel : wheels) { + Wheels.Add(FWheelTelemetryData(wheel)); + } + TelemetryData.Wheels = Wheels; + + return TelemetryData; + } + + #endif + + MSGPACK_DEFINE_ARRAY(speed, + steer, + throttle, + brake, + engine_rpm, + gear, + drag, + wheels); + }; + + } // namespace rpc +} // namespace carla \ No newline at end of file diff --git a/LibCarla/source/carla/rpc/WheelTelemetryData.h b/LibCarla/source/carla/rpc/WheelTelemetryData.h new file mode 100644 index 00000000000..2318af9081b --- /dev/null +++ b/LibCarla/source/carla/rpc/WheelTelemetryData.h @@ -0,0 +1,120 @@ +// Copyright (c) 2022 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 "carla/MsgPack.h" + +namespace carla { +namespace rpc { + + class WheelTelemetryData { + public: + + WheelTelemetryData() = default; + + WheelTelemetryData( + float tire_friction, + float lat_slip, + float long_slip, + float omega, + float tire_load, + float normalized_tire_load, + float torque, + float long_force, + float lat_force, + float normalized_long_force, + float normalized_lat_force) + : tire_friction(tire_friction), + lat_slip(lat_slip), + long_slip(long_slip), + omega(omega), + tire_load(tire_load), + normalized_tire_load(normalized_tire_load), + torque(torque), + long_force(long_force), + lat_force(lat_force), + normalized_long_force(normalized_long_force), + normalized_lat_force(normalized_lat_force) {} + + float tire_friction = 0.0f; + float lat_slip = 0.0f; + float long_slip = 0.0f; + float omega = 0.0f; + float tire_load = 0.0f; + float normalized_tire_load = 0.0f; + float torque = 0.0f; + float long_force = 0.0f; + float lat_force = 0.0f; + float normalized_long_force = 0.0f; + float normalized_lat_force = 0.0f; + + bool operator!=(const WheelTelemetryData &rhs) const { + return + tire_friction != rhs.tire_friction || + lat_slip != rhs.lat_slip || + long_slip != rhs.long_slip || + omega != rhs.omega || + tire_load != rhs.tire_load || + normalized_tire_load != rhs.normalized_tire_load || + torque != rhs.torque || + long_force != rhs.long_force || + lat_force != rhs.lat_force || + normalized_long_force != rhs.normalized_long_force || + normalized_lat_force != rhs.normalized_lat_force; + } + + bool operator==(const WheelTelemetryData &rhs) const { + return !(*this != rhs); + } +#ifdef LIBCARLA_INCLUDED_FROM_UE4 + + WheelTelemetryData(const FWheelTelemetryData &TelemetryData) + : tire_friction(TelemetryData.TireFriction), + lat_slip(TelemetryData.LatSlip), + long_slip(TelemetryData.LongSlip), + omega(TelemetryData.Omega), + tire_load(TelemetryData.TireLoad), + normalized_tire_load(TelemetryData.NormalizedTireLoad), + torque(TelemetryData.Torque), + long_force(TelemetryData.LongForce), + lat_force(TelemetryData.LatForce), + normalized_long_force(TelemetryData.NormalizedLongForce), + normalized_lat_force(TelemetryData.NormalizedLatForce) {} + + operator FWheelTelemetryData() const { + FWheelTelemetryData TelemetryData; + TelemetryData.TireFriction = tire_friction; + TelemetryData.LatSlip = lat_slip; + TelemetryData.LongSlip = long_slip; + TelemetryData.Omega = omega; + TelemetryData.TireLoad = tire_load; + TelemetryData.NormalizedTireLoad = normalized_tire_load; + TelemetryData.Torque = torque; + TelemetryData.LongForce = long_force; + TelemetryData.LatForce = lat_force; + TelemetryData.NormalizedLongForce = normalized_long_force; + TelemetryData.NormalizedLatForce = normalized_lat_force; + + return TelemetryData; + } +#endif + + MSGPACK_DEFINE_ARRAY(tire_friction, + lat_slip, + long_slip, + omega, + tire_load, + normalized_tire_load, + torque, + long_force, + lat_force, + normalized_long_force, + normalized_lat_force) + }; + +} // namespace rpc +} // namespace carla \ No newline at end of file diff --git a/PythonAPI/carla/include/PythonAPI.h b/PythonAPI/carla/include/PythonAPI.h index 353f4b45039..3b66eb8c136 100644 --- a/PythonAPI/carla/include/PythonAPI.h +++ b/PythonAPI/carla/include/PythonAPI.h @@ -17,7 +17,9 @@ #include #include #include +#include #include +#include #include #include #include @@ -543,6 +545,33 @@ namespace rpc { return out; } + inline std::ostream &operator<<(std::ostream &out, const WheelTelemetryData &telemetry) { + out << "WheelTelemetryData(tire_friction=" << std::to_string(telemetry.tire_friction) + << ", lat_slip=" << std::to_string(telemetry.lat_slip) + << ", long_slip=" << std::to_string(telemetry.long_slip) + << ", omega=" << std::to_string(telemetry.omega) + << ", tire_load=" << std::to_string(telemetry.tire_load) + << ", normalized_tire_load=" << std::to_string(telemetry.normalized_tire_load) + << ", torque=" << std::to_string(telemetry.torque) + << ", long_force=" << std::to_string(telemetry.long_force) + << ", lat_force=" << std::to_string(telemetry.lat_force) + << ", normalized_long_force=" << std::to_string(telemetry.normalized_long_force) + << ", normalized_lat_force=" << std::to_string(telemetry.normalized_lat_force) << ')'; + return out; + } + + inline std::ostream &operator<<(std::ostream &out, const VehicleTelemetryData &telemetry) { + out << "VehicleTelemetryData(speed=" << std::to_string(telemetry.speed) + << ", steer=" << std::to_string(telemetry.steer) + << ", throttle=" << std::to_string(telemetry.throttle) + << ", brake=" << std::to_string(telemetry.brake) + << ", engine_rpm=" << std::to_string(telemetry.engine_rpm) + << ", gear=" << std::to_string(telemetry.gear) + << ", drag=" << std::to_string(telemetry.drag) + << ", wheels=" << telemetry.wheels << ')'; + return out; + } + inline std::ostream &operator<<(std::ostream &out, const AckermannControllerSettings &settings) { out << "AckermannControllerSettings(speed_kp=" << std::to_string(settings.speed_kp) << ", speed_ki=" << std::to_string(settings.speed_ki) diff --git a/PythonAPI/carla/src/Actor.cpp b/PythonAPI/carla/src/Actor.cpp index da8f24ce13b..24b2e9d5788 100644 --- a/PythonAPI/carla/src/Actor.cpp +++ b/PythonAPI/carla/src/Actor.cpp @@ -187,6 +187,7 @@ void export_actor() { .def("apply_ackermann_controller_settings", &cc::Vehicle::ApplyAckermannControllerSettings, (arg("settings"))) .def("get_ackermann_controller_settings", CONST_CALL_WITHOUT_GIL(cc::Vehicle, GetAckermannControllerSettings)) .def("set_autopilot", CALL_WITHOUT_GIL_2(cc::Vehicle, SetAutopilot, bool, uint16_t), (arg("enabled") = true, arg("tm_port") = ctm::TM_DEFAULT_PORT)) + .def("get_telemetry_data", CONST_CALL_WITHOUT_GIL(cc::Vehicle, GetTelemetryData)) .def("show_debug_telemetry", &cc::Vehicle::ShowDebugTelemetry, (arg("enabled") = true)) .def("get_speed_limit", &cc::Vehicle::GetSpeedLimit) .def("get_traffic_light_state", &cc::Vehicle::GetTrafficLightState) diff --git a/PythonAPI/carla/src/Control.cpp b/PythonAPI/carla/src/Control.cpp index 482e13dc999..ef577725bfb 100644 --- a/PythonAPI/carla/src/Control.cpp +++ b/PythonAPI/carla/src/Control.cpp @@ -279,6 +279,55 @@ boost::python::object WalkerBoneControl_init(boost::python::tuple args, boost::p return res; } +static auto GetWheelsTelemetry(const carla::rpc::VehicleTelemetryData &self) { + const auto &wheels = self.wheels; + boost::python::object get_iter = boost::python::iterator>(); + boost::python::object iter = get_iter(wheels); + return boost::python::list(iter); +} + +static void SetWheelsTelemetry(carla::rpc::VehicleTelemetryData &self, const boost::python::list &list) { + std::vector wheels; + auto length = boost::python::len(list); + for (auto i = 0u; i < length; ++i) { + wheels.push_back(boost::python::extract(list[i])); + } + self.wheels = wheels; +} + +boost::python::object VehicleTelemetryData_init(boost::python::tuple args, boost::python::dict kwargs) { + // Args names + const char *args_names[] = { + "speed", + "steer", + "throttle", + "brake", + "engine_rpm", + "gear", + "wheels" + }; + const auto NUM_ARGUMENTS = sizeof(args_names) / sizeof(const char*); + + boost::python::object self = args[0]; + args = boost::python::tuple(args.slice(1, boost::python::_)); + + auto res = self.attr("__init__")(); + if (len(args) > 0) { + for (unsigned int i = 0; i < len(args); ++i) { + self.attr(args_names[i]) = args[i]; + } + } + + for (unsigned int i = 0; i < NUM_ARGUMENTS; ++i) { + if (kwargs.contains(args_names[i])) { + self.attr(args_names[i]) = kwargs[args_names[i]]; + } + } + + return res; +} + + void export_control() { using namespace boost::python; namespace cg = carla::geom; @@ -488,4 +537,43 @@ void export_control() { .def("__ne__", &cr::VehiclePhysicsControl::operator!=) .def(self_ns::str(self_ns::self)) ; + + class_("WheelTelemetryData") + .def(init<>()) + .def_readwrite("tire_friction", &cr::WheelTelemetryData::tire_friction) + .def_readwrite("lat_slip", &cr::WheelTelemetryData::lat_slip) + .def_readwrite("long_slip", &cr::WheelTelemetryData::long_slip) + .def_readwrite("omega", &cr::WheelTelemetryData::omega) + .def_readwrite("tire_load", &cr::WheelTelemetryData::tire_load) + .def_readwrite("normalized_tire_load", &cr::WheelTelemetryData::normalized_tire_load) + .def_readwrite("torque", &cr::WheelTelemetryData::torque) + .def_readwrite("long_force", &cr::WheelTelemetryData::long_force) + .def_readwrite("lat_force", &cr::WheelTelemetryData::lat_force) + .def_readwrite("normalized_long_force", &cr::WheelTelemetryData::normalized_long_force) + .def_readwrite("normalized_lat_force", &cr::WheelTelemetryData::normalized_lat_force) + .def("__eq__", &cr::WheelTelemetryData::operator==) + .def("__ne__", &cr::WheelTelemetryData::operator!=) + .def(self_ns::str(self_ns::self)) + ; + + class_ >("vector_of_wheels_telemetry") + .def(vector_indexing_suite >()) + .def(self_ns::str(self_ns::self)) + ; + + class_("VehicleTelemetryData", no_init) + .def("__init__", raw_function(VehicleTelemetryData_init)) + .def(init<>()) + .def_readwrite("speed", &cr::VehicleTelemetryData::speed) + .def_readwrite("steer", &cr::VehicleTelemetryData::steer) + .def_readwrite("throttle", &cr::VehicleTelemetryData::throttle) + .def_readwrite("brake", &cr::VehicleTelemetryData::brake) + .def_readwrite("engine_rpm", &cr::VehicleTelemetryData::engine_rpm) + .def_readwrite("gear", &cr::VehicleTelemetryData::gear) + .def_readwrite("drag", &cr::VehicleTelemetryData::drag) + .add_property("wheels", &GetWheelsTelemetry, &SetWheelsTelemetry) + .def("__eq__", &cr::VehicleTelemetryData::operator==) + .def("__ne__", &cr::VehicleTelemetryData::operator!=) + .def(self_ns::str(self_ns::self)) + ; } diff --git a/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Actor/CarlaActor.cpp b/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Actor/CarlaActor.cpp index 33d82f7e4e8..4e6a30b11b7 100644 --- a/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Actor/CarlaActor.cpp +++ b/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Actor/CarlaActor.cpp @@ -971,6 +971,25 @@ ECarlaServerResponse FVehicleActor::SetActorAutopilot(bool bEnabled, bool bKeepS return ECarlaServerResponse::Success; } +ECarlaServerResponse FVehicleActor::GetVehicleTelemetryData(FVehicleTelemetryData& TelemetryData) +{ + if (IsDormant()) + { + FVehicleTelemetryData EmptyTelemetryData; + TelemetryData = EmptyTelemetryData; + } + else + { + auto Vehicle = Cast(GetActor()); + if (Vehicle == nullptr) + { + return ECarlaServerResponse::NotAVehicle; + } + TelemetryData = Vehicle->GetVehicleTelemetryData(); + } + return ECarlaServerResponse::Success; +} + ECarlaServerResponse FVehicleActor::ShowVehicleDebugTelemetry(bool bEnabled) { if (IsDormant()) diff --git a/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Actor/CarlaActor.h b/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Actor/CarlaActor.h index 62afba47167..dd31288f722 100644 --- a/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Actor/CarlaActor.h +++ b/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Actor/CarlaActor.h @@ -9,6 +9,7 @@ #include "Carla/Actor/ActorInfo.h" #include "Carla/Actor/ActorData.h" #include "Carla/Vehicle/CarlaWheeledVehicle.h" +#include "Carla/Vehicle/VehicleTelemetryData.h" #include "Carla/Walker/WalkerController.h" #include "Carla/Traffic/TrafficLightState.h" #include "Carla/Server/CarlaServerResponse.h" @@ -323,6 +324,11 @@ class FCarlaActor return ECarlaServerResponse::ActorTypeMismatch; } + virtual ECarlaServerResponse GetVehicleTelemetryData(FVehicleTelemetryData&) + { + return ECarlaServerResponse::ActorTypeMismatch; + } + virtual ECarlaServerResponse ShowVehicleDebugTelemetry(bool) { return ECarlaServerResponse::ActorTypeMismatch; @@ -523,6 +529,8 @@ class FVehicleActor : public FCarlaActor virtual ECarlaServerResponse SetActorAutopilot(bool bEnabled, bool bKeepState = false) final; + virtual ECarlaServerResponse GetVehicleTelemetryData(FVehicleTelemetryData&) final; + virtual ECarlaServerResponse ShowVehicleDebugTelemetry(bool bEnabled) final; virtual ECarlaServerResponse EnableCarSim(const FString& SimfilePath) final; diff --git a/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Server/CarlaServer.cpp b/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Server/CarlaServer.cpp index ab846c6a57d..d789f7770ff 100644 --- a/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Server/CarlaServer.cpp +++ b/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Server/CarlaServer.cpp @@ -55,6 +55,7 @@ #include #include #include +#include #include #include #include @@ -1912,6 +1913,31 @@ BIND_SYNC(is_sensor_enabled_for_ros) << [this](carla::streaming::detail::stream_ return R::Success(); }; + BIND_SYNC(get_telemetry_data) << [this]( + cr::ActorId ActorId) -> R + { + REQUIRE_CARLA_EPISODE(); + FCarlaActor* CarlaActor = Episode->FindCarlaActor(ActorId); + if (!CarlaActor) + { + return RespondError( + "get_telemetry_data", + ECarlaServerResponse::ActorNotFound, + " Actor Id: " + FString::FromInt(ActorId)); + } + FVehicleTelemetryData TelemetryData; + ECarlaServerResponse Response = + CarlaActor->GetVehicleTelemetryData(TelemetryData); + if (Response != ECarlaServerResponse::Success) + { + return RespondError( + "get_telemetry_data", + Response, + " Actor Id: " + FString::FromInt(ActorId)); + } + return cr::VehicleTelemetryData(TelemetryData); + }; + BIND_SYNC(show_vehicle_debug_telemetry) << [this]( cr::ActorId ActorId, bool bEnabled) -> R diff --git a/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Vehicle/CarlaWheeledVehicle.cpp b/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Vehicle/CarlaWheeledVehicle.cpp index b3f0bc046ee..83af0c6c380 100644 --- a/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Vehicle/CarlaWheeledVehicle.cpp +++ b/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Vehicle/CarlaWheeledVehicle.cpp @@ -642,6 +642,56 @@ void ACarlaWheeledVehicle::DeactivateVelocityControl() VelocityControl->Deactivate(); } +FVehicleTelemetryData ACarlaWheeledVehicle::GetVehicleTelemetryData() const +{ + FVehicleTelemetryData TelemetryData; + + auto *MovementComponent = GetVehicleMovement(); + + // Vehicle telemetry data + TelemetryData.Speed = GetVehicleForwardSpeed() / 100.0f; // From cm/s to m/s + TelemetryData.Steer = LastAppliedControl.Steer; + TelemetryData.Throttle = LastAppliedControl.Throttle; + TelemetryData.Brake = LastAppliedControl.Brake; + TelemetryData.EngineRPM = MovementComponent->GetEngineRotationSpeed(); + TelemetryData.Gear = GetVehicleCurrentGear(); + TelemetryData.Drag = MovementComponent->DebugDragMagnitude / 100.0f; // kg*cm/s2 to Kg*m/s2 + + // Wheels telemetry data + FPhysXVehicleManager* MyVehicleManager = FPhysXVehicleManager::GetVehicleManagerFromScene(GetWorld()->GetPhysicsScene()); + + SCOPED_SCENE_READ_LOCK(MyVehicleManager->GetScene()); + PxWheelQueryResult* WheelsStates = MyVehicleManager->GetWheelsStates_AssumesLocked(MovementComponent); + check(WheelsStates); + + TArray Wheels; + for (uint32 w = 0; w < MovementComponent->PVehicle->mWheelsSimData.getNbWheels(); ++w) + { + FWheelTelemetryData WheelTelemetryData; + + WheelTelemetryData.TireFriction = WheelsStates[w].tireFriction; + WheelTelemetryData.LatSlip = FMath::RadiansToDegrees(WheelsStates[w].lateralSlip); + WheelTelemetryData.LongSlip = WheelsStates[w].longitudinalSlip; + WheelTelemetryData.Omega = MovementComponent->PVehicle->mWheelsDynData.getWheelRotationSpeed(w); + + UVehicleWheel* Wheel = MovementComponent->Wheels[w]; + WheelTelemetryData.TireLoad = Wheel->DebugTireLoad / 100.0f; + WheelTelemetryData.NormalizedTireLoad = Wheel->DebugNormalizedTireLoad; + WheelTelemetryData.Torque = Wheel->DebugWheelTorque / (100.0f * 100.0f); // From cm2 to m2 + WheelTelemetryData.LongForce = Wheel->DebugLongForce / 100.f; + WheelTelemetryData.LatForce = Wheel->DebugLatForce / 100.f; + WheelTelemetryData.NormalizedLongForce = (FMath::Abs(WheelTelemetryData.LongForce)*WheelTelemetryData.NormalizedTireLoad) / (WheelTelemetryData.TireLoad); + WheelTelemetryData.NormalizedLatForce = (FMath::Abs(WheelTelemetryData.LatForce)*WheelTelemetryData.NormalizedTireLoad) / (WheelTelemetryData.TireLoad); + + Wheels.Add(WheelTelemetryData); + } + + TelemetryData.Wheels = Wheels; + + return TelemetryData; + +} + void ACarlaWheeledVehicle::ShowDebugTelemetry(bool Enabled) { if (GetWorld()->GetFirstPlayerController()) diff --git a/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Vehicle/CarlaWheeledVehicle.h b/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Vehicle/CarlaWheeledVehicle.h index 88607a6a0dc..a6ff9982b15 100644 --- a/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Vehicle/CarlaWheeledVehicle.h +++ b/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Vehicle/CarlaWheeledVehicle.h @@ -14,6 +14,7 @@ #include "Carla/Vehicle/VehicleLightState.h" #include "Carla/Vehicle/VehicleInputPriority.h" #include "Carla/Vehicle/VehiclePhysicsControl.h" +#include "Vehicle/VehicleTelemetryData.h" #include "Carla/Vehicle/VehicleVelocityControl.h" #include "Carla/Vehicle/WheeledVehicleMovementComponentNW.h" #include "Carla/Vehicle/MovementComponents/BaseCarlaMovementComponent.h" @@ -237,6 +238,9 @@ class CARLA_API ACarlaWheeledVehicle : public AWheeledVehiclePawn UFUNCTION(Category = "CARLA Wheeled Vehicle", BlueprintCallable) void DeactivateVelocityControl(); + UFUNCTION(Category = "CARLA Wheeled Vehicle", BlueprintCallable) + FVehicleTelemetryData GetVehicleTelemetryData() const; + UFUNCTION(Category = "CARLA Wheeled Vehicle", BlueprintCallable) void ShowDebugTelemetry(bool Enabled); diff --git a/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Vehicle/VehicleTelemetryData.h b/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Vehicle/VehicleTelemetryData.h new file mode 100644 index 00000000000..b5c5aeb0e2e --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Vehicle/VehicleTelemetryData.h @@ -0,0 +1,78 @@ +// Copyright (c) 2022 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 "VehicleTelemetryData.generated.h" + +USTRUCT(BlueprintType) +struct FWheelTelemetryData +{ + GENERATED_USTRUCT_BODY() + + UPROPERTY(Category = "Wheel Telemetry Data", EditAnywhere, BlueprintReadWrite) + float TireFriction = 0.0f; + + UPROPERTY(Category = "Wheel Telemetry Data", EditAnywhere, BlueprintReadWrite) + float LatSlip = 0.0f; // degrees + + UPROPERTY(Category = "Wheel Telemetry Data", EditAnywhere, BlueprintReadWrite) + float LongSlip = 0.0f; + + UPROPERTY(Category = "Wheel Telemetry Data", EditAnywhere, BlueprintReadWrite) + float Omega = 0.0f; + + UPROPERTY(Category = "Wheel Telemetry Data", EditAnywhere, BlueprintReadWrite) + float TireLoad = 0.0f; + + UPROPERTY(Category = "Wheel Telemetry Data", EditAnywhere, BlueprintReadWrite) + float NormalizedTireLoad = 0.0f; + + UPROPERTY(Category = "Wheel Telemetry Data", EditAnywhere, BlueprintReadWrite) + float Torque = 0.0f; // [Nm] + + UPROPERTY(Category = "Wheel Telemetry Data", EditAnywhere, BlueprintReadWrite) + float LongForce = 0.0f; // [N] + + UPROPERTY(Category = "Wheel Telemetry Data", EditAnywhere, BlueprintReadWrite) + float LatForce = 0.0f; // [N] + + UPROPERTY(Category = "Wheel Telemetry Data", EditAnywhere, BlueprintReadWrite) + float NormalizedLongForce = 0.0f; + + UPROPERTY(Category = "Wheel Telemetry Data", EditAnywhere, BlueprintReadWrite) + float NormalizedLatForce = 0.0f; +}; + +USTRUCT(BlueprintType) +struct CARLA_API FVehicleTelemetryData +{ + GENERATED_BODY() + + UPROPERTY(Category = "Vehicle Telemetry Data", EditAnywhere, BlueprintReadWrite) + float Speed = 0.0f; // [m/s] + + UPROPERTY(Category = "Vehicle Telemetry Data", EditAnywhere, BlueprintReadWrite) + float Steer = 0.0f; + + UPROPERTY(Category = "Vehicle Telemetry Data", EditAnywhere, BlueprintReadWrite) + float Throttle = 0.0f; + + UPROPERTY(Category = "Vehicle Telemetry Data", EditAnywhere, BlueprintReadWrite) + float Brake = 0.0f; + + UPROPERTY(Category = "Vehicle Telemetry Data", EditAnywhere, BlueprintReadWrite) + float EngineRPM = 0.0f; + + UPROPERTY(Category = "Vehicle Telemetry Data", EditAnywhere, BlueprintReadWrite) + int32 Gear = 0.0f; + + UPROPERTY(Category = "Vehicle Telemetry Data", EditAnywhere, BlueprintReadWrite) + float Drag = 0.0f; // [N] + + UPROPERTY(Category = "Vehicle Engine Physics Control", EditAnywhere, BlueprintReadWrite) + TArray Wheels; +}; diff --git a/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Vehicle/WheeledVehicleMovementComponentNW.cpp b/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Vehicle/WheeledVehicleMovementComponentNW.cpp index c587b4327e4..8d14c857892 100644 --- a/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Vehicle/WheeledVehicleMovementComponentNW.cpp +++ b/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Vehicle/WheeledVehicleMovementComponentNW.cpp @@ -20,7 +20,7 @@ UWheeledVehicleMovementComponentNW::UWheeledVehicleMovementComponentNW(const FOb DefEngineData.InitDefaults(); EngineSetup.MOI = DefEngineData.EngineRevUpMOI; EngineSetup.MaxRPM = DefEngineData.MaxRPM; - + // -- We need to investigate about this -- EngineSetup.DampingRateFullThrottle = DefEngineData.mDampingRateFullThrottle;