Feature/unit tests/simple sensor simulator

.github/workflows/Docker.yaml

@@ -26,7 +26,11 @@ jobs:
     strategy:
       matrix:
         rosdistro: [humble]
-        arch: [amd64, arm64]
+        arch: [amd64]
+        # Build test for arm64 CPU is broken.
+        # This is a temporary solution and will be repaired in the future.
+        # See also https://github.com/tier4/scenario_simulator_v2/pull/1295
+        # arch: [amd64, arm64]
     steps:
       - name: Free Disk Space (Ubuntu)
         uses: jlumbroso/free-disk-space@main

.github/workflows/Release.yaml

@@ -130,8 +130,13 @@ jobs:
           name: ${{ steps.new_version.outputs.new_version }}
           body: ${{ steps.get_release_description.outputs.stdout }}
 
-      - name: Delete branch
+      - name: Delete branch if merged
         if: github.event.pull_request.merged == true
-        uses: SvanBoxel/delete-merged-branch@main
-        env:
-          GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }}
+        uses: actions/github-script@v5
+        with:
+          script: |
+            github.rest.git.deleteRef({
+              owner: context.repo.owner,
+              repo: context.repo.repo,
+              ref: `heads/${context.payload.pull_request.head.ref}`,
+            })

common/math/arithmetic/CHANGELOG.rst

@@ -13,6 +13,83 @@ Changelog for package arithmetic
 * Merge branch 'master' into refactor/drop_workflow
 * Contributors: Kotaro Yoshimoto
 
+2.2.2 (2024-06-28)
+------------------
+
+2.2.1 (2024-06-27)
+------------------
+* Merge remote-tracking branch 'origin/master' into fix/issue1276-re
+* Contributors: Masaya Kataoka
+
+2.2.0 (2024-06-24)
+------------------
+* Merge branch 'master' into feature/clear_route_api
+* Merge remote-tracking branch 'origin/master' into feature/clear_route_api
+* Merge branch 'master' into feature/clear_route_api
+* Merge branch 'master' into feature/clear_route_api
+* Contributors: Masaya Kataoka, Taiga
+
+2.1.11 (2024-06-24)
+-------------------
+* Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/miscellaneous
+* resolve merge confilct
+* resolve merge
+* Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/miscellaneous
+* remove arithmetic tests
+* floating_point comparison
+* Contributors: robomic
+
+2.1.10 (2024-06-24)
+-------------------
+* Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/misc_object_entity
+* Contributors: robomic
+
+2.1.9 (2024-06-24)
+------------------
+
+2.1.8 (2024-06-20)
+------------------
+* Merge branch 'master' into feature/simple_sensor_simulator_unit_test
+* Merge branch 'master' into feature/simple_sensor_simulator_unit_test
+* Contributors: Kotaro Yoshimoto, SzymonParapura
+
+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

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.2.2</version>
   <description>arithmetic library for scenario_simulator_v2</description>
   <maintainer email="[email protected]">Tatsuya Yamasaki</maintainer>
   <license>Apache License 2.0</license>

common/math/geometry/CHANGELOG.rst

@@ -13,6 +13,89 @@ Changelog for package geometry
 * Merge branch 'master' into refactor/drop_workflow
 * Contributors: Kotaro Yoshimoto
 
+2.2.2 (2024-06-28)
+------------------
+
+2.2.1 (2024-06-27)
+------------------
+* Merge remote-tracking branch 'origin/master' into fix/issue1276-re
+* Contributors: Masaya Kataoka
+
+2.2.0 (2024-06-24)
+------------------
+* Merge branch 'master' into feature/clear_route_api
+* Merge remote-tracking branch 'origin/master' into feature/clear_route_api
+* Merge branch 'master' into feature/clear_route_api
+* Merge branch 'master' into feature/clear_route_api
+* Contributors: Masaya Kataoka, Taiga
+
+2.1.11 (2024-06-24)
+-------------------
+* Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/miscellaneous
+* resolve merge confilct
+* resolve merge
+* Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/miscellaneous
+* Contributors: robomic
+
+2.1.10 (2024-06-24)
+-------------------
+* Merge branch 'master' of github.com:tier4/scenario_simulator_v2 into feature/unit_tests/misc_object_entity
+* Contributors: robomic
+
+2.1.9 (2024-06-24)
+------------------
+
+2.1.8 (2024-06-20)
+------------------
+* Merge branch 'master' into feature/simple_sensor_simulator_unit_test
+* Merge branch 'master' into feature/simple_sensor_simulator_unit_test
+* Contributors: Kotaro Yoshimoto, SzymonParapura
+
+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

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_

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_

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_

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_