diff --git a/planning/planning_validator/include/planning_validator/planning_validator.hpp b/planning/planning_validator/include/planning_validator/planning_validator.hpp index ef570b57773bb..037c7fb0f4e95 100644 --- a/planning/planning_validator/include/planning_validator/planning_validator.hpp +++ b/planning/planning_validator/include/planning_validator/planning_validator.hpp @@ -109,7 +109,7 @@ class PlanningValidator : public rclcpp::Node int diag_error_count_threshold_ = 0; bool display_on_terminal_ = true; - Updater diag_updater_{this}; + std::shared_ptr diag_updater_ = nullptr; PlanningValidatorStatus validation_status_; ValidationParams validation_params_; // for thresholds diff --git a/planning/planning_validator/src/planning_validator.cpp b/planning/planning_validator/src/planning_validator.cpp index 875781d47b25d..58af2c08ccb22 100644 --- a/planning/planning_validator/src/planning_validator.cpp +++ b/planning/planning_validator/src/planning_validator.cpp @@ -47,10 +47,6 @@ PlanningValidator::PlanningValidator(const rclcpp::NodeOptions & options) setupParameters(); - if (publish_diag_) { - setupDiag(); - } - logger_configure_ = std::make_unique(this); } @@ -112,39 +108,40 @@ void PlanningValidator::setStatus( void PlanningValidator::setupDiag() { + diag_updater_ = std::make_shared(this); auto & d = diag_updater_; - d.setHardwareID("planning_validator"); + d->setHardwareID("planning_validator"); std::string ns = "trajectory_validation_"; - d.add(ns + "finite", [&](auto & stat) { + d->add(ns + "finite", [&](auto & stat) { setStatus(stat, validation_status_.is_valid_finite_value, "infinite value is found"); }); - d.add(ns + "interval", [&](auto & stat) { + d->add(ns + "interval", [&](auto & stat) { setStatus(stat, validation_status_.is_valid_interval, "points interval is too long"); }); - d.add(ns + "relative_angle", [&](auto & stat) { + d->add(ns + "relative_angle", [&](auto & stat) { setStatus(stat, validation_status_.is_valid_relative_angle, "relative angle is too large"); }); - d.add(ns + "curvature", [&](auto & stat) { + d->add(ns + "curvature", [&](auto & stat) { setStatus(stat, validation_status_.is_valid_curvature, "curvature is too large"); }); - d.add(ns + "lateral_acceleration", [&](auto & stat) { + d->add(ns + "lateral_acceleration", [&](auto & stat) { setStatus(stat, validation_status_.is_valid_lateral_acc, "lateral acceleration is too large"); }); - d.add(ns + "acceleration", [&](auto & stat) { + d->add(ns + "acceleration", [&](auto & stat) { setStatus(stat, validation_status_.is_valid_longitudinal_max_acc, "acceleration is too large"); }); - d.add(ns + "deceleration", [&](auto & stat) { + d->add(ns + "deceleration", [&](auto & stat) { setStatus(stat, validation_status_.is_valid_longitudinal_min_acc, "deceleration is too large"); }); - d.add(ns + "steering", [&](auto & stat) { + d->add(ns + "steering", [&](auto & stat) { setStatus(stat, validation_status_.is_valid_steering, "expected steering is too large"); }); - d.add(ns + "steering_rate", [&](auto & stat) { + d->add(ns + "steering_rate", [&](auto & stat) { setStatus( stat, validation_status_.is_valid_steering_rate, "expected steering rate is too large"); }); - d.add(ns + "velocity_deviation", [&](auto & stat) { + d->add(ns + "velocity_deviation", [&](auto & stat) { setStatus( stat, validation_status_.is_valid_velocity_deviation, "velocity deviation is too large"); }); @@ -174,11 +171,15 @@ void PlanningValidator::onTrajectory(const Trajectory::ConstSharedPtr msg) if (!isDataReady()) return; + if (publish_diag_ && !diag_updater_) { + setupDiag(); // run setup after all data is ready. + } + debug_pose_publisher_->clearMarkers(); validate(*current_trajectory_); - diag_updater_.force_update(); + diag_updater_->force_update(); publishTrajectory();