分装好的api接口无法计算路程问题

面对底盘ROS分装好的api接口设备无法触碰到底层ROS话题问题，同时苦于在未导航前提前估计路程大小的我，在网上没有办法的情况下又苦恼了好几天，直到得到一个师兄的点拨，编写了如下的代码算是真正解决了不用ROS低层topic话题便可以获得估计的路程～ 【PS：这里需要换成自己机器人的api接口】
步骤如下：

1. 保存地图到本地
2. 建立处理图片代码（我理解为去除噪声）

def map_dealing(map_path: str) -> None:
    map_origin = cv2.imread(map_path, cv2.IMREAD_GRAYSCALE)
    width, high = np.shape(map_origin)
    for x in range(width):
        for y in range(high):
            if 0 <= map_origin[x, y] <= 204:
                map_origin[x, y] = 0
            else:
                map_origin[x, y] = 255
    num_labels, labels, stats, centroids = cv2.connectedComponentsWithStats(map_origin, connectivity=4)
    area = stats[:, 4:5]  # area
    max1 = np.sort(area, axis=0)[-1]  # first area label
    max_index = area.tolist().index(max1)
    max2 = np.sort(area, axis=0)[-2]  # second area label
    max2_index = area.tolist().index(max2)
    map_connectedcomponents = map_origin
    for x in range(width):
        for y in range(high):
            if labels[x, y] == max2_index:
                map_connectedcomponents[x, y] = 255
            else:
                map_connectedcomponents[x, y] = 204
    for x in range(width):
        for y in range(high):
            if map_origin[x, y] == 0:
                map_connectedcomponents[x, y] = 0
    map_binary = np.array(map_connectedcomponents)
    for x in range(width):
        for y in range(high):
            if map_binary[x, y] == 204:
                map_binary[x, y] = 0
    contours, hierarchy = cv2.findContours(map_binary, cv2.RETR_LIST, cv2.CHAIN_APPROX_SIMPLE)
    cv2.drawContours(map_connectedcomponents, contours, -1, 0, 2)
    map_name = map_path.split('.')
    cv2.imwrite(map_name[0] + 'Dealing' + '.png', map_connectedcomponents)

目的与结果如下：

变成如下：
3. 获取图像中所有白点，即可行点，并保存

4. 写 像素到真实世界的映射函数/真实世界到像素的映射函数

    def real_to_pix(self, real_x, real_y):
        """
        世界坐标转换成像素坐标

        :param real_x: 世界坐标x
        :param real_y: 世界坐标y
        :return: (像素坐标x, 像素坐标y)
        """
        return int((real_x - self.origin_x) / self.resolution), \
               int(self.height - (real_y - self.origin_y) / self.resolution)

    def pix_to_real(self, pix_x, pix_y):
        """
        像素坐标转换成世界坐标

        :param pix_x: 像素坐标x
        :param pix_y: 像素坐标y
        :return: (世界坐标x, 世界坐标y)
        """
        return self.resolution * pix_x + self.origin_x, self.origin_y - (pix_y - self.height) * self.resolution

import math
from water_api import WaterApi
water = WaterApi("192.168.10.10", 31001)
water.map_dealing("/home/water/下载/yolov5_d435i_detection/xindongyuan20230408.png")
# exit(0)
start = []
location = water.robot_status()
print(location['results']['current_pose'])
start.append(location['results']['current_pose']['x'])
start.append(location['results']['current_pose']['y'])import math

from water_api import WaterApi
water = WaterApi("192.168.10.10", 31001)
water.map_dealing("/home/water/下载/yolov5_d435i_detection/xindongyuan20230408.png")
# exit(0)
start = []
location = water.robot_status()
print(location['results']['current_pose'])
start.append(location['results']['current_pose']['x'])
start.append(location['results']['current_pose']['y'])
# water.move_location_pretend(-4.4617, -1.7089, -3.0321)
pix_start_x, pix_start_y = water.real_to_pix(start[0], start[1])

# print("plan:", plan['results']['distance'])   #  距离长度
#
import json
with open('/home/water/下载/yolov5_d435i_detection/position.json', 'r') as f:
    result = json.load(f)
    print(result['沙发'])
real_end_x, real_end_y = result['沙发']['x'], result['沙发']['y']
pix_end_x, pix_end_y = water.real_to_pix(real_end_x, real_end_y)
# import math
# print(math.sqrt(math.pow((0.2139 + 4.4617 ), 2) + math.pow(0.1294 + 1.7089, 2)))
import networkx as nx
import cv2
def dist(a, b):
    (x1, y1) = a
    (x2, y2) = b
    return math.sqrt((x1 - x2) ** 2 + (y1 - y2) ** 2)

