Skip to content

Commit

Permalink
feat: update trajectory visualizer (tier4#737)
Browse files Browse the repository at this point in the history 
* feat: update callback group of trajectory visualizer

Signed-off-by: tomoya.kimura <tomoya.kimura@tier4.jp>

* add velocity limit to trajectory visualizer

Signed-off-by: tomoya.kimura <tomoya.kimura@tier4.jp>

* ci(pre-commit): autofix

* rename behavior _path to behavior_velocity_path

Signed-off-by: tomoya.kimura <tomoya.kimura@tier4.jp>

* rename lane_change_path to behavior_path_path

Signed-off-by: tomoya.kimura <tomoya.kimura@tier4.jp>

* misc

Signed-off-by: tomoya.kimura <tomoya.kimura@tier4.jp>

* ci(pre-commit): autofix

Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
  • Loading branch information
@tkimura4 @pre-commit-ci
tkimura4 and pre-commit-ci[bot] authored Apr 21, 2022
1 parent 02ebf0b commit bad563d
Showing 1 changed file with 51 additions and 29 deletions.
80 changes: 51 additions & 29 deletions planning/motion_velocity_smoother/scripts/trajectory_visualizer.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,6 +31,7 @@
import tf2_ros
from tf2_ros.buffer import Buffer
from tf2_ros.transform_listener import TransformListener
from tier4_planning_msgs.msg import VelocityLimit

parser = argparse.ArgumentParser()
parser.add_argument("-l", "--length", help="max arclength in plot")
Expand Down Expand Up @@ -83,8 +84,8 @@ def __init__(self):
self.update_traj_raw = False
self.update_traj_resample = False
self.update_traj_final = False
self.update_lanechange_path = False
self.update_behavior_path = False
self.update_behavior_path_planner_path = False
self.update_behavior_velocity_planner_path = False
self.update_traj_ob_avoid = False
self.update_traj_ob_stop = False

Expand All @@ -95,15 +96,16 @@ def __init__(self):
self.self_pose_received = False
self.localization_vx = 0.0
self.vehicle_vx = 0.0
self.velocity_limit = None

self.trajectory_external_velocity_limited = Trajectory()
self.trajectory_lateral_acc_filtered = Trajectory()
self.trajectory_raw = Trajectory()
self.trajectory_time_resampled = Trajectory()
self.trajectory_final = Trajectory()

self.lane_change_path = PathWithLaneId()
self.behavior_path = Path()
self.behavior_path_planner_path = PathWithLaneId()
self.behavior_velocity_planner_path = Path()
self.obstacle_avoid_traj = Trajectory()
self.obstacle_stop_traj = Trajectory()

Expand All @@ -115,6 +117,10 @@ def __init__(self):
VelocityReport, "/vehicle/status/velocity_status", self.CallbackVehicleTwist, 1
)

self.sub_external_velocity_limit = self.create_subscription(
VelocityLimit, "/planning/scenario_planning/max_velocity", self.CallbackVelocityLimit, 1
)

