Merge branch 'master' into feat/add_new_vehicle_model

tier4 · Mar 7, 2025 · e06692d · e06692d
2 parents ae9fb99 + 2d3c826
commit e06692d
Show file tree

Hide file tree

Showing 61 changed files with 223 additions and 58 deletions.
diff --git a/common/math/arithmetic/CHANGELOG.rst b/common/math/arithmetic/CHANGELOG.rst
@@ -21,6 +21,11 @@ Changelog for package arithmetic
 * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context
 * Contributors: Masaya Kataoka
 
+12.1.2 (2025-03-07)
+-------------------
+* Merge branch 'master' into xtk/loc-noise-exp
+* Contributors: Tatsuya Yamasaki
+
 12.1.1 (2025-03-07)
 -------------------
 

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>12.1.1</version>
+  <version>12.1.2</version>
   <description>arithmetic library for scenario_simulator_v2</description>
   <maintainer email="tatsuya.yamasaki@tier4.jp">Tatsuya Yamasaki</maintainer>
   <license>Apache License 2.0</license>

diff --git a/common/math/geometry/CHANGELOG.rst b/common/math/geometry/CHANGELOG.rst
@@ -21,6 +21,11 @@ Changelog for package geometry
 * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context
 * Contributors: Masaya Kataoka
 
+12.1.2 (2025-03-07)
+-------------------
+* Merge branch 'master' into xtk/loc-noise-exp
+* Contributors: Tatsuya Yamasaki
+
 12.1.1 (2025-03-07)
 -------------------
 

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>12.1.1</version>
+  <version>12.1.2</version>
   <description>geometry math library for scenario_simulator_v2 application</description>
   <maintainer email="masaya.kataoka@tier4.jp">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
@@ -21,6 +21,11 @@ Changelog for package scenario_simulator_exception
 * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context
 * Contributors: Masaya Kataoka
 
+12.1.2 (2025-03-07)
+-------------------
+* Merge branch 'master' into xtk/loc-noise-exp
+* Contributors: Tatsuya Yamasaki
+
 12.1.1 (2025-03-07)
 -------------------
 

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>12.1.1</version>
+  <version>12.1.2</version>
   <description>Exception types for scenario simulator</description>
   <maintainer email="tatsuya.yamasaki@tier4.jp">Tatsuya Yamasaki</maintainer>
   <license>Apache License 2.0</license>

diff --git a/common/simple_junit/CHANGELOG.rst b/common/simple_junit/CHANGELOG.rst
@@ -21,6 +21,11 @@ Changelog for package junit_exporter
 * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context
 * Contributors: Masaya Kataoka
 
+12.1.2 (2025-03-07)
+-------------------
+* Merge branch 'master' into xtk/loc-noise-exp
+* Contributors: Tatsuya Yamasaki
+
 12.1.1 (2025-03-07)
 -------------------
 

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>12.1.1</version>
+  <version>12.1.2</version>
   <description>Lightweight JUnit library for ROS 2</description>
   <maintainer email="masaya.kataoka@tier4.jp">Masaya Kataoka</maintainer>
   <maintainer email="tatsuya.yamasaki@tier4.jp">Tatsuya Yamasaki</maintainer>

diff --git a/common/status_monitor/CHANGELOG.rst b/common/status_monitor/CHANGELOG.rst
@@ -21,6 +21,11 @@ Changelog for package status_monitor
 * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context
 * Contributors: Masaya Kataoka
 
+12.1.2 (2025-03-07)
+-------------------
+* Merge branch 'master' into xtk/loc-noise-exp
+* Contributors: Tatsuya Yamasaki
+
 12.1.1 (2025-03-07)
 -------------------
 

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>12.1.1</version>
+  <version>12.1.2</version>
   <description>none</description>
   <maintainer email="tatsuya.yamasaki@tier4.jp">Tatsuya Yamasaki</maintainer>
   <license>Apache License 2.0</license>

