起点（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
