MCZhi / Predictive-Decision

[ITSC 2023] Predictive Decision-making Framework with Interaction-aware Motion Forecasting Model
83 stars 13 forks source link

Some questions about the trajectory generation code #6

Closed wheltz closed 8 months ago

wheltz commented 8 months ago

I read the source code and found that the input obs.waypoint_paths of the planner already contains 51 waypoints. The trajectory sampling part will first use the interpolation method to form the route formed by these waypoints into a CubicSpline2D curve x=f(s),y=f( s), and then generate_trajectory_on_frenet will generate various trajectories giving different accelerations (due to the difference in acceleration, some new trajectories will indeed be generated). But I found the calculate_global_trajectory function still uses the CubicSpline2D curve formed by route to calculate fx and fy, which means that even if many trajectories are generated , but the waypoints from the route still need to be passed through in theory. So if I understand correctly, the waypoint provided by obs.waypoint_paths is actually similar to the target point of the carla simulator. I'm wondering if there's something wrong with my understanding. And if this is the case, can it be possible to use only two waypoints as the starting point and end point, and then generate more diverse trajectories? In addition, I don't quite understand why calculate_global_trajectory uses CubicSpline2D's i_yaw based on the original route to calculate fx and fy. I'm very sorry if there is any misunderstanding, I am not very familiar with the smarts simulator.

MCZhi commented 8 months ago

Hi, @wheltz. Thank you for your question.

To clarify, in our work, obs.waypoint_paths should be considered as reference paths, not target points. The waypoints provided by obs.waypoint_paths are sparse, with a 1m interval between them. However, we use a cubic spline curve to create a smooth and dense reference path, with a 0.1m interval between points. This allows us to generate trajectories based on Frenet frame on the cubic spline reference path.

While you can try directly sampling target points for obs.waypoint_paths to generate trajectories, it may not necessarily be more diverse.

For calculate_global_trajectory, we do need the yaw of the reference path to convert the Frenet coordinates to Cartesian coordinates. You can easily find more information on this by doing a quick Google search.

wheltz commented 8 months ago

Your reply is very helpful. Thank you a lot