Skip to content

Commit

Permalink
feat(data_collecting_tool): visualize collected acc and vel grid (#83)
Browse files Browse the repository at this point in the history 
* feat(data_collecting_tool): visualize collected acc and vel grid

Signed-off-by: kosuke55 <[email protected]>

* fix spell

Signed-off-by: kosuke55 <[email protected]>

* fix import

Signed-off-by: kosuke55 <[email protected]>

* add matplotlib depend

Signed-off-by: kosuke55 <[email protected]>

* add dependency

* update division

Signed-off-by: kosuke55 <[email protected]>

* fix blinking grid

Signed-off-by: kosuke55 <[email protected]>

---------

Signed-off-by: kosuke55 <[email protected]>
  • Loading branch information
@kosuke55
kosuke55 authored Jul 26, 2024
1 parent 97ed6ee commit a1aba06
Show file tree
Hide file tree
Showing 2 changed files with 88 additions and 21 deletions.
3 changes: 3 additions & 0 deletions control_data_collecting_tool/package.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,6 +23,9 @@
<depend>rviz_common</depend>
<depend>rviz_default_plugins</depend>

<exec_depend>python3-matplotlib</exec_depend>
<exec_depend>python3-seaborn</exec_depend>

<test_depend>ament_index_python</test_depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>autoware_lint_common</test_depend>
Expand Down
106 changes: 85 additions & 21 deletions control_data_collecting_tool/scripts/data_collecting_trajectory_publisher.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,8 +16,10 @@

from autoware_planning_msgs.msg import Trajectory
from autoware_planning_msgs.msg import TrajectoryPoint
from geometry_msgs.msg import AccelWithCovarianceStamped
from geometry_msgs.msg import Point
from geometry_msgs.msg import PolygonStamped
import matplotlib.pyplot as plt
from nav_msgs.msg import Odometry
import numpy as np
from numpy import arctan
Expand All @@ -28,14 +30,13 @@
import rclpy
from rclpy.node import Node
from scipy.spatial.transform import Rotation as R
import seaborn as sns
from visualization_msgs.msg import Marker
from visualization_msgs.msg import MarkerArray

debug_matplotlib_plot_flag = False
Differential_Smoothing_Flag = True
USE_CURVATURE_RADIUS_FLAG = False
if debug_matplotlib_plot_flag:
import matplotlib.pyplot as plt


def smooth_bounding(upper: np.ndarray, threshold: np.ndarray, x: np.ndarray):
Expand Down Expand Up @@ -133,6 +134,20 @@ class DataCollectingTrajectoryPublisher(Node):
def __init__(self):
super().__init__("data_collecting_trajectory_publisher")

# velocity and acceleration grid
self.num_bins = 10
self.v_min, self.v_max = 0, 13.8
self.a_min, self.a_max = -1, 1
self.collected_data_counts = np.zeros((self.num_bins, self.num_bins))
self.v_bins = np.linspace(self.v_min, self.v_max, self.num_bins + 1)
self.a_bins = np.linspace(self.a_min, self.a_max, self.num_bins + 1)
self.v_bin_centers = (self.v_bins[:-1] + self.v_bins[1:]) / 2
self.a_bin_centers = (self.a_bins[:-1] + self.a_bins[1:]) / 2
self.fig, self.axs = plt.subplots(2, 1, figsize=(10, 12))
self.grid_update_time_interval = 1
self.last_grid_update_time = None
plt.ion()

self.declare_parameter(
"max_lateral_accel",
0.5, # 0.3G
Expand Down Expand Up @@ -217,7 +232,13 @@ def __init__(self):
self.onOdometry,
1,
)
self.sub_odometry_

self.sub_acceleration_ = self.create_subscription(
AccelWithCovarianceStamped,
"/localization/acceleration",
self.onAcceleration,
1,
)

self.sub_data_collecting_area_ = self.create_subscription(
PolygonStamped,
Expand All @@ -233,6 +254,7 @@ def __init__(self):
self.timer = self.create_timer(self.timer_period_callback, self.timer_callback)

self._present_kinematic_state = None
self._present_acceleration = None

self.trajectory_position_data = None
self.trajectory_yaw_data = None
Expand All @@ -252,6 +274,9 @@ def __init__(self):
def onOdometry(self, msg):
self._present_kinematic_state = msg

def onAcceleration(self, msg):
self._present_acceleration = msg

def onDataCollectingArea(self, msg):
self._data_collecting_area_polygon = msg
self.updateNominalTargetTrajectory()
Expand Down Expand Up @@ -424,8 +449,52 @@ def updateNominalTargetTrajectory(self):

self.get_logger().info("update nominal target trajectory")

def count_observations(self, v, a):
v_bin = np.digitize(v, self.v_bins) - 1
a_bin = np.digitize(a, self.a_bins) - 1
if 0 <= v_bin < self.num_bins and 0 <= a_bin < self.num_bins:
self.collected_data_counts[v_bin, a_bin] += 1

def plot_data_collection_grid(self):
# do not update if enough time has not passed
if self.last_grid_update_time is not None:
time_elapsed = self.get_clock().now() - self.last_grid_update_time
if self.grid_update_time_interval > time_elapsed.nanoseconds / 1e9:
return
# 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,
annot=True,
cmap="coolwarm",
xticklabels=np.round(self.a_bin_centers, 2),
yticklabels=np.round(self.v_bin_centers, 2),
ax=self.axs[1],
linewidths=0.1,
linecolor="gray",
)
self.axs[1].set_xlabel("Acceleration bins")
self.axs[1].set_ylabel("Velocity bins")
self.axs[1].set_title("Counts of Observations in Each Grid Cell")
self.last_grid_update_time = self.get_clock().now()
self.fig.canvas.draw()

def timer_callback(self):
if self._present_kinematic_state is not None and self.trajectory_position_data is not None:
if (
self._present_kinematic_state is not None
and self._present_acceleration is not None
and self.trajectory_position_data is not None
):
# update velocity and acceleration bin if ego vehicle is moving
if self._present_kinematic_state.twist.twist.linear.x > 1e-3:
self.count_observations(
self._present_kinematic_state.twist.twist.linear.x,
self._present_acceleration.accel.accel.linear.x,
)

# [0] update nominal target trajectory if changing related ros2 params
target_longitudinal_velocity = (
self.get_parameter("target_longitudinal_velocity")
Expand Down Expand Up @@ -708,9 +777,8 @@ def timer_callback(self):

self.data_collecting_trajectory_marker_array_pub_.publish(marker_array)

# [99] debug plot
if debug_matplotlib_plot_flag:
plt.cla()
self.axs[0].cla()
step_size_array = np.sqrt(
((trajectory_position_data[1:] - trajectory_position_data[:-1]) ** 2).sum(
axis=1
Expand All @@ -726,46 +794,42 @@ def timer_callback(self):
timestamp[i] = timestamp[i - 1] + time_width_array[i - 1]
timestamp -= timestamp[nearestIndex]

plt.plot(0, present_linear_velocity[0], "o", label="current vel")
self.axs[0].plot(0, present_linear_velocity[0], "o", label="current vel")

plt.plot(
# distance[nearestIndex : nearestIndex + pub_traj_len],
self.axs[0].plot(
timestamp[nearestIndex : nearestIndex + pub_traj_len],
trajectory_longitudinal_velocity_data_without_limit[
nearestIndex : nearestIndex + pub_traj_len
],
"--",
label="target vel before applying limit",
)
plt.plot(
# distance[nearestIndex : nearestIndex + pub_traj_len],
self.axs[0].plot(
timestamp[nearestIndex : nearestIndex + pub_traj_len],
lateral_acc_limit[nearestIndex : nearestIndex + pub_traj_len],
"--",
label="lateral acc limit (always)",
)
plt.plot(
# distance[nearestIndex : nearestIndex + pub_traj_len],
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)",
)
plt.plot(
# distance[nearestIndex : nearestIndex + pub_traj_len],
self.axs[0].plot(
timestamp[nearestIndex : nearestIndex + pub_traj_len],
trajectory_longitudinal_velocity_data[
nearestIndex : nearestIndex + pub_traj_len
],
label="actual target vel",
)
plt.xlim([-0.5, 10.5])
plt.ylim([-0.5, 12.5])
self.axs[0].set_xlim([-0.5, 10.5])
self.axs[0].set_ylim([-0.5, 12.5])

# plt.xlabel("future driving distance [m]")
plt.xlabel("future timestamp [s]")
plt.ylabel("longitudinal_velocity [m/s]")
plt.legend(fontsize=8)
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()
plt.pause(0.01)


Expand Down

0 comments on commit a1aba06

Please sign in to comment.