From 355d053e91be213c2f348205924ffe6bcda15a42 Mon Sep 17 00:00:00 2001 From: t4-adc Date: Wed, 5 Mar 2025 14:51:23 +0900 Subject: [PATCH 1/4] remove the noises when ego stop. --- external/concealer/src/publisher.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/external/concealer/src/publisher.cpp b/external/concealer/src/publisher.cpp index b8efe713a26..28a7182a1ae 100644 --- a/external/concealer/src/publisher.cpp +++ b/external/concealer/src/publisher.cpp @@ -54,6 +54,12 @@ auto NormalDistribution::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) { + return odometry; + } Eigen::Vector3d local_position = Eigen::Vector3d(0.0, 0.0, 0.0); From 815dd9ea5cffa947f6b02d5d7bad4900781b5bd9 Mon Sep 17 00:00:00 2001 From: t4-adc Date: Wed, 5 Mar 2025 19:42:23 +0900 Subject: [PATCH 2/4] configurize the speed_threshold --- external/concealer/include/concealer/publisher.hpp | 2 ++ external/concealer/src/publisher.cpp | 5 +++-- test_runner/scenario_test_runner/config/parameters.yaml | 1 + 3 files changed, 6 insertions(+), 2 deletions(-) diff --git a/external/concealer/include/concealer/publisher.hpp b/external/concealer/include/concealer/publisher.hpp index 0d96162fde1..38684749394 100644 --- a/external/concealer/include/concealer/publisher.hpp +++ b/external/concealer/include/concealer/publisher.hpp @@ -49,6 +49,8 @@ struct NormalDistribution std::mt19937_64 engine; + double speed_threshold; + struct Error { std::normal_distribution additive, multiplicative; diff --git a/external/concealer/src/publisher.cpp b/external/concealer/src/publisher.cpp index 28a7182a1ae..dd320ff162e 100644 --- a/external/concealer/src/publisher.cpp +++ b/external/concealer/src/publisher.cpp @@ -33,6 +33,7 @@ NormalDistribution::NormalDistribution( } }()), engine(seed ? seed : device()), + speed_threshold(getParameter(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::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 index f05412ce17d..ca202025277 100644 --- 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: From 97d1e785529d2079dc6877620be900474c9a6fb5 Mon Sep 17 00:00:00 2001 From: t4-adc Date: Thu, 6 Mar 2025 14:01:58 +0900 Subject: [PATCH 3/4] remove unused space. --- external/concealer/src/publisher.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/external/concealer/src/publisher.cpp b/external/concealer/src/publisher.cpp index dd320ff162e..47861511897 100644 --- a/external/concealer/src/publisher.cpp +++ b/external/concealer/src/publisher.cpp @@ -56,7 +56,7 @@ auto NormalDistribution::operator()(nav_msgs::msg::Odom odometry.pose.pose.orientation.w, odometry.pose.pose.orientation.x, odometry.pose.pose.orientation.y, odometry.pose.pose.orientation.z); - const double speed = std::hypot( + const double speed = std::hypot( odometry.twist.twist.linear.x, odometry.twist.twist.linear.y, odometry.twist.twist.linear.z); if (speed < speed_threshold) { return odometry; From d87429adb4cb50d6e8a6b19e3caaa7fe0f943a80 Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Fri, 7 Mar 2025 11:56:45 +0900 Subject: [PATCH 4/4] Replace early-return with if-else Signed-off-by: yamacir-kit --- external/concealer/src/publisher.cpp | 69 ++++++++++++++-------------- 1 file changed, 35 insertions(+), 34 deletions(-) diff --git a/external/concealer/src/publisher.cpp b/external/concealer/src/publisher.cpp index 47861511897..56dede725c9 100644 --- a/external/concealer/src/publisher.cpp +++ b/external/concealer/src/publisher.cpp @@ -52,51 +52,52 @@ NormalDistribution::NormalDistribution( auto NormalDistribution::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); - - const double speed = std::hypot( - odometry.twist.twist.linear.x, odometry.twist.twist.linear.y, odometry.twist.twist.linear.z); - if (speed < speed_threshold) { + 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