diff --git a/external/concealer/CHANGELOG.rst b/external/concealer/CHANGELOG.rst
@@ -21,6 +21,17 @@ Changelog for package concealer
 * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context
 * Contributors: Masaya Kataoka
 
+12.1.2 (2025-03-07)
+-------------------
+* Merge pull request `#1542 <https://github.com/tier4/scenario_simulator_v2/issues/1542>`_ from tier4/xtk/loc-noise-exp
+  feat: disable the localization noises when ego speed is slow
+* Merge branch 'master' into xtk/loc-noise-exp
+* Replace early-return with if-else
+* remove unused space.
+* configurize the speed_threshold
+* remove the noises when ego stop.
+* Contributors: Tatsuya Yamasaki, t4-adc, yamacir-kit
+
 12.1.1 (2025-03-07)
 -------------------
 

diff --git a/external/concealer/include/concealer/publisher.hpp b/external/concealer/include/concealer/publisher.hpp
@@ -49,6 +49,8 @@ struct NormalDistribution<nav_msgs::msg::Odometry>
 
   std::mt19937_64 engine;
 
+  double speed_threshold;
+
   struct Error
   {
     std::normal_distribution<double> additive, multiplicative;

diff --git a/external/concealer/package.xml b/external/concealer/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>concealer</name>
-  <version>12.1.1</version>
+  <version>12.1.2</version>
   <description>Provides a class 'Autoware' to conceal miscellaneous things to simplify Autoware management of the simulator.</description>
   <maintainer email="tatsuya.yamasaki@tier4.jp">Tatsuya Yamasaki</maintainer>
   <license>Apache License 2.0</license>

diff --git a/external/concealer/src/publisher.cpp b/external/concealer/src/publisher.cpp
@@ -33,6 +33,7 @@ NormalDistribution<nav_msgs::msg::Odometry>::NormalDistribution(
     }
   }()),
   engine(seed ? seed : device()),
+  speed_threshold(getParameter<double>(node, topic + ".nav_msgs::msg::Odometry.speed_threshold")),
   position_local_x_error(node, topic + ".nav_msgs::msg::Odometry.pose.pose.position.local_x.error"),
   position_local_y_error(node, topic + ".nav_msgs::msg::Odometry.pose.pose.position.local_y.error"),
   position_local_z_error(node, topic + ".nav_msgs::msg::Odometry.pose.pose.position.local_z.error"),
