Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

feat: update trajectory visualizer #737

Merged
merged 8 commits into from
Apr 21, 2022
Merged

feat: update trajectory visualizer #737

Changes from 1 commit
Commits
Show all changes
8 commits
Select commit Hold shift + click to select a range
3fa33d8
feat: update callback group of trajectory visualizer
tkimura4 Apr 20, 2022
8f071bb
add velocity limit to trajectory visualizer
tkimura4 Apr 20, 2022
2a127de
ci(pre-commit): autofix
pre-commit-ci[bot] Apr 20, 2022
e201a9c
rename behavior _path to behavior_velocity_path
tkimura4 Apr 20, 2022
5ddfe3e
rename lane_change_path to behavior_path_path
tkimura4 Apr 20, 2022
018ba73
misc
tkimura4 Apr 20, 2022
838b554
Merge branch 'feature/update_smoother_vis' of github.com:tkimura4/aut…
tkimura4 Apr 20, 2022
53d7712
ci(pre-commit): autofix
pre-commit-ci[bot] Apr 20, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Prev Previous commit
Next Next commit
rename behavior _path to behavior_velocity_path 
Signed-off-by: tomoya.kimura <tomoya.kimura@tier4.jp>
  • Loading branch information
@tkimura4
tkimura4 committed Apr 20, 2022
commit e201a9c2a2822d172c6365ba1384c7f55f9ebaf1
24 changes: 12 additions & 12 deletions planning/motion_velocity_smoother/scripts/trajectory_visualizer.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -85,7 +85,7 @@ def __init__(self):
self.update_traj_resample = False
self.update_traj_final = False
self.update_lanechange_path = False
self.update_behavior_path = False
self.update_behavior_velocity_path = False
self.update_traj_ob_avoid = False
self.update_traj_ob_stop = False

Expand All @@ -105,7 +105,7 @@ def __init__(self):
self.trajectory_final = Trajectory()

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

Expand Down Expand Up @@ -218,17 +218,17 @@ def CallBackLaneDrivingTraj(self, cmd5, cmd6, cmd7, cmd8, cmd9):
print("CallBackLaneDrivingTraj called")
self.CallBackTrajFinal(cmd5)
self.CallBackLaneChangePath(cmd6)
self.CallBackBehaviorPath(cmd7)
self.CallBackBehaviorVelocityPath(cmd7)
self.CallbackObstacleAvoidTraj(cmd8)
self.CallbackObstacleStopTraj(cmd9)

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

def CallBackBehaviorPath(self, cmd):
self.behavior_path = cmd
self.update_behavior_path = True
def CallBackBehaviorVelocityPath(self, cmd):
self.behavior_velocity_path = cmd
self.update_behavior_velocity_path = True

def CallbackObstacleAvoidTraj(self, cmd):
self.obstacle_avoid_traj = cmd
Expand All @@ -241,7 +241,7 @@ 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.im2,) = self.ax1.plot([], [], label="1: behavior_velocity_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 Down Expand Up @@ -303,7 +303,7 @@ def plotTrajectoryVelocity(self, data):

# copy
lane_change_path = self.lane_change_path
behavior_path = self.behavior_path
behavior_velocity_path = self.behavior_velocity_path
obstacle_avoid_traj = self.obstacle_avoid_traj
obstacle_stop_traj = self.obstacle_stop_traj
trajectory_raw = self.trajectory_raw
Expand All @@ -321,11 +321,11 @@ def plotTrajectoryVelocity(self, data):
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_path:
x = self.CalcArcLengthPath(behavior_velocity_path)
y = self.ToVelListPath(behavior_velocity_path)
self.im2.set_data(x, y)
self.update_behavior_path = False
self.update_behavior_velocity_path = False

if self.update_traj_ob_avoid:
x = self.CalcArcLength(obstacle_avoid_traj)
Expand Down