def distance(x_start, y_start, x_end, y_end):
    image = cv2.imread('/home/water/下载/yolov5_d435i_detection/xindongyuan20230408Dealing.png')
    graph = nx.Graph()
    sum = 0
    for i in range(image.shape[0]):
        for j in range(image.shape[1]):
            if image[i][j][0] == 255 and image[i][j][1] == 255 and image[i][j][2] == 255:
                sum += 1
                graph.add_node((i, j))
                for di in range(-5, 6):  # -4, 5
                    for dj in range(-5, 6):
                        if graph.has_node((i + di, j + dj)):
                            graph.add_edge((i, j), (i + di, j + dj), weight=math.sqrt(di**2 + dj**2))
                '''if graph.has_node((i - 1, j)):
                    graph.add_edge((i, j), (i - 1, j))
                if graph.has_node((i + 1, j)):
                    graph.add_edge((i, j), (i + 1, j))
                if graph.has_node((i, j - 1)):
                    graph.add_edge((i, j), (i, j - 1))
                if graph.has_node((i, j + 1)):
                    graph.add_edge((i, j), (i, j + 1))'''
    assert graph.has_node((y_start, x_start))
    assert graph.has_node((y_end, x_end))
    print(len(graph.edges))
    print(len(graph.nodes))
    path = nx.astar_path(graph, (y_start, x_start), (y_end, x_end), heuristic=dist, weight='weight')  # weight='weight'
    ##########################
    # for i in range(len(path)):
    #     cv2.circle(image, (path[i][1], path[i][0]), 2, (0, 0, 255), -1)
    # cv2.imshow('img', image)
    # cv2.waitKey(100000)
    #############################3
    path_real = []
    for i in range(len(path)):
        x, y = water.pix_to_real(path[i][1], path[i][0])
        path_real.append([x, y])
    S = 0
    for i in range(len(path_real) - 1):
        S += dist(path_real[i], path_real[i+1]) * 1.2525
    return S

S = distance(pix_start_x, pix_start_y, pix_end_x, pix_end_y)
print("S:", S)
# water.move_location_pretend(-4.4617, -1.7089, -3.0321)
pix_start_x, pix_start_y = water.real_to_pix(start[0], start[1])

# print("plan:", plan['results']['distance'])   #  距离长度
#
import json
with open('/home/water/下载/yolov5_d435i_detection/position.json', 'r') as f:
    result = json.load(f)
    print(result['沙发'])
real_end_x, real_end_y = result['沙发']['x'], result['沙发']['y']
pix_end_x, pix_end_y = water.real_to_pix(real_end_x, real_end_y)
# import math
# print(math.sqrt(math.pow((0.2139 + 4.4617 ), 2) + math.pow(0.1294 + 1.7089, 2)))
import networkx as nx
import cv2
def dist(a, b):
    (x1, y1) = a
    (x2, y2) = b
    return math.sqrt((x1 - x2) ** 2 + (y1 - y2) ** 2)

def distance(x_start, y_start, x_end, y_end):
    image = cv2.imread('/home/water/下载/yolov5_d435i_detection/xindongyuan20230408Dealing.png')
    graph = nx.Graph()
    sum = 0
    for i in range(image.shape[0]):
        for j in range(image.shape[1]):
            if image[i][j][0] == 255 and image[i][j][1] == 255 and image[i][j][2] == 255:
                sum += 1
                graph.add_node((i, j))
                for di in range(-5, 6):  # -4, 5
                    for dj in range(-5, 6):
                        if graph.has_node((i + di, j + dj)):
                            graph.add_edge((i, j), (i + di, j + dj), weight=math.sqrt(di**2 + dj**2))
                '''if graph.has_node((i - 1, j)):
                    graph.add_edge((i, j), (i - 1, j))
                if graph.has_node((i + 1, j)):
                    graph.add_edge((i, j), (i + 1, j))
                if graph.has_node((i, j - 1)):
                    graph.add_edge((i, j), (i, j - 1))
                if graph.has_node((i, j + 1)):
                    graph.add_edge((i, j), (i, j + 1))'''
    assert graph.has_node((y_start, x_start))
    assert graph.has_node((y_end, x_end))
    print(len(graph.edges))
    print(len(graph.nodes))
    path = nx.astar_path(graph, (y_start, x_start), (y_end, x_end), heuristic=dist, weight='weight')  # weight='weight'
    ##########################
    # for i in range(len(path)):
    #     cv2.circle(image, (path[i][1], path[i][0]), 2, (0, 0, 255), -1)
    # cv2.imshow('img', image)
    # cv2.waitKey(100000)
    #############################3
    path_real = []
    for i in range(len(path)):
        x, y = water.pix_to_real(path[i][1], path[i][0])
        path_real.append([x, y])
    S = 0
    for i in range(len(path_real) - 1):
        S += dist(path_real[i], path_real[i+1]) * 1.2525
    return S

S = distance(pix_start_x, pix_start_y, pix_end_x, pix_end_y)
print("S:", S)

计算出来的结果：