I am working on modifying the example "Lane change for highway driving" and I want to adapt it to work in curved roads as it only works for straight segments of the road. I have modified the block "Update Global Plan and Deviation" and "Trajectory Generator" (Blocks inside LaneChangeTestBench/PlannerandControllerRefMdl/Motion Planner). However, it seems that the block "Trajectory Generator" only accepts roads that have two waypoints, and when I have a curved road, the vehicle only moves up from the first to the second defined waypoint. I modified the block "Update Global Plan and Deviation" to keep updating consistently the Waypoints to provide two at a time, but it doesn't work. I will appreciate any hints on how to solve this problem, or if there is a better approach that I'm missing. Thank you!
Extra: I'm using the Navigation Toolbox, MPC Toolbox, Automated Driving Toolbox
I get the following error when I try to update the next waypoint:
An error occurred while running the simulation and the simulation was terminated
Caused by:
