Fix some type declaring issue and add the function create_waypoint_node
Fix the type declaration of the "waypoint_t" again
Implement the push_waypoint_node function
Implement the free_waypoint_list function