From d1782a946eb970d2e44bd2a8c4c86f2de86f05aa Mon Sep 17 00:00:00 2001 From: kyoichi-sugahara Date: Thu, 6 Jun 2024 19:17:59 +0900 Subject: [PATCH 1/4] add autoware_ prefix to planning_validator Signed-off-by: kyoichi-sugahara --- .../src/test_planning_validator_pubsub.cpp | 27 ++++++++++--------- 1 file changed, 14 insertions(+), 13 deletions(-) diff --git a/planning/planning_validator/test/src/test_planning_validator_pubsub.cpp b/planning/planning_validator/test/src/test_planning_validator_pubsub.cpp index 8cf56525edbc5..2217ee12c322f 100644 --- a/planning/planning_validator/test/src/test_planning_validator_pubsub.cpp +++ b/planning/planning_validator/test/src/test_planning_validator_pubsub.cpp @@ -182,7 +182,7 @@ TEST(PlanningValidator, DiagCheckForTooLongIntervalTrajectory) TEST(PlanningValidator, DiagCheckSize) { - const auto diag_name = "planning_validator: trajectory_validation_size"; + const auto diag_name = "autoware_planning_validator: trajectory_validation_size"; const auto odom = generateDefaultOdometry(); runWithBadTrajectory(generateTrajectory(1.0, 1.0, 0.0, 0), odom, diag_name); runWithBadTrajectory(generateTrajectory(1.0, 1.0, 0.0, 1), odom, diag_name); @@ -192,7 +192,7 @@ TEST(PlanningValidator, DiagCheckSize) TEST(PlanningValidator, DiagCheckInterval) { - const auto diag_name = "planning_validator: trajectory_validation_interval"; + const auto diag_name = "autoware_planning_validator: trajectory_validation_interval"; const auto odom = generateDefaultOdometry(); // Larger interval than threshold -> must be NG @@ -216,7 +216,7 @@ TEST(PlanningValidator, DiagCheckInterval) TEST(PlanningValidator, DiagCheckRelativeAngle) { - const auto diag_name = "planning_validator: trajectory_validation_relative_angle"; + const auto diag_name = "autoware_planning_validator: trajectory_validation_relative_angle"; // TODO(Horibe): interval must be larger than min_interval used in planning_validator.cpp constexpr auto interval = 1.1; @@ -246,7 +246,7 @@ TEST(PlanningValidator, DiagCheckRelativeAngle) TEST(PlanningValidator, DiagCheckCurvature) { - const auto diag_name = "planning_validator: trajectory_validation_curvature"; + const auto diag_name = "autoware_planning_validator: trajectory_validation_curvature"; // TODO(Horibe): interval must be larger than min_interval used in planning_validator.cpp constexpr auto interval = 1.1; @@ -279,7 +279,7 @@ TEST(PlanningValidator, DiagCheckCurvature) TEST(PlanningValidator, DiagCheckLateralAcceleration) { - const auto diag_name = "planning_validator: trajectory_validation_lateral_acceleration"; + const auto diag_name = "autoware_planning_validator: trajectory_validation_lateral_acceleration"; constexpr double speed = 10.0; // Note: lateral_acceleration is speed^2 * curvature; @@ -303,7 +303,7 @@ TEST(PlanningValidator, DiagCheckLateralAcceleration) TEST(PlanningValidator, DiagCheckLongitudinalMaxAcc) { - const auto diag_name = "planning_validator: trajectory_validation_acceleration"; + const auto diag_name = "autoware_planning_validator: trajectory_validation_acceleration"; constexpr double speed = 1.0; // Larger acceleration than threshold -> must be NG @@ -325,7 +325,7 @@ TEST(PlanningValidator, DiagCheckLongitudinalMaxAcc) TEST(PlanningValidator, DiagCheckLongitudinalMinAcc) { - const auto diag_name = "planning_validator: trajectory_validation_deceleration"; + const auto diag_name = "autoware_planning_validator: trajectory_validation_deceleration"; constexpr double speed = 20.0; const auto test = [&](const auto acceleration, const bool expect_ok) { @@ -346,7 +346,7 @@ TEST(PlanningValidator, DiagCheckLongitudinalMinAcc) TEST(PlanningValidator, DiagCheckSteering) { - const auto diag_name = "planning_validator: trajectory_validation_steering"; + const auto diag_name = "autoware_planning_validator: trajectory_validation_steering"; // TODO(Horibe): interval must be larger than min_interval used in planning_validator.cpp constexpr auto interval = 1.1; @@ -370,7 +370,7 @@ TEST(PlanningValidator, DiagCheckSteering) TEST(PlanningValidator, DiagCheckSteeringRate) { - const auto diag_name = "planning_validator: trajectory_validation_steering_rate"; + const auto diag_name = "autoware_planning_validator: trajectory_validation_steering_rate"; // TODO(Horibe): interval must be larger than min_interval used in planning_validator.cpp constexpr auto interval = 1.1; @@ -394,7 +394,7 @@ TEST(PlanningValidator, DiagCheckSteeringRate) TEST(PlanningValidator, DiagCheckVelocityDeviation) { - const auto diag_name = "planning_validator: trajectory_validation_velocity_deviation"; + const auto diag_name = "autoware_planning_validator: trajectory_validation_velocity_deviation"; const auto test = [&](const auto trajectory_speed, const auto ego_speed, const bool expect_ok) { const auto trajectory = generateTrajectory(1.0, trajectory_speed, 0.0, 10); const auto ego_odom = generateDefaultOdometry(0.0, 0.0, ego_speed); @@ -412,7 +412,7 @@ TEST(PlanningValidator, DiagCheckVelocityDeviation) TEST(PlanningValidator, DiagCheckDistanceDeviation) { - const auto diag_name = "planning_validator: trajectory_validation_distance_deviation"; + const auto diag_name = "autoware_planning_validator: trajectory_validation_distance_deviation"; const auto test = [&](const auto ego_x, const auto ego_y, const bool expect_ok) { const auto trajectory = generateTrajectory(1.0, 3.0, 0.0, 10); const auto last_p = trajectory.points.back().pose.position; @@ -439,7 +439,7 @@ TEST(PlanningValidator, DiagCheckDistanceDeviation) TEST(PlanningValidator, DiagCheckLongitudinalDistanceDeviation) { const auto diag_name = - "planning_validator: trajectory_validation_longitudinal_distance_deviation"; + "autoware_planning_validator: trajectory_validation_longitudinal_distance_deviation"; const auto trajectory = generateTrajectory(1.0, 3.0, 0.0, 10); const auto test = [&](const auto ego_x, const auto ego_y, const bool expect_ok) { const auto ego_odom = generateDefaultOdometry(ego_x, ego_y, 0.0); @@ -469,7 +469,8 @@ TEST(PlanningValidator, DiagCheckLongitudinalDistanceDeviation) TEST(PlanningValidator, DiagCheckForwardTrajectoryLength) { - const auto diag_name = "planning_validator: trajectory_validation_forward_trajectory_length"; + const auto diag_name = + "autoware_planning_validator: trajectory_validation_forward_trajectory_length"; constexpr auto trajectory_v = 10.0; constexpr size_t trajectory_size = 10; constexpr auto ego_v = 10.0; From 281e81f38626c38023e618873ce24416eea13a53 Mon Sep 17 00:00:00 2001 From: kyoichi-sugahara Date: Fri, 7 Jun 2024 13:26:22 +0900 Subject: [PATCH 2/4] revert diag name Signed-off-by: kyoichi-sugahara --- .../src/test_planning_validator_pubsub.cpp | 26 +++++++++---------- 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/planning/planning_validator/test/src/test_planning_validator_pubsub.cpp b/planning/planning_validator/test/src/test_planning_validator_pubsub.cpp index 2217ee12c322f..57d609eef5197 100644 --- a/planning/planning_validator/test/src/test_planning_validator_pubsub.cpp +++ b/planning/planning_validator/test/src/test_planning_validator_pubsub.cpp @@ -182,7 +182,7 @@ TEST(PlanningValidator, DiagCheckForTooLongIntervalTrajectory) TEST(PlanningValidator, DiagCheckSize) { - const auto diag_name = "autoware_planning_validator: trajectory_validation_size"; + const auto diag_name = "planning_validator: trajectory_validation_size"; const auto odom = generateDefaultOdometry(); runWithBadTrajectory(generateTrajectory(1.0, 1.0, 0.0, 0), odom, diag_name); runWithBadTrajectory(generateTrajectory(1.0, 1.0, 0.0, 1), odom, diag_name); @@ -192,7 +192,7 @@ TEST(PlanningValidator, DiagCheckSize) TEST(PlanningValidator, DiagCheckInterval) { - const auto diag_name = "autoware_planning_validator: trajectory_validation_interval"; + const auto diag_name = "planning_validator: trajectory_validation_interval"; const auto odom = generateDefaultOdometry(); // Larger interval than threshold -> must be NG @@ -216,7 +216,7 @@ TEST(PlanningValidator, DiagCheckInterval) TEST(PlanningValidator, DiagCheckRelativeAngle) { - const auto diag_name = "autoware_planning_validator: trajectory_validation_relative_angle"; + const auto diag_name = "planning_validator: trajectory_validation_relative_angle"; // TODO(Horibe): interval must be larger than min_interval used in planning_validator.cpp constexpr auto interval = 1.1; @@ -246,7 +246,7 @@ TEST(PlanningValidator, DiagCheckRelativeAngle) TEST(PlanningValidator, DiagCheckCurvature) { - const auto diag_name = "autoware_planning_validator: trajectory_validation_curvature"; + const auto diag_name = "planning_validator: trajectory_validation_curvature"; // TODO(Horibe): interval must be larger than min_interval used in planning_validator.cpp constexpr auto interval = 1.1; @@ -279,7 +279,7 @@ TEST(PlanningValidator, DiagCheckCurvature) TEST(PlanningValidator, DiagCheckLateralAcceleration) { - const auto diag_name = "autoware_planning_validator: trajectory_validation_lateral_acceleration"; + const auto diag_name = "planning_validator: trajectory_validation_lateral_acceleration"; constexpr double speed = 10.0; // Note: lateral_acceleration is speed^2 * curvature; @@ -303,7 +303,7 @@ TEST(PlanningValidator, DiagCheckLateralAcceleration) TEST(PlanningValidator, DiagCheckLongitudinalMaxAcc) { - const auto diag_name = "autoware_planning_validator: trajectory_validation_acceleration"; + const auto diag_name = "planning_validator: trajectory_validation_acceleration"; constexpr double speed = 1.0; // Larger acceleration than threshold -> must be NG @@ -325,7 +325,7 @@ TEST(PlanningValidator, DiagCheckLongitudinalMaxAcc) TEST(PlanningValidator, DiagCheckLongitudinalMinAcc) { - const auto diag_name = "autoware_planning_validator: trajectory_validation_deceleration"; + const auto diag_name = "planning_validator: trajectory_validation_deceleration"; constexpr double speed = 20.0; const auto test = [&](const auto acceleration, const bool expect_ok) { @@ -346,7 +346,7 @@ TEST(PlanningValidator, DiagCheckLongitudinalMinAcc) TEST(PlanningValidator, DiagCheckSteering) { - const auto diag_name = "autoware_planning_validator: trajectory_validation_steering"; + const auto diag_name = "planning_validator: trajectory_validation_steering"; // TODO(Horibe): interval must be larger than min_interval used in planning_validator.cpp constexpr auto interval = 1.1; @@ -370,7 +370,7 @@ TEST(PlanningValidator, DiagCheckSteering) TEST(PlanningValidator, DiagCheckSteeringRate) { - const auto diag_name = "autoware_planning_validator: trajectory_validation_steering_rate"; + const auto diag_name = "planning_validator: trajectory_validation_steering_rate"; // TODO(Horibe): interval must be larger than min_interval used in planning_validator.cpp constexpr auto interval = 1.1; @@ -394,7 +394,7 @@ TEST(PlanningValidator, DiagCheckSteeringRate) TEST(PlanningValidator, DiagCheckVelocityDeviation) { - const auto diag_name = "autoware_planning_validator: trajectory_validation_velocity_deviation"; + const auto diag_name = "planning_validator: trajectory_validation_velocity_deviation"; const auto test = [&](const auto trajectory_speed, const auto ego_speed, const bool expect_ok) { const auto trajectory = generateTrajectory(1.0, trajectory_speed, 0.0, 10); const auto ego_odom = generateDefaultOdometry(0.0, 0.0, ego_speed); @@ -412,7 +412,7 @@ TEST(PlanningValidator, DiagCheckVelocityDeviation) TEST(PlanningValidator, DiagCheckDistanceDeviation) { - const auto diag_name = "autoware_planning_validator: trajectory_validation_distance_deviation"; + const auto diag_name = "planning_validator: trajectory_validation_distance_deviation"; const auto test = [&](const auto ego_x, const auto ego_y, const bool expect_ok) { const auto trajectory = generateTrajectory(1.0, 3.0, 0.0, 10); const auto last_p = trajectory.points.back().pose.position; @@ -439,7 +439,7 @@ TEST(PlanningValidator, DiagCheckDistanceDeviation) TEST(PlanningValidator, DiagCheckLongitudinalDistanceDeviation) { const auto diag_name = - "autoware_planning_validator: trajectory_validation_longitudinal_distance_deviation"; + "planning_validator: trajectory_validation_longitudinal_distance_deviation"; const auto trajectory = generateTrajectory(1.0, 3.0, 0.0, 10); const auto test = [&](const auto ego_x, const auto ego_y, const bool expect_ok) { const auto ego_odom = generateDefaultOdometry(ego_x, ego_y, 0.0); @@ -470,7 +470,7 @@ TEST(PlanningValidator, DiagCheckLongitudinalDistanceDeviation) TEST(PlanningValidator, DiagCheckForwardTrajectoryLength) { const auto diag_name = - "autoware_planning_validator: trajectory_validation_forward_trajectory_length"; + "planning_validator: trajectory_validation_forward_trajectory_length"; constexpr auto trajectory_v = 10.0; constexpr size_t trajectory_size = 10; constexpr auto ego_v = 10.0; From 3862fa40f42bb81d6c6e11713257bb23ef29558f Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Fri, 7 Jun 2024 04:31:17 +0000 Subject: [PATCH 3/4] style(pre-commit): autofix --- .../test/src/test_planning_validator_pubsub.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/planning/planning_validator/test/src/test_planning_validator_pubsub.cpp b/planning/planning_validator/test/src/test_planning_validator_pubsub.cpp index 57d609eef5197..8cf56525edbc5 100644 --- a/planning/planning_validator/test/src/test_planning_validator_pubsub.cpp +++ b/planning/planning_validator/test/src/test_planning_validator_pubsub.cpp @@ -469,8 +469,7 @@ TEST(PlanningValidator, DiagCheckLongitudinalDistanceDeviation) TEST(PlanningValidator, DiagCheckForwardTrajectoryLength) { - const auto diag_name = - "planning_validator: trajectory_validation_forward_trajectory_length"; + const auto diag_name = "planning_validator: trajectory_validation_forward_trajectory_length"; constexpr auto trajectory_v = 10.0; constexpr size_t trajectory_size = 10; constexpr auto ego_v = 10.0; From 55d6564ed5d365b44e2ce2efb9be6374c2bcc870 Mon Sep 17 00:00:00 2001 From: kyoichi-sugahara Date: Fri, 7 Jun 2024 14:23:02 +0900 Subject: [PATCH 4/4] use polling subscriber Signed-off-by: kyoichi-sugahara --- .../autoware_planning_validator/planning_validator.hpp | 4 +++- planning/planning_validator/src/planning_validator.cpp | 6 +++--- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/planning/planning_validator/include/autoware_planning_validator/planning_validator.hpp b/planning/planning_validator/include/autoware_planning_validator/planning_validator.hpp index 113c70fecb62c..476160f8ba2f2 100644 --- a/planning/planning_validator/include/autoware_planning_validator/planning_validator.hpp +++ b/planning/planning_validator/include/autoware_planning_validator/planning_validator.hpp @@ -18,6 +18,7 @@ #include "autoware_planning_validator/debug_marker.hpp" #include "autoware_planning_validator/msg/planning_validator_status.hpp" #include "tier4_autoware_utils/ros/logger_level_configure.hpp" +#include "tier4_autoware_utils/ros/polling_subscriber.hpp" #include "tier4_autoware_utils/system/stop_watch.hpp" #include "vehicle_info_util/vehicle_info_util.hpp" @@ -102,7 +103,8 @@ class PlanningValidator : public rclcpp::Node void setStatus(DiagnosticStatusWrapper & stat, const bool & is_ok, const std::string & msg); - rclcpp::Subscription::SharedPtr sub_kinematics_; + tier4_autoware_utils::InterProcessPollingSubscriber sub_kinematics_{ + this, "~/input/kinematics"}; rclcpp::Subscription::SharedPtr sub_traj_; rclcpp::Publisher::SharedPtr pub_traj_; rclcpp::Publisher::SharedPtr pub_status_; diff --git a/planning/planning_validator/src/planning_validator.cpp b/planning/planning_validator/src/planning_validator.cpp index aac50e24c5d5c..7adc4c6feb387 100644 --- a/planning/planning_validator/src/planning_validator.cpp +++ b/planning/planning_validator/src/planning_validator.cpp @@ -32,9 +32,6 @@ PlanningValidator::PlanningValidator(const rclcpp::NodeOptions & options) { using std::placeholders::_1; - sub_kinematics_ = create_subscription( - "~/input/kinematics", 1, - [this](const Odometry::ConstSharedPtr msg) { current_kinematics_ = msg; }); sub_traj_ = create_subscription( "~/input/trajectory", 1, std::bind(&PlanningValidator::onTrajectory, this, _1)); @@ -195,6 +192,9 @@ void PlanningValidator::onTrajectory(const Trajectory::ConstSharedPtr msg) current_trajectory_ = msg; + // receive data + current_kinematics_ = sub_kinematics_.takeData(); + if (!isDataReady()) return; if (publish_diag_ && !diag_updater_) {