旋转平移、手动修改控制点需要手动更新棋盘数据

laser/src/main/java/com/fdkankan/indoor/base/convert/FixRouteMap.java

@@ -109,6 +109,65 @@ public class FixRouteMap {
     }
 
 
+    /**
+     * 更新route数据
+     * @param orientation dataSet.orientation
+     * @param gpsLocation dataSet.location
+     * @param controlPoint 对应场景码控制点
+     * @param path final_freespace.csv 文件目录路径
+     * @return
+     * @throws Exception
+     */
+    public static List<String> updateRouteMap_2(double orientation, double[] gpsLocation, ControlPointCalculateEntity controlPoint, String path) throws Exception {
+
+        // dataSet.location的gps坐标转本地坐标
+        double[] ageLocation = GisCoordinateUtil.transformBLToLocation(gpsLocation[0], gpsLocation[1], controlPoint);
+
+        // 新的本地坐标, 转出来应该是原点， 很接近0
+        JSONObject newLocation = new JSONObject();
+        newLocation.put("x", ageLocation[0]);
+        newLocation.put("y", ageLocation[1]);
+        newLocation.put("z", gpsLocation[2]);
+        log.info("newLocation: {}", newLocation );
+
+
+        double[] newMatrix = getTransfromMatrix(orientation, newLocation);
+
+
+        // 读取棋盘的坐标，读文件final_freespace.csv，不要读数据库，最初的坐标
+        String routePath = path + "/final_freespace.csv";
+        log.info("routePath: {}", routePath);
+        if (!cn.hutool.core.io.FileUtil.isFile(routePath)){
+            log.info("输入路径有误， path: {}", routePath);
+            throw new BaseRuntimeException(MsgCode.e3001, "final_freespace.csv路径有误，文件不存在");
+        }
+        List<String> list = FileUtil.readFileByLines2(routePath);
+        log.info("数据读取成功");
+
+        // 修改棋盘每个顶点坐标，然后再入库
+        List<String> newRoute = new ArrayList<>();
+        for (String line : list) {
+            // 将每一行的坐标值获取处理进行修改
+            String[] split = line.split(" ");
+
+            double[] oldPoint = {Double.valueOf(split[0]), Double.valueOf(split[1]), Double.valueOf(split[2])};
+            double[] newPoint =  applyMatrix4(oldPoint,newMatrix);
+
+            // 重新组装每一行数据
+            String newLine = Arrays.asList(newPoint[0], newPoint[1], newPoint[2], split[3], split[4],
+                    split[5], split[6], split[7], split[8], split[9], split[10], split[11]).toString();
+            // 去逗号
+            newLine = newLine.replaceAll(",", "");
+            newLine = newLine.replace("[", "");
+            newLine = newLine.replace("]", "");
+            newRoute.add(newLine);
+        }
+        log.info("数据解析成功");
+        return newRoute;
+
+    }
+
+
 
     /**
      * 旋转平移

laser/src/main/java/com/fdkankan/indoor/base/convert/GetRoute.java

@@ -5,16 +5,23 @@ import java.util.List;
 
 
 import cn.hutool.core.util.StrUtil;
+import cn.hutool.extra.spring.SpringUtil;
 import com.fdkankan.indoor.base.convert.kesar.AStar;
 import com.fdkankan.indoor.base.convert.kesar.Coord;
 import com.fdkankan.indoor.base.convert.kesar.MapInfo;
 import com.fdkankan.indoor.base.convert.kesar.Node;
+import com.fdkankan.indoor.base.exception.BaseRuntimeException;
+import com.fdkankan.indoor.base.util.ResponseUtil;
 import com.fdkankan.indoor.core.entity.ControlPointCalculateEntity;
 import com.fdkankan.indoor.core.entity.ControlPointEntity;
 import com.fdkankan.indoor.core.entity.dto.RouteInputDto;
 import lombok.extern.slf4j.Slf4j;
 import net.sf.json.JSONArray;
 import net.sf.json.JSONObject;
+import org.springframework.beans.factory.annotation.Autowired;
+
+import javax.servlet.ServletResponse;
+
 @Slf4j
 public class GetRoute {
 
@@ -40,6 +47,7 @@ public class GetRoute {
 	private static AStar g_AStar = new AStar();
 
 
+
 //	/**
 //	 * 读取文件，获取文件内容
 //	 *   F:\test\project\age_laser\routeMap.txt
@@ -85,11 +93,13 @@ public class GetRoute {
 	 *   F:\test\project\age_laser\final_freespace.csv
 	 *
 	 *   list 是routeMap的结果集
+	 *
+	 *   , ServletResponse response
 	 * @return
 	 */
