上一篇写了地图的格式，主要是由-1，0，1组成的矩阵。有了地图，还要有初始点和目标点的位置才能进行路径规划。

位置获取

初始点和目标点主要是矩阵中的位置，但是从rivz中获取的是地图坐标，所以获取之后还要进行转换成栅格地图坐标。

查看初始点发布的话题和发布者

我选择的话题是/amcl_pose，接收的数据类型是PoseWithCovarianceStamped
查看数据类型：

[geometry_msgs/PoseWithCovarianceStamped]:
std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
geometry_msgs/PoseWithCovariance pose
  geometry_msgs/Pose pose
    geometry_msgs/Point position
      float64 x
      float64 y
      float64 z
    geometry_msgs/Quaternion orientation
      float64 x
      float64 y
      float64 z
      float64 w
  float64[36] covariance

对于最初始的路径规划，我们只需要x,y的坐标，其他的后续才能用到。

获取初始点

from geometry_msgs.msg import PoseWithCovarianceStamped



def getIniPose(self):
        '''
            获取初始坐标点
        '''
        self.IniPose = rospy.wait_for_message("/amcl_pose", PoseWithCovarianceStamped,timeout=None)
        self.init_x = self.IniPose.pose.pose.position.x
        self.init_y = self.IniPose.pose.pose.position.y
        self.start_point = self.worldToMap(self.init_x,self.init_y)

打印出来（0.03650604122402327 -0.025128637246311626）很显然这不是我们想要的点的格式。这里面的worldToMap函数是rviz坐标到矩阵位置的转换。

查看目标点发布的话题和发布者

选择的话题是/move_base_simple/goal, 数据类型为PoseStamped，这个获取的点就是在rviz菜单栏中选择的那个粉色的箭头(2D nav goal)标注的点

[geometry_msgs/PoseStamped]:
std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
geometry_msgs/Pose pose
  geometry_msgs/Point position
    float64 x
    float64 y
    float64 z
  geometry_msgs/Quaternion orientation
    float64 x
    float64 y
    float64 z
    float64 w

同理，这里只需要x,y

获取目标点

from geometry_msgs.msg import PoseStamped


    def getTarPose(self):
        '''
            获取目标坐标点
        '''
        self.TarPose = rospy.wait_for_message("/move_base_simple/goal", PoseStamped,timeout=None)
        self.tar_x = self.TarPose.pose.position.x
        self.tar_y = self.TarPose.pose.position.y
        self.final_point = self.worldToMap(self.tar_x,self.tar_y)

rviz地图坐标和栅格坐标的转换

获取地图大小

由于我的地图绘制完有大量的没有用到的区域，所以我自行给它截图了，截完之后机器人和地图的相对位置变了，需要重新调整配置文件(麻烦)，而且也要测量一下地图尺寸。
这里可以用rviz箭头获取目标点的方法测量四个角。

原理

经过测量，获取到地图的四个点坐标如下图，因为矩阵是左上角为0行0列，为了和矩阵对应，所以我们要把当前坐标系移动到左上角,这样就有转换公式：
xnew = 10.5 – x
ynew = -(-4.6-y)

下一步，任意给定（xnew,ynew）,如何找到对应的矩阵所在的位置。
这里就要用到分辨率，如果不知道分辨率是什么，不妨根据这个例子看看。可以通过获取矩阵的长宽，得到其是一个（210，378）的矩阵块。
我们把这两个矩阵的长和宽分别做比：
10.5/210 =0.05
18.9/378 = 0.05
这个0.05就是分辨率，也就是一个矩阵格子对应多少米，而且应该是向下取整，所以转换公式为：
x_m = (10.5 – x) / 0.05
y_m = -(-4.6-y) / 0.05

代码实现

pixwidth = 10.197194  #10.2
pixheight = 4.625010  #4.6



    def worldToMap(self,x,y):
        #将rviz地图坐标转换为栅格坐标
        #这里10.2和-4.6需要自动添加，目前不知道怎么添加
        mx = (int)((pixwidth-x) /self.resolution)
        my = (int)(-(-pixheight-y) /self.resolution)
        return [mx,my]

这样上游所有的数据就处理好了。