@@ -51,45 +52,52 @@ NormalDistribution<nav_msgs::msg::Odometry>::NormalDistribution(
 auto NormalDistribution<nav_msgs::msg::Odometry>::operator()(nav_msgs::msg::Odometry odometry)
   -> nav_msgs::msg::Odometry
 {
-  const Eigen::Quaterniond orientation = Eigen::Quaterniond(
-    odometry.pose.pose.orientation.w, odometry.pose.pose.orientation.x,
-    odometry.pose.pose.orientation.y, odometry.pose.pose.orientation.z);
+  if (const double speed = std::hypot(
+        odometry.twist.twist.linear.x, odometry.twist.twist.linear.y,
+        odometry.twist.twist.linear.z);
+      speed < speed_threshold) {
+    return odometry;
+  } else {
+    const Eigen::Quaterniond orientation = Eigen::Quaterniond(
+      odometry.pose.pose.orientation.w, odometry.pose.pose.orientation.x,
+      odometry.pose.pose.orientation.y, odometry.pose.pose.orientation.z);
 
-  Eigen::Vector3d local_position = Eigen::Vector3d(0.0, 0.0, 0.0);
+    Eigen::Vector3d local_position = Eigen::Vector3d(0.0, 0.0, 0.0);
 
-  local_position.x() = position_local_x_error.apply(engine, local_position.x());
-  local_position.y() = position_local_y_error.apply(engine, local_position.y());
-  local_position.z() = position_local_z_error.apply(engine, local_position.z());
+    local_position.x() = position_local_x_error.apply(engine, local_position.x());
+    local_position.y() = position_local_y_error.apply(engine, local_position.y());
+    local_position.z() = position_local_z_error.apply(engine, local_position.z());
 
-  const Eigen::Vector3d world_position = orientation.toRotationMatrix() * local_position;
+    const Eigen::Vector3d world_position = orientation.toRotationMatrix() * local_position;
 
-  odometry.pose.pose.position.x += world_position.x();
-  odometry.pose.pose.position.y += world_position.y();
-  odometry.pose.pose.position.z += world_position.z();
+    odometry.pose.pose.position.x += world_position.x();
+    odometry.pose.pose.position.y += world_position.y();
+    odometry.pose.pose.position.z += world_position.z();
 
-  Eigen::Vector3d euler = orientation.matrix().eulerAngles(0, 1, 2);
+    Eigen::Vector3d euler = orientation.matrix().eulerAngles(0, 1, 2);
 
-  euler.x() = orientation_r_error.apply(engine, euler.x());
-  euler.y() = orientation_p_error.apply(engine, euler.y());
-  euler.z() = orientation_y_error.apply(engine, euler.z());
+    euler.x() = orientation_r_error.apply(engine, euler.x());
+    euler.y() = orientation_p_error.apply(engine, euler.y());
+    euler.z() = orientation_y_error.apply(engine, euler.z());
 
-  const Eigen::Quaterniond q = Eigen::AngleAxisd(euler.x(), Eigen::Vector3d::UnitX()) *
-                               Eigen::AngleAxisd(euler.y(), Eigen::Vector3d::UnitY()) *
-                               Eigen::AngleAxisd(euler.z(), Eigen::Vector3d::UnitZ());
+    const Eigen::Quaterniond q = Eigen::AngleAxisd(euler.x(), Eigen::Vector3d::UnitX()) *
+                                 Eigen::AngleAxisd(euler.y(), Eigen::Vector3d::UnitY()) *
+                                 Eigen::AngleAxisd(euler.z(), Eigen::Vector3d::UnitZ());
 
-  odometry.pose.pose.orientation.x = q.x();
-  odometry.pose.pose.orientation.y = q.y();
-  odometry.pose.pose.orientation.z = q.z();
-  odometry.pose.pose.orientation.w = q.w();
+    odometry.pose.pose.orientation.x = q.x();
+    odometry.pose.pose.orientation.y = q.y();
+    odometry.pose.pose.orientation.z = q.z();
+    odometry.pose.pose.orientation.w = q.w();
 
-  odometry.twist.twist.linear.x = linear_x_error.apply(engine, odometry.twist.twist.linear.x);
-  odometry.twist.twist.linear.y = linear_y_error.apply(engine, odometry.twist.twist.linear.y);
-  odometry.twist.twist.linear.z = linear_z_error.apply(engine, odometry.twist.twist.linear.z);
+    odometry.twist.twist.linear.x = linear_x_error.apply(engine, odometry.twist.twist.linear.x);
+    odometry.twist.twist.linear.y = linear_y_error.apply(engine, odometry.twist.twist.linear.y);
+    odometry.twist.twist.linear.z = linear_z_error.apply(engine, odometry.twist.twist.linear.z);
 
-  odometry.twist.twist.angular.x = angular_x_error.apply(engine, odometry.twist.twist.angular.x);
-  odometry.twist.twist.angular.y = angular_y_error.apply(engine, odometry.twist.twist.angular.y);
-  odometry.twist.twist.angular.z = angular_z_error.apply(engine, odometry.twist.twist.angular.z);
+    odometry.twist.twist.angular.x = angular_x_error.apply(engine, odometry.twist.twist.angular.x);
+    odometry.twist.twist.angular.y = angular_y_error.apply(engine, odometry.twist.twist.angular.y);
+    odometry.twist.twist.angular.z = angular_z_error.apply(engine, odometry.twist.twist.angular.z);
 
-  return odometry;
+    return odometry;
+  }
 }
 }  // namespace concealer
