def test_get_pool_pix(self):
src_point = [114.4314, 30.523558]
self.baidu_obj = baidu_map.BaiduMap(src_point,
zoom=15,
scale=1,
map_type=baidu_map.MapType.gaode)
pool_cnts, (pool_cx,
pool_cy) = self.baidu_obj.get_pool_pix(b_show=False)
self.assertIsNot(pool_cnts, None)
def test_get_pool_name(self):
src_point = [114.4314, 30.523558]
self.baidu_obj = baidu_map.BaiduMap(src_point,
zoom=15,
scale=1,
map_type=baidu_map.MapType.gaode)
self.baidu_obj.get_pool_name()
self.assertEqual(type(self.baidu_obj.pool_name), str)
self.assertEqual(type(self.baidu_obj.address), str)
def test_scan_pool(self):
src_point = [114.4314, 30.523558]
self.baidu_obj = baidu_map.BaiduMap(src_point,
zoom=15,
scale=1,
map_type=baidu_map.MapType.gaode)
pool_cnts, (pool_cx,
pool_cy) = self.baidu_obj.get_pool_pix(b_show=False)
scan_cnts = self.baidu_obj.scan_pool(meter_gap=50,
safe_meter_distance=10,
b_show=False)
self.assertFalse(len(scan_cnts) < 0)
def get_path(baidu_map_obj=None,
mode=0,
target_lng_lats=None,
target_pixs=None,
b_show=False,
back_home=False,
map_connect=7,
pix_gap=30):
"""
根据设置模式返回高德地图上规划路径
:param baidu_map_obj 地图对象
:param mode 选择模式
:param target_lng_lats 目标经纬度集合，传入为高德经纬度
:param target_pixs 目标像素
:param b_show 是否显示图像
:param map_connect 搜索一个点最多连接数量
:param pix_gap 自动搜索像素间隔
mode
０ 到达目标点后停留
１ 到达多个目标点
２ 扫描整个湖泊
4 返航
"""
global path_matrix
if baidu_map_obj == None:
baidu_map_obj = baidu_map.BaiduMap(config.ship_gaode_lng_lat,
zoom=16,
scale=1,
map_type=baidu_map.MapType.gaode)
pool_cnts, (pool_cx,
pool_cy) = baidu_map_obj.get_pool_pix(b_show=False)
if pool_cnts is None:
return 'pool_cx is None'
baidu_map_obj.outpool_cnts_set = get_outpool_set(
np.array(baidu_map_obj.pool_cnts))
# 无GPS调试模式 以湖泊中心作为起点
if config.home_debug:
baidu_map_obj.ship_gaode_lng_lat = config.ship_gaode_lng_lat
baidu_map_obj.ship_gps = config.ship_gaode_lng_lat
if baidu_map_obj.ship_gps is None:
return 'no ship gps'
if mode == 0:
if target_lng_lats is None:
return 'target_pixs is None'
elif len(target_lng_lats) > 1:
return 'len(target_pixs) is >1 choose mode 1'
if baidu_map_obj.ship_pix is None:
if baidu_map_obj.ship_gaode_lng_lat is None:
baidu_map_obj.ship_gaode_lng_lat = baidu_map_obj.gps_to_gaode_lng_lat(
baidu_map_obj.ship_gps)
baidu_map_obj.ship_pix = baidu_map_obj.gaode_lng_lat_to_pix(
baidu_map_obj.ship_gaode_lng_lat)
s_start = tuple(baidu_map_obj.ship_pix)
s_goal = tuple(baidu_map_obj.gaode_lng_lat_to_pix(target_lng_lats[0]))
print('s_start,s_goal', s_start, s_goal)
# 判断是否能直线到达，不能则采用路径搜索
if not cross_outpool(s_start, s_goal, baidu_map_obj.pool_cnts):
try:
# astar = AStar(s_start, s_goal, "euclidean", baidu_map_obj.outpool_cnts_set)
# astar_path, visited = astar.searching()
rrt_conn = RrtConnect(s_start, s_goal, 1, 0.1, 10000,
baidu_map_obj.pool_cnts)
astar_path = rrt_conn.planning()
print('astar_path', astar_path)
return_pix_path = astar_path[::-1]
# 返航时添加
if back_home:
return_pix_path.append(astar_path)
print('原始长度', len(return_pix_path))
return_pix_path = multi_points_to_simple_points(
return_pix_path)
print('简化后长度', len(return_pix_path))
_, return_gaode_lng_lat_path = baidu_map_obj.pix_to_gps(
return_pix_path)
if b_show:
baidu_map_obj.show_img = cv2.polylines(
baidu_map_obj.show_img,
[np.array(astar_path, dtype=np.int32)], False,
(255, 0, 0), 1)
cv2.circle(baidu_map_obj.show_img, s_start, 5,
[255, 0, 255], -1)
cv2.circle(baidu_map_obj.show_img, s_goal, 5,
[255, 0, 255], -1)
baidu_map_obj.show_img = cv2.drawContours(
baidu_map_obj.show_img, [return_pix_path], -1,
(0, 0, 255), 3)
cv2.imshow('scan', baidu_map_obj.show_img)
cv2.waitKey(0)
cv2.destroyAllWindows()
return return_gaode_lng_lat_path
except Exception as e:
print('error ', e)
if b_show:
cv2.circle(baidu_map_obj.show_img, s_start, 5, [0, 255, 0],
-1)
cv2.circle(baidu_map_obj.show_img, s_goal, 5, [0, 0, 255],
-1)
cv2.imshow('scan', baidu_map_obj.show_img)
cv2.waitKey(0)
cv2.destroyAllWindows()
return 'can not fand path'
# 直接可达模式
else:
return_pix_path = []
return_pix_path.append(s_start)
return_pix_path.append(s_goal)
_, return_gaode_lng_lat_path = baidu_map_obj.pix_to_gps(
return_pix_path)
if b_show:
cv2.circle(baidu_map_obj.show_img, s_start, 5, [255, 255, 0],
-1)
cv2.circle(baidu_map_obj.show_img, s_goal, 5, [255, 255, 0],
-1)
baidu_map_obj.show_img = cv2.drawContours(
baidu_map_obj.show_img, np.array([return_pix_path]), -1,
(0, 0, 255), 3)
cv2.imshow('scan', baidu_map_obj.show_img)
cv2.waitKey(0)
cv2.destroyAllWindows()
# 返航时添加家的经纬度
if back_home:
if config.home_debug:
return_gaode_lng_lat_path.append(config.ship_gaode_lng_lat)
else:
return_gaode_lng_lat_path.append(
baidu_map_obj.init_ship_gaode_lng_lat)
return return_gaode_lng_lat_path
elif mode == 1:
if target_lng_lats is None:
return 'target_pixs is None'
elif len(target_lng_lats) <= 1:
return 'len(target_pixs) is<=1 choose mode 0'
if baidu_map_obj.ship_pix is None:
if baidu_map_obj.ship_gaode_lng_lat is None:
baidu_map_obj.ship_gaode_lng_lat = baidu_map_obj.gps_to_gaode_lng_lat(
baidu_map_obj.ship_gps)
baidu_map_obj.ship_pix = baidu_map_obj.gaode_lng_lat_to_pix(
baidu_map_obj.ship_gaode_lng_lat)
target_pixs = []
for target_lng_lat in target_lng_lats:
target_pixs.append(
baidu_map_obj.gaode_lng_lat_to_pix(target_lng_lat))
target_pixs.insert(0, baidu_map_obj.ship_pix)
distance_matrix = measure_distance(target_pixs,
baidu_map_obj.pool_cnts,
baidu_map_obj.outpool_cnts_set,
map_connect=config.find_points_num)
if back_home:
tsp_path = solve_tsp(distance_matrix, endpoints=(0, 0))
else:
tsp_path = solve_tsp(distance_matrix,
endpoints=(0, (len(target_pixs) - 1)))
path_points = []
print('path_matrix', path_matrix)
for index_i, val in enumerate(tsp_path):
if index_i < len(tsp_path) - 1:
if '%d_%d' % (val,
tsp_path[index_i + 1]) in path_matrix.keys():
path_points.extend(
path_matrix['%d_%d' % (val, tsp_path[index_i + 1])])
elif '%d_%d' % (tsp_path[index_i + 1],
val) in path_matrix.keys():
path_points.extend(
path_matrix['%d_%d' %
(tsp_path[index_i + 1], val)][::-1])
else:
path_points.append(target_pixs[val])
elif index_i == len(tsp_path) - 1:
if '%d_%d' % (val, tsp_path[index_i - 1]) in path_matrix.keys(
) or '%d_%d' % (val,
tsp_path[index_i - 1]) in path_matrix.keys():
pass
else:
path_points.append(target_pixs[val])
return_pix_path = path_points
print('原始长度', len(return_pix_path))
return_pix_path = multi_points_to_simple_points(return_pix_path)
print('简化后长度', len(return_pix_path))
_, return_gaode_lng_lat_path = baidu_map_obj.pix_to_gps(
return_pix_path)
if b_show:
baidu_map_obj.show_img = cv2.polylines(
baidu_map_obj.show_img,
[np.array(path_points, dtype=np.int32)], False, (255, 0, 0), 1)
cv2.imshow('scan', baidu_map_obj.show_img)
cv2.waitKey(0)
cv2.destroyAllWindows()
return return_gaode_lng_lat_path
elif mode == 2:
baidu_map_obj.scan_pool(baidu_map_obj.pool_cnts,
pix_gap=pix_gap,
b_show=False)
print('len(scan_cnt)', len(baidu_map_obj.scan_point_cnts))
distance_matrix = measure_distance(baidu_map_obj.scan_point_cnts,
baidu_map_obj.pool_cnts,
baidu_map_obj.outpool_cnts_set,
map_connect=map_connect)
tsp_path = solve_tsp(distance_matrix, endpoints=(0, 0))
print('tsp_path', tsp_path)
path_points = []
print('baidu_map_obj.pool_cnts', baidu_map_obj.pool_cnts)
print('path_matrix', path_matrix)
print('list path_matrix', list(path_matrix.keys()))
for index_i, val in enumerate(tsp_path):
if index_i < len(tsp_path) - 1:
if '%d_%d' % (val,
tsp_path[index_i + 1]) in path_matrix.keys():
path_points.extend(
path_matrix['%d_%d' % (val, tsp_path[index_i + 1])])
elif '%d_%d' % (tsp_path[index_i + 1],
val) in path_matrix.keys():
path_points.extend(
path_matrix['%d_%d' %
(tsp_path[index_i + 1], val)][::-1])
else:
path_points.append(baidu_map_obj.scan_point_cnts[val])
elif index_i == len(tsp_path) - 1:
if '%d_%d' % (val, tsp_path[index_i - 1]) in path_matrix.keys(
) or '%d_%d' % (val,
tsp_path[index_i - 1]) in path_matrix.keys():
pass
else:
pass
# path_points.append(baidu_map_obj.scan_point_cnts[val])
if b_show:
baidu_map_obj.show_img = cv2.polylines(
baidu_map_obj.show_img,
[np.array(path_points, dtype=np.int32)], False, (255, 0, 0), 1)
cv2.imshow('scan', baidu_map_obj.show_img)
cv2.waitKey(0)
cv2.destroyAllWindows()
return_pix_path = path_points
return_pix_path = path_points
print('原始长度', len(return_pix_path))
return_pix_path = multi_points_to_simple_points(return_pix_path)
print('简化后长度', len(return_pix_path))
_, return_gaode_lng_lat_path = baidu_map_obj.pix_to_gps(
return_pix_path)
return return_gaode_lng_lat_path
elif mode == 3:
pass
# back home
elif mode == 4:
if baidu_map_obj.init_ship_gps is None:
return 'ship init gps is None'
if baidu_map_obj.init_ship_pix is None:
if baidu_map_obj.init_ship_gaode_lng_lat is None:
baidu_map_obj.init_ship_gaode_lng_lat = baidu_map_obj.gps_to_gaode_lng_lat(
baidu_map_obj.init_ship_gps)
baidu_map_obj.init_ship_pix = baidu_map_obj.gaode_lng_lat_to_pix(
baidu_map_obj.init_ship_gaode_lng_lat)
baidu_map_obj.ship_gaode_lng_lat = baidu_map_obj.gps_to_gaode_lng_lat(
baidu_map_obj.ship_gps)
baidu_map_obj.ship_pix = baidu_map_obj.gaode_lng_lat_to_pix(
baidu_map_obj.ship_gaode_lng_lat)
s_start = tuple(baidu_map_obj.ship_pix)
s_goal = tuple(baidu_map_obj.init_ship_pix)
print('s_start,s_goal', s_start, s_goal)
# 判断是否能直线到达，不能则采用路径搜索
if not cross_outpool(s_start, s_goal, baidu_map_obj.pool_cnts):
rrt_conn = RrtConnect(s_start, s_goal, 1, 0.05, 100000,
baidu_map_obj.pool_cnts)
astar_path = rrt_conn.planning()
print('astar_path', astar_path)
baidu_map_obj.show_img = cv2.polylines(
baidu_map_obj.show_img, [np.array(astar_path, dtype=np.int32)],
False, (255, 0, 0), 1)
return_pix_path = astar_path[::-1]
return_pix_path = multi_points_to_simple_points(return_pix_path)
_, return_gaode_lng_lat_path = baidu_map_obj.pix_to_gps(
return_pix_path)
if b_show:
cv2.circle(baidu_map_obj.show_img, s_start, 5, [255, 0, 255],
-1)
cv2.circle(baidu_map_obj.show_img, s_goal, 5, [0, 0, 255], -1)
baidu_map_obj.show_img = cv2.drawContours(
baidu_map_obj.show_img, [return_pix_path], -1, (0, 0, 255),
3)
cv2.imshow('scan', baidu_map_obj.show_img)
cv2.waitKey(0)
cv2.destroyAllWindows()
return return_gaode_lng_lat_path
# except:
# if b_show:
# cv2.circle(baidu_map_obj.show_img, s_start, 5, [255, 255, 0], -1)
# cv2.circle(baidu_map_obj.show_img, s_goal, 5, [255, 255, 0], -1)
# baidu_map_obj.show_img = cv2.drawContours(baidu_map_obj.show_img, [return_pix_path], -1, (0, 0, 255), 3)
#
# cv2.imshow('scan', baidu_map_obj.show_img)
# cv2.waitKey(0)
# cv2.destroyAllWindows()
# return -3
# 直接可达模式
else:
return_pix_path = []
return_pix_path.append(s_start)
return_pix_path.append(s_goal)
_, return_gaode_lng_lat_path = baidu_map_obj.pix_to_gps(
return_pix_path)
if b_show:
cv2.circle(baidu_map_obj.show_img, s_start, 5, [255, 255, 0],
-1)
cv2.circle(baidu_map_obj.show_img, s_goal, 5, [255, 255, 0],
-1)
cv2.imshow('scan', baidu_map_obj.show_img)
cv2.waitKey(0)
cv2.destroyAllWindows()
return return_gaode_lng_lat_path
else:
return 1
def find_pool(self):
while True:
# 循环等待一定时间
time.sleep(config.check_status_interval)
# 若是用户没有点击点
if self.server_data_obj.mqtt_send_get_obj.pool_click_lng_lat is None or self.server_data_obj.mqtt_send_get_obj.pool_click_zoom is None:
# self.logger.info('没有点击湖，等待用户执行操作')
continue
# 查找与更新湖泊id
save_img_dir = os.path.join(config.root_path, 'baiduMap', 'imgs')
if not os.path.exists(save_img_dir):
os.mkdir(save_img_dir)
if self.current_map_type == baidu_map.MapType.gaode:
save_img_path = os.path.join(
save_img_dir, 'gaode_%f_%f_%i_%i.png' %
(self.server_data_obj.mqtt_send_get_obj.
pool_click_lng_lat[0], self.server_data_obj.
mqtt_send_get_obj.pool_click_lng_lat[1],
self.server_data_obj.mqtt_send_get_obj.pool_click_zoom,
1))
elif self.current_map_type == baidu_map.MapType.tecent:
save_img_path = os.path.join(
save_img_dir, 'tecent_%f_%f_%i_%i.png' %
(self.server_data_obj.mqtt_send_get_obj.
pool_click_lng_lat[0], self.server_data_obj.
mqtt_send_get_obj.pool_click_lng_lat[1],
self.server_data_obj.mqtt_send_get_obj.pool_click_zoom,
1))
elif self.current_map_type == baidu_map.MapType.baidu:
save_img_path = os.path.join(
save_img_dir, 'baidu_%f_%f_%i_%i.png' %
(self.server_data_obj.mqtt_send_get_obj.
pool_click_lng_lat[0], self.server_data_obj.
mqtt_send_get_obj.pool_click_lng_lat[1],
self.server_data_obj.mqtt_send_get_obj.pool_click_zoom,
1))
# 创建于查找湖泊
if len(self.data_define_obj.pool_code) <= 0 or not os.path.exists(
save_img_path):
# 创建地图对象
self.baidu_map_obj = baidu_map.BaiduMap(
lng_lat=self.server_data_obj.mqtt_send_get_obj.
pool_click_lng_lat,
zoom=self.server_data_obj.mqtt_send_get_obj.
pool_click_zoom,
logger=self.map_log,
map_type=self.current_map_type)
pool_cnts, (pool_cx,
pool_cy) = self.baidu_map_obj.get_pool_pix()
# 为NOne表示没有找到湖泊 继续换地图找
if pool_cnts is None:
if self.current_map_type == baidu_map.MapType.gaode:
self.current_map_type = baidu_map.MapType.tecent
continue
if self.current_map_type == baidu_map.MapType.tecent:
self.current_map_type = baidu_map.MapType.baidu
continue
if self.current_map_type == baidu_map.MapType.baidu:
self.current_map_type = baidu_map.MapType.gaode
# 若返回为None表示没找到湖 定义错误代码
is_collision = 1
pool_info_data = {
'deviceId':
config.ship_code,
'lng_lat':
self.server_data_obj.mqtt_send_get_obj.
pool_click_lng_lat,
'is_collision':
is_collision,
'zoom':
self.server_data_obj.mqtt_send_get_obj.
pool_click_zoom,
}
self.send(method='mqtt',
topic='pool_info_%s' % (config.ship_code),
data=pool_info_data,
qos=1)
self.logger.debug({'pool_info_data': pool_info_data})
continue
# 获取湖泊轮廓与中心点经纬度位置 _位置为提供前端直接绘图使用
_, self.baidu_map_obj.pool_lng_lats = self.baidu_map_obj.pix_to_gps(
pool_cnts)
_, self.baidu_map_obj.pool_center_lng_lat = self.baidu_map_obj.pix_to_gps(
[[pool_cx, pool_cy]])
self.logger.info({
'pool_center_lng_lat':
self.baidu_map_obj.pool_center_lng_lat
})
# 判断当前湖泊是否曾经出现，出现过则获取的ID 没出现过发送请求获取新ID
if isinstance(self.baidu_map_obj.pool_cnts, np.ndarray):
save_pool_cnts = self.baidu_map_obj.pool_cnts.tolist()
else:
save_pool_cnts = self.baidu_map_obj.pool_cnts
send_data = {
"longitudeLatitude":
json.dumps(self.baidu_map_obj.pool_center_lng_lat),
"mapData":
json.dumps(self.baidu_map_obj.pool_lng_lats),
"deviceId":
config.ship_code,
"pixData":
json.dumps(save_pool_cnts)
}
# 本地保存经纬度信息，放大1000000倍 用来只保存整数
save_pool_lng_lats = [[
int(i[0] * 1000000),
int(i[1] * 1000000)
] for i in self.baidu_map_obj.pool_lng_lats]
if not os.path.exists(config.local_map_data_path):
# 发送请求获取湖泊ID
self.logger.debug({'send_data': send_data})
try:
pool_id = self.send(method='http',
data=send_data,
url=config.http_save,
http_type='POST')
except Exception as e:
self.logger.error({'error': e})
continue
if isinstance(self.baidu_map_obj.pool_cnts, np.ndarray):
save_pool_cnts = self.baidu_map_obj.pool_cnts.tolist()
else:
save_pool_cnts = self.baidu_map_obj.pool_cnts
save_data = {
"mapList": [{
"id": pool_id,
"pool_center_lng_lat":
self.baidu_map_obj.pool_center_lng_lat,
"pool_lng_lats": save_pool_lng_lats,
"pool_cnts": save_pool_cnts
}]
}
self.logger.info({'pool_id': pool_id})
with open(config.local_map_data_path, 'w') as f:
json.dump(save_data, f)
else:
with open(config.local_map_data_path, 'r') as f:
local_map_data = json.load(f)
pool_id = baidu_map.is_in_contours(
(self.baidu_map_obj.lng_lat[0] * 1000000,
self.baidu_map_obj.lng_lat[1] * 1000000),
local_map_data)
if pool_id is not None:
self.logger.info({'在本地找到湖泊 poolid': pool_id})
# 不存在获取新的id
else:
try:
pool_id = self.send(method='http',
data=send_data,
url=config.http_save,
http_type='POST')
except Exception as e:
self.logger.error({'error': e})
self.logger.info({'新的湖泊 poolid': pool_id})
with open(config.local_map_data_path, 'w') as f:
# 以前存储键值
# local_map_data["mapList"].append({"id": pool_id,
# "longitudeLatitude": self.baidu_map_obj.pool_center_lng_lat,
# "mapData": self.baidu_map_obj.pool_lng_lat,
# "pool_cnt": pool_cnts.tolist()})
if isinstance(self.baidu_map_obj.pool_cnts,
np.ndarray):
save_pool_cnts = self.baidu_map_obj.pool_cnts.tolist(
)
local_map_data["mapList"].append({
"id":
pool_id,
"pool_center_lng_lat":
self.baidu_map_obj.pool_center_lng_lat,
"pool_lng_lats":
save_pool_lng_lats,
"pool_cnts":
save_pool_cnts
})
json.dump(local_map_data, f)
self.data_define_obj.pool_code = pool_id
pool_info_data = {
'deviceId':
config.ship_code,
'mapId':
self.data_define_obj.pool_code,
'lng_lat':
self.server_data_obj.mqtt_send_get_obj.pool_click_lng_lat,
# 'pool_lng_lats':self.baidu_map_obj.pool_lng_lats
}
self.send(method='mqtt',
topic='pool_info_%s' % (config.ship_code),
data=pool_info_data,
qos=1)
self.logger.info({'pool_info_data': pool_info_data})
def get_path(baidu_map_obj,
target_lng_lats,
b_show=False,
):
"""
根据设置模式返回高德地图上规划路径
:param baidu_map_obj 地图对象
:param target_lng_lats 目标经纬度集合，传入为高德经纬度
:param b_show 是否显示图像
"""
mode = len(target_lng_lats)
global path_matrix
if config.home_debug and baidu_map_obj is None:
baidu_map_obj = baidu_map.BaiduMap(config.ship_gaode_lng_lat, zoom=16, scale=1,
map_type=baidu_map.MapType.gaode)
pool_cnts, (pool_cx, pool_cy) = baidu_map_obj.get_pool_pix(b_show=False)
if pool_cnts is None:
return 'pool_cx is None'
if baidu_map_obj.ship_gaode_lng_lat is None:
return 'no ship gps'
search_safe_pix = int(config.path_search_safe_distance / baidu_map_obj.pix_2_meter)
print(config.path_search_safe_distance,baidu_map_obj.pix_2_meter)
# 单点
print('mode', mode)
if mode == 1:
baidu_map_obj.ship_pix = baidu_map_obj.gaode_lng_lat_to_pix(baidu_map_obj.ship_gaode_lng_lat)
row_start = tuple(baidu_map_obj.ship_pix)
row_goal = tuple(baidu_map_obj.gaode_lng_lat_to_pix(target_lng_lats[0]))
s_start = mod_point(row_start)
s_goal = mod_point(row_goal)
print('row_start,row_goal ', row_start, row_goal, 's_start,s_goal', s_start, s_goal,)
print('search_safe_pix', search_safe_pix)
# 判断是否能直线到达，不能则采用路径搜索
if not cross_outpool(s_start, s_goal, baidu_map_obj.pool_cnts, search_safe_pix):
astar = AStar(s_start, s_goal, "euclidean", baidu_map_obj.pool_cnts, search_safe_pix)
try:
astar_path, visited = astar.searching()
return_pix_path = astar_path[::-1]
simple_return_pix_path = multi_points_to_simple_points(return_pix_path)
print('原始长度', len(return_pix_path), '简化后长度', len(simple_return_pix_path))
_, return_gaode_lng_lat_path = baidu_map_obj.pix_to_gps(simple_return_pix_path)
if b_show:
baidu_map_obj.show_img = cv2.polylines(baidu_map_obj.show_img,
[np.array(astar_path, dtype=np.int32)], False, (255, 0, 0),
1)
cv2.circle(baidu_map_obj.show_img, s_start, 5, [255, 0, 255], -1)
cv2.circle(baidu_map_obj.show_img, s_goal, 5, [255, 0, 255], -1)
# baidu_map_obj.show_img = cv2.drawContours(baidu_map_obj.show_img, [return_pix_path], -1,
# (0, 0, 255), 3)
cv2.imshow('scan', baidu_map_obj.show_img)
cv2.waitKey(0)
cv2.destroyAllWindows()
return return_gaode_lng_lat_path
except Exception as e:
print('error ', e)
if b_show:
cv2.circle(baidu_map_obj.show_img, s_start, 5, [0, 255, 0], -1)
cv2.circle(baidu_map_obj.show_img, s_goal, 5, [0, 0, 255], -1)
cv2.imshow('scan', baidu_map_obj.show_img)
cv2.waitKey(0)
cv2.destroyAllWindows()
return 'can not fand path'
# 直接可达模式
else:
return_pix_path = []
# return_pix_path.append(row_start)
return_pix_path.append(row_goal)
_, return_gaode_lng_lat_path = baidu_map_obj.pix_to_gps(return_pix_path)
if b_show:
cv2.circle(baidu_map_obj.show_img, s_start, 5, [255, 255, 0], -1)
cv2.circle(baidu_map_obj.show_img, s_goal, 5, [255, 255, 0], -1)
baidu_map_obj.show_img = cv2.drawContours(baidu_map_obj.show_img, np.array([return_pix_path]), -1,
(0, 0, 255), 3)
cv2.imshow('scan', cv2.resize(baidu_map_obj.show_img, (512, 512)))
cv2.waitKey(0)
cv2.destroyAllWindows()
return return_gaode_lng_lat_path
# 多点
else:
if target_lng_lats is None:
return 'target_pixs is None'
elif len(target_lng_lats) <= 1:
return 'len(target_pixs) is<=1 choose mode 0'
baidu_map_obj.ship_pix = baidu_map_obj.gaode_lng_lat_to_pix(baidu_map_obj.ship_gaode_lng_lat)
target_pixs = []
for target_lng_lat in target_lng_lats:
target_pixs.append(baidu_map_obj.gaode_lng_lat_to_pix(target_lng_lat))
# 使用TSP优化路径
if config.b_tsp:
# 测量距离
distance_matrix = measure_distance(target_pixs, baidu_map_obj.pool_cnts, map_connect=config.find_points_num)
tsp_path = solve_tsp(distance_matrix)
if config.b_tsp:
tsp_path = solve_tsp(distance_matrix)
path_points = []
print('path_matrix', path_matrix)
for index_i, val in enumerate(tsp_path):
if index_i < len(tsp_path) - 1:
if '%d_%d' % (val, tsp_path[index_i + 1]) in path_matrix.keys():
path_points.extend(path_matrix['%d_%d' % (val, tsp_path[index_i + 1])])
elif '%d_%d' % (tsp_path[index_i + 1], val) in path_matrix.keys():
path_points.extend(path_matrix['%d_%d' % (tsp_path[index_i + 1], val)][::-1])
else:
path_points.append(target_pixs[val])
elif index_i == len(tsp_path) - 1:
if '%d_%d' % (val, tsp_path[index_i - 1]) in path_matrix.keys() or '%d_%d' % (
val, tsp_path[index_i - 1]) in path_matrix.keys():
pass
else:
path_points.append(target_pixs[val])
else:
path_points = []
for index, val in enumerate(target_lng_lats):
if index == 0:
row_start = tuple(baidu_map_obj.ship_pix)
row_goal = tuple(baidu_map_obj.gaode_lng_lat_to_pix(target_lng_lats[index]))
else:
row_start = tuple(baidu_map_obj.gaode_lng_lat_to_pix(target_lng_lats[index - 1]))
row_goal = tuple(baidu_map_obj.gaode_lng_lat_to_pix(target_lng_lats[index]))
s_start = mod_point(row_start)
s_goal = mod_point(row_goal)
print('row_start,row_goal', row_start, row_goal, 's_start,s_goal', s_start, s_goal)
# 判断是否能直线到达，不能则采用路径搜索
if not cross_outpool(s_start, s_goal, baidu_map_obj.pool_cnts):
astar = AStar(s_start, s_goal, "euclidean", baidu_map_obj.pool_cnts, search_safe_pix)
try:
astar_path, visited = astar.searching()
return_pix_path = astar_path[::-1]
# simple_return_pix_path = multi_points_to_simple_points(return_pix_path)
# print('原始长度', len(return_pix_path), '简化后长度', len(simple_return_pix_path))
# _, return_gaode_lng_lat_path = baidu_map_obj.pix_to_gps(simple_return_pix_path)
if b_show:
baidu_map_obj.show_img = cv2.polylines(baidu_map_obj.show_img,
[np.array(astar_path, dtype=np.int32)], False,
(255, 0, 0),
1)
cv2.circle(baidu_map_obj.show_img, s_start, 5, [255, 0, 255], -1)
cv2.circle(baidu_map_obj.show_img, s_goal, 5, [255, 0, 255], -1)
# baidu_map_obj.show_img = cv2.drawContours(baidu_map_obj.show_img, [return_pix_path], -1,
# (0, 0, 255), 3)
cv2.imshow('scan', baidu_map_obj.show_img)
cv2.waitKey(0)
cv2.destroyAllWindows()
path_points.extend(return_pix_path)
except Exception as e:
print('error ', e)
if b_show:
cv2.circle(baidu_map_obj.show_img, s_start, 5, [0, 255, 0], -1)
cv2.circle(baidu_map_obj.show_img, s_goal, 5, [0, 0, 255], -1)
cv2.imshow('scan', baidu_map_obj.show_img)
cv2.waitKey(0)
cv2.destroyAllWindows()
path_points.append(row_goal)
# 直接可达模式
else:
path_points.append(list(row_goal))
return_pix_path = path_points
# print('原始长度', len(return_pix_path))
# return_pix_path = multi_points_to_simple_points(return_pix_path)
# print('简化后长度', len(return_pix_path))
_, return_gaode_lng_lat_path = baidu_map_obj.pix_to_gps(return_pix_path)
if b_show:
baidu_map_obj.show_img = cv2.polylines(baidu_map_obj.show_img, [np.array(path_points, dtype=np.int32)],
False, (255, 0, 0), 1)
cv2.imshow('scan', baidu_map_obj.show_img)
cv2.waitKey(0)
cv2.destroyAllWindows()
return return_gaode_lng_lat_path
def find_pool(self):
while True:
# 循环等待一定时间
time.sleep(0.1)
for ship_code in server_config.ship_code_list:
save_map_path = os.path.join(server_config.save_map_dir, 'map_%s.json' % ship_code)
# 若是用户没有点击点
if self.server_data_obj_dict.get(ship_code).mqtt_send_get_obj.pool_click_lng_lat is None or \
self.server_data_obj_dict.get(ship_code).mqtt_send_get_obj.pool_click_zoom is None:
continue
# 查找与更新湖泊id
save_img_dir = os.path.join(config.root_path, 'statics', 'imgs')
if not os.path.exists(save_img_dir):
os.mkdir(save_img_dir)
# 超过1000张图片时候删除图片
save_img_name_list = os.listdir(save_img_dir)
if len(save_img_name_list) > 1000:
for i in save_img_name_list:
print(os.path.join(save_img_dir, i))
os.remove(os.path.join(save_img_dir, i))
if self.current_map_type == baidu_map.MapType.gaode:
save_img_path = os.path.join(
save_img_dir, 'gaode_%f_%f_%i_%i.png' %
(self.server_data_obj_dict.get(ship_code).mqtt_send_get_obj.pool_click_lng_lat[0],
self.server_data_obj_dict.get(ship_code).mqtt_send_get_obj.pool_click_lng_lat[1],
self.server_data_obj_dict.get(ship_code).mqtt_send_get_obj.pool_click_zoom,
1))
elif self.current_map_type == baidu_map.MapType.tecent:
save_img_path = os.path.join(
save_img_dir, 'tecent_%f_%f_%i_%i.png' %
(self.server_data_obj_dict.get(ship_code).mqtt_send_get_obj.pool_click_lng_lat[0],
self.server_data_obj_dict.get(ship_code).mqtt_send_get_obj.pool_click_lng_lat[1],
self.server_data_obj_dict.get(ship_code).mqtt_send_get_obj.pool_click_zoom,
1))
else:
save_img_path = os.path.join(
save_img_dir, 'baidu_%f_%f_%i_%i.png' %
(self.server_data_obj_dict.get(ship_code).mqtt_send_get_obj.pool_click_lng_lat[0],
self.server_data_obj_dict.get(ship_code).mqtt_send_get_obj.pool_click_lng_lat[1],
self.server_data_obj_dict.get(ship_code).mqtt_send_get_obj.pool_click_zoom,
1))
# 创建于查找湖泊
if self.data_define_obj_dict.get(ship_code).pool_code or not os.path.exists(save_img_path):
# 创建地图对象
if os.path.exists(save_img_path) and self.baidu_map_obj_dict.get(ship_code) is not None:
continue
else:
baidu_map_obj = baidu_map.BaiduMap(
lng_lat=self.server_data_obj_dict.get(ship_code).mqtt_send_get_obj.pool_click_lng_lat,
zoom=self.server_data_obj_dict.get(ship_code).mqtt_send_get_obj.pool_click_zoom,
logger=self.map_log,
map_type=self.current_map_type)
self.baidu_map_obj_dict.update({ship_code: baidu_map_obj})
pool_cnts, (pool_cx, pool_cy) = self.baidu_map_obj_dict.get(ship_code).get_pool_pix()
# 为None表示没有找到湖泊 继续换地图找
if pool_cnts is None:
if self.current_map_type == baidu_map.MapType.gaode:
self.current_map_type = baidu_map.MapType.tecent
continue
if self.current_map_type == baidu_map.MapType.tecent:
self.current_map_type = baidu_map.MapType.baidu
continue
if self.current_map_type == baidu_map.MapType.baidu:
self.current_map_type = baidu_map.MapType.gaode
# 若返回为None表示没找到湖 定义错误代码
is_collision = 1
pool_info_data = {
'deviceId': ship_code,
'lng_lat': self.server_data_obj_dict.get(
ship_code).mqtt_send_get_obj.pool_click_lng_lat,
'is_collision': is_collision,
'zoom': self.server_data_obj_dict.get(ship_code).mqtt_send_get_obj.pool_click_zoom,
}
self.send(
method='mqtt',
topic='pool_info_%s' % ship_code,
ship_code=ship_code,
data=pool_info_data,
qos=1)
self.logger.debug({'pool_info_data': pool_info_data})
continue
# 获取湖泊轮廓与中心点经纬度位置 _位置为提供前端直接绘图使用
_, self.baidu_map_obj_dict.get(ship_code).pool_lng_lats = self.baidu_map_obj_dict.get(
ship_code).pix_to_gps(pool_cnts)
_, self.baidu_map_obj_dict.get(ship_code).pool_center_lng_lat = self.baidu_map_obj_dict.get(
ship_code).pix_to_gps([[pool_cx, pool_cy]])
self.logger.info(
{'pool_center_lng_lat': self.baidu_map_obj_dict.get(ship_code).pool_center_lng_lat})
self.baidu_map_obj_dict.get(ship_code).get_pool_name()
if self.baidu_map_obj_dict.get(ship_code).pool_name is not None:
config.pool_name = self.baidu_map_obj_dict.get(ship_code).pool_name
else:
config.pool_name = self.baidu_map_obj_dict.get(ship_code).address
self.logger.info({'config.pool_name': config.pool_name})
# 判断当前湖泊是否曾经出现，出现过则获取的ID 没出现过发送请求获取新ID
if isinstance(self.baidu_map_obj_dict.get(ship_code).pool_cnts, np.ndarray):
save_pool_cnts = self.baidu_map_obj_dict.get(ship_code).pool_cnts.tolist()
else:
save_pool_cnts = self.baidu_map_obj_dict.get(ship_code).pool_cnts
send_data = {
"longitudeLatitude": json.dumps(
self.baidu_map_obj_dict.get(ship_code).pool_center_lng_lat),
"mapData": json.dumps(
self.baidu_map_obj_dict.get(ship_code).pool_lng_lats),
"deviceId": ship_code,
"name": config.pool_name,
"pixData": json.dumps(save_pool_cnts)}
# 本地保存经纬度信息，放大1000000倍 用来只保存整数
save_pool_lng_lats = [[int(i[0] * 1000000), int(i[1] * 1000000)]
for i in self.baidu_map_obj_dict.get(ship_code).pool_lng_lats]
if not os.path.exists(save_map_path):
# 发送请求获取湖泊ID
self.logger.debug({'send_data': send_data})
try:
pool_id = self.send(
method='http',
data=send_data,
ship_code=ship_code,
url=config.http_save,
http_type='POST')
except Exception as e:
self.logger.error({'error': e})
continue
if isinstance(self.baidu_map_obj_dict.get(ship_code).pool_cnts, np.ndarray):
save_pool_cnts = self.baidu_map_obj_dict.get(ship_code).pool_cnts.tolist()
else:
save_pool_cnts = self.baidu_map_obj_dict.get(ship_code).pool_cnts
save_data = {
"mapList": [
{
"id": pool_id,
"pool_center_lng_lat": self.baidu_map_obj_dict.get(ship_code).pool_center_lng_lat,
"pool_lng_lats": save_pool_lng_lats,
"pool_cnts": save_pool_cnts}]}
self.logger.info({'pool_id': pool_id})
with open(save_map_path, 'w') as f:
json.dump(save_data, f)
else:
with open(save_map_path, 'r') as f:
local_map_data = json.load(f)
pool_id = baidu_map.is_in_contours(
(self.baidu_map_obj_dict.get(ship_code).lng_lat[0] * 1000000,
self.baidu_map_obj_dict.get(ship_code).lng_lat[1] * 1000000),
local_map_data)
print('pool_id', pool_id)
if pool_id is not None:
self.logger.info({'在本地找到湖泊 poolid': pool_id})
# 不存在获取新的id
else:
try:
self.logger.info({config.http_save: send_data})
pool_id = self.send(
method='http',
data=send_data,
ship_code=ship_code,
url=config.http_save,
http_type='POST')
except Exception as e:
# self.logger.info({config.http_save: send_data})
self.logger.error({'error': e})
self.logger.info({'新的湖泊 poolid': pool_id})
with open(save_map_path, 'w') as f:
# 以前存储键值
# local_map_data["mapList"].append({"id": pool_id,
# "longitudeLatitude": self.baidu_map_obj.pool_center_lng_lat,
# "mapData": self.baidu_map_obj.pool_lng_lat,
# "pool_cnt": pool_cnts.tolist()})
if isinstance(self.baidu_map_obj_dict.get(ship_code).pool_cnts, np.ndarray):
save_pool_cnts = self.baidu_map_obj_dict.get(ship_code).pool_cnts.tolist()
local_map_data["mapList"].append(
{
"id": pool_id,
"pool_center_lng_lat": self.baidu_map_obj_dict.get(
ship_code).pool_center_lng_lat,
"pool_lng_lats": save_pool_lng_lats,
"pool_cnts": save_pool_cnts})
json.dump(local_map_data, f)
self.data_define_obj_dict.get(ship_code).pool_code = pool_id
pool_info_data = {
'deviceId': ship_code,
'mapId': self.data_define_obj_dict.get(ship_code).pool_code,
'lng_lat': self.server_data_obj_dict.get(ship_code).mqtt_send_get_obj.pool_click_lng_lat,
# 'pool_lng_lats':self.baidu_map_obj.pool_lng_lats
}
self.send(
method='mqtt',
topic='pool_info_%s' %
(ship_code),
ship_code=ship_code,
data=pool_info_data,
qos=1)
config.write_setting(True)
config.update_base_setting()
# 在基础设置中发送湖泊名称
if self.server_data_obj_dict.get(ship_code).mqtt_send_get_obj.b_pool_click:
if self.server_data_obj_dict.get(ship_code).mqtt_send_get_obj.base_setting_data is None:
self.logger.error(
{
'base_setting_data is None': self.server_data_obj_dict.get(
ship_code).mqtt_send_get_obj.base_setting_data})
else:
self.server_data_obj_dict.get(ship_code).mqtt_send_get_obj.base_setting_data.update(
{'info_type': 3})
send_data = self.server_data_obj_dict.get(ship_code).mqtt_send_get_obj.base_setting_data
send_data.update({'pool_name': config.pool_name})
self.send(method='mqtt',
topic='base_setting_%s' % (config.ship_code),
ship_code=ship_code,
data=send_data,
qos=0)
self.logger.info({'base_setting': send_data})
self.server_data_obj_dict.get(ship_code).mqtt_send_get_obj.b_pool_click = 0