# BUFFER_SIZE = 65536*100
optimizer_debug = "/planning/scenario_planning/motion_velocity_smoother/debug/"
self.sub1 = message_filters.Subscriber(
Expand Down Expand Up @@ -144,12 +150,12 @@ def __init__(self):
self.sub9 = message_filters.Subscriber(self, Trajectory, lane_driving + "/trajectory")

self.ts1 = message_filters.ApproximateTimeSynchronizer(
[self.sub1, self.sub2, self.sub3, self.sub4, self.sub5], 30, 0.5
[self.sub1, self.sub2, self.sub3, self.sub4], 30, 0.5
)
self.ts1.registerCallback(self.CallbackMotionVelOptTraj)

self.ts2 = message_filters.ApproximateTimeSynchronizer(
[self.sub6, self.sub7, self.sub8, self.sub9], 30, 1, 0
[self.sub5, self.sub6, self.sub7, self.sub8, self.sub9], 30, 1, 0
)
self.ts2.registerCallback(self.CallBackLaneDrivingTraj)

Expand Down Expand Up @@ -178,13 +184,15 @@ def CallbackLocalizationTwist(self, cmd):
def CallbackVehicleTwist(self, cmd):
self.vehicle_vx = cmd.longitudinal_velocity

def CallbackMotionVelOptTraj(self, cmd1, cmd2, cmd3, cmd4, cmd5):
def CallbackVelocityLimit(self, cmd):
self.velocity_limit = cmd.max_velocity

def CallbackMotionVelOptTraj(self, cmd1, cmd2, cmd3, cmd4):
print("CallbackMotionVelOptTraj called")
self.CallBackTrajExVelLim(cmd1)
self.CallBackTrajLatAccFiltered(cmd2)
self.CallBackTrajRaw(cmd3)
self.CallBackTrajTimeResampled(cmd4)
self.CallBackTrajFinal(cmd5)

def CallBackTrajExVelLim(self, cmd):
self.trajectory_external_velocity_limited = cmd
Expand All @@ -206,20 +214,21 @@ def CallBackTrajFinal(self, cmd):
self.trajectory_final = cmd
self.update_traj_final = True

def CallBackLaneDrivingTraj(self, cmd6, cmd7, cmd8, cmd9):
def CallBackLaneDrivingTraj(self, cmd5, cmd6, cmd7, cmd8, cmd9):
print("CallBackLaneDrivingTraj called")
self.CallBackLaneChangePath(cmd6)
self.CallBackBehaviorPath(cmd7)
self.CallBackTrajFinal(cmd5)
self.CallBackLBehaviorPathPlannerPath(cmd6)
self.CallBackBehaviorVelocityPlannerPath(cmd7)
self.CallbackObstacleAvoidTraj(cmd8)
self.CallbackObstacleStopTraj(cmd9)

def CallBackLaneChangePath(self, cmd):
self.lane_change_path = cmd
self.update_lanechange_path = True
def CallBackLBehaviorPathPlannerPath(self, cmd):
self.behavior_path_planner_path = cmd
self.update_behavior_path_planner_path = True

def CallBackBehaviorPath(self, cmd):
self.behavior_path = cmd
self.update_behavior_path = True
def CallBackBehaviorVelocityPlannerPath(self, cmd):
self.behavior_velocity_planner_path = cmd
self.update_behavior_velocity_planner_path = True

def CallbackObstacleAvoidTraj(self, cmd):
self.obstacle_avoid_traj = cmd
Expand All @@ -231,8 +240,10 @@ def CallbackObstacleStopTraj(self, cmd):

def setPlotTrajectoryVelocity(self):
self.ax1 = plt.subplot(1, 1, 1) # row, col, index(<raw*col)
(self.im1,) = self.ax1.plot([], [], label="0: lane_change_path", marker="")
(self.im2,) = self.ax1.plot([], [], label="1: behavior_path", marker="", ls="--")
(self.im1,) = self.ax1.plot([], [], label="0: behavior_path_planner_path", marker="")
(self.im2,) = self.ax1.plot(
[], [], label="1: behavior_velocity_planner_path", marker="", ls="--"
)
(self.im3,) = self.ax1.plot([], [], label="2: obstacle_avoid_traj", marker="", ls="-.")
(self.im4,) = self.ax1.plot([], [], label="3: obstacle_stop_traj", marker="", ls="--")
(self.im5,) = self.ax1.plot([], [], label="4-1: opt input", marker="", ls="--")
Expand All @@ -248,6 +259,9 @@ def setPlotTrajectoryVelocity(self):
(self.im11,) = self.ax1.plot(
[], [], label="vehicle twist vx", color="k", marker="+", ls=":", markersize=10
)

(self.im12,) = self.ax1.plot([], [], label="external velocity limit", color="k", marker="")

self.ax1.set_title("trajectory's velocity")
self.ax1.legend()
self.ax1.set_xlim([0, PLOT_MAX_ARCLENGTH])
Expand All @@ -265,6 +279,7 @@ def setPlotTrajectoryVelocity(self):
self.im9,
self.im10,
self.im11,
self.im12,
)

def plotTrajectoryVelocity(self, data):
Expand All @@ -283,12 +298,13 @@ def plotTrajectoryVelocity(self, data):
self.im9,
self.im10,
self.im11,
self.im12,
)
print("plot start")

# copy
lane_change_path = self.lane_change_path
behavior_path = self.behavior_path
behavior_path_planner_path = self.behavior_path_planner_path
behavior_velocity_planner_path = self.behavior_velocity_planner_path
obstacle_avoid_traj = self.obstacle_avoid_traj
obstacle_stop_traj = self.obstacle_stop_traj
trajectory_raw = self.trajectory_raw
Expand All @@ -297,20 +313,20 @@ def plotTrajectoryVelocity(self, data):
trajectory_time_resampled = self.trajectory_time_resampled
trajectory_final = self.trajectory_final

if self.update_lanechange_path:
x = self.CalcArcLengthPathWLid(lane_change_path)
y = self.ToVelListPathWLid(lane_change_path)
if self.update_behavior_path_planner_path:
x = self.CalcArcLengthPathWLid(behavior_path_planner_path)
y = self.ToVelListPathWLid(behavior_path_planner_path)
self.im1.set_data(x, y)
self.update_lanechange_path = False
self.update_behavior_path_planner_path = False
if len(y) != 0:
self.max_vel = max(10.0, np.max(y))
self.min_vel = np.min(y)

if self.update_behavior_path:
x = self.CalcArcLengthPath(behavior_path)
y = self.ToVelListPath(behavior_path)
if self.update_behavior_velocity_planner_path:
x = self.CalcArcLengthPath(behavior_velocity_planner_path)
y = self.ToVelListPath(behavior_velocity_planner_path)
self.im2.set_data(x, y)
self.update_behavior_path = False
self.update_behavior_velocity_planner_path = False

if self.update_traj_ob_avoid:
x = self.CalcArcLength(obstacle_avoid_traj)
Expand Down Expand Up @@ -360,6 +376,11 @@ def plotTrajectoryVelocity(self, data):
self.im10.set_data(x_closest, self.localization_vx)
self.im11.set_data(x_closest, self.vehicle_vx)

if self.velocity_limit is not None:
x = [0, PLOT_MAX_ARCLENGTH]
y = [self.velocity_limit, self.velocity_limit]
self.im12.set_data(x, y)

# change y-range
self.ax1.set_ylim([self.min_vel - 1.0, self.max_vel + 1.0])

Expand All @@ -375,6 +396,7 @@ def plotTrajectoryVelocity(self, data):
self.im9,
self.im10,
self.im11,
self.im12,
)

def CalcArcLength(self, traj):
Expand Down

0 comments on commit bad563d

Please sign in to comment.