Bone Transformation Mismatch Fix

Carrying over the changes from UE4

LibCarla/source/carla/client/Vehicle.cpp

@@ -103,6 +103,10 @@ namespace client {
     return GetEpisode().Lock()->GetVehicleLightState(*this).GetLightStateEnum();
   }
 
+  std::vector<geom::Transform> Vehicle::GetVehicleBoneWorldTransforms() const {
+    return GetEpisode().Lock()->GetVehicleBoneWorldTransforms(*this);
+  }
+
   float Vehicle::GetSpeedLimit() const {
     return GetEpisode().Lock()->GetActorSnapshot(*this).state.vehicle_data.speed_limit;
   }

LibCarla/source/carla/client/Vehicle.h

@@ -106,6 +106,8 @@ namespace client {
     /// received in the last tick.
     float GetSpeedLimit() const;
 
+    std::vector<geom::Transform> GetVehicleBoneWorldTransforms() const;
+
     /// Return the state of the traffic light currently affecting this vehicle.
     ///
     /// @return Green If no traffic light is affecting the vehicle.

LibCarla/source/carla/client/detail/Client.cpp

@@ -559,6 +559,11 @@ namespace detail {
     return _pimpl->CallAndWait<std::vector<std::pair<carla::ActorId, uint32_t>>>("get_vehicle_light_states");
   }
 
+  std::vector<geom::Transform> Client::GetVehicleBoneWorldTransforms(rpc::ActorId actor) {
+    using return_t = std::vector<geom::Transform>;
+    return _pimpl->CallAndWait<return_t>("get_vehicle_bone_world_transforms", actor);
+  }
+
   std::vector<ActorId> Client::GetGroupTrafficLights(rpc::ActorId traffic_light) {
     using return_t = std::vector<ActorId>;
     return _pimpl->CallAndWait<return_t>("get_group_traffic_lights", traffic_light);

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

@@ -166,6 +166,8 @@ namespace detail {
     rpc::VehiclePhysicsControl GetVehiclePhysicsControl(rpc::ActorId vehicle) const;
 
     rpc::VehicleLightState GetVehicleLightState(rpc::ActorId vehicle) const;
+    
+    std::vector<geom::Transform> GetVehicleBoneWorldTransforms(rpc::ActorId actor);
 
     void ApplyPhysicsControlToVehicle(
         rpc::ActorId vehicle,

LibCarla/source/carla/client/detail/Episode.cpp

@@ -14,6 +14,8 @@
 
 #include <exception>
 
+class FPoseSnapShot;
+
 namespace carla {
 namespace client {
 namespace detail {

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

@@ -271,6 +271,10 @@ namespace detail {
       return _client.GetVehicleLightState(vehicle.GetId());
     }
 
+    std::vector<geom::Transform> GetVehicleBoneWorldTransforms(const Vehicle &vehicle) {
+      return _client.GetVehicleBoneWorldTransforms(vehicle.GetId());
+    }
+
     /// Returns all the BBs of all the elements of the level
     std::vector<geom::BoundingBox> GetLevelBBs(uint8_t queried_tag) const {
       return _client.GetLevelBBs(queried_tag);

PythonAPI/carla/src/Actor.cpp

@@ -196,6 +196,7 @@ void export_actor() {
       .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("get_vehicle_bone_world_transforms", &cc::Vehicle::GetVehicleBoneWorldTransforms)
       .def(self_ns::str(self_ns::self))
   ;
 

Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Server/CarlaServer.cpp

@@ -69,6 +69,7 @@
 #include "EngineUtils.h"
 #include "GameFramework/CharacterMovementComponent.h"
 #include "Misc/FileHelper.h"
+#include "Animation/PoseSnapshot.h"
 #include <util/ue-header-guard-end.h>
 
 #include <vector>
@@ -1645,6 +1646,25 @@ BIND_SYNC(is_sensor_enabled_for_ros) << [this](carla::streaming::detail::stream_
     return R<void>::Success();
   };
 
+  BIND_SYNC(get_vehicle_bone_world_transforms) << [this](
+      cr::ActorId ActorId) -> R<std::vector<cr::Transform>>
+  {
+    REQUIRE_CARLA_EPISODE();
+    FCarlaActor* CarlaActor = Episode->FindCarlaActor(ActorId);
+    if (!CarlaActor)
+    {
+      return RespondError(
+          "get_vehicle_bone_world_transforms",
+          ECarlaServerResponse::ActorNotFound,
+          " Actor Id: " + FString::FromInt(ActorId));
+    }
+    else
+    {
+      ACarlaWheeledVehicle* CarlaVehicle = Cast<ACarlaWheeledVehicle>(CarlaActor->GetActor());
+      return MakeVectorFromTArray<cr::Transform>(CarlaVehicle->GetWorldTransformedPose().LocalTransforms);
+    }
+  };
+
   // ~~ Apply control ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 
   BIND_SYNC(apply_control_to_vehicle) << [this](

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

@@ -163,6 +163,20 @@ void ACarlaWheeledVehicle::BeginPlay()
   AddReferenceToManager();
 }
 
+void ACarlaWheeledVehicle::TickActor(float DeltaTime, enum ELevelTick TickType, FActorTickFunction& ThisTickFunction){
+  Super::TickActor(DeltaTime, TickType, ThisTickFunction);
+
+  FPoseSnapshot pose;
+  GetMesh()->SnapshotPose(pose);
+  for(FTransform &transform : pose.LocalTransforms)
+  {
+    transform *= GetMesh()->GetComponentTransform();
+  }
+
+  WorldTransformedPose = pose;
+
+}
+
 bool ACarlaWheeledVehicle::IsInVehicleRange(const FVector& Location) const
 {
   TRACE_CPUPROFILER_EVENT_SCOPE(ACarlaWheeledVehicle::IsInVehicleRange);
@@ -978,3 +992,16 @@ void ACarlaWheeledVehicle::SetPhysicsConstraintAngle(
   Component->ConstraintInstance.AngularRotationOffset = NewAngle;
 }
 
+FPoseSnapshot ACarlaWheeledVehicle::GetWorldTransformedPose()
+{
+  if(WorldTransformedPose.bIsValid == false)
+  {
+    SetActorTickEnabled(true);
+    GetMesh()->SnapshotPose(WorldTransformedPose);
+    for(FTransform &transform : WorldTransformedPose.LocalTransforms)
+    {
+      transform *= GetMesh()->GetComponentTransform();
+    }
+  }
+  return WorldTransformedPose;
+}

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

@@ -304,6 +304,7 @@ public:
 protected:
 
   virtual void BeginPlay() override;
+  virtual void TickActor(float DeltaTime, enum ELevelTick TickType, FActorTickFunction& ThisTickFunction) override;
   virtual void EndPlay(const EEndPlayReason::Type EndPlayReason);
 
   UFUNCTION(BlueprintImplementableEvent)
@@ -410,6 +411,9 @@ public:
 
   virtual FVector GetVelocity() const override;
 
+  UFUNCTION()
+  FPoseSnapshot GetWorldTransformedPose();
+
 //-----CARSIM--------------------------------
   UPROPERTY(Category="CARLA Wheeled Vehicle", EditAnywhere)
   float CarSimOriginOffset = 150.f;
@@ -465,6 +469,7 @@ private:
 public:
   float SpeedAnim { 0.0f };
   float RotationAnim { 0.0f };
+  FPoseSnapshot WorldTransformedPose;
 
   UFUNCTION(Category = "CARLA Wheeled Vehicle", BlueprintCallable)
   float GetSpeedAnim() const { return SpeedAnim; }