diff --git a/external/embree_vendor/CHANGELOG.rst b/external/embree_vendor/CHANGELOG.rst
@@ -24,6 +24,11 @@ Changelog for package embree_vendor
 * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context
 * Contributors: Masaya Kataoka
 
+12.1.2 (2025-03-07)
+-------------------
+* Merge branch 'master' into xtk/loc-noise-exp
+* Contributors: Tatsuya Yamasaki
+
 12.1.1 (2025-03-07)
 -------------------
 

diff --git a/external/embree_vendor/package.xml b/external/embree_vendor/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>embree_vendor</name>
-  <version>12.1.1</version>
+  <version>12.1.2</version>
   <description>vendor packages for intel raytracing kernel library</description>
   <maintainer email="ms.kataoka@gmail.com">masaya</maintainer>
   <license>Apache 2.0</license>

diff --git a/map/kashiwanoha_map/CHANGELOG.rst b/map/kashiwanoha_map/CHANGELOG.rst
@@ -21,6 +21,11 @@ Changelog for package kashiwanoha_map
 * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context
 * Contributors: Masaya Kataoka
 
+12.1.2 (2025-03-07)
+-------------------
+* Merge branch 'master' into xtk/loc-noise-exp
+* Contributors: Tatsuya Yamasaki
+
 12.1.1 (2025-03-07)
 -------------------
 

diff --git a/map/kashiwanoha_map/package.xml b/map/kashiwanoha_map/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>kashiwanoha_map</name>
-  <version>12.1.1</version>
+  <version>12.1.2</version>
   <description>map package for kashiwanoha</description>
   <maintainer email="masaya.kataoka@tier4.jp">Masaya Kataoka</maintainer>
   <license>Apache License 2.0</license>

diff --git a/map/simple_cross_map/CHANGELOG.rst b/map/simple_cross_map/CHANGELOG.rst
@@ -9,6 +9,11 @@ Changelog for package simple_cross_map
 * Merge branch 'master' into feature/publish_empty_context
 * Contributors: Masaya Kataoka
 
+12.1.2 (2025-03-07)
+-------------------
+* Merge branch 'master' into xtk/loc-noise-exp
+* Contributors: Tatsuya Yamasaki
+
 12.1.1 (2025-03-07)
 -------------------
 

diff --git a/map/simple_cross_map/package.xml b/map/simple_cross_map/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_cross_map</name>
-  <version>12.1.1</version>
+  <version>12.1.2</version>
   <description>map package for simple cross</description>
   <maintainer email="masaya.kataoka@tier4.jp">Masaya Kataoka</maintainer>
   <license>Apache License 2.0</license>

diff --git a/mock/cpp_mock_scenarios/CHANGELOG.rst b/mock/cpp_mock_scenarios/CHANGELOG.rst
@@ -21,6 +21,11 @@ Changelog for package cpp_mock_scenarios
 * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context
 * Contributors: Masaya Kataoka
 
+12.1.2 (2025-03-07)
+-------------------
+* Merge branch 'master' into xtk/loc-noise-exp
+* Contributors: Tatsuya Yamasaki
+
 12.1.1 (2025-03-07)
 -------------------
 

diff --git a/mock/cpp_mock_scenarios/package.xml b/mock/cpp_mock_scenarios/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>cpp_mock_scenarios</name>
-  <version>12.1.1</version>
+  <version>12.1.2</version>
   <description>C++ mock scenarios</description>
   <maintainer email="masaya.kataoka@tier4.jp">masaya</maintainer>
   <license>Apache License 2.0</license>

