diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index d4d70f18..2b8b3ba8 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -15,7 +15,7 @@ common/tier4_screen_capture_rviz_plugin/** kyoichi.sugahara@tier4.jp satoshi.ota common/tier4_simulated_clock_rviz_plugin/** maxime.clement@tier4.jp common/tier4_string_viewer_rviz_plugin/** satoshi.ota@tier4.jp common/tier4_target_object_type_rviz_plugin/** takamasa.horibe@tier4.jp -control/control_debug_tools/** takayuki.murooka@tier4.jp zhe.shen@tier4.jp +control/control_debug_tools/** takayuki.murooka@tier4.jp zhe.shen@tier4.jp kosuke.takeuchi@tier4.jp temkei.kem@tier4.jp control/stop_accel_evaluator/** takayuki.murooka@tier4.jp control/vehicle_cmd_analyzer/** taichi.hirano@tier4.jp control_data_collecting_tool/** asei.inoue@proxima-ai-tech.com kosuke.takeuchi@tier4.jp masayuki.aino@proxima-ai-tech.com takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp toru.hishinuma@proxima-ai-tech.com diff --git a/control/control_debug_tools/package.xml b/control/control_debug_tools/package.xml index 2d938f13..0e23681d 100644 --- a/control/control_debug_tools/package.xml +++ b/control/control_debug_tools/package.xml @@ -6,6 +6,8 @@ The control_debug_tools package Zhe Shen Takayuki Murooka + Kosuke Takeuchi + Temkei Kem Apache License 2.0 Zhe Shen diff --git a/control/control_debug_tools/scripts/lateral_deviation_monitor.py b/control/control_debug_tools/scripts/lateral_deviation_monitor.py index 00ea2cfe..0675a5f6 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: