Port Telemetry Data (see #5153)

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

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);
   }

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);
 

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<carla::rpc::VehicleTelemetryData>("get_telemetry_data", vehicle);
+  }
+
   void Client::ShowVehicleDebugTelemetry(rpc::ActorId vehicle, const bool enabled) {
     _pimpl->AsyncCall("show_vehicle_debug_telemetry", vehicle, enabled);
   }

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);

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);
     }

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 <https://opensource.org/licenses/MIT>.
+
+#pragma once
+
+#include "carla/MsgPack.h"
+#include "carla/rpc/WheelTelemetryData.h"
+
+#include <vector>
+
+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<WheelTelemetryData> 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<WheelTelemetryData> 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<WheelTelemetryData>();
+        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<FWheelTelemetryData> 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

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 <https://opensource.org/licenses/MIT>.
+
+#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

PythonAPI/carla/include/PythonAPI.h

@@ -17,7 +17,9 @@
 #include <carla/rpc/VehicleAckermannControl.h>
 #include <carla/rpc/VehicleControl.h>
 #include <carla/rpc/VehiclePhysicsControl.h>
+#include <carla/rpc/VehicleTelemetryData.h>
 #include <carla/rpc/WheelPhysicsControl.h>
+#include <carla/rpc/WheelTelemetryData.h>
 #include <carla/rpc/WalkerControl.h>
 #include <carla/rpc/WalkerBoneControlIn.h>
 #include <carla/rpc/WalkerBoneControlOut.h>
@@ -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)

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)