From e55939aad70f2af208095a2f791ecbfd2bbea6a9 Mon Sep 17 00:00:00 2001 From: Kasunori-Nakajima Date: Wed, 20 Nov 2024 15:40:50 +0900 Subject: [PATCH] fix(lareral_deviation_monitor): update metrics msgs Signed-off-by: Kasunori-Nakajima --- .../scripts/lateral_deviation_monitor.py | 18 +++++++----------- 1 file changed, 7 insertions(+), 11 deletions(-) diff --git a/control/control_debug_tools/scripts/lateral_deviation_monitor.py b/control/control_debug_tools/scripts/lateral_deviation_monitor.py index 00ea2cfe..be465741 100755 --- a/control/control_debug_tools/scripts/lateral_deviation_monitor.py +++ b/control/control_debug_tools/scripts/lateral_deviation_monitor.py @@ -18,11 +18,11 @@ from autoware_control_msgs.msg import Control from autoware_vehicle_msgs.msg import SteeringReport -from diagnostic_msgs.msg import DiagnosticArray import matplotlib.pyplot as plt import rclpy from rclpy.node import Node from termcolor import colored +from tier4_metric_msgs.msg import MetricArray class SteeringAndLateralDeviationMonitor(Node): @@ -38,7 +38,7 @@ def __init__(self, plot=False): ) self.create_subscription( - DiagnosticArray, "/control/control_evaluator/metrics", self.metrics_callback, 10 + MetricArray, "/control/control_evaluator/metrics", self.metrics_callback, 10 ) self.control_steering_angle = None @@ -77,14 +77,11 @@ def steering_status_callback(self, msg): self.update_steering_diff() self.update_max_values() - def metrics_callback(self, msg): - for status in msg.status: - if status.name == "lateral_deviation": - for value in status.values: - if value.key == "metric_value": - self.lateral_deviation = float(value.value) - self.update_max_values() - break + def metrics_callback(self, msgs): + for msg in msgs.metric_array: + if msg.name == "lateral_deviation": + self.lateral_deviation = float(msg.value) + self.update_max_values() def update_steering_diff(self): if self.control_steering_angle is not None and self.vehicle_steering_angle is not None: @@ -185,7 +182,6 @@ def main(args=None): if monitor.plot: plt.ion() # Interactive mode on for real-time plot updates rclpy.spin(monitor) - monitor.destroy_node() rclpy.shutdown() if monitor.plot: