RobotecAI · robomic · May 29, 2023 · May 29, 2023 · May 29, 2023 · Jun 20, 2023
diff --git a/common/math/arithmetic/CHANGELOG.rst b/common/math/arithmetic/CHANGELOG.rst
@@ -13,6 +13,43 @@ Changelog for package arithmetic
 * Merge branch 'master' into refactor/drop_workflow
 * Contributors: Kotaro Yoshimoto
 
+2.1.7 (2024-06-19)
+------------------
+* Merge branch 'master' into feature/improve-ros-parameter-handling
+* Merge branch 'master' into feature/improve-ros-parameter-handling
+* Contributors: Masaya Kataoka, Mateusz Palczuk
+
+2.1.6 (2024-06-18)
+------------------
+
+2.1.5 (2024-06-18)
+------------------
+
+2.1.4 (2024-06-14)
+------------------
+* Merge branch 'master' into fix/remove_quaternion_operation
+* Merge branch 'master' into fix/remove_quaternion_operation
+* Contributors: Masaya Kataoka
+
+2.1.3 (2024-06-14)
+------------------
+* Merge branch 'master' into fix/issue1276
+* Contributors: Masaya Kataoka
+
+2.1.2 (2024-06-13)
+------------------
+* Merge branch 'master' into fix/interpreter/fault-injection-action
+* Merge branch 'master' into fix/interpreter/fault-injection-action
+* Merge branch 'master' into fix/interpreter/fault-injection-action
+* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action
+* Merge branch 'master' into fix/interpreter/fault-injection-action
+* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action
+* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action
+* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action
+* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action
+* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action
+* Contributors: Tatsuya Yamasaki, yamacir-kit
+
 2.1.1 (2024-06-11)
 ------------------
 * Merge branch 'master' into fix/reorder

diff --git a/common/math/arithmetic/package.xml b/common/math/arithmetic/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>arithmetic</name>
-  <version>2.1.1</version>
+  <version>2.1.7</version>
   <description>arithmetic library for scenario_simulator_v2</description>
   <maintainer email="[email protected]">Tatsuya Yamasaki</maintainer>
   <license>Apache License 2.0</license>

diff --git a/common/math/geometry/CHANGELOG.rst b/common/math/geometry/CHANGELOG.rst
@@ -13,6 +13,51 @@ Changelog for package geometry
 * Merge branch 'master' into refactor/drop_workflow
 * Contributors: Kotaro Yoshimoto
 
+2.1.7 (2024-06-19)
+------------------
+* Merge branch 'master' into feature/improve-ros-parameter-handling
+* Merge branch 'master' into feature/improve-ros-parameter-handling
+* Contributors: Masaya Kataoka, Mateusz Palczuk
+
+2.1.6 (2024-06-18)
+------------------
+
+2.1.5 (2024-06-18)
+------------------
+
+2.1.4 (2024-06-14)
+------------------
+* Merge pull request `#1281 <https://github.com/tier4/scenario_simulator_v2/issues/1281>`_ from tier4/fix/remove_quaternion_operation
+  Remove quaternion_operation
+* Merge branch 'master' into fix/remove_quaternion_operation
+* Update common/math/geometry/include/geometry/quaternion/get_rotation.hpp
+* fix
+* Merge branch 'master' into fix/remove_quaternion_operation
+* Remove quaternion_operation
+* Contributors: Masaya Kataoka, Taiga Takano
+
+2.1.3 (2024-06-14)
+------------------
+* Merge branch 'master' into fix/issue1276
+* Contributors: Masaya Kataoka
+
+2.1.2 (2024-06-13)
+------------------
+* Merge branch 'master' into fix/interpreter/fault-injection-action
+* Merge branch 'master' into fix/interpreter/fault-injection-action
+* Merge branch 'master' into fix/interpreter/fault-injection-action
+* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action
+* Merge branch 'master' into fix/interpreter/fault-injection-action
+* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action
+* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action
+* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action
+* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action
+* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action
+* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action
+* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action
+* Merge remote-tracking branch 'origin/master' into fix/interpreter/fault-injection-action
+* Contributors: Tatsuya Yamasaki, yamacir-kit
+
 2.1.1 (2024-06-11)
 ------------------
 * Merge branch 'master' into fix/reorder

