From 90863d89f47757d7d136ee766456cca45421fa33 Mon Sep 17 00:00:00 2001 From: yk1109 <150131701+YoshihiroKogure@users.noreply.github.com> Date: Tue, 1 Oct 2024 18:48:33 +0900 Subject: [PATCH] feat(control_data_collecting_tool): improve `/data_collecting_trajectory_publisher` (#126) * add data_collecting_plotter Signed-off-by: Yoshihiro Kogure * add an arrow in the tangential direction Signed-off-by: Yoshihiro Kogure --------- Signed-off-by: Yoshihiro Kogure --- control_data_collecting_tool/CMakeLists.txt | 1 + control_data_collecting_tool/README.md | 16 + .../config/param.yaml | 17 +- .../control_data_collecting_tool.launch.py | 6 + .../scripts/data_collecting_plotter.py | 310 ++++++++++++++++++ .../data_collecting_trajectory_publisher.py | 196 ++++++----- 6 files changed, 441 insertions(+), 105 deletions(-) create mode 100755 control_data_collecting_tool/scripts/data_collecting_plotter.py diff --git a/control_data_collecting_tool/CMakeLists.txt b/control_data_collecting_tool/CMakeLists.txt index 331366c5..6d817fcd 100644 --- a/control_data_collecting_tool/CMakeLists.txt +++ b/control_data_collecting_tool/CMakeLists.txt @@ -7,6 +7,7 @@ autoware_package() install(PROGRAMS scripts/data_collecting_pure_pursuit_trajectory_follower.py scripts/data_collecting_trajectory_publisher.py + scripts/data_collecting_plotter.py DESTINATION lib/${PROJECT_NAME} ) diff --git a/control_data_collecting_tool/README.md b/control_data_collecting_tool/README.md index f8e7d14e..4f853eec 100644 --- a/control_data_collecting_tool/README.md +++ b/control_data_collecting_tool/README.md @@ -144,3 +144,19 @@ ROS 2 params in `/data_collecting_pure_pursuit_trajectory_follower` node: | `steer_noise_amp` | `double` | Steer command additional sine noise amplitude [rad] | 0.01 | | `steer_noise_max_period` | `double` | Steer command additional sine noise maximum period [s] | 5.0 | | `steer_noise_min_period` | `double` | Steer command additional sine noise minimum period [s] | 20.0 | + +ROS 2 params in `/data_collecting_plotter` node: + +| Name | Type | Description | Default value | +| :---------------------- | :------- | :-------------------------------------------------------------------------------------------------- | :------------- | +| `COURSE_NAME` | `string` | Course name [`eight_course`, `u_shaped_return`, `straight_line_positive`, `straight_line_negative`] | `eight_course` | +| `NUM_BINS_V` | `int` | Number of bins of velocity in heatmap | 10 | +| `NUM_BINS_STEER` | `int` | Number of bins of steer in heatmap | 10 | +| `NUM_BINS_ACCELERATION` | `int` | Number of bins of acceleration in heatmap | 10 | +| `V_MIN` | `double` | Minimum velocity in heatmap [m/s] | 0.0 | +| `V_MAX` | `double` | Maximum velocity in heatmap [m/s] | 11.5 | +| `STEER_MIN` | `double` | Minimum steer in heatmap [rad] | -1.0 | +| `STEER_MAX` | `double` | Maximum steer in heatmap [rad] | 1.0 | +| `A_MIN` | `double` | Minimum acceleration in heatmap [m/ss] | -1.0 | +| `A_MAX` | `double` | Maximum acceleration in heatmap [m/ss] | 1.0 | +| `wheel_base` | `double` | Wheel base [m] | 2.79 | diff --git a/control_data_collecting_tool/config/param.yaml b/control_data_collecting_tool/config/param.yaml index cc6177f6..d96553ac 100644 --- a/control_data_collecting_tool/config/param.yaml +++ b/control_data_collecting_tool/config/param.yaml @@ -1,7 +1,16 @@ -common_parameters: +data_collecting_plotter: ros__parameters: + NUM_BINS_V: 10 + NUM_BINS_STEER: 10 + NUM_BINS_A: 10 + V_MIN: 0.0 + V_MAX: 11.5 + STEER_MIN: -1.0 + STEER_MAX: 1.0 + A_MIN: -1.0 + A_MAX: 1.0 + wheel_base: 2.79 - acc_kp: 1.0 data_collecting_trajectory_publisher: ros__parameters: @@ -20,6 +29,8 @@ data_collecting_trajectory_publisher: A_MIN: -1.0 A_MAX: 1.0 + wheel_base: 2.79 + acc_kp: 1.0 max_lateral_accel: 0.5 lateral_error_threshold: 2.0 yaw_error_threshold: 0.50 @@ -33,6 +44,8 @@ data_collecting_trajectory_publisher: data_collecting_pure_pursuit_trajectory_follower: ros__parameters: pure_pursuit_type: linearized + wheel_base: 2.79 + acc_kp: 1.0 lookahead_time: 2.0 min_lookahead: 2.0 linearized_pure_pursuit_steer_kp_param: 2.0 diff --git a/control_data_collecting_tool/launch/control_data_collecting_tool.launch.py b/control_data_collecting_tool/launch/control_data_collecting_tool.launch.py index f1b15476..ba3ea314 100644 --- a/control_data_collecting_tool/launch/control_data_collecting_tool.launch.py +++ b/control_data_collecting_tool/launch/control_data_collecting_tool.launch.py @@ -39,6 +39,12 @@ def generate_launch_description(): name="data_collecting_trajectory_publisher", parameters=[param_file_path], ), + Node( + package="control_data_collecting_tool", + executable="data_collecting_plotter.py", + name="data_collecting_plotter", + parameters=[param_file_path], + ), ] ) diff --git a/control_data_collecting_tool/scripts/data_collecting_plotter.py b/control_data_collecting_tool/scripts/data_collecting_plotter.py new file mode 100755 index 00000000..30f1f2cc --- /dev/null +++ b/control_data_collecting_tool/scripts/data_collecting_plotter.py @@ -0,0 +1,310 @@ +#!/usr/bin/env python3 + +# Copyright 2024 Proxima Technology Inc, TIER IV +# +# 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 threading + +from geometry_msgs.msg import AccelWithCovarianceStamped +import matplotlib.pyplot as plt +from nav_msgs.msg import Odometry +import numpy as np +from numpy import arctan2 +from rcl_interfaces.msg import ParameterDescriptor +import rclpy +from rclpy.callback_groups import ReentrantCallbackGroup +from rclpy.node import Node +import seaborn as sns + + +class DataCollectingPlotter(Node): + def __init__(self): + super().__init__("data_collecting_plotter") + + self.declare_parameter( + "NUM_BINS_V", + 10, + ParameterDescriptor(description="Number of bins of velocity in heatmap"), + ) + + self.declare_parameter( + "NUM_BINS_STEER", + 10, + ParameterDescriptor(description="Number of bins of steer in heatmap"), + ) + + self.declare_parameter( + "NUM_BINS_A", + 10, + ParameterDescriptor(description="Number of bins of acceleration in heatmap"), + ) + + self.declare_parameter( + "V_MIN", + 0.0, + ParameterDescriptor(description="Maximum velocity in heatmap [m/s]"), + ) + + self.declare_parameter( + "V_MAX", + 1.0, + ParameterDescriptor(description="Minimum steer in heatmap [m/s]"), + ) + + self.declare_parameter( + "STEER_MIN", + -1.0, + ParameterDescriptor(description="Maximum steer in heatmap [rad]"), + ) + + self.declare_parameter( + "STEER_MAX", + 1.0, + ParameterDescriptor(description="Maximum steer in heatmap [rad]"), + ) + + self.declare_parameter( + "A_MIN", + -1.0, + ParameterDescriptor(description="Minimum acceleration in heatmap [m/ss]"), + ) + + self.declare_parameter( + "A_MAX", + 1.0, + ParameterDescriptor(description="Maximum acceleration in heatmap [m/ss]"), + ) + + self.declare_parameter( + "wheel_base", + 2.79, # sample_vehicle_launch/sample_vehicle_description/config/vehicle_info.param.yaml + ParameterDescriptor(description="Wheel base [m]"), + ) + + self.sub_odometry_ = self.create_subscription( + Odometry, + "/localization/kinematic_state", + self.onOdometry, + 1, + ) + + self.sub_acceleration_ = self.create_subscription( + AccelWithCovarianceStamped, + "/localization/acceleration", + self.onAcceleration, + 1, + ) + + self._present_kinematic_state = None + self._present_acceleration = None + + # velocity and acceleration grid + # velocity and steer grid + self.num_bins_v = self.get_parameter("NUM_BINS_V").get_parameter_value().integer_value + self.num_bins_steer = ( + self.get_parameter("NUM_BINS_STEER").get_parameter_value().integer_value + ) + self.num_bins_a = self.get_parameter("NUM_BINS_A").get_parameter_value().integer_value + self.v_min, self.v_max = ( + self.get_parameter("V_MIN").get_parameter_value().double_value, + self.get_parameter("V_MAX").get_parameter_value().double_value, + ) + self.steer_min, self.steer_max = ( + self.get_parameter("STEER_MIN").get_parameter_value().double_value, + self.get_parameter("STEER_MAX").get_parameter_value().double_value, + ) + self.a_min, self.a_max = ( + self.get_parameter("A_MIN").get_parameter_value().double_value, + self.get_parameter("A_MAX").get_parameter_value().double_value, + ) + + self.collected_data_counts_of_vel_acc = np.zeros((self.num_bins_v, self.num_bins_a)) + self.collected_data_counts_of_vel_steer = np.zeros((self.num_bins_v, self.num_bins_steer)) + + self.v_bins = np.linspace(self.v_min, self.v_max, self.num_bins_v + 1) + self.steer_bins = np.linspace(self.steer_min, self.steer_max, self.num_bins_steer + 1) + self.a_bins = np.linspace(self.a_min, self.a_max, self.num_bins_a + 1) + + self.v_bin_centers = (self.v_bins[:-1] + self.v_bins[1:]) / 2 + self.steer_bin_centers = (self.steer_bins[:-1] + self.steer_bins[1:]) / 2 + self.a_bin_centers = (self.a_bins[:-1] + self.a_bins[1:]) / 2 + + self.vel_hist = np.zeros(200) + self.acc_hist = np.zeros(200) + + self.vel_hist_for_plot = np.zeros(200) + self.acc_hist_for_plot = np.zeros(200) + + # create callback groups + self.callback_group = ReentrantCallbackGroup() + self.lock = threading.Lock() + + # callback for data count + self.timer_period_callback = 0.033 # 30ms + self.timer_counter = self.create_timer( + self.timer_period_callback, + self.timer_callback_counter, + callback_group=self.callback_group, + ) + + # callback for plot + self.grid_update_time_interval = 5.0 + self.timer_plotter = self.create_timer( + self.grid_update_time_interval, + self.timer_callback_plotter, + callback_group=self.callback_group, + ) + + self.fig, self.axs = plt.subplots(3, 1, figsize=(12, 20)) + plt.ion() + + self.collected_data_counts_of_vel_acc_for_plot = np.zeros( + (self.num_bins_v, self.num_bins_a) + ) + self.collected_data_counts_of_vel_steer_for_plot = np.zeros( + (self.num_bins_v, self.num_bins_steer) + ) + + self.v_bin_centers_for_plot = (self.v_bins[:-1] + self.v_bins[1:]) / 2 + self.steer_bin_centers_for_plot = (self.steer_bins[:-1] + self.steer_bins[1:]) / 2 + self.a_bin_centers_for_plot = (self.a_bins[:-1] + self.a_bins[1:]) / 2 + + def onOdometry(self, msg): + self._present_kinematic_state = msg + + def onAcceleration(self, msg): + self._present_acceleration = msg + + def count_observations(self, v, a, steer): + v_bin = np.digitize(v, self.v_bins) - 1 + steer_bin = np.digitize(steer, self.steer_bins) - 1 + a_bin = np.digitize(a, self.a_bins) - 1 + + if 0 <= v_bin < self.num_bins_v and 0 <= a_bin < self.num_bins_a: + self.collected_data_counts_of_vel_acc[v_bin, a_bin] += 1 + + if 0 <= v_bin < self.num_bins_v and 0 <= steer_bin < self.num_bins_steer: + self.collected_data_counts_of_vel_steer[v_bin, steer_bin] += 1 + + def timer_callback_plotter(self): + with self.lock: + self.collected_data_counts_of_vel_acc_for_plot = ( + self.collected_data_counts_of_vel_acc.copy() + ) + self.collected_data_counts_of_vel_steer_for_plot = ( + self.collected_data_counts_of_vel_steer.copy() + ) + + self.acc_hist_for_plot = self.acc_hist.copy() + self.vel_hist_for_plot = self.vel_hist.copy() + + self.v_bin_centers_for_plot = self.v_bin_centers + self.steer_bin_centers_for_plot = self.steer_bin_centers + self.a_bin_centers_for_plot = self.a_bin_centers + + self.plot_data_collection_grid() + plt.pause(0.01) + + def timer_callback_counter(self): + if self._present_kinematic_state is not None and self._present_acceleration is not None: + # calculate steer + angular_z = self._present_kinematic_state.twist.twist.angular.z + wheel_base = self.get_parameter("wheel_base").get_parameter_value().double_value + current_steer = arctan2( + wheel_base * angular_z, self._present_kinematic_state.twist.twist.linear.x + ) + + current_vel = self._present_kinematic_state.twist.twist.linear.x + current_acc = self._present_acceleration.accel.accel.linear.x + + # update velocity and acceleration bin if ego vehicle is moving + if self._present_kinematic_state.twist.twist.linear.x > 1e-3: + with self.lock: + self.count_observations( + current_vel, + current_acc, + current_steer, + ) + + self.acc_hist[:-1] = 1.0 * self.acc_hist[1:] + self.acc_hist[-1] = current_acc + self.vel_hist[:-1] = 1.0 * self.vel_hist[1:] + self.vel_hist[-1] = current_vel + + def plot_data_collection_grid(self): + self.axs[0].cla() + self.axs[0].scatter(self.acc_hist_for_plot, self.vel_hist_for_plot) + self.axs[0].plot(self.acc_hist_for_plot, self.vel_hist_for_plot) + self.axs[0].set_xlim([-2.0, 2.0]) + self.axs[0].set_ylim([0.0, self.v_max + 1.0]) + self.axs[0].set_xlabel("Acceleration") + self.axs[0].set_ylabel("Velocity") + + # update collected acceleration and velocity grid + for collection in self.axs[1].collections: + if collection.colorbar is not None: + collection.colorbar.remove() + self.axs[1].cla() + + self.heatmap = sns.heatmap( + self.collected_data_counts_of_vel_acc_for_plot.T, + annot=True, + cmap="coolwarm", + xticklabels=np.round(self.v_bin_centers_for_plot, 2), + yticklabels=np.round(self.a_bin_centers_for_plot, 2), + ax=self.axs[1], + linewidths=0.1, + linecolor="gray", + ) + + self.axs[1].set_xlabel("Velocity bins") + self.axs[1].set_ylabel("Acceleration bins") + + for collection in self.axs[2].collections: + if collection.colorbar is not None: + collection.colorbar.remove() + self.axs[2].cla() + + self.heatmap = sns.heatmap( + self.collected_data_counts_of_vel_steer_for_plot.T, + annot=True, + cmap="coolwarm", + xticklabels=np.round(self.v_bin_centers_for_plot, 2), + yticklabels=np.round(self.steer_bin_centers_for_plot, 2), + ax=self.axs[2], + linewidths=0.1, + linecolor="gray", + ) + + # update collected steer and velocity grid + self.axs[2].set_xlabel("Velocity bins") + self.axs[2].set_ylabel("Steer bins") + + self.fig.canvas.draw() + + plt.pause(0.01) + + +def main(args=None): + rclpy.init(args=args) + + data_collecting_plot = DataCollectingPlotter() + rclpy.spin(data_collecting_plot) + + data_collecting_plot.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/control_data_collecting_tool/scripts/data_collecting_trajectory_publisher.py b/control_data_collecting_tool/scripts/data_collecting_trajectory_publisher.py index 00af7fe8..d8c396ac 100755 --- a/control_data_collecting_tool/scripts/data_collecting_trajectory_publisher.py +++ b/control_data_collecting_tool/scripts/data_collecting_trajectory_publisher.py @@ -14,9 +14,6 @@ # See the License for the specific language governing permissions and # limitations under the License. - -import threading - from autoware_planning_msgs.msg import Trajectory from autoware_planning_msgs.msg import TrajectoryPoint from geometry_msgs.msg import AccelWithCovarianceStamped @@ -31,7 +28,6 @@ from numpy import sin from rcl_interfaces.msg import ParameterDescriptor import rclpy -from rclpy.callback_groups import ReentrantCallbackGroup from rclpy.node import Node from scipy.spatial.transform import Rotation as R import seaborn as sns @@ -39,7 +35,6 @@ from visualization_msgs.msg import MarkerArray debug_matplotlib_plot_flag = False -data_counts_matplotlib_plot_flag = True Differential_Smoothing_Flag = True USE_CURVATURE_RADIUS_FLAG = False @@ -509,42 +504,12 @@ def __init__(self): self.vel_hist = np.zeros(200) self.acc_hist = np.zeros(200) - self.vel_hist_for_plot = np.zeros(200) - self.acc_hist_for_plot = np.zeros(200) - - self.callback_group = ReentrantCallbackGroup() - self.lock = threading.Lock() - self.timer_period_callback = 0.03 # 30ms self.traj_step = 0.1 - self.timer_traj = self.create_timer( - self.timer_period_callback, self.timer_callback_traj, callback_group=self.callback_group - ) - - if data_counts_matplotlib_plot_flag: - self.collected_data_counts_of_vel_acc_for_plot = np.zeros( - (self.num_bins_v, self.num_bins_a) - ) - self.collected_data_counts_of_vel_steer_for_plot = np.zeros( - (self.num_bins_v, self.num_bins_steer) - ) - - self.v_bin_centers_for_plot = (self.v_bins[:-1] + self.v_bins[1:]) / 2 - self.steer_bin_centers_for_plot = (self.steer_bins[:-1] + self.steer_bins[1:]) / 2 - self.a_bin_centers_for_plot = (self.a_bins[:-1] + self.a_bins[1:]) / 2 - - self.fig, self.axs = plt.subplots(3, 1, figsize=(12, 20)) - self.grid_update_time_interval = 2.0 - plt.ion() - - self.timer_plot = self.create_timer( - self.grid_update_time_interval, - self.timer_callback_plot, - callback_group=self.callback_group, - ) + self.timer_traj = self.create_timer(self.timer_period_callback, self.timer_callback_traj) if debug_matplotlib_plot_flag: - self.fig_debug, self.axs_debug = plt.subplots(figsize=(12, 6)) + self.fig, self.axs = plt.subplots(4, 1, figsize=(12, 20)) plt.ion() self._present_kinematic_state = None @@ -962,76 +927,52 @@ def count_observations(self, v, a, steer): self.collected_data_counts_of_vel_steer[v_bin, steer_bin] += 1 def plot_data_collection_grid(self): - self.axs[0].cla() - self.axs[0].scatter(self.acc_hist_for_plot, self.vel_hist_for_plot) - self.axs[0].plot(self.acc_hist_for_plot, self.vel_hist_for_plot) - self.axs[0].set_xlim([-2.0, 2.0]) - self.axs[0].set_ylim([0.0, self.v_max + 1.0]) - self.axs[0].set_xlabel("Acceleration") - self.axs[0].set_ylabel("Velocity") + self.axs[1].cla() + self.axs[1].scatter(self.acc_hist, self.vel_hist) + self.axs[1].plot(self.acc_hist, self.vel_hist) + self.axs[1].set_xlim([-2.0, 2.0]) + self.axs[1].set_ylim([0.0, self.v_max + 1.0]) + self.axs[1].set_xlabel("Acceleration") + self.axs[1].set_ylabel("Velocity") # update collected acceleration and velocity grid - for collection in self.axs[1].collections: + for collection in self.axs[2].collections: if collection.colorbar is not None: collection.colorbar.remove() - self.axs[1].cla() + self.axs[2].cla() self.heatmap = sns.heatmap( - self.collected_data_counts_of_vel_acc_for_plot.T, + self.collected_data_counts_of_vel_acc.T, annot=True, cmap="coolwarm", - xticklabels=np.round(self.v_bin_centers_for_plot, 2), - yticklabels=np.round(self.a_bin_centers_for_plot, 2), - ax=self.axs[1], + xticklabels=np.round(self.v_bin_centers, 2), + yticklabels=np.round(self.a_bin_centers, 2), + ax=self.axs[2], linewidths=0.1, linecolor="gray", ) - self.axs[1].set_xlabel("Velocity bins") - self.axs[1].set_ylabel("Acceleration bins") + self.axs[2].set_xlabel("Velocity bins") + self.axs[2].set_ylabel("Acceleration bins") - for collection in self.axs[2].collections: + for collection in self.axs[3].collections: if collection.colorbar is not None: collection.colorbar.remove() - self.axs[2].cla() + self.axs[3].cla() self.heatmap = sns.heatmap( - self.collected_data_counts_of_vel_steer_for_plot.T, + self.collected_data_counts_of_vel_steer.T, annot=True, cmap="coolwarm", - xticklabels=np.round(self.v_bin_centers_for_plot, 2), - yticklabels=np.round(self.steer_bin_centers_for_plot, 2), - ax=self.axs[2], + xticklabels=np.round(self.v_bin_centers, 2), + yticklabels=np.round(self.steer_bin_centers, 2), + ax=self.axs[3], linewidths=0.1, linecolor="gray", ) - # update collected steer and velocity grid - self.axs[2].set_xlabel("Velocity bins") - self.axs[2].set_ylabel("Steer bins") - - self.fig.canvas.draw() - - plt.pause(0.005) - - def timer_callback_plot(self): - with self.lock: - self.collected_data_counts_of_vel_acc_for_plot = ( - self.collected_data_counts_of_vel_acc.copy() - ) - self.collected_data_counts_of_vel_steer_for_plot = ( - self.collected_data_counts_of_vel_steer.copy() - ) - - self.acc_hist_for_plot = self.acc_hist.copy() - self.vel_hist_for_plot = self.vel_hist.copy() - - self.v_bin_centers_for_plot = self.v_bin_centers - self.steer_bin_centers_for_plot = self.steer_bin_centers - self.a_bin_centers_for_plot = self.a_bin_centers - - self.plot_data_collection_grid() - plt.pause(0.01) + self.axs[3].set_xlabel("Velocity bins") + self.axs[3].set_ylabel("Steer bins") def timer_callback_traj(self): if ( @@ -1048,12 +989,11 @@ def timer_callback_traj(self): # update velocity and acceleration bin if ego vehicle is moving if self._present_kinematic_state.twist.twist.linear.x > 1e-3: - with self.lock: - self.count_observations( - self._present_kinematic_state.twist.twist.linear.x, - self._present_acceleration.accel.accel.linear.x, - steer, - ) + self.count_observations( + self._present_kinematic_state.twist.twist.linear.x, + self._present_acceleration.accel.accel.linear.x, + steer, + ) # [0] update nominal target trajectory if changing related ros2 params target_longitudinal_velocity = ( @@ -1165,8 +1105,7 @@ def timer_callback_traj(self): self.one_round_progress_rate = 1.0 * nearestIndex / len(trajectory_position_data) # set target velocity - with self.lock: - target_vel = self.get_target_velocity(nearestIndex) + target_vel = self.get_target_velocity(nearestIndex) trajectory_longitudinal_velocity_data = np.array( [target_vel for _ in range(len(trajectory_longitudinal_velocity_data))] @@ -1345,10 +1284,57 @@ def timer_callback_traj(self): marker_array.markers.append(marker_traj2) + # [6-2c] add arrow + marker_arrow = Marker() + marker_arrow.type = marker_arrow.ARROW + marker_arrow.id = 2 + marker_arrow.header.frame_id = "map" + + marker_arrow.action = marker_arrow.ADD + + marker_arrow.scale.x = 0.5 + marker_arrow.scale.y = 2.5 + marker_arrow.scale.z = 0.0 + + marker_arrow.color.a = 1.0 + marker_arrow.color.r = 1.0 + marker_arrow.color.g = 0.0 + marker_arrow.color.b = 1.0 + + marker_arrow.lifetime.nanosec = 500000000 + marker_arrow.frame_locked = True + + tangent_vec = np.array( + [ + tmp_traj.points[1].pose.position.x - tmp_traj.points[0].pose.position.x, + tmp_traj.points[1].pose.position.y - tmp_traj.points[0].pose.position.y, + ] + ) + + marker_arrow.points = [] + + start_marker_point = Point() + start_marker_point.x = tmp_traj.points[0].pose.position.x + start_marker_point.y = tmp_traj.points[0].pose.position.y + start_marker_point.z = 0.0 + marker_arrow.points.append(start_marker_point) + + end_marker_point = Point() + end_marker_point.x = tmp_traj.points[0].pose.position.x + 5.0 * tangent_vec[ + 0 + ] / np.linalg.norm(tangent_vec) + end_marker_point.y = tmp_traj.points[0].pose.position.y + 5.0 * tangent_vec[ + 1 + ] / np.linalg.norm(tangent_vec) + end_marker_point.z = 0.0 + marker_arrow.points.append(end_marker_point) + + marker_array.markers.append(marker_arrow) + self.data_collecting_trajectory_marker_array_pub_.publish(marker_array) if debug_matplotlib_plot_flag: - self.axs_debug.cla() + self.axs[0].cla() step_size_array = np.sqrt( ((trajectory_position_data[1:] - trajectory_position_data[:-1]) ** 2).sum( axis=1 @@ -1366,9 +1352,9 @@ def timer_callback_traj(self): timestamp[i] = timestamp[i - 1] + time_width_array[i - 1] timestamp -= timestamp[nearestIndex] - self.axs_debug.plot(0, present_linear_velocity[0], "o", label="current vel") + self.axs[0].plot(0, present_linear_velocity[0], "o", label="current vel") - self.axs_debug.plot( + self.axs[0].plot( timestamp[nearestIndex : nearestIndex + pub_traj_len], trajectory_longitudinal_velocity_data_without_limit[ nearestIndex : nearestIndex + pub_traj_len @@ -1376,31 +1362,35 @@ def timer_callback_traj(self): "--", label="target vel before applying limit", ) - self.axs_debug.plot( + self.axs[0].plot( timestamp[nearestIndex : nearestIndex + pub_traj_len], lateral_acc_limit[nearestIndex : nearestIndex + pub_traj_len], "--", label="lateral acc limit (always)", ) - self.axs_debug.plot( + self.axs[0].plot( timestamp[nearestIndex : nearestIndex + pub_traj_len], velocity_limit_by_tracking_error * np.ones(pub_traj_len), "--", label="vel limit by tracking error (only when exceeding threshold)", ) - self.axs_debug.plot( + self.axs[0].plot( timestamp[nearestIndex : nearestIndex + pub_traj_len], trajectory_longitudinal_velocity_data[ nearestIndex : nearestIndex + pub_traj_len ], label="actual target vel", ) - self.axs_debug.set_xlim([-0.5, 10.5]) - self.axs_debug.set_ylim([-0.5, 12.5]) + self.axs[0].set_xlim([-0.5, 10.5]) + self.axs[0].set_ylim([-0.5, 12.5]) + + self.axs[0].set_xlabel("future timestamp [s]") + self.axs[0].set_ylabel("longitudinal_velocity [m/s]") + self.axs[0].legend(fontsize=8) + + self.plot_data_collection_grid() - self.axs_debug.set_xlabel("future timestamp [s]") - self.axs_debug.set_ylabel("longitudinal_velocity [m/s]") - self.axs_debug.legend(fontsize=8) + self.fig.canvas.draw() plt.pause(0.01)