Lap Counting

[ugsfume]

In trackdrive and autocross, the autonomous vehicle has to complete a certain number of laps, no more no less. It has to stop within a certain distance after the final lap. Completion of a lap will be indicated by a pair of large orange cones placed at the start/stop position. To Do:

• Develop a function that counts the lap when the car passes the large orange orange cones.
• Develop a function that tells the car to stop when the mission is completed

Notes: The coordinates of the large orange cones can be acquired from the ROStopic /cone_list

[ugsfume]

Lap counting has been implemented. Using the first two large orange cones detected to set a vector and compute the cross product of the car vector, a sign change indicates line crossing. A distance threshold is implemented to avoid false lap counts. There's a cooldown period after lap count increments to avoid double counting due to noise.

[ugsfume]

This lap counting method is used in Trackdrive, Autocross, and Acceleration.

However, there are two large orange cones at the starting line of Acceleration. They were misidentified as the finish line during the F24 Silverstone runs causing the car to go into AS-FINISH after crossing the starting line. To avoid this, implement a distance threshold for large orange cones detection, comparing their distance from the world's origin(the car's starting position).