diff --git a/common/math/geometry/include/geometry/quaternion/euler_to_quaternion.hpp b/common/math/geometry/include/geometry/quaternion/euler_to_quaternion.hpp
@@ -0,0 +1,46 @@
+// Copyright 2015 TIER IV, Inc. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef GEOMETRY__QUATERNION__EULER_TO_QUATERNION_HPP_
+#define GEOMETRY__QUATERNION__EULER_TO_QUATERNION_HPP_
+
+#include <tf2/LinearMath/Matrix3x3.h>
+
+#include <cmath>
+#include <geometry/vector3/is_like_vector3.hpp>
+#include <geometry_msgs/msg/quaternion.hpp>
+
+namespace math
+{
+namespace geometry
+{
+template <
+  typename T, std::enable_if_t<std::conjunction_v<IsLikeVector3<T>>, std::nullptr_t> = nullptr>
+auto convertEulerAngleToQuaternion(const T & v)
+{
+  const double & roll = v.x;
+  const double & pitch = v.y;
+  const double & yaw = v.z;
+  tf2::Quaternion tf_quat;
+  tf_quat.setRPY(roll, pitch, yaw);
+  return geometry_msgs::build<geometry_msgs::msg::Quaternion>()
+    .x(tf_quat.x())
+    .y(tf_quat.y())
+    .z(tf_quat.z())
+    .w(tf_quat.w());
+}
+}  // namespace geometry
+}  // namespace math
+
+#endif  // GEOMETRY__QUATERNION__EULER_TO_QUATERNION_HPP_
diff --git a/common/math/geometry/include/geometry/quaternion/get_rotation.hpp b/common/math/geometry/include/geometry/quaternion/get_rotation.hpp
@@ -0,0 +1,43 @@
+// Copyright 2015 TIER IV, Inc. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef GEOMETRY__QUATERNION__GET_ROTATION_HPP_
+#define GEOMETRY__QUATERNION__GET_ROTATION_HPP_
+
+#include <tf2/LinearMath/Matrix3x3.h>
+
+#include <cmath>
+#include <geometry/quaternion/is_like_quaternion.hpp>
+#include <geometry/quaternion/operator.hpp>
+
+namespace math
+{
+namespace geometry
+{
+template <
+  typename T, typename U,
+  std::enable_if_t<std::conjunction_v<IsLikeQuaternion<T>, IsLikeQuaternion<U>>, std::nullptr_t> =
+    nullptr>
+auto getRotation(T from, U to)
+{
+  // This function gets the conjugate of the quaternion.
+  from.x *= -1;
+  from.y *= -1;
+  from.z *= -1;
+  return from * to;
+}
+}  // namespace geometry
+}  // namespace math
+
+#endif  // GEOMETRY__QUATERNION__GET_ROTATION_HPP_
diff --git a/common/math/geometry/include/geometry/quaternion/get_rotation_matrix.hpp b/common/math/geometry/include/geometry/quaternion/get_rotation_matrix.hpp
@@ -0,0 +1,50 @@
+// Copyright 2015 TIER IV, Inc. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef GEOMETRY__QUATERNION__GET_ROTATION_MATRIX_HPP_
+#define GEOMETRY__QUATERNION__GET_ROTATION_MATRIX_HPP_
+
+#include <tf2/LinearMath/Matrix3x3.h>
+
+#include <cmath>
+#include <geometry/quaternion/is_like_quaternion.hpp>
+
+#define EIGEN_MPL2_ONLY
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+namespace math
+{
+namespace geometry
+{
+template <
+  typename T, std::enable_if_t<std::conjunction_v<IsLikeQuaternion<T>>, std::nullptr_t> = nullptr>
+auto getRotationMatrix(T quat)
+{
+  auto x = quat.x;
+  auto y = quat.y;
+  auto z = quat.z;
+  auto w = quat.w;
+  Eigen::Matrix3d ret(3, 3);
+  // clang-format off
+  ret << x * x - y * y - z * z + w * w,  2 * (x * y - z * w),            2 * (z * x + w * y),
+         2 * (x * y + z * w),           -x * x + y * y - z * z + w * w,  2 * (y * z - x * w), 
+         2 * (z * x - w * y),            2 * (y * z + w * x),           -x * x - y * y + z * z + w * w;
+  // clang-format on
+  return ret;
+}
+}  // namespace geometry
+}  // namespace math
+
+#endif  // GEOMETRY__QUATERNION__GET_ROTATION_MATRIX_HPP_
diff --git a/common/math/geometry/include/geometry/quaternion/quaternion_to_euler.hpp b/common/math/geometry/include/geometry/quaternion/quaternion_to_euler.hpp
@@ -0,0 +1,41 @@
+// Copyright 2015 TIER IV, Inc. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef GEOMETRY__QUATERNION__QUATERNION_TO_EULER_HPP_
+#define GEOMETRY__QUATERNION__QUATERNION_TO_EULER_HPP_
+
+#include <tf2/LinearMath/Matrix3x3.h>
+
+#include <cmath>
+#include <geometry/quaternion/is_like_quaternion.hpp>
+#include <geometry_msgs/msg/vector3.hpp>
+
+namespace math
+{
+namespace geometry
+{
+template <
+  typename T, std::enable_if_t<std::conjunction_v<IsLikeQuaternion<T>>, std::nullptr_t> = nullptr>
+auto convertQuaternionToEulerAngle(const T & q)
+{
+  tf2::Quaternion tf_quat(q.x, q.y, q.z, q.w);
+  tf2::Matrix3x3 mat(tf_quat);
+  double roll, pitch, yaw;
+  mat.getRPY(roll, pitch, yaw);
+  return geometry_msgs::build<geometry_msgs::msg::Vector3>().x(roll).y(pitch).z(yaw);
+}
+}  // namespace geometry
+}  // namespace math
+
+#endif  // GEOMETRY__QUATERNION__QUATERNION_TO_EULER_HPP_
diff --git a/common/math/geometry/include/geometry/quaternion/slerp.hpp b/common/math/geometry/include/geometry/quaternion/slerp.hpp
@@ -0,0 +1,58 @@
+// Copyright 2015 TIER IV, Inc. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef GEOMETRY__QUATERNION__SLERP_HPP_
+#define GEOMETRY__QUATERNION__SLERP_HPP_
+
+#include <cmath>
+#include <geometry/quaternion/is_like_quaternion.hpp>
+#include <geometry_msgs/msg/quaternion.hpp>
+namespace math
+{
+namespace geometry
+{
+template <
+  typename T, typename U, typename V,
+  std::enable_if_t<
+    std::conjunction_v<IsLikeQuaternion<T>, IsLikeQuaternion<U>, std::is_scalar<V>>,
+    std::nullptr_t> = nullptr>
+auto slerp(T quat1, U quat2, V t)
+{
+  double qr = quat1.w * quat2.w + quat1.x * quat2.x + quat1.y * quat2.y + quat1.z * quat2.z;
+  double ss = 1.0 - qr * qr;
+  constexpr double e = std::numeric_limits<double>::epsilon();
+  if (std::fabs(ss) <= e) {
+    return geometry_msgs::build<geometry_msgs::msg::Quaternion>()
+      .x(quat1.x)
+      .y(quat1.y)
+      .z(quat1.z)
+      .w(quat1.w);
+  } else {
+    double sp = std::sqrt(ss);
+    double ph = std::acos(qr);
+    double pt = ph * t;
+    double t1 = std::sin(pt) / sp;
+    double t0 = std::sin(ph - pt) / sp;
+
+    return geometry_msgs::build<geometry_msgs::msg::Quaternion>()
+      .x(quat1.x * t0 + quat2.x * t1)
+      .y(quat1.y * t0 + quat2.y * t1)
+      .z(quat1.z * t0 + quat2.z * t1)
+      .w(quat1.w * t0 + quat2.w * t1);
+  }
+}
+}  // namespace geometry
+}  // namespace math
+
+#endif  // GEOMETRY__QUATERNION__SLERP_HPP_
diff --git a/common/math/geometry/include/geometry/spline/hermite_curve.hpp b/common/math/geometry/include/geometry/spline/hermite_curve.hpp
@@ -16,7 +16,6 @@
 #define GEOMETRY__SPLINE__HERMITE_CURVE_HPP_
 
 #include <gtest/gtest.h>
