diff --git a/planning/planning_debug_tools/scripts/trajectory_visualizer.py b/planning/planning_debug_tools/scripts/trajectory_visualizer.py index 917afc08..146f4019 100755 --- a/planning/planning_debug_tools/scripts/trajectory_visualizer.py +++ b/planning/planning_debug_tools/scripts/trajectory_visualizer.py @@ -94,13 +94,15 @@ def __init__(self): self.update_ex_vel_lim = False self.update_lat_acc_fil = False self.update_steer_rate_fil = False - self.update_traj_raw = False - self.update_traj_resample = False - self.update_traj_final = False + self.update_raw_traj = False + self.update_resample_traj = False + self.update_final_traj = 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 + self.update_path_smoother_path = False + self.update_path_optimizer_traj = False + self.update_motion_velocity_planner_traj = False + self.update_obstacle_stop_planner_traj = False self.tf_buffer = Buffer(node=self) self.tf_listener = TransformListener(self.tf_buffer, self, spin_thread=True) @@ -120,8 +122,10 @@ def __init__(self): self.behavior_path_planner_path = PathWithLaneId() self.behavior_velocity_planner_path = Path() + self.path_smoother_path = Path() self.path_optimizer_traj = Trajectory() - self.obstacle_stop_traj = Trajectory() + self.motion_velocity_planner_traj = Trajectory() + self.obstacle_stop_planner_traj = Trajectory() self.plotted = [False] * 9 self.sub_localization_kinematics = self.create_subscription( @@ -137,44 +141,77 @@ def __init__(self): # BUFFER_SIZE = 65536*100 optimizer_debug = "/planning/scenario_planning/velocity_smoother/debug/" - self.sub1 = message_filters.Subscriber( + self.sub_velocity_smoother_external_velocity_limited = message_filters.Subscriber( self, Trajectory, optimizer_debug + "trajectory_external_velocity_limited" ) - self.sub2 = message_filters.Subscriber( + self.sub_velocity_smoother_lateral_acc_filtered = message_filters.Subscriber( self, Trajectory, optimizer_debug + "trajectory_lateral_acc_filtered" ) - self.sub3 = message_filters.Subscriber(self, Trajectory, optimizer_debug + "trajectory_raw") - self.sub4 = message_filters.Subscriber( + self.sub_velocity_smoother_traj_raw = message_filters.Subscriber( + self, Trajectory, optimizer_debug + "trajectory_raw" + ) + self.sub_velocity_smoother_resample = message_filters.Subscriber( self, Trajectory, optimizer_debug + "trajectory_time_resampled" ) - self.sub41 = message_filters.Subscriber( + self.sub_velocity_smoother_steering_rate = message_filters.Subscriber( self, Trajectory, optimizer_debug + "trajectory_steering_rate_limited" ) - self.sub5 = message_filters.Subscriber( + self.sub_final_traj = message_filters.Subscriber( self, Trajectory, "/planning/scenario_planning/trajectory" ) lane_driving = "/planning/scenario_planning/lane_driving" - self.sub6 = message_filters.Subscriber( + self.sub_behavior_path_planner = message_filters.Subscriber( self, PathWithLaneId, lane_driving + "/behavior_planning/path_with_lane_id" ) - self.sub7 = message_filters.Subscriber(self, Path, lane_driving + "/behavior_planning/path") - self.sub8 = message_filters.Subscriber( + self.sub_behavior_velocity_planner = message_filters.Subscriber( + self, Path, lane_driving + "/behavior_planning/path" + ) + self.sub_path_smoother = message_filters.Subscriber( + self, + Path, + lane_driving + "/motion_planning/path_smoother/path", + ) + self.sub_path_optimizer = message_filters.Subscriber( self, Trajectory, lane_driving + "/motion_planning/path_optimizer/trajectory", ) - self.sub9 = message_filters.Subscriber(self, Trajectory, lane_driving + "/trajectory") + self.sub_motion_velocity_planner = message_filters.Subscriber( + self, Trajectory, lane_driving + "/motion_planning/motion_velocity_planner/trajectory" + ) + self.sub_obstacle_stop_planner = message_filters.Subscriber( + self, Trajectory, lane_driving + "/trajectory" + ) self.ts1 = message_filters.ApproximateTimeSynchronizer( - [self.sub1, self.sub2, self.sub3, self.sub4, self.sub41], 30, 0.5 + [ + self.sub_velocity_smoother_external_velocity_limited, + self.sub_velocity_smoother_lateral_acc_filtered, + self.sub_velocity_smoother_traj_raw, + self.sub_velocity_smoother_resample, + self.sub_velocity_smoother_steering_rate, + ], + 30, + 0.5, ) self.ts1.registerCallback(self.CallbackMotionVelOptTraj) self.ts2 = message_filters.ApproximateTimeSynchronizer( - [self.sub5, self.sub6, self.sub7, self.sub8, self.sub9], 30, 1, 0 + [ + self.sub_final_traj, + self.sub_behavior_path_planner, + self.sub_behavior_velocity_planner, + self.sub_path_smoother, + self.sub_path_optimizer, + self.sub_motion_velocity_planner, + self.sub_obstacle_stop_planner, + ], + 30, + 1, + 0, ) - self.ts2.registerCallback(self.CallBackLaneDrivingTraj) + self.ts2.registerCallback(self.CallbackLaneDrivingTraj) # main process if PLOT_TYPE == "VEL_ACC_JERK": @@ -205,86 +242,113 @@ def CallbackVelocityLimit(self, cmd): def CallbackMotionVelOptTraj(self, cmd1, cmd2, cmd3, cmd4, cmd5): self.get_logger().info("CallbackMotionVelOptTraj called") - self.CallBackTrajExVelLim(cmd1) - self.CallBackTrajLatAccFiltered(cmd2) - self.CallBackTrajRaw(cmd3) - self.CallBackTrajTimeResampled(cmd4) - self.CallBackTrajSteerRateFiltered(cmd5) + self.CallbackTrajExVelLim(cmd1) + self.CallbackTrajLatAccFiltered(cmd2) + self.CallbackTrajRaw(cmd3) + self.CallbackTrajTimeResampled(cmd4) + self.CallbackTrajSteerRateFiltered(cmd5) - def CallBackTrajExVelLim(self, cmd): + def CallbackTrajExVelLim(self, cmd): self.trajectory_external_velocity_limited = cmd self.update_ex_vel_lim = True - def CallBackTrajLatAccFiltered(self, cmd): + def CallbackTrajLatAccFiltered(self, cmd): self.trajectory_lateral_acc_filtered = cmd self.update_lat_acc_fil = True - def CallBackTrajSteerRateFiltered(self, cmd): + def CallbackTrajSteerRateFiltered(self, cmd): self.trajectory_steer_rate_filtered = cmd self.update_steer_rate_fil = True - def CallBackTrajRaw(self, cmd): + def CallbackTrajRaw(self, cmd): self.trajectory_raw = cmd - self.update_traj_raw = True + self.update_raw_traj = True - def CallBackTrajTimeResampled(self, cmd): + def CallbackTrajTimeResampled(self, cmd): self.trajectory_time_resampled = cmd - self.update_traj_resample = True + self.update_resample_traj = True - def CallBackTrajFinal(self, cmd): + def CallbackTrajFinal(self, cmd): self.trajectory_final = cmd - self.update_traj_final = True - - def CallBackLaneDrivingTraj(self, cmd5, cmd6, cmd7, cmd8, cmd9): - self.get_logger().info("CallBackLaneDrivingTraj called") - self.CallBackTrajFinal(cmd5) - self.CallBackLBehaviorPathPlannerPath(cmd6) - self.CallBackBehaviorVelocityPlannerPath(cmd7) - self.CallbackObstacleAvoidTraj(cmd8) - self.CallbackObstacleStopTraj(cmd9) - - def CallBackLBehaviorPathPlannerPath(self, cmd): + self.update_final_traj = True + + def CallbackLaneDrivingTraj(self, cmd5, cmd6, cmd7, cmd8, cmd9, cmd10, cmd11): + self.get_logger().info("CallbackLaneDrivingTraj called") + self.CallbackTrajFinal(cmd5) + self.CallbackLBehaviorPathPlannerPath(cmd6) + self.CallbackBehaviorVelocityPlannerPath(cmd7) + self.CallbackPathSmootherPath(cmd8) + self.CallbackPathOptimizerTraj(cmd9) + self.CallbackObstacleStopPlannerTraj(cmd10) + self.CallbackMotionVelocityPlannerTraj(cmd11) + + def CallbackLBehaviorPathPlannerPath(self, cmd): self.behavior_path_planner_path = cmd self.update_behavior_path_planner_path = True - def CallBackBehaviorVelocityPlannerPath(self, cmd): + def CallbackBehaviorVelocityPlannerPath(self, cmd): self.behavior_velocity_planner_path = cmd self.update_behavior_velocity_planner_path = True - def CallbackObstacleAvoidTraj(self, cmd): + def CallbackPathSmootherPath(self, cmd): + self.path_smoother_path = cmd + self.update_path_smoother_path = True + + def CallbackPathOptimizerTraj(self, cmd): self.path_optimizer_traj = cmd - self.update_traj_ob_avoid = True + self.update_path_optimizer_traj = True + + def CallbackMotionVelocityPlannerTraj(self, cmd): + self.motion_velocity_planner_traj = cmd + self.update_motion_velocity_planner_traj = True - def CallbackObstacleStopTraj(self, cmd): - self.obstacle_stop_traj = cmd - self.update_traj_ob_stop = True + def CallbackObstacleStopPlannerTraj(self, cmd): + self.obstacle_stop_planner_traj = cmd + self.update_obstacle_stop_planner_traj = True def setPlotTrajectoryVelocity(self): self.ax1 = plt.subplot(1, 1, 1) # row, col, index(