Get bone position

LibCarla/source/carla/client/Actor.cpp

@@ -8,6 +8,7 @@
 
 #include "carla/Logging.h"
 #include "carla/client/detail/Simulator.h"
+#include <stdio.h>
 
 namespace carla {
 namespace client {
@@ -36,6 +37,10 @@ namespace client {
     return GetEpisode().Lock()->GetActorAcceleration(*this);
   }
 
+  rpc::Keypoints Actor::GetKeypoints() const {
+    return GetEpisode().Lock()->GetActorSnapshot(*this).keypoints;
+  }
+
   void Actor::SetLocation(const geom::Location &location) {
     GetEpisode().Lock()->SetActorLocation(*this, location);
   }

LibCarla/source/carla/client/Actor.h

@@ -10,6 +10,7 @@
 #include "carla/Memory.h"
 #include "carla/client/detail/ActorState.h"
 #include "carla/profiler/LifetimeProfiled.h"
+#include "carla/rpc/Keypoints.h"
 
 namespace carla {
 namespace client {
@@ -58,6 +59,8 @@ namespace client {
     /// acceleration calculated after the actor's velocity.
     geom::Vector3D GetAcceleration() const;
 
+    rpc::Keypoints GetKeypoints() const;
+
     /// Teleport the actor to @a location.
     void SetLocation(const geom::Location &location);
 

LibCarla/source/carla/client/ActorSnapshot.h

@@ -21,6 +21,7 @@ namespace client {
     geom::Vector3D angular_velocity;
     geom::Vector3D acceleration;
     sensor::data::ActorDynamicState::TypeDependentState state;
+    sensor::data::detail::PackedKeypoints keypoints;
   };
 
 } // namespace client

LibCarla/source/carla/client/Walker.h

@@ -34,6 +34,7 @@ namespace client {
     /// received in the last tick.
     Control GetWalkerControl() const;
 
+
   private:
 
     Control _control;

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

@@ -28,7 +28,8 @@ namespace detail {
               actor.velocity,
               actor.angular_velocity,
               actor.acceleration,
-              actor.state});
+              actor.state,
+              actor.keypoints});
       DEBUG_ASSERT(result.second);
     }
   }

LibCarla/source/carla/rpc/Keypoints.h

@@ -0,0 +1,30 @@
+// Copyright (c) 2017 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/geom/Vector3D.h"
+#include <utility>
+#include <vector>
+
+namespace carla {
+namespace rpc {
+
+  class Keypoints {
+  // The keypoints are just a vector of pairs <string, Vector3D>
+  public:
+
+    Keypoints() = default;
+
+    std::vector<std::pair<std::string, geom::Vector3D>> keypoints;
+
+    MSGPACK_DEFINE_ARRAY(keypoints)
+
+  };
+
+} // namespace rpc
+} // namespace carla

LibCarla/source/carla/sensor/data/ActorDynamicState.h

@@ -12,8 +12,10 @@
 #include "carla/rpc/TrafficLightState.h"
 #include "carla/rpc/VehicleControl.h"
 #include "carla/rpc/WalkerControl.h"
+#include "carla/rpc/Keypoints.h"
 
 #include <cstdint>
+#include <stdio.h>
 
 namespace carla {
 namespace sensor {
@@ -102,6 +104,56 @@ namespace detail {
     uint32_t pole_index;
   };
 #pragma pack(pop)
+
+#pragma pack(push, 1)
+  struct PackedKeypoint {
+    // Helper structure that stores the name of a bone
+    // (up to 24 chars) and its position (a float 3)
+    PackedKeypoint() = default;
+
+    char name[24];
+    float location[3];
+  };
+#pragma pack(pop)
+
+#pragma pack(push, 1)
+struct PackedKeypoints {
+  // Helper class that stores up to 72 keypoints (yeah it's quite heavy,
+  // This is 2kB right there...)
+
+  PackedKeypoints() = default;
+  PackedKeypoint keypoints[72];
+  int count=0;
+
+
+  void addKeypoint(std::string name, float x, float y, float z){
+      if (count >= 72){
+          return;
+      }
+      strncpy(keypoints[count].name, name.c_str(), sizeof(keypoints[count]));
+      keypoints[count].location[0] = x;
+      keypoints[count].location[1] = y;
+      keypoints[count].location[2] = z;
+      count ++;
+  }
+
+  operator rpc::Keypoints() const {
+      rpc::Keypoints kps;
+      for (int i=0; i<count; i++)
+      {
+          geom::Vector3D v(keypoints[i].location[0], keypoints[i].location[1],
+                     keypoints[i].location[2]);
+          std::string name = std::string(keypoints[i].name);
+          std::pair<std::string, geom::Vector3D> pair1(name, v);
+          kps.keypoints.push_back(pair1);
+      }
+    return kps;
+  };
+
+};
+#pragma pack(pop)
+
+
 } // namespace detail
 
 #pragma pack(push, 1)
@@ -124,16 +176,19 @@ namespace detail {
       detail::VehicleData vehicle_data;
       detail::PackedWalkerControl walker_control;
     } state;
+
+    detail::PackedKeypoints keypoints;
+
   };
 
 #pragma pack(pop)
 
