RobotecAI · robomic · Oct 30, 2024 · Oct 30, 2024 · Oct 30, 2024 · Oct 30, 2024
diff --git a/.github/workflows/BuildAndRun.yaml b/.github/workflows/BuildAndRun.yaml
@@ -26,7 +26,6 @@ jobs:
     name: BuildAndRun
     runs-on: ${{ matrix.runs_on }}
     timeout-minutes: 180
-    container: ros:${{ matrix.rosdistro }}
     env:
       DEBIAN_FRONTEND: noninteractive
     strategy:
@@ -36,6 +35,11 @@ jobs:
         runs_on: [ubuntu-22.04, ubuntu-22.04-arm]
         cmake_build_type: [RelWithDebInfo, Release] # Debug build type is currently unavailable. @TODO Fix problem and add Debug build.
     steps:
+      - name: free disk
+        uses: jlumbroso/free-disk-space@main
+        with:
+          tool-cache: false
+
       - name: Suppress warnings
         run: git config --global --add safe.directory '*'
 
@@ -51,7 +55,24 @@ jobs:
         with:
           repository: RobotecAI/scenario_simulator_v2_scenarios
           path: src/scenario_simulator_v2_scenarios
-
+
+      - name: Cache apt and rosdep
+        id: cache-apt-rosdep
+        uses: actions/cache@v4
+        with:
+          path: |
+            /var/cache/apt/archives
+            /var/lib/apt/lists
+            /etc/ros/rosdep
+          key: ${{ runner.os }}-ros-${{ matrix.rosdistro }}-apt-${{ hashFiles('**/package.xml', '**/dependency_*.repos') }}
+          restore-keys: |
+            ${{ runner.os }}-ros-${{ matrix.rosdistro }}-apt-
+
+      - name: Setup ROS environment
+        uses: ros-tooling/[email protected]
+        with:
+          required-ros-distributions: ${{ matrix.rosdistro }}
+
       - name: Search packages in this repository
         id: list_packages
         run: |
@@ -67,12 +88,16 @@ jobs:
 
       - name: Resolve rosdep and install colcon mixin
         run: |
-          apt-get update
-          apt-get install -y python3-pip python3-colcon-lcov-result lcov unzip gcovr
+          sudo apt-get update
+          sudo apt-get install -y python3-pip python3-colcon-lcov-result lcov unzip gcovr libunwind-dev
           rosdep update --include-eol-distros
           rosdep install -iy --from-paths src --rosdistro ${{ matrix.rosdistro }}
           colcon mixin add default https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml
           colcon mixin update default
