configurize the speed_threshold

tier4 · Mar 5, 2025 · 815dd9e · 815dd9e
1 parent 355d053
commit 815dd9e
Show file tree

Hide file tree

Showing 3 changed files with 6 additions and 2 deletions.
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/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"),
@@ -54,10 +55,10 @@ auto NormalDistribution<nav_msgs::msg::Odometry>::operator()(nav_msgs::msg::Odom
   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);
-  // calculate the speed 
+
   const double speed =  std::hypot(
     odometry.twist.twist.linear.x, odometry.twist.twist.linear.y, odometry.twist.twist.linear.z);
-  if (speed < 0.1) {
+  if (speed < speed_threshold) {
     return odometry;
   }
 

diff --git a/test_runner/scenario_test_runner/config/parameters.yaml b/test_runner/scenario_test_runner/config/parameters.yaml
@@ -6,6 +6,7 @@
       version: 20240605 # architecture_type suffix (mandatory)
       seed: 0 # If 0 is specified, a random seed value will be generated for each run.
       nav_msgs::msg::Odometry:
+        speed_threshold: 0.1
         pose:
           pose:
             position: