FIXME(soblin): convergence should be applied from nearest_idx ?

* @param waypoint_chunks WaypointGroupChunk defined on the lanelet_sequence * @param lanelet_sequence consecutive lanelet sequence * @param s_start

NOTE: rear_to_cog is supposed to be positive

@brief convert LinearRing2d to Polygon2d * @param footprint LinearRing2d * @return Polygon2d */

Build a single-point cluster cloud at (px, py, pz) with the given source frame.

* Create a curved trajectory (quarter circle) * * Example ASCII diagram: * * v=3m/s v=3m/s v=3m/s * (0,2)----(1,2) * | * | * v

Core logic for creating control command based on current odom and traj

@brief make a single enveloped polygon from the all trajectory polygons

* @brief Calculate the delta z between a point and a range. Return 0.0 if the point is within the * range. * @param z The point to calculate the dis

* @brief Get the end of the interpolation range. */

Assert that every covariance entry except the six diagonal indices is exactly zero.

* @brief extract points included in all clusters as a single pointcloud */

* @brief Calculate the z-range from the bounds of a lanelet. * @param lanelet The lanelet to derive the z-range from. * @return The z-range as a pai