-	public static JSONArray getRoute(List<String> list, RouteInputDto dto, ControlPointCalculateEntity controlPoint){
+	public static JSONArray getRoute(List<String> list, RouteInputDto dto, ControlPointCalculateEntity controlPoint, ServletResponse response){
 		try {
-			JSONArray maps = readMap(list, dto);
+			JSONArray maps = readMap(list, dto, response);
 			MapInfo info=new MapInfo(maps,start,end);
 			List<Node> path = g_AStar.start(info);
 			JSONArray jsonArray = convertFromPath(path, dto, controlPoint);
@@ -102,7 +112,7 @@ public class GetRoute {
 
 
 
-	private static JSONArray readMap(List<String> list, RouteInputDto dto) throws Exception {
+	private static JSONArray readMap(List<String> list, RouteInputDto dto, ServletResponse response) throws Exception {
 //		List<String> list = FileUtil.readFileByLines2(inputFilePath);
 		JSONArray maps = new JSONArray();
 
@@ -170,6 +180,35 @@ public class GetRoute {
 			}
 		}
 
+		com.alibaba.fastjson.JSONObject jsonObject = new com.alibaba.fastjson.JSONObject();
+		if(startDistance>1.08){
+
+//			jsonObject.put("msg", "Could not create route: No navigation graph node found near start.");
+//			jsonObject.put("exception", "de.navvis.iv.routing.exception.NoNavgraphNearRouteStartException");
+			log.error("路径规划错误start： {}", jsonObject);
+
+			// 直接返回Response信息
+
+			ResponseUtil.writeResponse(response, jsonObject);
+			return null;
+
+//			return {"msg":"Could not create route: No navigation graph node found near start.","exception":"de.navvis.iv.routing.exception.NoNavgraphNearRouteStartException"}
+		}
+		else if(endDistance>1.08){
+//			jsonObject.put("msg", "Could not create route: No navigation graph node found near end.");
+//			jsonObject.put("exception", "de.navvis.iv.routing.exception.NoNavgraphNearRouteStartException");
+			log.error("路径规划错误end： {}", jsonObject);
+			ResponseUtil.writeResponse(response, jsonObject);
+			return null;
+//			return {"msg":"Could not create route: No navigation graph node found near end.","exception":"de.navvis.iv.routing.exception.NoNavgraphNearRouteStartException"}
+		}
+
+//		if (!jsonObject.isEmpty()){
+//			ResponseUtil.writeResponse(response, jsonObject);
+//			return null;
+//		}
+
+
 		JSONObject virtualStart = maps.getJSONObject(minStartId);
 		JSONObject virtualEnd = maps.getJSONObject(minEndId);
 		Coord startVirtualCoord = new Coord(virtualStart.getDouble("x"), virtualStart.getDouble("y"),virtualStart.getDouble("z"));
@@ -302,7 +341,6 @@ public class GetRoute {
 				item.put("instruction", instruction);
 
 				instruction = new JSONObject();
-				instruction.put("type", "source_projection_to_navgraph");
 				item.put("instruction", instruction);
 			} else {
 				Node prenode = path.get(i-1);
@@ -313,7 +351,7 @@ public class GetRoute {
 				item.put("distance", distance);
 				if(i == path.size()-1) {
 					instruction = new JSONObject();
-					instruction.put("type", "destination_projection_to_navgraph");
+					//instruction.put("type", "destination_projection_to_navgraph");
 					item.put("instruction", instruction);
 
 //					virtualEndPosition[0] = node.coord.x;
@@ -354,246 +392,21 @@ public class GetRoute {
 	}
 
 
-//	private static JSONArray convertFromPath(List<Node> mappath, RouteInputDto dto, ControlPointEntity controlPoint) {
-//		double defaultdistance = 0.36;
-//		if(mappath == null||mappath.size() == 0) {
-//			return null;
-//		}
-//
-//		// 起始点
-//		Double startX = dto.getSource_longitude();
-//		Double startY = dto.getSource_latitude();
-//		Double startZ = dto.getSource_z();
-//
-//		// 终点
-//		Double endX = dto.getDestination_longitude();
-//		Double endY = dto.getDestination_latitude();
-//		Double endZ = dto.getDestination_z();
-//
-//		if(mappath.size()>5){
-//
-//			//从前三个和后三个路径点开始优化，防止绕来绕去
-//			Node tempNode = mappath.get(mappath.size()-2);
-//			double dis = g_AStar.calcH(tempNode.coord,new Coord(startX,startY,startZ));
-//			if(dis<defaultdistance){
-//				mappath.remove(mappath.size()-1);
-//			}
-//
-//			tempNode = mappath.get(mappath.size()-2);
-//			dis = g_AStar.calcH(tempNode.coord,new Coord(startX,startY,startZ));
-//			if(dis<defaultdistance){
-//				mappath.remove(mappath.size()-1);
-//			}
-//
-//			tempNode = mappath.get(0);
-//			dis = g_AStar.calcH(tempNode.coord,new Coord(startX,startY,startZ));
-//			if(dis<defaultdistance){
-//				mappath.remove(0);
-//			}
-//
-//			tempNode = mappath.get(0);
-//			dis = g_AStar.calcH(tempNode.coord,new Coord(startX,startY,startZ));
-//			if(dis<defaultdistance){
-//				mappath.remove(0);
-//			}
-//		}
-//
-//		List<Node> path = new ArrayList<Node>();
-////		for(int i = mappath.size()-1;i>-1;--i) {
-////			Node node = mappath.get(i);
-////			path.add(node);
-////		}
-//
-//		// 点的距离
-//		for(int i = mappath.size()-1;i>-1;) {
-//			Node node = mappath.get(i);
-//			if(mappath.size()>4){
-//				i -= 3;
-//			}
-//			else{
-//				--i;
-//			}
-//			path.add(node);
-//		}
-//
-//		JSONArray route = new JSONArray();
-//
-//		//起点不在path上，path的第一个点对应的是格子
-//		JSONObject start = new JSONObject();
-//
-//
-//		double[] startPosition = {startX,startY};
-//		startPosition = TransformGPS.convertLocationToGis(startPosition, controlPoint);
-//		start.put("longitude", startPosition[0]);
-//		start.put("latitude", startPosition[1]);
-//		start.put("z", startZ);
-//		Double[] location = new Double[3];
-//
-//
-//		location[0] = startPosition[0];
-//		location[1] = startPosition[1];
-//		location[2] = startZ;
-//
-//		start.put("location", location);
-//		start.put("distance", 0);
-//		start.put("distance_to_previous", 0);
-//		start.put("instruction", null);
-//		route.add(start);
-//
-//		Double[] virtualEndPosition = new Double[3];
-//		for(int i=0;i<path.size();++i) {
-//			Node node = path.get(i);
-//			JSONObject item = new JSONObject();
-//
-//			//转经纬度
-//			double[] position = {node.coord.x,node.coord.y};
-//			position = TransformGPS.convertLocationToGis(position, controlPoint);
-//			item.put("longitude", position[0]);
-//			item.put("latitude", position[1]);
-//
-//
-//			item.put("z", node.coord.z);
-//			location = new Double[3];
-//
-//
-//			location[0] = position[0];
-//			location[1] = position[1];
-//
-//			location[2] = node.coord.z;
-//			item.put("location", location);
-//			item.put("id", node.id);
-//
-//			JSONObject instruction = null;
-//
-//			virtualEndPosition[0] = node.coord.x;
-//			virtualEndPosition[1] = node.coord.y;
-//			virtualEndPosition[2] = node.coord.z;
-//
-//			if(i == 0) {
-//				item.put("distance", g_AStar.calcH(node.coord,new Coord(startX,startY,startZ)));
-//				item.put("distance_to_previous", g_AStar.calcH(node.coord,new Coord(startX,startY,startZ)));
-//				item.put("instruction", instruction);
-//
-//				instruction = new JSONObject();
-//				instruction.put("type", "source_projection_to_navgraph");
-//				item.put("instruction", instruction);
-//			} else {
-//				Node prenode = path.get(i-1);
-//				JSONObject preitem = route.getJSONObject(i);
-//				Double distance_to_previous = g_AStar.calcH(node.coord,prenode.coord);
-//				item.put("distance_to_previous", distance_to_previous);
-//				Double distance = preitem.getDouble("distance")+distance_to_previous;
-//				item.put("distance", distance);
-//				if(i == path.size()-1) {
-//					instruction = new JSONObject();
-//					instruction.put("type", "destination_projection_to_navgraph");
-//					item.put("instruction", instruction);
-//
-////					virtualEndPosition[0] = node.coord.x;
-////					virtualEndPosition[1] = node.coord.y;
-////					virtualEndPosition[2] = node.coord.z;
-//
-//				}
-//				else {
-//					item.put("instruction", instruction);
-//				}
-//				System.out.println("distance:"+distance_to_previous);
-//			}
-//
-//			route.add(item);
-//		}
-//
-//		JSONObject endItem = route.getJSONObject(route.size()-1);
-//		JSONObject end = new JSONObject();
-//		double[] endPosition = {endX,endY};
-//		endPosition = TransformGPS.convertLocationToGis(endPosition, controlPoint);
-//
-//		end.put("longitude", endPosition[0]);
-//		end.put("latitude", endPosition[1]);
-//		end.put("z", endZ);
-//		location = new Double[3];
-//		location[0] = endPosition[0];
-//		location[1] = endPosition[1];
-//		location[2] = endZ;
-//		end.put("location", location);
-//		Double enddistance = g_AStar.calcH(new Coord(virtualEndPosition[0],virtualEndPosition[1],virtualEndPosition[2]),new Coord(endX,endY,endZ));
-//		end.put("distance", enddistance+(float)endItem.getDouble("distance"));
-//		end.put("distance_to_previous", enddistance);
-//		end.put("instruction", null);
-//		route.add(end);
-//
-//		return route;
-//	}
 
 
-	private static JSONArray readMap() throws Exception {
-		List<String> list = FileUtil.readFileByLines2(inputFilePath);
-		JSONArray maps = new JSONArray();
-		
-		Coord _start = new Coord(startX,startY,startZ);
-		Coord _end = new Coord(endX,endY,endZ);
-
-		Double startDistance=1000.0;
-		Double endDistance = 1000.0;
-		
-		for(int i=0;i<list.size();++i) {
-			String str = list.get(i);
-			String[] strArray = str.trim().split(" ");
-			JSONObject item = new JSONObject();
-			//item.put("id", i);
-			item.put("x", strArray[0]);
-			item.put("y", strArray[1]);
-			item.put("z", strArray[2]);
-			item.put("weight", strArray[3]);
-			
-			String linkedIds = "";
-			for(int j = 4;j<strArray.length;++j) {
-				if(Integer.valueOf(strArray[j])>0) {
-					linkedIds += strArray[j]+",";
-				}
-			}
-			
-			item.put("linkedIds", linkedIds.substring(0, linkedIds.length()-1));
-			maps.add(item);
-			
-			Coord coord = new Coord(Double.valueOf(strArray[0]),Double.valueOf(strArray[1]),Double.valueOf(strArray[2]));
-			Double _startDistance = g_AStar.calcH(_start, coord);
-			if(_startDistance<startDistance) {
-				minStartId = i;
-				startDistance = _startDistance;
-			}
-			Double _endDistance = g_AStar.calcH(_end, coord);
-			if(_endDistance<endDistance) {
-				minEndId = i;
-				endDistance = _endDistance;
-			}
-		}
-		
-		JSONObject virtualStart = maps.getJSONObject(minStartId);
-		JSONObject virtualEnd = maps.getJSONObject(minEndId);
-		Coord startVirtualCoord = new Coord(virtualStart.getDouble("x"), virtualStart.getDouble("y"),virtualStart.getDouble("z"));
-		Coord endVirtualCoord = new Coord(virtualEnd.getDouble("x"), virtualEnd.getDouble("y"),virtualEnd.getDouble("z"));
-		Double startH = g_AStar.calcH(startVirtualCoord, endVirtualCoord);
-		
-		start = new Node(minStartId,0.0,startVirtualCoord, null, 0.0, startH);
-		end = new Node(minEndId,0.0,endVirtualCoord, null, 0.0, 0.0);
-		
-		return maps;
-	}
-	
 	private static JSONArray convertFromPath(List<Node> mappath) {
 		if(mappath == null||mappath.size() == 0) {
 			return null;
 		}
-		
+
 		List<Node> path = new ArrayList<Node>();
 		for(int i = mappath.size()-1;i>-1;--i) {
 			Node node = mappath.get(i);
 			path.add(node);
 		}
-		
+
 		JSONArray route = new JSONArray();
-		
+
 		//起点不在path上，path的第一个点对应的是格子
 		JSONObject start = new JSONObject();
 		start.put("longitude", startX);
@@ -608,7 +421,7 @@ public class GetRoute {
 		start.put("distance_to_previous", 0);
 		start.put("instruction", null);
 		route.add(start);
-		
+
 		for(int i=0;i<path.size();++i) {
 			Node node = path.get(i);
 			JSONObject item = new JSONObject();
@@ -621,16 +434,15 @@ public class GetRoute {
 			location[2] = node.coord.z;
 			item.put("location", location);
 			item.put("id", node.id);
-			
+
 			JSONObject instruction = null;
-			
+
 			if(i == 0) {
 				item.put("distance", g_AStar.calcH(node.coord,new Coord(startX,startY,startZ)));
 				item.put("distance_to_previous", g_AStar.calcH(node.coord,new Coord(startX,startY,startZ)));
 				item.put("instruction", instruction);
-				
+
 				instruction = new JSONObject();
-				instruction.put("type", "source_projection_to_navgraph");
 				item.put("instruction", instruction);
 			}
 			else {
@@ -642,7 +454,7 @@ public class GetRoute {
 				item.put("distance", distance);
 				if(i == path.size()-1) {
 					instruction = new JSONObject();
-					instruction.put("type", "destination_projection_to_navgraph");
+					//instruction.put("type", "destination_projection_to_navgraph");
 					item.put("instruction", instruction);
 				}
 				else {
@@ -650,10 +462,10 @@ public class GetRoute {
 				}
 				System.out.println("distance:"+distance_to_previous);
 			}
-			
+
 			route.add(item);
 		}
-		
+
 		JSONObject endItem = route.getJSONObject(route.size()-1);
 		JSONObject end = new JSONObject();
 		end.put("longitude", endX);
@@ -669,16 +481,33 @@ public class GetRoute {
 		end.put("distance_to_previous", enddistance);
 		end.put("instruction", null);
 		route.add(end);
-		
+
 		return route;
 	}
-	
-	public static void main(String[] args) throws Exception
-	{
-		JSONArray maps = readMap();
-		MapInfo info=new MapInfo(maps,start,end);
-		List<Node> path = g_AStar.start(info);
-		convertFromPath(path);
-	}
 
+//	public static void main(String[] args) throws Exception
+//	{
+//		JSONArray maps = readMap();
+//		MapInfo info=new MapInfo(maps,start,end);
+//		List<Node> path = g_AStar.start(info);
+//		convertFromPath(path);
+//	}
+
+
+	/**
+	 * 封装异常信息
+	 */
+	private void exceptionMsg(ServletResponse response) {
+		// 认证出现异常，传递错误信息msg
+		// 获取应用异常(该Cause是导致抛出此throwable(异常)的throwable(异常))
+		com.alibaba.fastjson.JSONObject jsonObject = new com.alibaba.fastjson.JSONObject();
+
+		jsonObject.put("code", 5002);
+		jsonObject.put("msg", "token invalid");
+
+//        jsonObject.put("timestamp", LocalDateTime.now());
+		// 直接返回Response信息
+		ResponseUtil.writeResponse(response, jsonObject);
+
+	}
 }

laser/src/main/java/com/fdkankan/indoor/base/util/ResponseUtil.java

@@ -0,0 +1,39 @@
+package com.fdkankan.indoor.base.util;
+
+import com.alibaba.fastjson.JSONObject;
+
+import javax.servlet.ServletOutputStream;
+import javax.servlet.ServletResponse;
+import java.io.IOException;
+import java.io.PrintWriter;
+import java.time.LocalDateTime;
+
+/**
+ * Created by owen on 2022/1/7 0007 11:31
+ * 直接响应输出
+ */
+public class ResponseUtil {
+
+
+
+    /**
+     * 无需转发，直接返回Response信息
+     *
+     */
+    public static void writeResponse(ServletResponse response, JSONObject msg) {
+        response.setCharacterEncoding("UTF-8");
+        response.setContentType("application/json; charset=utf-8");
+        ServletOutputStream outputStream = null;
+        try {
+            outputStream = response.getOutputStream();
+        } catch (IOException e) {
+            e.printStackTrace();
+        }
+        assert outputStream != null;
+        PrintWriter printWriter = new PrintWriter(outputStream, true);
+//        printWriter.write(msg.toString());//直接将json输出到页面
+        printWriter.flush();
+        printWriter.close();
+
+    }
+}

laser/src/main/java/com/fdkankan/indoor/core/controller/ControlPointController.java

@@ -29,6 +29,7 @@ public class ControlPointController {
      * 	"gpsControlCoordinate2": [113.59571900944,22.366554639591],
      * 	"id": "t1"
      * }
+     * 2022-1-7 手动修改控制点需要更新棋盘数据
      * @param param
      * @return
      */
@@ -36,8 +37,6 @@ public class ControlPointController {
     @ApiOperation(value = "新增/修改控制点")
     @PostMapping("indoor/{sceneCode}/api/controlPoint/save")
     public Result save(@PathVariable String sceneCode,  @RequestBody ControlPointDto param) {
-//        param.setId(sceneCode);
-//        String from = "web";
         return entityService.save(param, sceneCode);
     }
 

laser/src/main/java/com/fdkankan/indoor/core/controller/FilterController.java

@@ -64,6 +64,12 @@ public class FilterController {
     }
 
 
+    /**
+     * 2022-1-7 数据集校准（旋转平移是会调用此接口）
+     * @param sceneCode
+     * @param param
+     * @return
+     */
     @WebControllerLog(description = "更新热点")
     @ApiOperation(value = "更新热点")
     @PutMapping("indoor/{sceneCode}/api/images")

laser/src/main/java/com/fdkankan/indoor/core/service/RouteService.java

@@ -2,6 +2,8 @@ package com.fdkankan.indoor.core.service;
 
 import com.fdkankan.indoor.base.util.Result;
 import com.fdkankan.indoor.core.entity.ControlPointCalculateEntity;
+import com.fdkankan.indoor.core.entity.DataSetEntity;
+import com.fdkankan.indoor.core.entity.SpecialPointEntity;
 import com.fdkankan.indoor.core.entity.dto.RouteInputDto;
 import com.fdkankan.indoor.core.entity.RouteEntity;
 
@@ -18,4 +20,7 @@ public interface RouteService {
     RouteEntity findBySceneCode(String sceneCode);
 
     void updateRouteByControlPointCalculate(Double[] location, Double orientation, String sceneCode, ControlPointCalculateEntity controlPointDto, String laserDataPath);
+
+
+    void fixRouteMap(Double[] location, Double orientation, String sceneCode) throws Exception;
 }

laser/src/main/java/com/fdkankan/indoor/core/service/impl/ControlPointServiceImpl.java

@@ -7,12 +7,16 @@ import com.fdkankan.indoor.base.exception.BaseRuntimeException;
 import com.fdkankan.indoor.base.util.Result;
 import com.fdkankan.indoor.core.entity.ControlPointCalculateEntity;
 import com.fdkankan.indoor.core.entity.ControlPointEntity;
+import com.fdkankan.indoor.core.entity.DataSetEntity;
 import com.fdkankan.indoor.core.entity.InitEntity;
 import com.fdkankan.indoor.core.entity.dto.ControlPointDto;
+import com.fdkankan.indoor.core.entity.po.DataSetPo;
 import com.fdkankan.indoor.core.mapper.ControlPointCalculateMapper;
 import com.fdkankan.indoor.core.mapper.ControlPointMapper;
 import com.fdkankan.indoor.core.service.ControlPointService;
+import com.fdkankan.indoor.core.service.DataSetService;
 import com.fdkankan.indoor.core.service.InitService;
+import com.fdkankan.indoor.core.service.RouteService;
 import lombok.extern.slf4j.Slf4j;
 import org.junit.Test;
 import org.springframework.beans.BeanUtils;
@@ -36,8 +40,14 @@ public class ControlPointServiceImpl implements ControlPointService {
     @Autowired
     InitService initService;
 
+    @Autowired
+    RouteService routeService;
+
+    @Autowired
+    DataSetService dataSetService;
+
     @Override
-    public Result save(ControlPointDto param, String sceneCode) {
+    public Result save(ControlPointDto param, String sceneCode)  {
 
 
         ControlPointEntity entity = this.findById(sceneCode);
@@ -56,8 +66,24 @@ public class ControlPointServiceImpl implements ControlPointService {
 
         // 初始化step2 数据
         initService.initDataStep2(sceneCode, "web", true);
-        log.info("初始化step2 完成");
-        return Result.success();
+
+        // 修改控制点需要修改棋盘数据
+        DataSetEntity dataSetEntity = dataSetService.findById(sceneCode);
+        List<DataSetPo> data = dataSetEntity.getData();
+        DataSetPo dataSetPo = data.get(0);
+        Double[] location = dataSetPo.getLocation();
+        Double orientation = dataSetPo.getOrientation();
+        try {
+            routeService.fixRouteMap(location, orientation, sceneCode);
+            log.info("手动修改控制点更新棋盘数据完成");
+
+            log.info("初始化step2 完成");
+            return Result.success();
+        } catch (Exception e) {
+            log.error("手动修改控制点更新棋盘数据异常");
+            e.printStackTrace();
+        }
+        return null;
     }
 
     @Override

laser/src/main/java/com/fdkankan/indoor/core/service/impl/DataSetServiceImpl.java

@@ -60,6 +60,9 @@ public class DataSetServiceImpl extends IBaseServiceImpl implements DataSetServi
     @Autowired
     TiledMapService tiledMapService;
 
+    @Autowired
+    SpecialPointService specialPointService;
+
 
     @Override
     public void save(DataSetEntity entity) {
@@ -77,6 +80,77 @@ public class DataSetServiceImpl extends IBaseServiceImpl implements DataSetServi
 
 
 
+//    @Override
+//    public Result update(String sceneCode, List<DataSetPo> param) {
+//
+//        DataSetEntity entity = findById(sceneCode);
+//        List<DataSetPo> data = entity.getData();
+//        log.info("db数据量： {}", data.size());
+//
+//
+//        Integer maxId = 0;
+//        if (data.size() != 0){
+//            // 将热点id提取出来做过滤条件
+//            List<Integer> argIds = param.stream().map(DataSetPo::getId).collect(Collectors.toList());
+//            log.info("参数id数量： {}", argIds.size());
+//
+//            data = data.stream().filter(p -> !argIds.contains(p.getId())).collect(Collectors.toList());
+//            log.info("不需要更新的数据量： {}", data.size());
+//
+//            // 过滤后数据为空
+//            if (data.size() != 0){
+//                // 获取最大id
+//                Optional<DataSetPo> max = data.stream().max(Comparator.comparingInt(DataSetPo::getId));
+//                maxId = max.get().getId();
+//            }
+//
+//        }
+//
+//        // 合并两个流, 处理id
+//
+//        // 处理的id
+//        List<Integer> newIds = new ArrayList<>();
+//        int i = 1;
+//        for (DataSetPo dto : param) {
+//            if (dto.getId() == null) {
+//                maxId ++;
+//                log.info("处理后maxId: {}", maxId);
+//                dto.setId(maxId);
+//            }
+//
+//            newIds.add(dto.getId());
+//            data.add(dto);
+//            i ++;
+//        }
+//
+//        log.info("需要更新db数据量： {}", data.size());
+//
+//        // 更新db
+//        entity.setUpdateTime(LocalDateTime.now());
+//        entity.setData(data);
+//        dataSetMapper.save(entity);
+//        log.info("更新dataSet数据完成");
+//
+//
+//        // 2021-09-16 处理比较高度与是否在响应的楼层， 不在情况下修改dateSet , filter表响应的siteModelId
+//        executeDateSetBySetModelIds(sceneCode);
+//
+//        return Result.success(getDataBySceneCodeAndDataSetId(sceneCode, newIds));
+//
+//        // 2021-08-31 不需处理更新RouteMap并存入数据库
+////        try {
+////            fixRouteMap(location, orientation, sceneCode);
+//////            return Result.success(auth(sceneCode).getData());
+////            // 返回修改的数组
+////            return Result.success(getDataBySceneCodeAndDataSetId(sceneCode, newIds));
+////        } catch (Exception e) {
+////            e.printStackTrace();
+////        }
+//
+////        return null;
+//    }
+
+
     @Override
     public Result update(String sceneCode, List<DataSetPo> param) {
 
@@ -132,19 +206,22 @@ public class DataSetServiceImpl extends IBaseServiceImpl implements DataSetServi
         // 2021-09-16 处理比较高度与是否在响应的楼层， 不在情况下修改dateSet , filter表响应的siteModelId
         executeDateSetBySetModelIds(sceneCode);
 
-        return Result.success(getDataBySceneCodeAndDataSetId(sceneCode, newIds));
+//        return Result.success(getDataBySceneCodeAndDataSetId(sceneCode, newIds));
 
-        // 2021-08-31 不需处理更新RouteMap并存入数据库
-//        try {
-//            fixRouteMap(location, orientation, sceneCode);
-////            return Result.success(auth(sceneCode).getData());
-//            // 返回修改的数组
-//            return Result.success(getDataBySceneCodeAndDataSetId(sceneCode, newIds));
-//        } catch (Exception e) {
-//            e.printStackTrace();
-//        }
-//
-//        return null;
+        // 2022-1-7 旋转平移重算棋盘数据
+        try {
+            DataSetPo dataSetPo = data.get(0);
+            Double[] location = dataSetPo.getLocation();
+            Double orientation = dataSetPo.getOrientation();
+
+            routeService.fixRouteMap(location, orientation, sceneCode);
+            // 返回修改的数组
+            return Result.success(getDataBySceneCodeAndDataSetId(sceneCode, newIds));
+        } catch (Exception e) {
+            e.printStackTrace();
+        }
+
+        return null;
     }
 
 
@@ -373,28 +450,28 @@ public class DataSetServiceImpl extends IBaseServiceImpl implements DataSetServi
 
     }
 
-    /**
-     *
-     * @param location
-     * @param orientation 改数组了 2021.08.26
-     * @param sceneCode
-     * @throws Exception
-     */
+
+//    /**
+//     * 2022-1-7 重算棋盘数据
+//     * @param location dataSet.location
+//     * @param orientation dataSet.orientation
+//     * @param sceneCode
+//     * @throws Exception
+//     */
 //    private void fixRouteMap(Double[] location, Double orientation, String sceneCode) throws Exception {
 //        String initKey = TypeConstant.REDIS_INIT_PATH + sceneCode;
 //        log.info("initKey: {}", initKey);
 //        String initPath = redisPath(sceneCode);
 //
-//        initPath = initPath + "/laserData";
 //        log.info("initPath: {}", initPath);
 //
-//        ControlPointEntity controlPointEntity = controlPointService.findById(sceneCode);
-//        if (controlPointEntity == null) {
-//            throw new BaseRuntimeException("控制点数据不存： {}" +sceneCode);
+//        ControlPointCalculateEntity calculateEntity = controlPointCalculateService.findById(sceneCode);
+//        if (calculateEntity == null) {
+//            throw new BaseRuntimeException("控制点计算数据不存： {}" +sceneCode);
 //        }
 //        log.info("有控制点");
 //        double[] gpsLocation = {location[0], location[1], location[2]};
-//        List<String> routeMap = FixRouteMap.updateRouteMap(orientation, gpsLocation, controlPointEntity, initPath);
+//        List<String> routeMap = FixRouteMap.updateRouteMap(orientation, gpsLocation, calculateEntity, initPath);
 //
 //        RouteEntity routeEntity = routeService.findBySceneCode(sceneCode);
 //        routeEntity.setData(routeMap);

laser/src/main/java/com/fdkankan/indoor/core/service/impl/RouteServiceImpl.java

@@ -3,17 +3,18 @@ package com.fdkankan.indoor.core.service.impl;
 import cn.hutool.core.collection.ListUtil;
 import cn.hutool.core.date.DateUtil;
 import cn.hutool.core.io.FileUtil;
+import cn.hutool.core.util.ObjectUtil;
 import com.fdkankan.indoor.base.constant.ConfigConstant;
 import com.fdkankan.indoor.base.constant.MsgCode;
+import com.fdkankan.indoor.base.constant.TypeConstant;
 import com.fdkankan.indoor.base.convert.FixRouteMap;
 import com.fdkankan.indoor.base.convert.GetRoute;
 import com.fdkankan.indoor.base.convert.GisCoordinateUtil;
+import com.fdkankan.indoor.base.convert.MatrixUtil;
 import com.fdkankan.indoor.base.exception.BaseRuntimeException;
 import com.fdkankan.indoor.base.util.Result;
-import com.fdkankan.indoor.core.entity.ControlPointCalculateEntity;
-import com.fdkankan.indoor.core.entity.ControlPointEntity;
+import com.fdkankan.indoor.core.entity.*;
 import com.fdkankan.indoor.core.entity.dto.RouteInputDto;
-import com.fdkankan.indoor.core.entity.RouteEntity;
 import com.fdkankan.indoor.core.mapper.RouteMapper;
 import com.fdkankan.indoor.core.service.ControlPointCalculateService;
 import com.fdkankan.indoor.core.service.ControlPointService;
@@ -24,6 +25,7 @@ import net.sf.json.JSONObject;
 import org.springframework.beans.factory.annotation.Autowired;
 import org.springframework.stereotype.Service;
 
+import javax.servlet.ServletResponse;
 import java.time.LocalDateTime;
 import java.util.ArrayList;
 import java.util.HashMap;
@@ -49,12 +51,49 @@ public class RouteServiceImpl extends IBaseServiceImpl implements RouteService {
     @Autowired
     ControlPointCalculateService controlPointCalculateService;
 
+
+    @Autowired
+    ServletResponse response;
+
+
+
+    /**
+     * 2022-1-7 重算棋盘数据
+     * @param location dataSet.location
+     * @param orientation dataSet.orientation
+     * @param sceneCode
+     * @throws Exception
+     */
+    public void fixRouteMap(Double[] location, Double orientation, String sceneCode) throws Exception {
+        String initKey = TypeConstant.REDIS_INIT_PATH + sceneCode;
+        log.info("initKey: {}", initKey);
+        String initPath = redisPath(sceneCode);
+
+        log.info("initPath: {}", initPath);
+
+        ControlPointCalculateEntity calculateEntity = controlPointCalculateService.findById(sceneCode);
+        if (calculateEntity == null) {
+            throw new BaseRuntimeException("控制点计算数据不存： {}" +sceneCode);
+        }
+        log.info("有控制点");
+        double[] gpsLocation = {location[0], location[1], location[2]};
+        List<String> routeMap = FixRouteMap.updateRouteMap(orientation, gpsLocation, calculateEntity, initPath);
+
+        RouteEntity routeEntity = this.findBySceneCode(sceneCode);
+        routeEntity.setData(routeMap);
+        routeEntity.setUpdateTime(LocalDateTime.now());
+        this.save(routeEntity);
+        log.info("更新Route数据完成");
+
+    }
+
     @Override
     public void save(RouteEntity entity) {
         entity.setUpdateTime(LocalDateTime.now());
         entityMapper.save(entity);
     }
 
+
     @Override
     public Result getRoute(String sceneCode, RouteInputDto param) {
         List<String> data = getDataBySceneCode(sceneCode);
@@ -72,7 +111,7 @@ public class RouteServiceImpl extends IBaseServiceImpl implements RouteService {
 
         // route数据：头尾数据是没有id值的，其他都有
 //        JSONArray route = GetRoute.getRoute(data, param, controlPointEntity);
-        JSONArray route = GetRoute.getRoute(data, locationDto, controlPointEntity);
+        JSONArray route = GetRoute.getRoute(data, locationDto, controlPointEntity, response);
 
 
         // 2021-08-12 测试用