-#include <quaternion_operation/quaternion_operation.h>
 
 #include <geometry/solver/polynomial_solver.hpp>
 #include <geometry_msgs/msg/point.hpp>

diff --git a/common/math/geometry/package.xml b/common/math/geometry/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>geometry</name>
-  <version>2.1.1</version>
+  <version>2.1.7</version>
   <description>geometry math library for scenario_simulator_v2 application</description>
   <maintainer email="[email protected]">Masaya Kataoka</maintainer>
   <license>Apache License 2.0</license>
@@ -12,10 +12,10 @@
 
   <depend>boost</depend>
   <depend>geometry_msgs</depend>
-  <depend>quaternion_operation</depend>
   <depend>rclcpp</depend>
   <depend>scenario_simulator_exception</depend>
   <depend>tf2_geometry_msgs</depend>
+  <depend>tf2_ros</depend>
   <depend>traffic_simulator_msgs</depend>
 
   <test_depend>ament_cmake_clang_format</test_depend>

diff --git a/common/math/geometry/src/bounding_box.cpp b/common/math/geometry/src/bounding_box.cpp
@@ -12,8 +12,6 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include <quaternion_operation/quaternion_operation.h>
-
 #include <geometry/bounding_box.hpp>
 #include <geometry/polygon/polygon.hpp>
 

diff --git a/common/math/geometry/src/intersection/collision.cpp b/common/math/geometry/src/intersection/collision.cpp
@@ -12,8 +12,6 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include <quaternion_operation/quaternion_operation.h>
-
 #include <boost/assert.hpp>
 #include <boost/assign/list_of.hpp>
 #include <boost/geometry.hpp>