diff --git a/planning/motion_velocity_smoother/config/default_motion_velocity_smoother.param.yaml b/planning/motion_velocity_smoother/config/default_motion_velocity_smoother.param.yaml index b2592316f658f..0e8c90705c265 100644 --- a/planning/motion_velocity_smoother/config/default_motion_velocity_smoother.param.yaml +++ b/planning/motion_velocity_smoother/config/default_motion_velocity_smoother.param.yaml @@ -50,3 +50,6 @@ # system over_stop_velocity_warn_thr: 1.389 # used to check if the optimization exceeds the input velocity on the stop point + + # diagnostics + enable_diagnostics_when_smoothing_failure: false diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp index 9e6a2d1fc12ee..e2f97462fd314 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp +++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp @@ -33,6 +33,8 @@ #include "tier4_autoware_utils/ros/self_pose_listener.hpp" #include "tier4_autoware_utils/system/stop_watch.hpp" +#include + #include "autoware_auto_planning_msgs/msg/trajectory.hpp" #include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" #include "nav_msgs/msg/odometry.hpp" @@ -129,6 +131,8 @@ class MotionVelocitySmootherNode : public rclcpp::Node double ego_nearest_dist_threshold; // for ego's closest index calculation double ego_nearest_yaw_threshold; // for ego's closest index calculation + bool enable_diagnostics_when_smoothing_failure; + resampling::ResampleParam post_resample_param; AlgorithmType algorithm_type; // Option : JerkFiltered, Linf, L2 } node_param_{}; @@ -180,11 +184,10 @@ class MotionVelocitySmootherNode : public rclcpp::Node AlgorithmType getAlgorithmType(const std::string & algorithm_name) const; - TrajectoryPoints calcTrajectoryVelocity(const TrajectoryPoints & traj_input) const; + TrajectoryPoints calcTrajectoryVelocity(const TrajectoryPoints & traj_input); bool smoothVelocity( - const TrajectoryPoints & input, const size_t input_closest, - TrajectoryPoints & traj_smoothed) const; + const TrajectoryPoints & input, const size_t input_closest, TrajectoryPoints & traj_smoothed); std::pair calcInitialMotion( const TrajectoryPoints & input_traj, const size_t input_closest) const; @@ -242,6 +245,13 @@ class MotionVelocitySmootherNode : public rclcpp::Node bool isReverse(const TrajectoryPoints & points) const; void flipVelocity(TrajectoryPoints & points) const; void publishStopWatchTime(); + + // diagnostics + diagnostic_updater::Updater diagnostic_updater_{this}; + bool failure_to_apply_smoother_{false}; + + void setupDiagnosticUpdater(); + void checkSmootherErrors(diagnostic_updater::DiagnosticStatusWrapper & stat); }; } // namespace motion_velocity_smoother diff --git a/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp b/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp index 5dd7556d5d951..209ad2624c7f9 100644 --- a/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp +++ b/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp @@ -129,6 +129,44 @@ MotionVelocitySmootherNode::MotionVelocitySmootherNode(const rclcpp::NodeOptions pub_velocity_limit_->publish(max_vel_msg); clock_ = get_clock(); + + // Diagnostics + setupDiagnosticUpdater(); +} + +void MotionVelocitySmootherNode::setupDiagnosticUpdater() +{ + diagnostic_updater_.setHardwareID("smoother"); + if (node_param_.enable_diagnostics_when_smoothing_failure) { + diagnostic_updater_.add( + "smoother_errors", this, &MotionVelocitySmootherNode::checkSmootherErrors); + } + diagnostic_updater_.setPeriod(0.1); +} + +void MotionVelocitySmootherNode::checkSmootherErrors( + diagnostic_updater::DiagnosticStatusWrapper & stat) +{ + diagnostic_msgs::msg::DiagnosticStatus status; + status.level = diagnostic_msgs::msg::DiagnosticStatus::OK; + status.message = "OK"; + + // guard + if (current_odometry_ptr_ == nullptr) { + return; + } + + if (current_odometry_ptr_->twist.twist.linear.x < 1e-3) { + failure_to_apply_smoother_ = false; + } + + if (failure_to_apply_smoother_) { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 3000 /*ms*/, "Failure to apply smoother"); + status.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + status.message = "Failure to apply smoother"; + } + + stat.summary(status); } rcl_interfaces::msg::SetParametersResult MotionVelocitySmootherNode::onParameter( @@ -274,6 +312,8 @@ void MotionVelocitySmootherNode::initCommonParam() p.post_resample_param.sparse_min_interval_distance = declare_parameter("post_sparse_min_interval_distance", 1.0); p.algorithm_type = getAlgorithmType(declare_parameter("algorithm_type", "JerkFiltered")); + p.enable_diagnostics_when_smoothing_failure = + declare_parameter("enable_diagnostics_when_smoothing_failure", false); } void MotionVelocitySmootherNode::publishTrajectory(const TrajectoryPoints & trajectory) const @@ -484,7 +524,7 @@ void MotionVelocitySmootherNode::updateDataForExternalVelocityLimit() } TrajectoryPoints MotionVelocitySmootherNode::calcTrajectoryVelocity( - const TrajectoryPoints & traj_input) const + const TrajectoryPoints & traj_input) { TrajectoryPoints output{}; // velocity is optimized by qp solver @@ -530,8 +570,7 @@ TrajectoryPoints MotionVelocitySmootherNode::calcTrajectoryVelocity( } bool MotionVelocitySmootherNode::smoothVelocity( - const TrajectoryPoints & input, const size_t input_closest, - TrajectoryPoints & traj_smoothed) const + const TrajectoryPoints & input, const size_t input_closest, TrajectoryPoints & traj_smoothed) { // Calculate initial motion for smoothing const auto [initial_motion, type] = calcInitialMotion(input, input_closest); @@ -579,6 +618,10 @@ bool MotionVelocitySmootherNode::smoothVelocity( if (!smoother_->apply( initial_motion.vel, initial_motion.acc, clipped, traj_smoothed, debug_trajectories)) { RCLCPP_WARN(get_logger(), "Fail to solve optimization."); + if (node_param_.enable_diagnostics_when_smoothing_failure) { + failure_to_apply_smoother_ = true; + diagnostic_updater_.force_update(); + } } // Set 0 velocity after input-stop-point diff --git a/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp b/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp index d9b06bcd4da72..02e179cbcd4cf 100644 --- a/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp +++ b/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp @@ -199,7 +199,7 @@ bool AnalyticalJerkConstrainedSmoother::apply( applyMaxVelocity(0.0, bwd_start_index, filtered_trajectory.size() - 1, filtered_trajectory); output = filtered_trajectory; RCLCPP_DEBUG(logger_, "-------------------- Finish --------------------"); - return true; + return false; } applyMaxVelocity(decel_target_vel, bwd_start_index, decel_target_index, reference_trajectory); RCLCPP_DEBUG(logger_, "Apply forward jerk filter from: %ld", bwd_start_index); diff --git a/system/system_error_monitor/config/diagnostic_aggregator/planning.param.yaml b/system/system_error_monitor/config/diagnostic_aggregator/planning.param.yaml index 4f0fb2b6c15d9..99e12b865b9aa 100644 --- a/system/system_error_monitor/config/diagnostic_aggregator/planning.param.yaml +++ b/system/system_error_monitor/config/diagnostic_aggregator/planning.param.yaml @@ -55,3 +55,13 @@ path: collision_check contains: [": collision_check"] timeout: 1.0 + + smoother_errors: + type: diagnostic_aggregator/AnalyzerGroup + path: smoother_errors + analyzers: + smoother_errors: + type: diagnostic_aggregator/GenericAnalyzer + path: smoother_errors + contains: [": smoother_errors"] + timeout: 1.0