From 5d7245aa2b105f947680fd60168da2a8a17ff9b3 Mon Sep 17 00:00:00 2001 From: Jorge Virgos Date: Fri, 7 Feb 2025 12:42:29 +0100 Subject: [PATCH] Bone Transformation Mismatch Fix Carrying over the changes from UE4 --- LibCarla/source/carla/client/Vehicle.cpp | 4 +++ LibCarla/source/carla/client/Vehicle.h | 2 ++ .../source/carla/client/detail/Client.cpp | 5 ++++ LibCarla/source/carla/client/detail/Client.h | 2 ++ .../source/carla/client/detail/Episode.cpp | 2 ++ .../source/carla/client/detail/Simulator.h | 4 +++ PythonAPI/carla/src/Actor.cpp | 1 + .../Carla/Source/Carla/Server/CarlaServer.cpp | 20 ++++++++++++++ .../Carla/Vehicle/CarlaWheeledVehicle.cpp | 27 +++++++++++++++++++ .../Carla/Vehicle/CarlaWheeledVehicle.h | 5 ++++ 10 files changed, 72 insertions(+) diff --git a/LibCarla/source/carla/client/Vehicle.cpp b/LibCarla/source/carla/client/Vehicle.cpp index 201afa239..c541a603e 100644 --- a/LibCarla/source/carla/client/Vehicle.cpp +++ b/LibCarla/source/carla/client/Vehicle.cpp @@ -103,6 +103,10 @@ namespace client { return GetEpisode().Lock()->GetVehicleLightState(*this).GetLightStateEnum(); } + std::vector Vehicle::GetVehicleBoneWorldTransforms() const { + return GetEpisode().Lock()->GetVehicleBoneWorldTransforms(*this); + } + float Vehicle::GetSpeedLimit() const { return GetEpisode().Lock()->GetActorSnapshot(*this).state.vehicle_data.speed_limit; } diff --git a/LibCarla/source/carla/client/Vehicle.h b/LibCarla/source/carla/client/Vehicle.h index a086e775f..2290ec2f7 100644 --- a/LibCarla/source/carla/client/Vehicle.h +++ b/LibCarla/source/carla/client/Vehicle.h @@ -106,6 +106,8 @@ namespace client { /// received in the last tick. float GetSpeedLimit() const; + std::vector GetVehicleBoneWorldTransforms() const; + /// Return the state of the traffic light currently affecting this vehicle. /// /// @return Green If no traffic light is affecting the vehicle. diff --git a/LibCarla/source/carla/client/detail/Client.cpp b/LibCarla/source/carla/client/detail/Client.cpp index 76143d24d..836ce32df 100644 --- a/LibCarla/source/carla/client/detail/Client.cpp +++ b/LibCarla/source/carla/client/detail/Client.cpp @@ -559,6 +559,11 @@ namespace detail { return _pimpl->CallAndWait>>("get_vehicle_light_states"); } + std::vector Client::GetVehicleBoneWorldTransforms(rpc::ActorId actor) { + using return_t = std::vector; + return _pimpl->CallAndWait("get_vehicle_bone_world_transforms", actor); + } + std::vector Client::GetGroupTrafficLights(rpc::ActorId traffic_light) { using return_t = std::vector; return _pimpl->CallAndWait("get_group_traffic_lights", traffic_light); diff --git a/LibCarla/source/carla/client/detail/Client.h b/LibCarla/source/carla/client/detail/Client.h index 1761636ca..ace2d44c4 100644 --- a/LibCarla/source/carla/client/detail/Client.h +++ b/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 GetVehicleBoneWorldTransforms(rpc::ActorId actor); void ApplyPhysicsControlToVehicle( rpc::ActorId vehicle, diff --git a/LibCarla/source/carla/client/detail/Episode.cpp b/LibCarla/source/carla/client/detail/Episode.cpp index 43eb0e5b7..78a64f07f 100644 --- a/LibCarla/source/carla/client/detail/Episode.cpp +++ b/LibCarla/source/carla/client/detail/Episode.cpp @@ -14,6 +14,8 @@ #include +class FPoseSnapShot; + namespace carla { namespace client { namespace detail { diff --git a/LibCarla/source/carla/client/detail/Simulator.h b/LibCarla/source/carla/client/detail/Simulator.h index 8c0225b1a..f73d3c178 100644 --- a/LibCarla/source/carla/client/detail/Simulator.h +++ b/LibCarla/source/carla/client/detail/Simulator.h @@ -271,6 +271,10 @@ namespace detail { return _client.GetVehicleLightState(vehicle.GetId()); } + std::vector GetVehicleBoneWorldTransforms(const Vehicle &vehicle) { + return _client.GetVehicleBoneWorldTransforms(vehicle.GetId()); + } + /// Returns all the BBs of all the elements of the level std::vector GetLevelBBs(uint8_t queried_tag) const { return _client.GetLevelBBs(queried_tag); diff --git a/PythonAPI/carla/src/Actor.cpp b/PythonAPI/carla/src/Actor.cpp index da8f24ce1..31f540a37 100644 --- a/PythonAPI/carla/src/Actor.cpp +++ b/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)) ; diff --git a/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Server/CarlaServer.cpp b/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Server/CarlaServer.cpp index ab846c6a5..1af7c2667 100644 --- a/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Server/CarlaServer.cpp +++ b/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 #include @@ -1645,6 +1646,25 @@ BIND_SYNC(is_sensor_enabled_for_ros) << [this](carla::streaming::detail::stream_ return R::Success(); }; + BIND_SYNC(get_vehicle_bone_world_transforms) << [this]( + cr::ActorId ActorId) -> R> + { + 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(CarlaActor->GetActor()); + return MakeVectorFromTArray(CarlaVehicle->GetWorldTransformedPose().LocalTransforms); + } + }; + // ~~ Apply control ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ BIND_SYNC(apply_control_to_vehicle) << [this]( diff --git a/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Vehicle/CarlaWheeledVehicle.cpp b/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Vehicle/CarlaWheeledVehicle.cpp index b3f0bc046..def62de88 100644 --- a/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Vehicle/CarlaWheeledVehicle.cpp +++ b/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; +} \ No newline at end of file diff --git a/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Vehicle/CarlaWheeledVehicle.h b/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Vehicle/CarlaWheeledVehicle.h index 88607a6a0..be63cc8d0 100644 --- a/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Vehicle/CarlaWheeledVehicle.h +++ b/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; }