diff --git a/openscenario/openscenario_experimental_catalog/CHANGELOG.rst b/openscenario/openscenario_experimental_catalog/CHANGELOG.rst
@@ -21,6 +21,11 @@ Changelog for package openscenario_experimental_catalog
 * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context
 * Contributors: Masaya Kataoka
 
+12.1.2 (2025-03-07)
+-------------------
+* Merge branch 'master' into xtk/loc-noise-exp
+* Contributors: Tatsuya Yamasaki
+
 12.1.1 (2025-03-07)
 -------------------
 

diff --git a/openscenario/openscenario_experimental_catalog/package.xml b/openscenario/openscenario_experimental_catalog/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>openscenario_experimental_catalog</name>
-  <version>12.1.1</version>
+  <version>12.1.2</version>
   <description>TIER IV experimental catalogs for OpenSCENARIO</description>
   <maintainer email="tatsuya.yamasaki@tier4.jp">Tatsuya Yamasaki</maintainer>
   <license>Apache License 2.0</license>

diff --git a/openscenario/openscenario_interpreter/CHANGELOG.rst b/openscenario/openscenario_interpreter/CHANGELOG.rst
@@ -32,6 +32,11 @@ Changelog for package openscenario_interpreter
 * add publish_empty_context parameter
 * Contributors: Masaya Kataoka
 
+12.1.2 (2025-03-07)
+-------------------
+* Merge branch 'master' into xtk/loc-noise-exp
+* Contributors: Tatsuya Yamasaki
+
 12.1.1 (2025-03-07)
 -------------------
 

diff --git a/openscenario/openscenario_interpreter/package.xml b/openscenario/openscenario_interpreter/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>openscenario_interpreter</name>
-  <version>12.1.1</version>
+  <version>12.1.2</version>
   <description>OpenSCENARIO 1.2.0 interpreter package for Autoware</description>
   <maintainer email="tatsuya.yamasaki@tier4.jp">Tatsuya Yamasaki</maintainer>
   <license>Apache License 2.0</license>

diff --git a/openscenario/openscenario_interpreter_example/CHANGELOG.rst b/openscenario/openscenario_interpreter_example/CHANGELOG.rst
@@ -21,6 +21,11 @@ Changelog for package openscenario_interpreter_example
 * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context
 * Contributors: Masaya Kataoka
 
+12.1.2 (2025-03-07)
+-------------------
+* Merge branch 'master' into xtk/loc-noise-exp
+* Contributors: Tatsuya Yamasaki
+
 12.1.1 (2025-03-07)
 -------------------
 

diff --git a/openscenario/openscenario_interpreter_example/package.xml b/openscenario/openscenario_interpreter_example/package.xml
@@ -3,7 +3,7 @@
 
 <package format="3">
   <name>openscenario_interpreter_example</name>
-  <version>12.1.1</version>
+  <version>12.1.2</version>
   <description>Examples for some TIER IV OpenSCENARIO Interpreter's features</description>
   <maintainer email="tatsuya.yamasaki@tier4.jp">Tatsuya Yamasaki</maintainer>
   <license>Apache License 2.0</license>

diff --git a/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst b/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst
@@ -21,6 +21,11 @@ Changelog for package openscenario_interpreter_msgs
 * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context
 * Contributors: Masaya Kataoka
 
+12.1.2 (2025-03-07)
+-------------------
+* Merge branch 'master' into xtk/loc-noise-exp
+* Contributors: Tatsuya Yamasaki
+
 12.1.1 (2025-03-07)
 -------------------
 

diff --git a/openscenario/openscenario_interpreter_msgs/package.xml b/openscenario/openscenario_interpreter_msgs/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>openscenario_interpreter_msgs</name>
-  <version>12.1.1</version>
+  <version>12.1.2</version>
   <description>ROS message types for package openscenario_interpreter</description>
   <maintainer email="tatsuya.yamasaki@tier4.jp">Yamasaki Tatsuya</maintainer>
   <license>Apache License 2.0</license>