diff --git a/control/autoware_autonomous_emergency_braking/src/node.cpp b/control/autoware_autonomous_emergency_braking/src/node.cpp index 0e1ce4bf3c77d..846a7654d7313 100644 --- a/control/autoware_autonomous_emergency_braking/src/node.cpp +++ b/control/autoware_autonomous_emergency_braking/src/node.cpp @@ -401,6 +401,7 @@ void AEB::onCheckCollision(DiagnosticStatusWrapper & stat) { MarkerArray debug_markers; MarkerArray virtual_wall_marker; + auto metrics = MetricArray(); checkCollision(debug_markers); if (!collision_data_keeper_.checkCollisionExpired()) { @@ -418,14 +419,12 @@ void AEB::onCheckCollision(DiagnosticStatusWrapper & stat) } addVirtualStopWallMarker(virtual_wall_marker); - // publish metrics - auto metric = Metric(); - metric.name = "decision"; - metric.value = "brake"; - auto metrics = MetricArray(); - metrics.stamp = get_clock()->now(); - metrics.metric_array.push_back(metric); - metrics_pub_->publish(metrics); + { + auto metric = Metric(); + metric.name = "decision"; + metric.value = "brake"; + metrics.metric_array.push_back(metric); + } } else { const std::string error_msg = "[AEB]: No Collision"; @@ -436,6 +435,9 @@ void AEB::onCheckCollision(DiagnosticStatusWrapper & stat) // publish debug markers debug_marker_publisher_->publish(debug_markers); virtual_wall_publisher_->publish(virtual_wall_marker); + // publish metrics + metrics.stamp = get_clock()->now(); + metrics_pub_->publish(metrics); } bool AEB::checkCollision(MarkerArray & debug_markers) diff --git a/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp b/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp index 6d893b8f79ded..28034eed5f89c 100644 --- a/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp +++ b/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp @@ -221,11 +221,10 @@ void ControlEvaluatorNode::onTimer() AddKinematicStateMetricMsg(*odom, *acc); } - if (!metrics_msg_.metric_array.empty()) { - metrics_msg_.stamp = now(); - metrics_pub_->publish(metrics_msg_); - metrics_msg_ = MetricArrayMsg{}; - } + // Publish metrics + metrics_msg_.stamp = now(); + metrics_pub_->publish(metrics_msg_); + metrics_msg_ = MetricArrayMsg{}; } } // namespace control_diagnostics diff --git a/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp b/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp index a1e77e751d7e1..a1d7c75891f4a 100644 --- a/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp +++ b/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp @@ -243,11 +243,10 @@ void PlanningEvaluatorNode::onTimer() onModifiedGoal(modified_goal_msg, ego_state_ptr); } - if (!metrics_msg_.metric_array.empty()) { - metrics_msg_.stamp = now(); - metrics_pub_->publish(metrics_msg_); - metrics_msg_ = MetricArrayMsg{}; - } + // Publish metrics + metrics_msg_.stamp = now(); + metrics_pub_->publish(metrics_msg_); + metrics_msg_ = MetricArrayMsg{}; } void PlanningEvaluatorNode::onTrajectory( diff --git a/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp b/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp index 389fa30b4a1e8..140396a09e042 100644 --- a/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp @@ -912,9 +912,7 @@ void PlannerInterface::publishMetrics(const rclcpp::Time & current_time) addMetrics(debug_data_ptr_->stop_metrics); addMetrics(debug_data_ptr_->slow_down_metrics); addMetrics(debug_data_ptr_->cruise_metrics); - if (!metrics_msg.metric_array.empty()) { - metrics_pub_->publish(metrics_msg); - } + metrics_pub_->publish(metrics_msg); clearMetrics(); } diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp index 65adcb2683ecc..0e8a36977fec8 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp @@ -307,9 +307,7 @@ void MotionVelocityPlannerNode::on_trajectory( processing_time_publisher_->publish(processing_time_msg); std::shared_ptr metrics = planner_manager_.get_metrics(get_clock()->now()); - if (!metrics->metric_array.empty()) { - metrics_pub_->publish(*metrics); - } + metrics_pub_->publish(*metrics); planner_manager_.clear_metrics(); }