diff --git a/control/control_debug_tools/scripts/lateral_deviation_monitor.py b/control/control_debug_tools/scripts/lateral_deviation_monitor.py new file mode 100755 index 00000000..4c7b87c0 --- /dev/null +++ b/control/control_debug_tools/scripts/lateral_deviation_monitor.py @@ -0,0 +1,174 @@ +#!/usr/bin/env python3 + +# Copyright 2024 TIER IV, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import time + +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 + + +class SteeringAndLateralDeviationMonitor(Node): + def __init__(self, plot=False): + super().__init__("steering_and_lateral_deviation_monitor") + + self.create_subscription( + Control, "/control/command/control_cmd", self.control_cmd_callback, 10 + ) + + self.create_subscription( + SteeringReport, "/vehicle/status/steering_status", self.steering_status_callback, 10 + ) + + self.create_subscription( + DiagnosticArray, "/control/control_evaluator/metrics", self.metrics_callback, 10 + ) + + self.control_steering_angle = None + self.vehicle_steering_angle = None + self.lateral_deviation = None + self.steering_diff = None + + # For plotting + self.plot = plot + if self.plot: + self.start_time = time.time() + self.time_data = [] + self.control_steering_data = [] + self.vehicle_steering_data = [] + self.lateral_deviation_data = [] + self.steering_diff_data = [] + self.fig, self.ax = plt.subplots(3, 1, figsize=(10, 10)) + + # Set the window title + self.fig.canvas.manager.set_window_title("Steering and Lateral Deviation Monitor") + + def control_cmd_callback(self, msg): + self.control_steering_angle = msg.lateral.steering_tire_angle + self.update_steering_diff() + self.display_values() + + def steering_status_callback(self, msg): + self.vehicle_steering_angle = msg.steering_tire_angle + self.update_steering_diff() + self.display_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.display_values() + break + + def update_steering_diff(self): + if self.control_steering_angle is not None and self.vehicle_steering_angle is not None: + self.steering_diff = self.control_steering_angle - self.vehicle_steering_angle + + def display_values(self): + if ( + self.control_steering_angle is not None + and self.vehicle_steering_angle is not None + and self.lateral_deviation is not None + and self.steering_diff is not None + ): + self.get_logger().info( + f"\n{'-'*40}\n" + f"{colored('Control Steering Angle: ', 'red')}{self.control_steering_angle:.2f} [rad]\n" + f"{colored('Vehicle Steering Angle: ', 'green')}{self.vehicle_steering_angle:.2f} [rad]\n" + f"{colored('Steering Diff: ', 'magenta')}{self.steering_diff:.2f} [rad]\n" + f"{colored('Lateral Deviation: ', 'blue')}{self.lateral_deviation:.4f} [m]\n" + f"{'-'*40}\n" + ) + if self.plot: + self.update_plot() + + def update_plot(self): + current_time = time.time() - self.start_time + self.time_data.append(current_time) + self.control_steering_data.append(self.control_steering_angle) + self.vehicle_steering_data.append(self.vehicle_steering_angle) + self.lateral_deviation_data.append(self.lateral_deviation) + self.steering_diff_data.append(self.steering_diff) + + # Keep only the last specified seconds of data + if current_time > 60: + while self.time_data and self.time_data[0] < current_time - 30: + self.time_data.pop(0) + self.control_steering_data.pop(0) + self.vehicle_steering_data.pop(0) + self.lateral_deviation_data.pop(0) + self.steering_diff_data.pop(0) + + # Clear previous data + self.ax[0].clear() + self.ax[1].clear() + self.ax[2].clear() + + # Plot steering angles on the same graph + self.ax[0].plot( + self.time_data, self.control_steering_data, label="Control Steering Angle", color="red" + ) + self.ax[0].plot( + self.time_data, + self.vehicle_steering_data, + label="Vehicle Steering Angle", + color="green", + ) + self.ax[0].set_ylabel("Steering Angle [rad]") + self.ax[0].set_xlabel("Time [s]") + self.ax[0].legend() + + # Plot steering difference on a separate graph + self.ax[1].plot( + self.time_data, self.steering_diff_data, label="Steering Diff", color="magenta" + ) + self.ax[1].set_ylabel("Steering Diff [rad]") + self.ax[1].set_xlabel("Time [s]") + self.ax[1].legend() + + # Plot lateral deviation on another graph + self.ax[2].plot( + self.time_data, self.lateral_deviation_data, label="Lateral Deviation", color="blue" + ) + self.ax[2].set_ylabel("Lateral Deviation [m]") + self.ax[2].set_xlabel("Time [s]") + self.ax[2].legend() + + plt.pause(0.001) + + +def main(args=None): + rclpy.init(args=args) + monitor = SteeringAndLateralDeviationMonitor(plot=True) + if monitor.plot: + plt.ion() # Interactive mode on for real-time plot updates + rclpy.spin(monitor) + + monitor.destroy_node() + rclpy.shutdown() + if monitor.plot: + plt.ioff() + plt.show() + + +if __name__ == "__main__": + main()