+          df -h
+          sudo apt-get clean
+          sudo rm -rf /var/lib/apt/lists/*
+          df -h
         shell: bash
 
       - name: Install Build Wrapper

diff --git a/common/agnocast_wrapper/CHANGELOG.rst b/common/agnocast_wrapper/CHANGELOG.rst
@@ -5,6 +5,14 @@ Changelog for package agnocast_wrapper
 16.4.2 (2025-05-23)
 -------------------
 
+17.6.1 (2025-10-02)
+-------------------
+
+17.6.0 (2025-09-29)
+-------------------
+* Merge branch 'master' into fix/noise_v4
+* Contributors: Kotaro Yoshimoto
+
 17.5.3 (2025-09-17)
 -------------------
 * Merge branch 'master' into fix/macro-xml-validation

diff --git a/common/agnocast_wrapper/package.xml b/common/agnocast_wrapper/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>agnocast_wrapper</name>
-  <version>17.5.3</version>
+  <version>17.6.1</version>
   <description>A wrapper package for agnocast</description>
   <maintainer email="[email protected]">Mateusz Palczuk</maintainer>
   <license>Apache License 2.0</license>

diff --git a/common/get_parameter/CHANGELOG.rst b/common/get_parameter/CHANGELOG.rst
@@ -5,6 +5,14 @@ Changelog for package get_parameter
 16.4.2 (2025-05-23)
 -------------------
 
+17.6.1 (2025-10-02)
+-------------------
+
+17.6.0 (2025-09-29)
+-------------------
+* Merge branch 'master' into fix/noise_v4
+* Contributors: Kotaro Yoshimoto
+
 17.5.3 (2025-09-17)
 -------------------
 * Merge branch 'master' into fix/macro-xml-validation

diff --git a/common/get_parameter/package.xml b/common/get_parameter/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>get_parameter</name>
-  <version>17.5.3</version>
+  <version>17.6.1</version>
   <description>Thin wrapper for rclcpp::Node::get_parameter</description>
   <maintainer email="[email protected]">Tatsuya Yamasaki</maintainer>
   <license>Apache License 2.0</license>

diff --git a/common/math/arithmetic/CHANGELOG.rst b/common/math/arithmetic/CHANGELOG.rst
@@ -24,6 +24,14 @@ Changelog for package arithmetic
 16.4.2 (2025-05-23)
 -------------------
 
+17.6.1 (2025-10-02)
+-------------------
+
+17.6.0 (2025-09-29)
+-------------------
+* Merge branch 'master' into fix/noise_v4
+* Contributors: Kotaro Yoshimoto
+
 17.5.3 (2025-09-17)
 -------------------
 * Merge branch 'master' into fix/macro-xml-validation

diff --git a/common/math/arithmetic/include/arithmetic/floating_point/comparison.hpp b/common/math/arithmetic/include/arithmetic/floating_point/comparison.hpp
@@ -23,35 +23,27 @@ namespace math
 namespace arithmetic
 {
 template <typename T>
-auto isApproximatelyEqualTo(T a, T b)
+constexpr auto isApproximatelyEqualTo(T a, T b) -> bool
 {
   return std::abs(a - b) <=
          (std::numeric_limits<T>::epsilon() * std::max(std::abs(a), std::abs(b)));
 }
 
 template <typename T>
-auto isEssentiallyEqualTo(T a, T b)
+constexpr auto isEssentiallyEqualTo(T a, T b) -> bool
 {
   return std::abs(a - b) <=
          (std::numeric_limits<T>::epsilon() * std::min(std::abs(a), std::abs(b)));
 }
 
-template <typename T, typename... Ts>
-auto isDefinitelyLessThan(T a, T b, Ts... xs)
+template <typename T>
+constexpr auto isDefinitelyLessThan(T a, T b) -> bool
 {
-  auto compare = [](T a, T b) {
-    return (b - a) > (std::numeric_limits<T>::epsilon() * std::max(std::abs(a), std::abs(b)));
-  };
-
-  if constexpr (0 < sizeof...(Ts)) {
-    return compare(a, b) and compare(b, xs...);
-  } else {
-    return compare(a, b);
-  }
+  return (b - a) > (std::numeric_limits<T>::epsilon() * std::max(std::abs(a), std::abs(b)));
 }
 
 template <typename T>
-auto isDefinitelyGreaterThan(T a, T b)
+constexpr auto isDefinitelyGreaterThan(T a, T b) -> bool
 {
   return (a - b) > (std::numeric_limits<T>::epsilon() * std::max(std::abs(a), std::abs(b)));
 }

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>17.5.3</version>
+  <version>17.6.1</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
@@ -24,6 +24,14 @@ Changelog for package geometry
 16.4.2 (2025-05-23)
 -------------------
 
+17.6.1 (2025-10-02)
+-------------------
+
+17.6.0 (2025-09-29)
+-------------------
+* Merge branch 'master' into fix/noise_v4
+* Contributors: Kotaro Yoshimoto
+
 17.5.3 (2025-09-17)
 -------------------
 * Merge branch 'master' into fix/macro-xml-validation

diff --git a/common/math/geometry/include/geometry/vector3/is_finite.hpp b/common/math/geometry/include/geometry/vector3/is_finite.hpp
@@ -0,0 +1,32 @@
+// 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__VECTOR3__IS_FINITE_HPP_
+#define GEOMETRY__VECTOR3__IS_FINITE_HPP_
+
+#include <geometry/vector3/is_like_vector3.hpp>
+
+namespace math
+{
+namespace geometry
+{
+template <typename T, std::enable_if_t<IsLikeVector3<T>::value, std::nullptr_t> = nullptr>
+auto isFinite(const T & v) -> bool
+{
+  return std::isfinite(v.x) and std::isfinite(v.y) and std::isfinite(v.z);
+}
+}  // namespace geometry
+}  // namespace math
+
+#endif  // GEOMETRY__VECTOR3__IS_FINITE_HPP_
diff --git a/common/math/geometry/include/geometry/vector3/rotate.hpp b/common/math/geometry/include/geometry/vector3/rotate.hpp
@@ -0,0 +1,44 @@
+// 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__VECTOR3__ROTATE_HPP_
+#define GEOMETRY__VECTOR3__ROTATE_HPP_
+
+#include <Eigen/Geometry>
+#include <geometry/quaternion/is_like_quaternion.hpp>
+#include <geometry/vector3/is_like_vector3.hpp>
+
+namespace math
+{
+namespace geometry
+{
+template <
+  typename V, typename Q,
+  std::enable_if_t<std::conjunction_v<IsLikeVector3<V>, IsLikeQuaternion<Q>>, std::nullptr_t> =
+    nullptr>
+auto rotate(const V & vector, const Q & quaternion)
+{
+  const Eigen::Quaterniond eigen_quaternion(quaternion.w, quaternion.x, quaternion.y, quaternion.z);
+  const Eigen::Vector3d eigen_vector(vector.x, vector.y, vector.z);
+  const Eigen::Vector3d eigen_rotated_vector = eigen_quaternion * eigen_vector;
+  V rotated_vector;
+  rotated_vector.x = eigen_rotated_vector.x();
+  rotated_vector.y = eigen_rotated_vector.y();
+  rotated_vector.z = eigen_rotated_vector.z();
+  return rotated_vector;
+}
+}  // namespace geometry
+}  // namespace math
+
+#endif  // GEOMETRY__VECTOR3__ROTATE_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>17.5.3</version>
+  <version>17.6.1</version>
   <description>geometry math library for scenario_simulator_v2 application</description>
   <maintainer email="[email protected]">Masaya Kataoka</maintainer>
   <license>Apache License 2.0</license>

diff --git a/common/scenario_simulator_exception/CHANGELOG.rst b/common/scenario_simulator_exception/CHANGELOG.rst
@@ -24,6 +24,14 @@ Changelog for package scenario_simulator_exception
 16.4.2 (2025-05-23)
 -------------------
 
+17.6.1 (2025-10-02)
+-------------------
+
+17.6.0 (2025-09-29)
+-------------------
+* Merge branch 'master' into fix/noise_v4
+* Contributors: Kotaro Yoshimoto
+
 17.5.3 (2025-09-17)
 -------------------
 * Merge branch 'master' into fix/macro-xml-validation

diff --git a/common/scenario_simulator_exception/package.xml b/common/scenario_simulator_exception/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>scenario_simulator_exception</name>
-  <version>17.5.3</version>
+  <version>17.6.1</version>
   <description>Exception types for scenario simulator</description>
   <maintainer email="[email protected]">Tatsuya Yamasaki</maintainer>
   <license>Apache License 2.0</license>

diff --git a/common/simple_junit/CHANGELOG.rst b/common/simple_junit/CHANGELOG.rst
@@ -24,6 +24,14 @@ Changelog for package junit_exporter
 16.4.2 (2025-05-23)
 -------------------
 
+17.6.1 (2025-10-02)
+-------------------
+
+17.6.0 (2025-09-29)
+-------------------
+* Merge branch 'master' into fix/noise_v4
+* Contributors: Kotaro Yoshimoto
+
 17.5.3 (2025-09-17)
 -------------------
 * Merge branch 'master' into fix/macro-xml-validation

diff --git a/common/simple_junit/package.xml b/common/simple_junit/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>simple_junit</name>
-  <version>17.5.3</version>
+  <version>17.6.1</version>
   <description>Lightweight JUnit library for ROS 2</description>
   <maintainer email="[email protected]">Masaya Kataoka</maintainer>
   <maintainer email="[email protected]">Tatsuya Yamasaki</maintainer>

diff --git a/common/status_monitor/CHANGELOG.rst b/common/status_monitor/CHANGELOG.rst
@@ -24,6 +24,14 @@ Changelog for package status_monitor
 16.4.2 (2025-05-23)
 -------------------
 
+17.6.1 (2025-10-02)
+-------------------
+
+17.6.0 (2025-09-29)
+-------------------
+* Merge branch 'master' into fix/noise_v4
+* Contributors: Kotaro Yoshimoto
+
 17.5.3 (2025-09-17)
 -------------------
 * Merge branch 'master' into fix/macro-xml-validation

diff --git a/common/status_monitor/package.xml b/common/status_monitor/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>status_monitor</name>
-  <version>17.5.3</version>
+  <version>17.6.1</version>
   <description>none</description>
   <maintainer email="[email protected]">Tatsuya Yamasaki</maintainer>
   <license>Apache License 2.0</license>