-static_assert(
-    sizeof(ActorDynamicState) == 93u,
-    "Invalid ActorDynamicState size! "
-    "If you modified this class please update the size here, else you may "
-    "comment this assert, but your platform may have compatibility issues "
-    "connecting to other platforms.");
+// static_assert(
+//     sizeof(ActorDynamicState) == 204u,
+//     "Invalid ActorDynamicState size! "
+//     "If you modified this class please update the size here, else you may "
+//     "comment this assert, but your platform may have compatibility issues "
+//     "connecting to other platforms.");
 
 } // namespace data
 } // namespace sensor

PythonAPI/carla/source/libcarla/Actor.cpp

@@ -10,6 +10,7 @@
 #include <carla/client/Walker.h>
 #include <carla/client/WalkerAIController.h>
 #include <carla/rpc/TrafficLightState.h>
+#include <carla/rpc/Keypoints.h>
 
 #include <boost/python/suite/indexing/vector_indexing_suite.hpp>
 
@@ -24,6 +25,11 @@ namespace client {
     return out;
   }
 
+  // std::ostream &operator<<(std::ostream &out, const carla::rpc::Keypoints &keypoints) {
+  //   out << "Keypoints(keypoints(" << keypoints.keypoints.size() << "))";
+  //   return out;
+  // }
+
 } // namespace client
 } // namespace carla
 
@@ -49,6 +55,18 @@ static void ApplyControl(carla::client::Walker &self, const ControlT &control) {
   self.ApplyControl(control);
 }
 
