Extend ROS2 support Step 1: Update geometry data types (#9425)

* Extend ROS2 support Step 1: Update geometry data types

To extend and simplify the native ROS2 support, some additional geometry
data types are introducued. Furthermore, some

- Introduced geom::AngularVelocity, geom::Acceleration,
  geom::Quaternion and geom::Velocity to ease the
  Unreal -> Carla -> ROS2 data conversion
- Added some Math functions for Quaternions
- Renamed MakeSafeUnitVector->MakeUnitVector
  (no 'unsafe' variant allowed anymore)
- Renamed RotateVector -> RotatedVector to reflect that a rotated vector
  is returned and the original vector is NOT rotated
- Added Math::GetVectorAngle(), Math::GetVectorAngleAbs() and supporting
  functions. Use within MotionPlanStage to get correct angle delta.

BUGFIX:
When introducing the rotations via Quaternions it became obvious that
the pitch and roll roations have been incorrect up to now. Therefore,
the RightHandedVector3D class was introduced to support the conversion
Unreal -> CARLA
- Fixed geom::Rotation::RotateVector() rotation directions
  of pitch and roll!
- Added RightHandedVector3D.h (internal class deployed for implicit
  correct rotation of CARLA left handed vectors)
- Allow to comment TransformationMatrix access out by ifdef to prevent
from erroneous misuse, since the TransformationMatrix does NOT provide
  a right-handed rotation as the user might expect.
  (Per default access is still enabled)

* use geom types on Actor functions

Author: berndgassmann

.gitignore

@@ -38,6 +38,7 @@ build/
 *CodeLitePreProcessor.txt
 .aria2c.input
 .codelite
+.debug
 .gdb_history
 .gtest
 .idea

CHANGELOG.md

@@ -4,6 +4,8 @@
  * Improved the turning behavior of vehicles controlled by the TrafficManager, making them smoother.
  * Added a hybrid solid-state LiDAR with adjustable parameters (blueprint attributes)
  * Fix OpenDrive Builder lane width
+ * Introduced geom::AngularVelocity, geom::Velocity, geom::Acceleration, geom::Quaternion types
+ * Fixed geom::Rotation::RotateVector() rotation directions of pitch and roll
 
 ## CARLA 0.9.16
 

LibCarla/source/carla/client/Actor.cpp

@@ -20,15 +20,15 @@ namespace client {
     return GetEpisode().Lock()->GetActorTransform(*this);
   }
 
-  geom::Vector3D Actor::GetVelocity() const {
+  geom::Velocity Actor::GetVelocity() const {
     return GetEpisode().Lock()->GetActorVelocity(*this);
   }
 
-  geom::Vector3D Actor::GetAngularVelocity() const {
+  geom::AngularVelocity Actor::GetAngularVelocity() const {
     return GetEpisode().Lock()->GetActorAngularVelocity(*this);
   }
 
-  geom::Vector3D Actor::GetAcceleration() const {
+  geom::Acceleration Actor::GetAcceleration() const {
     return GetEpisode().Lock()->GetActorAcceleration(*this);
   }
 

LibCarla/source/carla/client/Actor.h

@@ -9,6 +9,9 @@
 #include "carla/Debug.h"
 #include "carla/Memory.h"
 #include "carla/client/detail/ActorState.h"
+#include "carla/geom/Acceleration.h"
+#include "carla/geom/AngularVelocity.h"
+#include "carla/geom/Velocity.h"
 #include "carla/profiler/LifetimeProfiled.h"
 
 namespace carla {
@@ -45,19 +48,19 @@ namespace client {
     ///
     /// @note This function does not call the simulator, it returns the
     /// velocity received in the last tick.
-    geom::Vector3D GetVelocity() const;
+    geom::Velocity GetVelocity() const;
 
     /// Return the current 3D angular velocity of the actor.
     ///
     /// @note This function does not call the simulator, it returns the
     /// angular velocity received in the last tick.
-    geom::Vector3D GetAngularVelocity() const;
+    geom::AngularVelocity GetAngularVelocity() const;
 
     /// Return the current 3D acceleration of the actor.
     ///
     /// @note This function does not call the simulator, it returns the
     /// acceleration calculated after the actor's velocity.
-    geom::Vector3D GetAcceleration() const;
+    geom::Acceleration GetAcceleration() const;
 
     geom::BoundingBox GetBoundingBox() const;
 

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

@@ -381,7 +381,7 @@ namespace detail {
       if (attachment_type == rpc::AttachmentType::SpringArm ||
           attachment_type == rpc::AttachmentType::SpringArmGhost)
       {
-        const auto a = transform.location.MakeSafeUnitVector(std::numeric_limits<float>::epsilon());
+        const auto a = transform.location.MakeUnitVector(std::numeric_limits<float>::epsilon());
         const auto z = geom::Vector3D(0.0f, 0.f, 1.0f);
         constexpr float OneEps = 1.0f - std::numeric_limits<float>::epsilon();
         if (geom::Math::Dot(a, z) > OneEps) {

LibCarla/source/carla/geom/Acceleration.h

@@ -0,0 +1,98 @@
+// 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/geom/Vector3D.h"
+#include "carla/geom/Vector3DInt.h"
+#include "carla/geom/Math.h"
+
+#ifdef LIBCARLA_INCLUDED_FROM_UE4
+#include <compiler/enable-ue4-macros.h>
+#include "Math/Vector.h"
+#include <compiler/disable-ue4-macros.h>
+#endif // LIBCARLA_INCLUDED_FROM_UE4
+
+namespace carla {
+namespace geom {
+
+  /**
+   * Stores the acceleration vector of an entity in m/s²
+  */
+  class Acceleration : public Vector3D {
+  public:
+
+    // =========================================================================
+    // -- Constructors ---------------------------------------------------------
+    // =========================================================================
+
+    Acceleration() = default;
+
+    using Vector3D::Vector3D;
+
+    Acceleration(const Vector3D &rhs) : Vector3D(rhs) {}
+
+    Acceleration(const Vector3DInt &rhs) :
+        Vector3D(static_cast<float>(rhs.x),
+                 static_cast<float>(rhs.y),
+                 static_cast<float>(rhs.z)) {}
+
+    // =========================================================================
+    // -- Arithmetic operators -------------------------------------------------
+    // =========================================================================
+
+    Acceleration &operator+=(const Acceleration &rhs) {
+      static_cast<Vector3D &>(*this) += static_cast<const Vector3D &>(rhs);
+      return *this;
+    }
+
+    friend Acceleration operator+(Acceleration lhs, const Acceleration &rhs) {
+      lhs += rhs;
+      return lhs;
+    }
+
+    Acceleration &operator-=(const Acceleration &rhs) {
+      static_cast<Vector3D &>(*this) -= static_cast<const Vector3D &>(rhs);
+      return *this;
+    }
+
+    friend Acceleration operator-(Acceleration lhs, const Acceleration &rhs) {
+      lhs -= rhs;
+      return lhs;
+    }
+
+    // =========================================================================
+    // -- Comparison operators -------------------------------------------------
+    // =========================================================================
+
+    bool operator==(const Acceleration &rhs) const {
+      return static_cast<const Vector3D &>(*this) == static_cast<const Vector3D &>(rhs);
+    }
+
+    bool operator!=(const Acceleration &rhs) const {
+      return !(*this == rhs);
+    }
+
+    // =========================================================================
+    // -- Conversions to UE4 types ---------------------------------------------
+    // =========================================================================
+
+#ifdef LIBCARLA_INCLUDED_FROM_UE4
+
+    // from cm/s² to m/s².
+    Acceleration(const FVector &vector)
+      : Acceleration(1e-2f * vector.X, 1e-2f * vector.Y, 1e-2f * vector.Z) {}
+
+    // from m/s² to cm/s²
+    operator FVector() const {
+      return FVector{1e2f * x, 1e2f * y, 1e2f * z}; 
+    }
+
+#endif // LIBCARLA_INCLUDED_FROM_UE4
+  };
+
+} // namespace geom
+} // namespace carla

LibCarla/source/carla/geom/AngularVelocity.h

@@ -0,0 +1,98 @@
+// 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/geom/Vector3D.h"
+#include "carla/geom/Vector3DInt.h"
+#include "carla/geom/Math.h"
+
+#ifdef LIBCARLA_INCLUDED_FROM_UE4
+#include <compiler/enable-ue4-macros.h>
+#include "Math/Vector.h"
+#include <compiler/disable-ue4-macros.h>
+#endif // LIBCARLA_INCLUDED_FROM_UE4
+
+namespace carla {
+namespace geom {
+
+  /**
+   * Stores the angular velocity vector in deg/s
+  */
+  class AngularVelocity : public Vector3D {
+  public:
+
+    // =========================================================================
+    // -- Constructors ---------------------------------------------------------
+    // =========================================================================
+
+    AngularVelocity() = default;
+
+    using Vector3D::Vector3D;
+
+    AngularVelocity(const Vector3D &rhs) : Vector3D(rhs) {}
+
+    AngularVelocity(const Vector3DInt &rhs) :
+        Vector3D(static_cast<float>(rhs.x),
+                 static_cast<float>(rhs.y),
+                 static_cast<float>(rhs.z)) {}
+
+    // =========================================================================
+    // -- Arithmetic operators -------------------------------------------------
+    // =========================================================================
+
+    AngularVelocity &operator+=(const AngularVelocity &rhs) {
+      static_cast<Vector3D &>(*this) += static_cast<const Vector3D &>(rhs);
+      return *this;
+    }
+
+    friend AngularVelocity operator+(AngularVelocity lhs, const AngularVelocity &rhs) {
+      lhs += rhs;
+      return lhs;
+    }
+
+    AngularVelocity &operator-=(const AngularVelocity &rhs) {
+      static_cast<Vector3D &>(*this) -= static_cast<const Vector3D &>(rhs);
+      return *this;
+    }
+
+    friend AngularVelocity operator-(AngularVelocity lhs, const AngularVelocity &rhs) {
+      lhs -= rhs;
+      return lhs;
+    }
+
+    // =========================================================================
+    // -- Comparison operators -------------------------------------------------
+    // =========================================================================
+
+    bool operator==(const AngularVelocity &rhs) const {
+      return static_cast<const Vector3D &>(*this) == static_cast<const Vector3D &>(rhs);
+    }
+
+    bool operator!=(const AngularVelocity &rhs) const {
+      return !(*this == rhs);
+    }
+
+    // =========================================================================
+    // -- Conversions to UE4 types ---------------------------------------------
+    // =========================================================================
+
+#ifdef LIBCARLA_INCLUDED_FROM_UE4
+
+    // in future potentially transform deg/s -> rad/s?
+    AngularVelocity(const FVector &vector)
+      : Vector3D(vector.X, vector.Y, vector.Z) {}
+
+    // in future potentially transform rad/s -> deg/s?
+    operator FVector() const {
+      return FVector{x, y, z}; 
+    }
+
+#endif // LIBCARLA_INCLUDED_FROM_UE4
+  };
+
+} // namespace geom
+} // namespace carla

LibCarla/source/carla/geom/BoundingBox.h

@@ -83,14 +83,14 @@ namespace geom {
     std::array<Location, 8> GetLocalVertices() const {
 
         return {{
-            location + Location(rotation.RotateVector({-extent.x,-extent.y,-extent.z})),
-            location + Location(rotation.RotateVector({-extent.x,-extent.y, extent.z})),
-            location + Location(rotation.RotateVector({-extent.x, extent.y,-extent.z})),
-            location + Location(rotation.RotateVector({-extent.x, extent.y, extent.z})),
-            location + Location(rotation.RotateVector({ extent.x,-extent.y,-extent.z})),
-            location + Location(rotation.RotateVector({ extent.x,-extent.y, extent.z})),
-            location + Location(rotation.RotateVector({ extent.x, extent.y,-extent.z})),
-            location + Location(rotation.RotateVector({ extent.x, extent.y, extent.z}))
+            location + Location(rotation.RotatedVector(Location(-extent.x,-extent.y,-extent.z))),
+            location + Location(rotation.RotatedVector(Location(-extent.x,-extent.y, extent.z))),
+            location + Location(rotation.RotatedVector(Location(-extent.x, extent.y,-extent.z))),
+            location + Location(rotation.RotatedVector(Location(-extent.x, extent.y, extent.z))),
+            location + Location(rotation.RotatedVector(Location( extent.x,-extent.y,-extent.z))),
+            location + Location(rotation.RotatedVector(Location( extent.x,-extent.y, extent.z))),
+            location + Location(rotation.RotatedVector(Location( extent.x, extent.y,-extent.z))),
+            location + Location(rotation.RotatedVector(Location( extent.x, extent.y, extent.z)))
         }};
     }
 

LibCarla/source/carla/geom/Math.cpp

@@ -7,12 +7,38 @@
 #include "carla/geom/Math.h"
 
 #include "carla/geom/Rotation.h"
+#include "carla/geom/Quaternion.h"
 
 namespace carla {
 namespace geom {
 
-  double Math::GetVectorAngle(const Vector3D &a, const Vector3D &b) {
-    return std::acos(Dot(a, b) / (a.Length() * b.Length()));
+  float Math::SineVectorAngleFromUnitVectors(Vector3D const &a_unit, Vector3D const &b_unit)
+  {
+    auto const cross = Math::Cross(a_unit, b_unit);
+    Vector3D const to_up(0.f, 0.f, 1.f);
+    auto const sine_angle_abs = Math::Length(cross);
+    if ( std::signbit(Math::Dot(cross, to_up) ) )
+    {
+      return -sine_angle_abs;
+    }
+    else
+    {
+      return sine_angle_abs;
+    }
+  }
+
+  float Math::GetVectorAngleAbs(const Vector3D &a, const Vector3D &b) {
+    float cosine_vector_angle = Math::CosineVectorAngle(a, b);
+    return std::acos(cosine_vector_angle);
+  }
+
+  float Math::GetVectorAngle(const Vector3D &a, const Vector3D &b) {
+    auto const a_unit = a.MakeUnitVector();
+    auto const b_unit = b.MakeUnitVector();
+    auto const cosine_vector_angle = Math::CosineVectorAngleFromUnitVectors(a_unit, b_unit);
+    auto const sine_vector_angle = Math::SineVectorAngleFromUnitVectors(a_unit, b_unit);
+    auto const angle = std::atan2(sine_vector_angle, cosine_vector_angle);
+    return angle;
   }
 
   std::pair<float, float> Math::DistanceSegmentToPoint(
@@ -114,39 +140,17 @@ namespace geom {
     return Vector3D(p.x * c - p.y * s, p.x * s + p.y * c, 0.0f);
   }
 
-  Vector3D Math::GetForwardVector(const Rotation &rotation) {
-    const float cp = std::cos(ToRadians(rotation.pitch));
-    const float sp = std::sin(ToRadians(rotation.pitch));
-    const float cy = std::cos(ToRadians(rotation.yaw));
-    const float sy = std::sin(ToRadians(rotation.yaw));
-    return {cy * cp, sy * cp, sp};
-  }
+  Vector3D GetForwardVector(const Rotation &rotation) { return rotation.GetForwardVector(); }
 
-  Vector3D Math::GetRightVector(const Rotation &rotation) {
-    const float cy = std::cos(ToRadians(rotation.yaw));
-    const float sy = std::sin(ToRadians(rotation.yaw));
-    const float cr = std::cos(ToRadians(rotation.roll));
-    const float sr = std::sin(ToRadians(rotation.roll));
-    const float cp = std::cos(ToRadians(rotation.pitch));
-    const float sp = std::sin(ToRadians(rotation.pitch));
-    return {
-         cy * sp * sr - sy * cr,
-         sy * sp * sr + cy * cr,
-        -cp * sr};
-  }
+  Vector3D GetRightVector(const Rotation &rotation) { return rotation.GetRightVector(); }
 
-  Vector3D Math::GetUpVector(const Rotation &rotation) {
-    const float cy = std::cos(ToRadians(rotation.yaw));
-    const float sy = std::sin(ToRadians(rotation.yaw));
-    const float cr = std::cos(ToRadians(rotation.roll));
-    const float sr = std::sin(ToRadians(rotation.roll));
-    const float cp = std::cos(ToRadians(rotation.pitch));
-    const float sp = std::sin(ToRadians(rotation.pitch));
-    return {
-        -cy * sp * cr - sy * sr,
-        -sy * sp * cr + cy * sr,
-        cp * cr};
-  }
+  Vector3D GetUpVector(const Rotation &rotation) { return rotation.GetUpVector(); }
+
+  Vector3D GetForwardVector(const Quaternion &quaternion) { return quaternion.GetForwardVector(); }
+
+  Vector3D GetRightVector(const Quaternion &quaternion) { return quaternion.GetRightVector(); }
+
+  Vector3D GetUpVector(const Quaternion &quaternion) { return quaternion.GetUpVector(); }
 
   std::vector<int> Math::GenerateRange(int a, int b) {
     std::vector<int> result;