diff --git a/LibCarla/source/carla/client/Walker.cpp b/LibCarla/source/carla/client/Walker.cpp index 056c18f3c..8275966c7 100644 --- a/LibCarla/source/carla/client/Walker.cpp +++ b/LibCarla/source/carla/client/Walker.cpp @@ -25,5 +25,10 @@ namespace client { Walker::Control Walker::GetWalkerControl() const { return GetEpisode().Lock()->GetActorSnapshot(*this).state.walker_control; } + + // Walker::Control Walker::GetBonesTransform() const { + Walker::BoneControl Walker::GetBonesTransform() { + return GetEpisode().Lock()->GetBonesTransform(*this); + } } // namespace client } // namespace carla diff --git a/LibCarla/source/carla/client/Walker.h b/LibCarla/source/carla/client/Walker.h index aaf174a32..ea97a5794 100644 --- a/LibCarla/source/carla/client/Walker.h +++ b/LibCarla/source/carla/client/Walker.h @@ -32,6 +32,9 @@ namespace client { /// received in the last tick. Control GetWalkerControl() const; + // Walker::Control GetBonesTransform() const; + BoneControl GetBonesTransform(); + private: Control _control; diff --git a/LibCarla/source/carla/client/detail/Client.cpp b/LibCarla/source/carla/client/detail/Client.cpp index 570894293..7f0e1944e 100644 --- a/LibCarla/source/carla/client/detail/Client.cpp +++ b/LibCarla/source/carla/client/detail/Client.cpp @@ -439,6 +439,11 @@ namespace detail { _pimpl->AsyncCall("apply_bone_control_to_walker", walker, control); } + rpc::WalkerBoneControl Client::GetBonesTransform(rpc::ActorId walker) { + auto res = _pimpl->CallAndWait("get_bones_transform", walker); + return res; + } + void Client::SetTrafficLightState( rpc::ActorId traffic_light, const rpc::TrafficLightState traffic_light_state) { diff --git a/LibCarla/source/carla/client/detail/Client.h b/LibCarla/source/carla/client/detail/Client.h index d929f8edf..9b3802ead 100644 --- a/LibCarla/source/carla/client/detail/Client.h +++ b/LibCarla/source/carla/client/detail/Client.h @@ -271,6 +271,9 @@ namespace detail { rpc::ActorId walker, const rpc::WalkerBoneControl &control); + rpc::WalkerBoneControl GetBonesTransform( + rpc::ActorId walker); + void SetTrafficLightState( rpc::ActorId traffic_light, const rpc::TrafficLightState trafficLightState); diff --git a/LibCarla/source/carla/client/detail/Simulator.h b/LibCarla/source/carla/client/detail/Simulator.h index 7f47f6535..0680c5c09 100644 --- a/LibCarla/source/carla/client/detail/Simulator.h +++ b/LibCarla/source/carla/client/detail/Simulator.h @@ -461,6 +461,10 @@ namespace detail { _client.ApplyBoneControlToWalker(walker.GetId(), control); } + rpc::WalkerBoneControl GetBonesTransform(Walker &walker) { + return _client.GetBonesTransform(walker.GetId()); + } + void ApplyPhysicsControlToVehicle(Vehicle &vehicle, const rpc::VehiclePhysicsControl &physicsControl) { _client.ApplyPhysicsControlToVehicle(vehicle.GetId(), physicsControl); } diff --git a/PythonAPI/carla/source/libcarla/Actor.cpp b/PythonAPI/carla/source/libcarla/Actor.cpp index 617698969..4f1c18535 100644 --- a/PythonAPI/carla/source/libcarla/Actor.cpp +++ b/PythonAPI/carla/source/libcarla/Actor.cpp @@ -190,6 +190,7 @@ void export_actor() { .def("apply_control", &ApplyControl, (arg("control"))) .def("apply_control", &ApplyControl, (arg("control"))) .def("get_control", &cc::Walker::GetWalkerControl) + .def("get_bones", &cc::Walker::GetBonesTransform) .def(self_ns::str(self_ns::self)) ; diff --git a/PythonAPI/carla/source/libcarla/Control.cpp b/PythonAPI/carla/source/libcarla/Control.cpp index 39d24c094..cf46369e6 100644 --- a/PythonAPI/carla/source/libcarla/Control.cpp +++ b/PythonAPI/carla/source/libcarla/Control.cpp @@ -304,6 +304,17 @@ void export_control() { .def(self_ns::str(self_ns::self)) ; + class_("bone_transform") + .def(init<>()) + .def_readwrite("name", &std::pair::first) + .def_readwrite("transform", &std::pair::second) + .def(self_ns::str(self_ns::self)) + ; + + class_>("vector_of_bones") + .def(boost::python::vector_indexing_suite>()) + ; + class_("WalkerBoneControl") .def("__init__", raw_function(WalkerBoneControl_init)) .def(init<>()) diff --git a/PythonAPI/carla/source/libcarla/libcarla.cpp b/PythonAPI/carla/source/libcarla/libcarla.cpp index 83b344b59..f56ea5c99 100644 --- a/PythonAPI/carla/source/libcarla/libcarla.cpp +++ b/PythonAPI/carla/source/libcarla/libcarla.cpp @@ -180,6 +180,12 @@ namespace std { return PrintList(out, vector_of_stuff); } + template + std::ostream &operator<<(std::ostream &out, const std::pair &data) { + out << "(" << data.first << "," << data.second << ")"; + return out; + } + } // namespace std static carla::time_duration TimeDurationFromSeconds(double seconds) { diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/CarlaActor.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/CarlaActor.cpp index 419ef2ecf..5dcb1d404 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/CarlaActor.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/CarlaActor.cpp @@ -1269,3 +1269,25 @@ ECarlaServerResponse FWalkerActor::ApplyBoneControlToWalker( } return ECarlaServerResponse::Success; } + +ECarlaServerResponse FWalkerActor::GetBonesTransform(FWalkerBoneControl& Bones) +{ + if (IsDormant()) + { + } + else + { + auto Pawn = Cast(GetActor()); + if (Pawn == nullptr) + { + return ECarlaServerResponse::NotAWalker; + } + auto Controller = Cast(Pawn->GetController()); + if (Controller == nullptr) + { + return ECarlaServerResponse::WalkerIncompatibleController; + } + Controller->GetBonesTransform(Bones); + } + return ECarlaServerResponse::Success; +} diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/CarlaActor.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/CarlaActor.h index 5c6dbc9b4..4568bb924 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/CarlaActor.h +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/CarlaActor.h @@ -372,6 +372,11 @@ public: return ECarlaServerResponse::ActorTypeMismatch; } + virtual ECarlaServerResponse GetBonesTransform(FWalkerBoneControl&) + { + return ECarlaServerResponse::ActorTypeMismatch; + } + virtual ECarlaServerResponse FreezeTrafficLight(bool) { return ECarlaServerResponse::ActorTypeMismatch; @@ -549,6 +554,8 @@ public: virtual ECarlaServerResponse GetWalkerControl(FWalkerControl&) final; virtual ECarlaServerResponse ApplyBoneControlToWalker(const FWalkerBoneControl&) final; + + virtual ECarlaServerResponse GetBonesTransform(FWalkerBoneControl&) final; }; class FOtherActor : public FCarlaActor diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Server/CarlaServer.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Server/CarlaServer.cpp index f8896b8c6..4cb01a821 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Server/CarlaServer.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Server/CarlaServer.cpp @@ -32,6 +32,7 @@ #include #include #include +#include #include #include #include @@ -1313,6 +1314,40 @@ void FCarlaServer::FPimpl::BindActions() return R::Success(); }; + BIND_SYNC(get_bones_transform) << [this]( + cr::ActorId ActorId) -> R + { + REQUIRE_CARLA_EPISODE(); + FCarlaActor* CarlaActor = Episode->FindCarlaActor(ActorId); + if (!CarlaActor) + { + return RespondError( + "get_bones_transform", + ECarlaServerResponse::ActorNotFound, + " Actor Id: " + FString::FromInt(ActorId)); + } + FWalkerBoneControl Bones; + ECarlaServerResponse Response = + CarlaActor->GetBonesTransform(Bones); + if (Response != ECarlaServerResponse::Success) + { + return RespondError( + "get_bones_transform", + Response, + " Actor Id: " + FString::FromInt(ActorId)); + } + + std::vector BoneData; + for (auto Bone : Bones.BoneTransforms) + { + std::pair Data; + Data.first = std::string(TCHAR_TO_UTF8(*Bone.Get<0>())); + Data.second = Bone.Get<1>(); + BoneData.push_back(Data); + } + return carla::rpc::WalkerBoneControl(BoneData); + }; + BIND_SYNC(set_actor_autopilot) << [this]( cr::ActorId ActorId, bool bEnabled) -> R diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Walker/WalkerController.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Walker/WalkerController.cpp index 622fe1850..f04b317a5 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Walker/WalkerController.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Walker/WalkerController.cpp @@ -101,12 +101,43 @@ void AWalkerController::SetManualBones(const bool bIsEnabled) else { UPoseableMeshComponent *PoseableMesh = PoseableMeshes.IsValidIndex(0) ? PoseableMeshes[0] : nullptr; - PoseableMesh->SetVisibility(false); + if (PoseableMesh) + PoseableMesh->SetVisibility(false); SkeletalMesh->SetVisibility(true); } } } +void AWalkerController::GetBonesTransform(FWalkerBoneControl &WalkerBones) +{ + auto *Character = GetCharacter(); + TArray PoseableMeshes; + TArray SkeletalMeshes; + Character->GetComponents(PoseableMeshes, false); + Character->GetComponents(SkeletalMeshes, false); + USkeletalMeshComponent *SkeletalMesh = SkeletalMeshes.IsValidIndex(0) ? SkeletalMeshes[0] : nullptr; + if (!SkeletalMesh) return; + + UPoseableMeshComponent *PoseableMesh = + PoseableMeshes.IsValidIndex(0) ? PoseableMeshes[0] : AddNewBoneComponent(Character, + SkeletalMesh->GetRelativeTransform().GetLocation(), + SkeletalMesh->GetRelativeTransform().GetRotation().Rotator()); + PoseableMesh->SetSkeletalMesh(SkeletalMesh->SkeletalMesh); + PoseableMesh->SetVisibility(false); + PoseableMesh->CopyPoseFromSkeletalComponent(SkeletalMesh); + + FPoseSnapshot Snap; + SkeletalMesh->SnapshotPose(Snap); + for (int i=0; iGetBoneTransformByName(Snap.BoneNames[i], EBoneSpaces::WorldSpace); + WalkerBones.BoneTransforms.Add(Snap.BoneNames[i].ToString(), Trans); + // FVector Loc = SkeletalMesh->GetBoneLocation(Snap.BoneNames[i], EBoneSpaces::WorldSpace); + // Trans = Snap.LocalTransforms[i]; + // Trans.SetLocation(Loc); + } +} + void AWalkerController::ControlTickVisitor::operator()(const FWalkerControl &WalkerControl) { auto *Character = Controller->GetCharacter(); diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Walker/WalkerController.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Walker/WalkerController.h index e7e0e74ff..dd2a0ab7a 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Walker/WalkerController.h +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Walker/WalkerController.h @@ -13,6 +13,7 @@ #include "GameFramework/Controller.h" #include +#include #include #include @@ -76,6 +77,9 @@ public: UFUNCTION(BlueprintCallable) void SetManualBones(const bool bIsEnabled); + UFUNCTION(BlueprintCallable) + void GetBonesTransform(FWalkerBoneControl &WalkerBones); + private: boost::variant Control;