+static auto GetKeypoints(const carla::rpc::Keypoints &self) {
+  namespace py = boost::python;
+  auto keypoints = self.keypoints;
+  py::list result;
+  for (auto kp : keypoints) {
+    auto name = kp.first;
+    auto location = kp.second;
+    result.append(py::make_tuple(name, location));
+  }
+  return result;
+}
+
 void export_actor() {
   using namespace boost::python;
   namespace cc = carla::client;
@@ -59,6 +77,11 @@ void export_actor() {
       .def(self_ns::str(self_ns::self))
   ;
 
+  class_<cr::Keypoints>("Keypoints")
+    .def(init<>())
+    .add_property("keypoints", &GetKeypoints)
+  ;
+
   class_<cc::Actor, boost::noncopyable, boost::shared_ptr<cc::Actor>>("Actor", no_init)
   // work-around, force return copy to resolve Actor instead of ActorState.
       .add_property("id", CALL_RETURNING_COPY(cc::Actor, GetId))
@@ -78,6 +101,7 @@ void export_actor() {
       .def("get_transform", &cc::Actor::GetTransform)
       .def("get_velocity", &cc::Actor::GetVelocity)
       .def("get_angular_velocity", &cc::Actor::GetAngularVelocity)
+      .def("get_keypoints", &cc::Actor::GetKeypoints)
       .def("get_acceleration", &cc::Actor::GetAcceleration)
       .def("set_location", &cc::Actor::SetLocation, (arg("location")))
       .def("set_transform", &cc::Actor::SetTransform, (arg("transform")))

Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/WorldObserver.cpp

@@ -9,6 +9,9 @@
 
 #include "Carla/Traffic/TrafficLightBase.h"
 #include "Carla/Walker/WalkerController.h"
+#include "Components/PoseableMeshComponent.h"
+#include "Components/PrimitiveComponent.h"
+#include "GameFramework/CharacterMovementComponent.h"
 
 #include "CoreGlobals.h"
 
@@ -57,6 +60,7 @@ static auto FWorldObserver_GetActorState(const FActorView &View, const FActorReg
     if (Controller != nullptr)
     {
       state.walker_control = carla::rpc::WalkerControl{Controller->GetWalkerControl()};
+
     }
   }
   else if (AType::TrafficLight == View.GetActorType())
@@ -78,6 +82,31 @@ static auto FWorldObserver_GetActorState(const FActorView &View, const FActorReg
   return state;
 }
 
+static auto FWorldObserver_GetActorKeyPoints(const FActorView &View, const FActorRegistry &Registry)
+{
+  carla::sensor::data::detail::PackedKeypoints keypoints;
+  auto Pawn = Cast<APawn>(View.GetActor());
+  if (Pawn != nullptr)
+  {
+    TArray<USkeletalMeshComponent *> SkeletalMeshes;
+    Pawn->GetComponents<USkeletalMeshComponent>(SkeletalMeshes, false);
+    USkeletalMeshComponent *SkeletalMesh = SkeletalMeshes.IsValidIndex(0) ? SkeletalMeshes[0] : nullptr;
+
+    if (SkeletalMesh)
+    {
+        int32 NumBones = SkeletalMesh->GetNumBones();
+        for (int32 i = 0; i < NumBones; ++i)
+        {
+          FName const BoneName = SkeletalMesh->GetBoneName(i);
+          std::string key = std::string(TCHAR_TO_UTF8(*BoneName.ToString()));
+          FVector location = SkeletalMesh->GetBoneLocation(BoneName, EBoneSpaces::Type::WorldSpace);
+          keypoints.addKeypoint(key, location.X, location.Y, location.Z);
+      	}
+    }
+  }
+  return keypoints;
+}
+
 static carla::geom::Vector3D FWorldObserver_GetAngularVelocity(const AActor &Actor)
 {
   const auto RootComponent = Cast<UPrimitiveComponent>(Actor.GetRootComponent());
@@ -138,7 +167,8 @@ static carla::Buffer FWorldObserver_Serialize(
       carla::geom::Vector3D{Velocity.X, Velocity.Y, Velocity.Z},
       FWorldObserver_GetAngularVelocity(*View.GetActor()),
       FWorldObserver_GetAcceleration(View, Velocity, DeltaSeconds),
-      FWorldObserver_GetActorState(View, Registry)
+      FWorldObserver_GetActorState(View, Registry),
+      FWorldObserver_GetActorKeyPoints(View, Registry)
     };
     write_data(info);
   }

Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Walker/WalkerController.cpp

@@ -103,6 +103,8 @@ void AWalkerController::SetManualBones(const bool bIsEnabled)
       UPoseableMeshComponent *PoseableMesh = PoseableMeshes.IsValidIndex(0) ? PoseableMeshes[0] : nullptr;
       PoseableMesh->SetVisibility(false);
       SkeletalMesh->SetVisibility(true);
+      FName BoneName = FName("crl_arm__R");
+      UE_LOG(LogCarla, Warning, TEXT("222--> Bone's location is %s"), *PoseableMesh->GetBoneLocationByName(BoneName, EBoneSpaces::Type::ComponentSpace).ToString());
     }
   }
 }
@@ -137,6 +139,8 @@ void AWalkerController::ControlTickVisitor::operator()(FWalkerBoneControl &Walke
     {
       FName BoneName = FName(*pair.Key);
       PoseableMesh->SetBoneTransformByName(BoneName, pair.Value, EBoneSpaces::Type::ComponentSpace);
+      UE_LOG(LogCarla, Warning, TEXT("Bone's location is %s"), *PoseableMesh->GetBoneLocationByName(BoneName, EBoneSpaces::Type::ComponentSpace).ToString());
+      UE_LOG(LogCarla, Warning, TEXT("Bone name %s"), *BoneName.ToString());
     }
     WalkerBoneControl.BoneTransforms.Empty();
   }

Util/BuildTools/Package.sh

@@ -46,7 +46,7 @@ done
 # -- Package project -----------------------------------------------------------
 # ==============================================================================
 
-REPOSITORY_TAG=$(get_git_repository_version)
+REPOSITORY_TAG="0.9.5" # $(get_git_repository_version)
 
 BUILD_FOLDER=${CARLA_DIST_FOLDER}/${REPOSITORY_TAG}
 

Util/BuildTools/Setup.sh

@@ -346,7 +346,7 @@ unset RECAST_BASENAME
 # -- Generate Version.h --------------------------------------------------------
 # ==============================================================================
 
-CARLA_VERSION=$(get_git_repository_version)
+CARLA_VERSION="0.9.5" # $(get_git_repository_version)
 
 log "CARLA version ${CARLA_VERSION}."