f1tenth仿真2

起点（0.192，0.201）
终点（9.902，5.148）

起点（9.902，5.148）
终点（-13.289，7.058）

起点（-13.289，7.058）
终点（-13.289，0.201）

起点（-13.289，0.201）

#! /usr/bin/env python
import time
from numba import jit
import math
import rospy
import numpy as np
import matplotlib.pyplot as plt
from nav_msgs.msg import OccupancyGrid
from geometry_msgs.msg import PoseWithCovarianceStamped
from geometry_msgs.msg import PoseStamped
from nav_msgs.msg import Path
import csv


class MapMatrix:
    """
        说明:
            1