diff --git a/.gitignore b/.gitignore
index 9e20bc1..f42a9cb 100644
--- a/.gitignore
+++ b/.gitignore
@@ -2,3 +2,4 @@
 **/target/**
 logs/
 *.iml
+*.exe

diff --git a/.gitignore b/.gitignore
index 9e20bc1..f42a9cb 100644
--- a/.gitignore
+++ b/.gitignore
@@ -2,3 +2,4 @@
 **/target/**
 logs/
 *.iml
+*.exe
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java b/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
index c7b5d28..a849528 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
@@ -28,6 +28,7 @@
     String KEY_CUR_TASK_ID = "CUR_TASK_ID_";
     String KEY_CUR_TASK_INIT = "CUR_TASK_INIT_";
 
+    String KEY_COST_MAP = "KEY_COST_MAP";
     /**
      * PSD数据ID
      */
@@ -41,6 +42,7 @@
      */
     String KEY_CUR_XY_POINT = "KEY_CUR_XY_POINT_";
     String KEY_CUR_ESTIMATE_GRID = "KEY_CUR_ESTIMATE_GRID_";
+    String KEY_PLANING_FLAG = "KEY_PLANING_FLAG_";
     /**
      * 机器人巡点栅格
      */
@@ -55,6 +57,7 @@
      * 探测器配置参数缓存
      */
     String KEY_CONFIG_PARAMS_ID = "KEY_CONFIG_PARAMS_ID_";
+    String KEY_CONFIG_COSTMAP_RESOLUTION_SET = "KEY_CONFIG_COSTMAP_RESOLUTION_SET_";
 
     /**
      * 暂停状态缓存信息

diff --git a/.gitignore b/.gitignore
index 9e20bc1..f42a9cb 100644
--- a/.gitignore
+++ b/.gitignore
@@ -2,3 +2,4 @@
 **/target/**
 logs/
 *.iml
+*.exe
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java b/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
index c7b5d28..a849528 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
@@ -28,6 +28,7 @@
     String KEY_CUR_TASK_ID = "CUR_TASK_ID_";
     String KEY_CUR_TASK_INIT = "CUR_TASK_INIT_";
 
+    String KEY_COST_MAP = "KEY_COST_MAP";
     /**
      * PSD数据ID
      */
@@ -41,6 +42,7 @@
      */
     String KEY_CUR_XY_POINT = "KEY_CUR_XY_POINT_";
     String KEY_CUR_ESTIMATE_GRID = "KEY_CUR_ESTIMATE_GRID_";
+    String KEY_PLANING_FLAG = "KEY_PLANING_FLAG_";
     /**
      * 机器人巡点栅格
      */
@@ -55,6 +57,7 @@
      * 探测器配置参数缓存
      */
     String KEY_CONFIG_PARAMS_ID = "KEY_CONFIG_PARAMS_ID_";
+    String KEY_CONFIG_COSTMAP_RESOLUTION_SET = "KEY_CONFIG_COSTMAP_RESOLUTION_SET_";
 
     /**
      * 暂停状态缓存信息
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
index c4c905d..3bb475f 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
@@ -1,9 +1,14 @@
 package com.casic.missiles.modular.robot.controller;
 
+import cn.hutool.core.lang.Assert;
 import cn.hutool.core.util.StrUtil;
 import com.casic.missiles.core.base.controller.BaseController;
+import com.casic.missiles.core.cache.CacheUtil;
 import com.casic.missiles.core.util.SpringContextHolder;
+import com.casic.missiles.model.exception.RequestValidException;
+import com.casic.missiles.model.response.ResponseData;
 import com.casic.missiles.model.response.dto.ResponseDataDTO;
+import com.casic.missiles.modular.constants.RobotDictConstants;
 import com.casic.missiles.modular.robot.IBaseRobotService;
 import com.casic.missiles.modular.robot.dto.RobotInfoSetDTO;
 import com.casic.missiles.modular.robot.model.RobotInfo;
@@ -19,10 +24,16 @@
 import io.swagger.annotations.ApiOperation;
 import io.swagger.annotations.ApiParam;
 import lombok.SneakyThrows;
+import lombok.extern.slf4j.Slf4j;
+import org.apache.ibatis.annotations.Param;
+import org.springframework.beans.factory.annotation.Value;
 import org.springframework.web.bind.annotation.*;
+import org.springframework.web.multipart.MultipartFile;
 
 import javax.servlet.http.HttpServletResponse;
+import java.io.File;
 import java.util.List;
+import java.util.Optional;
 
 /**
  * 机器人操作类
@@ -30,6 +41,7 @@
  * @author lwh
  * @date 2023-10-30
  */
+@Slf4j
 @Api(tags = "机器人控制接口")
 @RestController
 @RequestMapping("/robot/opt")
@@ -61,6 +73,28 @@
         return baseRobotService.execCmdHandler(request, InstructCodeEnums.CHANGE_PCD);
     }
 
+    @Value("${casic.file.uploadPath}")
+    private String filePath;
+
+    @SneakyThrows
+    @ApiOperation(value = "PCD上传接口")
+    @PostMapping(value = "/uploadPcd")
+    public ResponseData uploadPcd(@RequestParam("file") MultipartFile file, @ApiParam("机器人ID,url拼接传参") String robotId) {
+        Assert.isFalse(StrUtil.isEmpty(robotId), () -> new RequestValidException("机器ID不能为空", Optional.of(new Throwable().getStackTrace()[2])));
+        RobotInfo info = robotInfoService.getById(robotId);
+        if (file != null && !file.isEmpty()) {
+            File pcdFile = new File(filePath + "finalCloud_" + info.getId() + ".pcd");
+            try {
+                file.transferTo(pcdFile);
+            } catch (Exception e) {
+                log.error(e.getMessage());
+            }
+            return ResponseData.success();
+        } else {
+            return ResponseData.error("附件上传失败!");
+        }
+    }
+
     @SneakyThrows
     @ApiOperation(value = "PCD下载接口")
     @PostMapping(value = "/pcdDownload")
@@ -136,6 +170,10 @@
     @ApiOperation(value = "启动或关闭循迹功能", notes = "返回 1成功 0失败  wait 启动时是否等待，0代表循迹启动即运动，不等待；1代表循迹启动原地等待。若无此字段，默认循迹启动不等待直接运动")
     @PostMapping(value = "/startTrack")
     public ResponseDataDTO<Integer> startTrack(@RequestBody StartTrackRequest request) {
+        request.setIsTask(1);
+        if (request.getStepLen() == null) {
+            request.setStepLen(2);
+        }
         baseRobotService.startTrack(request);
         return ResponseDataDTO.success();
     }
@@ -156,6 +194,13 @@
         MessageRequestDTO<CrlNavRequest> messageDTO = new MessageRequestDTO();
         messageDTO.setMsg(request);
         ResponseDataDTO<Integer> list = baseRobotService.getRobotRouteOptService().robotCmd(info, InstructCodeEnums.CTRL_NAV, messageDTO);
+        if (request.getAction() == 1) {
+            CostMapResolutionRequest costMap = new CostMapResolutionRequest();
+            costMap.setResolution(info.getResolution().doubleValue());
+            request.setRobotId(request.getRobotId());
+            //是否下发分辨率
+            CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CONFIG_COSTMAP_RESOLUTION_SET + request.getRobotId(), costMap);
+        }
         return list;
     }
 

diff --git a/.gitignore b/.gitignore
index 9e20bc1..f42a9cb 100644
--- a/.gitignore
+++ b/.gitignore
@@ -2,3 +2,4 @@
 **/target/**
 logs/
 *.iml
+*.exe
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java b/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
index c7b5d28..a849528 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
@@ -28,6 +28,7 @@
     String KEY_CUR_TASK_ID = "CUR_TASK_ID_";
     String KEY_CUR_TASK_INIT = "CUR_TASK_INIT_";
 
+    String KEY_COST_MAP = "KEY_COST_MAP";
     /**
      * PSD数据ID
      */
@@ -41,6 +42,7 @@
      */
     String KEY_CUR_XY_POINT = "KEY_CUR_XY_POINT_";
     String KEY_CUR_ESTIMATE_GRID = "KEY_CUR_ESTIMATE_GRID_";
+    String KEY_PLANING_FLAG = "KEY_PLANING_FLAG_";
     /**
      * 机器人巡点栅格
      */
@@ -55,6 +57,7 @@
      * 探测器配置参数缓存
      */
     String KEY_CONFIG_PARAMS_ID = "KEY_CONFIG_PARAMS_ID_";
+    String KEY_CONFIG_COSTMAP_RESOLUTION_SET = "KEY_CONFIG_COSTMAP_RESOLUTION_SET_";
 
     /**
      * 暂停状态缓存信息
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
index c4c905d..3bb475f 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
@@ -1,9 +1,14 @@
 package com.casic.missiles.modular.robot.controller;
 
+import cn.hutool.core.lang.Assert;
 import cn.hutool.core.util.StrUtil;
 import com.casic.missiles.core.base.controller.BaseController;
+import com.casic.missiles.core.cache.CacheUtil;
 import com.casic.missiles.core.util.SpringContextHolder;
+import com.casic.missiles.model.exception.RequestValidException;
+import com.casic.missiles.model.response.ResponseData;
 import com.casic.missiles.model.response.dto.ResponseDataDTO;
+import com.casic.missiles.modular.constants.RobotDictConstants;
 import com.casic.missiles.modular.robot.IBaseRobotService;
 import com.casic.missiles.modular.robot.dto.RobotInfoSetDTO;
 import com.casic.missiles.modular.robot.model.RobotInfo;
@@ -19,10 +24,16 @@
 import io.swagger.annotations.ApiOperation;
 import io.swagger.annotations.ApiParam;
 import lombok.SneakyThrows;
+import lombok.extern.slf4j.Slf4j;
+import org.apache.ibatis.annotations.Param;
+import org.springframework.beans.factory.annotation.Value;
 import org.springframework.web.bind.annotation.*;
+import org.springframework.web.multipart.MultipartFile;
 
 import javax.servlet.http.HttpServletResponse;
+import java.io.File;
 import java.util.List;
+import java.util.Optional;
 
 /**
  * 机器人操作类
@@ -30,6 +41,7 @@
  * @author lwh
  * @date 2023-10-30
  */
+@Slf4j
 @Api(tags = "机器人控制接口")
 @RestController
 @RequestMapping("/robot/opt")
@@ -61,6 +73,28 @@
         return baseRobotService.execCmdHandler(request, InstructCodeEnums.CHANGE_PCD);
     }
 
+    @Value("${casic.file.uploadPath}")
+    private String filePath;
+
+    @SneakyThrows
+    @ApiOperation(value = "PCD上传接口")
+    @PostMapping(value = "/uploadPcd")
+    public ResponseData uploadPcd(@RequestParam("file") MultipartFile file, @ApiParam("机器人ID,url拼接传参") String robotId) {
+        Assert.isFalse(StrUtil.isEmpty(robotId), () -> new RequestValidException("机器ID不能为空", Optional.of(new Throwable().getStackTrace()[2])));
+        RobotInfo info = robotInfoService.getById(robotId);
+        if (file != null && !file.isEmpty()) {
+            File pcdFile = new File(filePath + "finalCloud_" + info.getId() + ".pcd");
+            try {
+                file.transferTo(pcdFile);
+            } catch (Exception e) {
+                log.error(e.getMessage());
+            }
+            return ResponseData.success();
+        } else {
+            return ResponseData.error("附件上传失败!");
+        }
+    }
+
     @SneakyThrows
     @ApiOperation(value = "PCD下载接口")
     @PostMapping(value = "/pcdDownload")
@@ -136,6 +170,10 @@
     @ApiOperation(value = "启动或关闭循迹功能", notes = "返回 1成功 0失败  wait 启动时是否等待，0代表循迹启动即运动，不等待；1代表循迹启动原地等待。若无此字段，默认循迹启动不等待直接运动")
     @PostMapping(value = "/startTrack")
     public ResponseDataDTO<Integer> startTrack(@RequestBody StartTrackRequest request) {
+        request.setIsTask(1);
+        if (request.getStepLen() == null) {
+            request.setStepLen(2);
+        }
         baseRobotService.startTrack(request);
         return ResponseDataDTO.success();
     }
@@ -156,6 +194,13 @@
         MessageRequestDTO<CrlNavRequest> messageDTO = new MessageRequestDTO();
         messageDTO.setMsg(request);
         ResponseDataDTO<Integer> list = baseRobotService.getRobotRouteOptService().robotCmd(info, InstructCodeEnums.CTRL_NAV, messageDTO);
+        if (request.getAction() == 1) {
+            CostMapResolutionRequest costMap = new CostMapResolutionRequest();
+            costMap.setResolution(info.getResolution().doubleValue());
+            request.setRobotId(request.getRobotId());
+            //是否下发分辨率
+            CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CONFIG_COSTMAP_RESOLUTION_SET + request.getRobotId(), costMap);
+        }
         return list;
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java
new file mode 100644
index 0000000..5317651
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java
@@ -0,0 +1,86 @@
+package com.casic.missiles.modular.robot.controller;
+
+import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
+import com.baomidou.mybatisplus.extension.plugins.pagination.Page;
+import com.casic.missiles.core.base.controller.BaseController;
+import com.casic.missiles.core.page.PageFactory;
+import com.casic.missiles.core.page.PageInfoBT;
+import com.casic.missiles.model.form.IdForms;
+import com.casic.missiles.model.response.dto.ResponseDataDTO;
+import com.casic.missiles.modular.robot.model.TaskGridInfo;
+import com.casic.missiles.modular.robot.service.ITaskGridInfoService;
+import com.casic.missiles.modular.robot.vo.TaskGridInfoVO;
+import io.swagger.annotations.Api;
+import io.swagger.annotations.ApiOperation;
+import org.springframework.web.bind.annotation.*;
+
+import java.util.List;
+
+/**
+ * 任务网格信息Controller
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+@Api(tags = "任务网格信息")
+@RestController
+@RequestMapping("/robot/taskGridInfo")
+public class TaskGridInfoController extends BaseController {
+
+    private final ITaskGridInfoService taskGridInfoService;
+
+    public TaskGridInfoController(ITaskGridInfoService taskGridInfoService) {
+        this.taskGridInfoService = taskGridInfoService;
+    }
+
+    @ApiOperation(value = "列表查询")
+    @GetMapping(value = "/list")
+    public ResponseDataDTO<List<TaskGridInfo>> list(TaskGridInfoVO taskGridInfoVO) {
+        QueryWrapper<TaskGridInfo> query = taskGridInfoVO.genQuery();
+        return ResponseDataDTO.success(taskGridInfoService.list(query));
+    }
+
+    @ApiOperation(value = "分页查询")
+    @GetMapping(value = "/listPage")
+    public ResponseDataDTO<PageInfoBT<TaskGridInfo>> listPage(TaskGridInfoVO taskGridInfoVO) {
+        Page<TaskGridInfo> page = PageFactory.defaultPage();
+        QueryWrapper<TaskGridInfo> query = taskGridInfoVO.genQuery();
+        page = taskGridInfoService.page(page, query);
+        return ResponseDataDTO.success(super.packForBT(page));
+    }
+
+
+    @ApiOperation(value = "新增接口")
+    @PostMapping(value = "/add")
+    public ResponseDataDTO add(@RequestBody TaskGridInfo taskGridInfo) {
+        taskGridInfoService.save(taskGridInfo);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "修改接口")
+    @PostMapping(value = "/update")
+    public ResponseDataDTO update(@RequestBody TaskGridInfo taskGridInfo) {
+        taskGridInfoService.updateById(taskGridInfo);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "删除接口")
+    @GetMapping(value = "/delete")
+    public ResponseDataDTO delete(String id) {
+        taskGridInfoService.removeById(id);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation("批量删除")
+    @PostMapping("/batchDelete")
+    public ResponseDataDTO batchDelete(@RequestBody IdForms ids) {
+        taskGridInfoService.removeByIds(ids.getIds());
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "详情查询")
+    @GetMapping(value = "/detail")
+    public ResponseDataDTO<TaskGridInfo> detail(String id) {
+        return ResponseDataDTO.success(taskGridInfoService.getById(id));
+    }
+}

diff --git a/.gitignore b/.gitignore
index 9e20bc1..f42a9cb 100644
--- a/.gitignore
+++ b/.gitignore
@@ -2,3 +2,4 @@
 **/target/**
 logs/
 *.iml
+*.exe
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java b/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
index c7b5d28..a849528 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
@@ -28,6 +28,7 @@
     String KEY_CUR_TASK_ID = "CUR_TASK_ID_";
     String KEY_CUR_TASK_INIT = "CUR_TASK_INIT_";
 
+    String KEY_COST_MAP = "KEY_COST_MAP";
     /**
      * PSD数据ID
      */
@@ -41,6 +42,7 @@
      */
     String KEY_CUR_XY_POINT = "KEY_CUR_XY_POINT_";
     String KEY_CUR_ESTIMATE_GRID = "KEY_CUR_ESTIMATE_GRID_";
+    String KEY_PLANING_FLAG = "KEY_PLANING_FLAG_";
     /**
      * 机器人巡点栅格
      */
@@ -55,6 +57,7 @@
      * 探测器配置参数缓存
      */
     String KEY_CONFIG_PARAMS_ID = "KEY_CONFIG_PARAMS_ID_";
+    String KEY_CONFIG_COSTMAP_RESOLUTION_SET = "KEY_CONFIG_COSTMAP_RESOLUTION_SET_";
 
     /**
      * 暂停状态缓存信息
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
index c4c905d..3bb475f 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
@@ -1,9 +1,14 @@
 package com.casic.missiles.modular.robot.controller;
 
+import cn.hutool.core.lang.Assert;
 import cn.hutool.core.util.StrUtil;
 import com.casic.missiles.core.base.controller.BaseController;
+import com.casic.missiles.core.cache.CacheUtil;
 import com.casic.missiles.core.util.SpringContextHolder;
+import com.casic.missiles.model.exception.RequestValidException;
+import com.casic.missiles.model.response.ResponseData;
 import com.casic.missiles.model.response.dto.ResponseDataDTO;
+import com.casic.missiles.modular.constants.RobotDictConstants;
 import com.casic.missiles.modular.robot.IBaseRobotService;
 import com.casic.missiles.modular.robot.dto.RobotInfoSetDTO;
 import com.casic.missiles.modular.robot.model.RobotInfo;
@@ -19,10 +24,16 @@
 import io.swagger.annotations.ApiOperation;
 import io.swagger.annotations.ApiParam;
 import lombok.SneakyThrows;
+import lombok.extern.slf4j.Slf4j;
+import org.apache.ibatis.annotations.Param;
+import org.springframework.beans.factory.annotation.Value;
 import org.springframework.web.bind.annotation.*;
+import org.springframework.web.multipart.MultipartFile;
 
 import javax.servlet.http.HttpServletResponse;
+import java.io.File;
 import java.util.List;
+import java.util.Optional;
 
 /**
  * 机器人操作类
@@ -30,6 +41,7 @@
  * @author lwh
  * @date 2023-10-30
  */
+@Slf4j
 @Api(tags = "机器人控制接口")
 @RestController
 @RequestMapping("/robot/opt")
@@ -61,6 +73,28 @@
         return baseRobotService.execCmdHandler(request, InstructCodeEnums.CHANGE_PCD);
     }
 
+    @Value("${casic.file.uploadPath}")
+    private String filePath;
+
+    @SneakyThrows
+    @ApiOperation(value = "PCD上传接口")
+    @PostMapping(value = "/uploadPcd")
+    public ResponseData uploadPcd(@RequestParam("file") MultipartFile file, @ApiParam("机器人ID,url拼接传参") String robotId) {
+        Assert.isFalse(StrUtil.isEmpty(robotId), () -> new RequestValidException("机器ID不能为空", Optional.of(new Throwable().getStackTrace()[2])));
+        RobotInfo info = robotInfoService.getById(robotId);
+        if (file != null && !file.isEmpty()) {
+            File pcdFile = new File(filePath + "finalCloud_" + info.getId() + ".pcd");
+            try {
+                file.transferTo(pcdFile);
+            } catch (Exception e) {
+                log.error(e.getMessage());
+            }
+            return ResponseData.success();
+        } else {
+            return ResponseData.error("附件上传失败!");
+        }
+    }
+
     @SneakyThrows
     @ApiOperation(value = "PCD下载接口")
     @PostMapping(value = "/pcdDownload")
@@ -136,6 +170,10 @@
     @ApiOperation(value = "启动或关闭循迹功能", notes = "返回 1成功 0失败  wait 启动时是否等待，0代表循迹启动即运动，不等待；1代表循迹启动原地等待。若无此字段，默认循迹启动不等待直接运动")
     @PostMapping(value = "/startTrack")
     public ResponseDataDTO<Integer> startTrack(@RequestBody StartTrackRequest request) {
+        request.setIsTask(1);
+        if (request.getStepLen() == null) {
+            request.setStepLen(2);
+        }
         baseRobotService.startTrack(request);
         return ResponseDataDTO.success();
     }
@@ -156,6 +194,13 @@
         MessageRequestDTO<CrlNavRequest> messageDTO = new MessageRequestDTO();
         messageDTO.setMsg(request);
         ResponseDataDTO<Integer> list = baseRobotService.getRobotRouteOptService().robotCmd(info, InstructCodeEnums.CTRL_NAV, messageDTO);
+        if (request.getAction() == 1) {
+            CostMapResolutionRequest costMap = new CostMapResolutionRequest();
+            costMap.setResolution(info.getResolution().doubleValue());
+            request.setRobotId(request.getRobotId());
+            //是否下发分辨率
+            CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CONFIG_COSTMAP_RESOLUTION_SET + request.getRobotId(), costMap);
+        }
         return list;
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java
new file mode 100644
index 0000000..5317651
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java
@@ -0,0 +1,86 @@
+package com.casic.missiles.modular.robot.controller;
+
+import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
+import com.baomidou.mybatisplus.extension.plugins.pagination.Page;
+import com.casic.missiles.core.base.controller.BaseController;
+import com.casic.missiles.core.page.PageFactory;
+import com.casic.missiles.core.page.PageInfoBT;
+import com.casic.missiles.model.form.IdForms;
+import com.casic.missiles.model.response.dto.ResponseDataDTO;
+import com.casic.missiles.modular.robot.model.TaskGridInfo;
+import com.casic.missiles.modular.robot.service.ITaskGridInfoService;
+import com.casic.missiles.modular.robot.vo.TaskGridInfoVO;
+import io.swagger.annotations.Api;
+import io.swagger.annotations.ApiOperation;
+import org.springframework.web.bind.annotation.*;
+
+import java.util.List;
+
+/**
+ * 任务网格信息Controller
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+@Api(tags = "任务网格信息")
+@RestController
+@RequestMapping("/robot/taskGridInfo")
+public class TaskGridInfoController extends BaseController {
+
+    private final ITaskGridInfoService taskGridInfoService;
+
+    public TaskGridInfoController(ITaskGridInfoService taskGridInfoService) {
+        this.taskGridInfoService = taskGridInfoService;
+    }
+
+    @ApiOperation(value = "列表查询")
+    @GetMapping(value = "/list")
+    public ResponseDataDTO<List<TaskGridInfo>> list(TaskGridInfoVO taskGridInfoVO) {
+        QueryWrapper<TaskGridInfo> query = taskGridInfoVO.genQuery();
+        return ResponseDataDTO.success(taskGridInfoService.list(query));
+    }
+
+    @ApiOperation(value = "分页查询")
+    @GetMapping(value = "/listPage")
+    public ResponseDataDTO<PageInfoBT<TaskGridInfo>> listPage(TaskGridInfoVO taskGridInfoVO) {
+        Page<TaskGridInfo> page = PageFactory.defaultPage();
+        QueryWrapper<TaskGridInfo> query = taskGridInfoVO.genQuery();
+        page = taskGridInfoService.page(page, query);
+        return ResponseDataDTO.success(super.packForBT(page));
+    }
+
+
+    @ApiOperation(value = "新增接口")
+    @PostMapping(value = "/add")
+    public ResponseDataDTO add(@RequestBody TaskGridInfo taskGridInfo) {
+        taskGridInfoService.save(taskGridInfo);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "修改接口")
+    @PostMapping(value = "/update")
+    public ResponseDataDTO update(@RequestBody TaskGridInfo taskGridInfo) {
+        taskGridInfoService.updateById(taskGridInfo);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "删除接口")
+    @GetMapping(value = "/delete")
+    public ResponseDataDTO delete(String id) {
+        taskGridInfoService.removeById(id);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation("批量删除")
+    @PostMapping("/batchDelete")
+    public ResponseDataDTO batchDelete(@RequestBody IdForms ids) {
+        taskGridInfoService.removeByIds(ids.getIds());
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "详情查询")
+    @GetMapping(value = "/detail")
+    public ResponseDataDTO<TaskGridInfo> detail(String id) {
+        return ResponseDataDTO.success(taskGridInfoService.getById(id));
+    }
+}
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
index e82a792..6992274 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
@@ -98,7 +98,7 @@
     public ResponseDataDTO<AlgorithmResponse> test() {
         AlgorithmResponse response = new AlgorithmResponse();
         response.setTaskHeatMap(new TaskHeatMap());
-        algorithmService.readAlgorithmResponse("step3.txt",response,123L,null);
+        algorithmService.readAlgorithmResponse("step3.txt",null,response,123L,null);
         sendWebSocket(InstructStatusMsgEnums.HEAT_MAP, "1", response.getTaskHeatMap());
         return ResponseDataDTO.success(response);
     }
@@ -121,7 +121,7 @@
     @GetMapping(value = "/testGrid")
     public ResponseDataDTO<List<RouteInfo>> testGrid(@RequestBody PathInitDTO pathInitDTO) {
         RobotInfo robotInfo = infoService.getById(pathInitDTO.getRobotId());
-        GridPointDTO gridPointDTO = algorithmService.convertSlamToGrid(new BigDecimal(pathInitDTO.getX()), new BigDecimal(10), new BigDecimal(4), robotInfo);
+        GridPointDTO gridPointDTO = algorithmService.convertSlamToGrid(new BigDecimal(pathInitDTO.getXNow()), new BigDecimal(10), new BigDecimal(4), robotInfo);
         return ResponseDataDTO.success(algorithmService.convertGridCenterPoint(gridPointDTO, new BigDecimal(10), robotInfo));
     }
 

diff --git a/.gitignore b/.gitignore
index 9e20bc1..f42a9cb 100644
--- a/.gitignore
+++ b/.gitignore
@@ -2,3 +2,4 @@
 **/target/**
 logs/
 *.iml
+*.exe
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java b/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
index c7b5d28..a849528 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
@@ -28,6 +28,7 @@
     String KEY_CUR_TASK_ID = "CUR_TASK_ID_";
     String KEY_CUR_TASK_INIT = "CUR_TASK_INIT_";
 
+    String KEY_COST_MAP = "KEY_COST_MAP";
     /**
      * PSD数据ID
      */
@@ -41,6 +42,7 @@
      */
     String KEY_CUR_XY_POINT = "KEY_CUR_XY_POINT_";
     String KEY_CUR_ESTIMATE_GRID = "KEY_CUR_ESTIMATE_GRID_";
+    String KEY_PLANING_FLAG = "KEY_PLANING_FLAG_";
     /**
      * 机器人巡点栅格
      */
@@ -55,6 +57,7 @@
      * 探测器配置参数缓存
      */
     String KEY_CONFIG_PARAMS_ID = "KEY_CONFIG_PARAMS_ID_";
+    String KEY_CONFIG_COSTMAP_RESOLUTION_SET = "KEY_CONFIG_COSTMAP_RESOLUTION_SET_";
 
     /**
      * 暂停状态缓存信息
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
index c4c905d..3bb475f 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
@@ -1,9 +1,14 @@
 package com.casic.missiles.modular.robot.controller;
 
+import cn.hutool.core.lang.Assert;
 import cn.hutool.core.util.StrUtil;
 import com.casic.missiles.core.base.controller.BaseController;
+import com.casic.missiles.core.cache.CacheUtil;
 import com.casic.missiles.core.util.SpringContextHolder;
+import com.casic.missiles.model.exception.RequestValidException;
+import com.casic.missiles.model.response.ResponseData;
 import com.casic.missiles.model.response.dto.ResponseDataDTO;
+import com.casic.missiles.modular.constants.RobotDictConstants;
 import com.casic.missiles.modular.robot.IBaseRobotService;
 import com.casic.missiles.modular.robot.dto.RobotInfoSetDTO;
 import com.casic.missiles.modular.robot.model.RobotInfo;
@@ -19,10 +24,16 @@
 import io.swagger.annotations.ApiOperation;
 import io.swagger.annotations.ApiParam;
 import lombok.SneakyThrows;
+import lombok.extern.slf4j.Slf4j;
+import org.apache.ibatis.annotations.Param;
+import org.springframework.beans.factory.annotation.Value;
 import org.springframework.web.bind.annotation.*;
+import org.springframework.web.multipart.MultipartFile;
 
 import javax.servlet.http.HttpServletResponse;
+import java.io.File;
 import java.util.List;
+import java.util.Optional;
 
 /**
  * 机器人操作类
@@ -30,6 +41,7 @@
  * @author lwh
  * @date 2023-10-30
  */
+@Slf4j
 @Api(tags = "机器人控制接口")
 @RestController
 @RequestMapping("/robot/opt")
@@ -61,6 +73,28 @@
         return baseRobotService.execCmdHandler(request, InstructCodeEnums.CHANGE_PCD);
     }
 
+    @Value("${casic.file.uploadPath}")
+    private String filePath;
+
+    @SneakyThrows
+    @ApiOperation(value = "PCD上传接口")
+    @PostMapping(value = "/uploadPcd")
+    public ResponseData uploadPcd(@RequestParam("file") MultipartFile file, @ApiParam("机器人ID,url拼接传参") String robotId) {
+        Assert.isFalse(StrUtil.isEmpty(robotId), () -> new RequestValidException("机器ID不能为空", Optional.of(new Throwable().getStackTrace()[2])));
+        RobotInfo info = robotInfoService.getById(robotId);
+        if (file != null && !file.isEmpty()) {
+            File pcdFile = new File(filePath + "finalCloud_" + info.getId() + ".pcd");
+            try {
+                file.transferTo(pcdFile);
+            } catch (Exception e) {
+                log.error(e.getMessage());
+            }
+            return ResponseData.success();
+        } else {
+            return ResponseData.error("附件上传失败!");
+        }
+    }
+
     @SneakyThrows
     @ApiOperation(value = "PCD下载接口")
     @PostMapping(value = "/pcdDownload")
@@ -136,6 +170,10 @@
     @ApiOperation(value = "启动或关闭循迹功能", notes = "返回 1成功 0失败  wait 启动时是否等待，0代表循迹启动即运动，不等待；1代表循迹启动原地等待。若无此字段，默认循迹启动不等待直接运动")
     @PostMapping(value = "/startTrack")
     public ResponseDataDTO<Integer> startTrack(@RequestBody StartTrackRequest request) {
+        request.setIsTask(1);
+        if (request.getStepLen() == null) {
+            request.setStepLen(2);
+        }
         baseRobotService.startTrack(request);
         return ResponseDataDTO.success();
     }
@@ -156,6 +194,13 @@
         MessageRequestDTO<CrlNavRequest> messageDTO = new MessageRequestDTO();
         messageDTO.setMsg(request);
         ResponseDataDTO<Integer> list = baseRobotService.getRobotRouteOptService().robotCmd(info, InstructCodeEnums.CTRL_NAV, messageDTO);
+        if (request.getAction() == 1) {
+            CostMapResolutionRequest costMap = new CostMapResolutionRequest();
+            costMap.setResolution(info.getResolution().doubleValue());
+            request.setRobotId(request.getRobotId());
+            //是否下发分辨率
+            CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CONFIG_COSTMAP_RESOLUTION_SET + request.getRobotId(), costMap);
+        }
         return list;
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java
new file mode 100644
index 0000000..5317651
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java
@@ -0,0 +1,86 @@
+package com.casic.missiles.modular.robot.controller;
+
+import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
+import com.baomidou.mybatisplus.extension.plugins.pagination.Page;
+import com.casic.missiles.core.base.controller.BaseController;
+import com.casic.missiles.core.page.PageFactory;
+import com.casic.missiles.core.page.PageInfoBT;
+import com.casic.missiles.model.form.IdForms;
+import com.casic.missiles.model.response.dto.ResponseDataDTO;
+import com.casic.missiles.modular.robot.model.TaskGridInfo;
+import com.casic.missiles.modular.robot.service.ITaskGridInfoService;
+import com.casic.missiles.modular.robot.vo.TaskGridInfoVO;
+import io.swagger.annotations.Api;
+import io.swagger.annotations.ApiOperation;
+import org.springframework.web.bind.annotation.*;
+
+import java.util.List;
+
+/**
+ * 任务网格信息Controller
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+@Api(tags = "任务网格信息")
+@RestController
+@RequestMapping("/robot/taskGridInfo")
+public class TaskGridInfoController extends BaseController {
+
+    private final ITaskGridInfoService taskGridInfoService;
+
+    public TaskGridInfoController(ITaskGridInfoService taskGridInfoService) {
+        this.taskGridInfoService = taskGridInfoService;
+    }
+
+    @ApiOperation(value = "列表查询")
+    @GetMapping(value = "/list")
+    public ResponseDataDTO<List<TaskGridInfo>> list(TaskGridInfoVO taskGridInfoVO) {
+        QueryWrapper<TaskGridInfo> query = taskGridInfoVO.genQuery();
+        return ResponseDataDTO.success(taskGridInfoService.list(query));
+    }
+
+    @ApiOperation(value = "分页查询")
+    @GetMapping(value = "/listPage")
+    public ResponseDataDTO<PageInfoBT<TaskGridInfo>> listPage(TaskGridInfoVO taskGridInfoVO) {
+        Page<TaskGridInfo> page = PageFactory.defaultPage();
+        QueryWrapper<TaskGridInfo> query = taskGridInfoVO.genQuery();
+        page = taskGridInfoService.page(page, query);
+        return ResponseDataDTO.success(super.packForBT(page));
+    }
+
+
+    @ApiOperation(value = "新增接口")
+    @PostMapping(value = "/add")
+    public ResponseDataDTO add(@RequestBody TaskGridInfo taskGridInfo) {
+        taskGridInfoService.save(taskGridInfo);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "修改接口")
+    @PostMapping(value = "/update")
+    public ResponseDataDTO update(@RequestBody TaskGridInfo taskGridInfo) {
+        taskGridInfoService.updateById(taskGridInfo);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "删除接口")
+    @GetMapping(value = "/delete")
+    public ResponseDataDTO delete(String id) {
+        taskGridInfoService.removeById(id);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation("批量删除")
+    @PostMapping("/batchDelete")
+    public ResponseDataDTO batchDelete(@RequestBody IdForms ids) {
+        taskGridInfoService.removeByIds(ids.getIds());
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "详情查询")
+    @GetMapping(value = "/detail")
+    public ResponseDataDTO<TaskGridInfo> detail(String id) {
+        return ResponseDataDTO.success(taskGridInfoService.getById(id));
+    }
+}
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
index e82a792..6992274 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
@@ -98,7 +98,7 @@
     public ResponseDataDTO<AlgorithmResponse> test() {
         AlgorithmResponse response = new AlgorithmResponse();
         response.setTaskHeatMap(new TaskHeatMap());
-        algorithmService.readAlgorithmResponse("step3.txt",response,123L,null);
+        algorithmService.readAlgorithmResponse("step3.txt",null,response,123L,null);
         sendWebSocket(InstructStatusMsgEnums.HEAT_MAP, "1", response.getTaskHeatMap());
         return ResponseDataDTO.success(response);
     }
@@ -121,7 +121,7 @@
     @GetMapping(value = "/testGrid")
     public ResponseDataDTO<List<RouteInfo>> testGrid(@RequestBody PathInitDTO pathInitDTO) {
         RobotInfo robotInfo = infoService.getById(pathInitDTO.getRobotId());
-        GridPointDTO gridPointDTO = algorithmService.convertSlamToGrid(new BigDecimal(pathInitDTO.getX()), new BigDecimal(10), new BigDecimal(4), robotInfo);
+        GridPointDTO gridPointDTO = algorithmService.convertSlamToGrid(new BigDecimal(pathInitDTO.getXNow()), new BigDecimal(10), new BigDecimal(4), robotInfo);
         return ResponseDataDTO.success(algorithmService.convertGridCenterPoint(gridPointDTO, new BigDecimal(10), robotInfo));
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/TaskGridInfoMapper.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/TaskGridInfoMapper.java
new file mode 100644
index 0000000..5c3a70b
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/TaskGridInfoMapper.java
@@ -0,0 +1,23 @@
+package com.casic.missiles.modular.robot.dao;
+
+
+import com.baomidou.mybatisplus.core.mapper.BaseMapper;
+import com.casic.missiles.modular.robot.model.TaskGridInfo;
+import org.apache.ibatis.annotations.Param;
+import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
+import com.baomidou.mybatisplus.extension.plugins.pagination.Page;
+import java.util.List;
+/**
+ * <p>
+ *  任务网格信息 Mapper 接口
+ * </p>
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+public interface TaskGridInfoMapper extends BaseMapper<TaskGridInfo> {
+/**
+* 任务网格信息 分页检索
+*/
+List<TaskGridInfo> selectTaskGridInfoPage( @Param("page") Page<TaskGridInfo> page,@Param("ew" ) QueryWrapper<TaskGridInfo> query);
+}

diff --git a/.gitignore b/.gitignore
index 9e20bc1..f42a9cb 100644
--- a/.gitignore
+++ b/.gitignore
@@ -2,3 +2,4 @@
 **/target/**
 logs/
 *.iml
+*.exe
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java b/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
index c7b5d28..a849528 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
@@ -28,6 +28,7 @@
     String KEY_CUR_TASK_ID = "CUR_TASK_ID_";
     String KEY_CUR_TASK_INIT = "CUR_TASK_INIT_";
 
+    String KEY_COST_MAP = "KEY_COST_MAP";
     /**
      * PSD数据ID
      */
@@ -41,6 +42,7 @@
      */
     String KEY_CUR_XY_POINT = "KEY_CUR_XY_POINT_";
     String KEY_CUR_ESTIMATE_GRID = "KEY_CUR_ESTIMATE_GRID_";
+    String KEY_PLANING_FLAG = "KEY_PLANING_FLAG_";
     /**
      * 机器人巡点栅格
      */
@@ -55,6 +57,7 @@
      * 探测器配置参数缓存
      */
     String KEY_CONFIG_PARAMS_ID = "KEY_CONFIG_PARAMS_ID_";
+    String KEY_CONFIG_COSTMAP_RESOLUTION_SET = "KEY_CONFIG_COSTMAP_RESOLUTION_SET_";
 
     /**
      * 暂停状态缓存信息
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
index c4c905d..3bb475f 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
@@ -1,9 +1,14 @@
 package com.casic.missiles.modular.robot.controller;
 
+import cn.hutool.core.lang.Assert;
 import cn.hutool.core.util.StrUtil;
 import com.casic.missiles.core.base.controller.BaseController;
+import com.casic.missiles.core.cache.CacheUtil;
 import com.casic.missiles.core.util.SpringContextHolder;
+import com.casic.missiles.model.exception.RequestValidException;
+import com.casic.missiles.model.response.ResponseData;
 import com.casic.missiles.model.response.dto.ResponseDataDTO;
+import com.casic.missiles.modular.constants.RobotDictConstants;
 import com.casic.missiles.modular.robot.IBaseRobotService;
 import com.casic.missiles.modular.robot.dto.RobotInfoSetDTO;
 import com.casic.missiles.modular.robot.model.RobotInfo;
@@ -19,10 +24,16 @@
 import io.swagger.annotations.ApiOperation;
 import io.swagger.annotations.ApiParam;
 import lombok.SneakyThrows;
+import lombok.extern.slf4j.Slf4j;
+import org.apache.ibatis.annotations.Param;
+import org.springframework.beans.factory.annotation.Value;
 import org.springframework.web.bind.annotation.*;
+import org.springframework.web.multipart.MultipartFile;
 
 import javax.servlet.http.HttpServletResponse;
+import java.io.File;
 import java.util.List;
+import java.util.Optional;
 
 /**
  * 机器人操作类
@@ -30,6 +41,7 @@
  * @author lwh
  * @date 2023-10-30
  */
+@Slf4j
 @Api(tags = "机器人控制接口")
 @RestController
 @RequestMapping("/robot/opt")
@@ -61,6 +73,28 @@
         return baseRobotService.execCmdHandler(request, InstructCodeEnums.CHANGE_PCD);
     }
 
+    @Value("${casic.file.uploadPath}")
+    private String filePath;
+
+    @SneakyThrows
+    @ApiOperation(value = "PCD上传接口")
+    @PostMapping(value = "/uploadPcd")
+    public ResponseData uploadPcd(@RequestParam("file") MultipartFile file, @ApiParam("机器人ID,url拼接传参") String robotId) {
+        Assert.isFalse(StrUtil.isEmpty(robotId), () -> new RequestValidException("机器ID不能为空", Optional.of(new Throwable().getStackTrace()[2])));
+        RobotInfo info = robotInfoService.getById(robotId);
+        if (file != null && !file.isEmpty()) {
+            File pcdFile = new File(filePath + "finalCloud_" + info.getId() + ".pcd");
+            try {
+                file.transferTo(pcdFile);
+            } catch (Exception e) {
+                log.error(e.getMessage());
+            }
+            return ResponseData.success();
+        } else {
+            return ResponseData.error("附件上传失败!");
+        }
+    }
+
     @SneakyThrows
     @ApiOperation(value = "PCD下载接口")
     @PostMapping(value = "/pcdDownload")
@@ -136,6 +170,10 @@
     @ApiOperation(value = "启动或关闭循迹功能", notes = "返回 1成功 0失败  wait 启动时是否等待，0代表循迹启动即运动，不等待；1代表循迹启动原地等待。若无此字段，默认循迹启动不等待直接运动")
     @PostMapping(value = "/startTrack")
     public ResponseDataDTO<Integer> startTrack(@RequestBody StartTrackRequest request) {
+        request.setIsTask(1);
+        if (request.getStepLen() == null) {
+            request.setStepLen(2);
+        }
         baseRobotService.startTrack(request);
         return ResponseDataDTO.success();
     }
@@ -156,6 +194,13 @@
         MessageRequestDTO<CrlNavRequest> messageDTO = new MessageRequestDTO();
         messageDTO.setMsg(request);
         ResponseDataDTO<Integer> list = baseRobotService.getRobotRouteOptService().robotCmd(info, InstructCodeEnums.CTRL_NAV, messageDTO);
+        if (request.getAction() == 1) {
+            CostMapResolutionRequest costMap = new CostMapResolutionRequest();
+            costMap.setResolution(info.getResolution().doubleValue());
+            request.setRobotId(request.getRobotId());
+            //是否下发分辨率
+            CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CONFIG_COSTMAP_RESOLUTION_SET + request.getRobotId(), costMap);
+        }
         return list;
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java
new file mode 100644
index 0000000..5317651
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java
@@ -0,0 +1,86 @@
+package com.casic.missiles.modular.robot.controller;
+
+import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
+import com.baomidou.mybatisplus.extension.plugins.pagination.Page;
+import com.casic.missiles.core.base.controller.BaseController;
+import com.casic.missiles.core.page.PageFactory;
+import com.casic.missiles.core.page.PageInfoBT;
+import com.casic.missiles.model.form.IdForms;
+import com.casic.missiles.model.response.dto.ResponseDataDTO;
+import com.casic.missiles.modular.robot.model.TaskGridInfo;
+import com.casic.missiles.modular.robot.service.ITaskGridInfoService;
+import com.casic.missiles.modular.robot.vo.TaskGridInfoVO;
+import io.swagger.annotations.Api;
+import io.swagger.annotations.ApiOperation;
+import org.springframework.web.bind.annotation.*;
+
+import java.util.List;
+
+/**
+ * 任务网格信息Controller
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+@Api(tags = "任务网格信息")
+@RestController
+@RequestMapping("/robot/taskGridInfo")
+public class TaskGridInfoController extends BaseController {
+
+    private final ITaskGridInfoService taskGridInfoService;
+
+    public TaskGridInfoController(ITaskGridInfoService taskGridInfoService) {
+        this.taskGridInfoService = taskGridInfoService;
+    }
+
+    @ApiOperation(value = "列表查询")
+    @GetMapping(value = "/list")
+    public ResponseDataDTO<List<TaskGridInfo>> list(TaskGridInfoVO taskGridInfoVO) {
+        QueryWrapper<TaskGridInfo> query = taskGridInfoVO.genQuery();
+        return ResponseDataDTO.success(taskGridInfoService.list(query));
+    }
+
+    @ApiOperation(value = "分页查询")
+    @GetMapping(value = "/listPage")
+    public ResponseDataDTO<PageInfoBT<TaskGridInfo>> listPage(TaskGridInfoVO taskGridInfoVO) {
+        Page<TaskGridInfo> page = PageFactory.defaultPage();
+        QueryWrapper<TaskGridInfo> query = taskGridInfoVO.genQuery();
+        page = taskGridInfoService.page(page, query);
+        return ResponseDataDTO.success(super.packForBT(page));
+    }
+
+
+    @ApiOperation(value = "新增接口")
+    @PostMapping(value = "/add")
+    public ResponseDataDTO add(@RequestBody TaskGridInfo taskGridInfo) {
+        taskGridInfoService.save(taskGridInfo);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "修改接口")
+    @PostMapping(value = "/update")
+    public ResponseDataDTO update(@RequestBody TaskGridInfo taskGridInfo) {
+        taskGridInfoService.updateById(taskGridInfo);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "删除接口")
+    @GetMapping(value = "/delete")
+    public ResponseDataDTO delete(String id) {
+        taskGridInfoService.removeById(id);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation("批量删除")
+    @PostMapping("/batchDelete")
+    public ResponseDataDTO batchDelete(@RequestBody IdForms ids) {
+        taskGridInfoService.removeByIds(ids.getIds());
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "详情查询")
+    @GetMapping(value = "/detail")
+    public ResponseDataDTO<TaskGridInfo> detail(String id) {
+        return ResponseDataDTO.success(taskGridInfoService.getById(id));
+    }
+}
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
index e82a792..6992274 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
@@ -98,7 +98,7 @@
     public ResponseDataDTO<AlgorithmResponse> test() {
         AlgorithmResponse response = new AlgorithmResponse();
         response.setTaskHeatMap(new TaskHeatMap());
-        algorithmService.readAlgorithmResponse("step3.txt",response,123L,null);
+        algorithmService.readAlgorithmResponse("step3.txt",null,response,123L,null);
         sendWebSocket(InstructStatusMsgEnums.HEAT_MAP, "1", response.getTaskHeatMap());
         return ResponseDataDTO.success(response);
     }
@@ -121,7 +121,7 @@
     @GetMapping(value = "/testGrid")
     public ResponseDataDTO<List<RouteInfo>> testGrid(@RequestBody PathInitDTO pathInitDTO) {
         RobotInfo robotInfo = infoService.getById(pathInitDTO.getRobotId());
-        GridPointDTO gridPointDTO = algorithmService.convertSlamToGrid(new BigDecimal(pathInitDTO.getX()), new BigDecimal(10), new BigDecimal(4), robotInfo);
+        GridPointDTO gridPointDTO = algorithmService.convertSlamToGrid(new BigDecimal(pathInitDTO.getXNow()), new BigDecimal(10), new BigDecimal(4), robotInfo);
         return ResponseDataDTO.success(algorithmService.convertGridCenterPoint(gridPointDTO, new BigDecimal(10), robotInfo));
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/TaskGridInfoMapper.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/TaskGridInfoMapper.java
new file mode 100644
index 0000000..5c3a70b
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/TaskGridInfoMapper.java
@@ -0,0 +1,23 @@
+package com.casic.missiles.modular.robot.dao;
+
+
+import com.baomidou.mybatisplus.core.mapper.BaseMapper;
+import com.casic.missiles.modular.robot.model.TaskGridInfo;
+import org.apache.ibatis.annotations.Param;
+import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
+import com.baomidou.mybatisplus.extension.plugins.pagination.Page;
+import java.util.List;
+/**
+ * <p>
+ *  任务网格信息 Mapper 接口
+ * </p>
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+public interface TaskGridInfoMapper extends BaseMapper<TaskGridInfo> {
+/**
+* 任务网格信息 分页检索
+*/
+List<TaskGridInfo> selectTaskGridInfoPage( @Param("page") Page<TaskGridInfo> page,@Param("ew" ) QueryWrapper<TaskGridInfo> query);
+}
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/mapping/TaskGridInfoMapper.xml b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/mapping/TaskGridInfoMapper.xml
new file mode 100644
index 0000000..be573e0
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/mapping/TaskGridInfoMapper.xml
@@ -0,0 +1,80 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!DOCTYPE mapper PUBLIC "-//mybatis.org//DTD Mapper 3.0//EN" "http://mybatis.org/dtd/mybatis-3-mapper.dtd">
+<mapper namespace="com.casic.missiles.modular.robot.dao.TaskGridInfoMapper">
+    <!-- 通用查询映射结果 -->
+    <resultMap id="BaseResultMap" type="com.casic.missiles.modular.robot.model.TaskGridInfo">
+        <result column="id" property="id"/>
+        <result column="create_time" property="createTime"/>
+        <result column="update_time" property="updateTime"/>
+        <result column="task_id" property="taskId"/>
+        <result column="estimate_grid_x" property="estimateGridX"/>
+        <result column="estimate_grid_y" property="estimateGridY"/>
+        <result column="neutron_count" property="neutronCount"/>
+        <result column="cur_grid_x" property="curGridX"/>
+        <result column="cur_grid_y" property="curGridY"/>
+    </resultMap>
+    <!-- 通用查询结果列 -->
+    <sql id="Base_Column_List">
+        id
+        ,
+                    create_time as createTime ,
+                    update_time as updateTime ,
+                    task_id as taskId ,
+                    estimate_grid_x as estimateGridX ,
+                    estimate_grid_y as estimateGridY ,
+                    neutron_count as neutronCount ,
+                    cur_grid_x as curGridX ,
+                    cur_grid_y as curGridY
+    </sql>
+    <sql id="Base_Column_Alias">
+        a
+        .
+        id
+        ,
+            a.create_time
+            as createTime ,
+            a.update_time
+            as updateTime ,
+            a.task_id
+            as taskId ,
+            a.estimate_grid_x
+            as estimateGridX ,
+            a.estimate_grid_y
+            as estimateGridY ,
+            a.neutron_count
+            as neutronCount ,
+            a.cur_grid_x
+            as curGridX ,
+            a.cur_grid_y
+            as curGridY
+    </sql>
+    <select id="selectTaskGridInfoPage" resultType="com.casic.missiles.modular.robot.model.TaskGridInfo">
+        select
+        <include refid="Base_Column_Alias"/>
+        from task_grid_info a
+        <where>
+            <if test="ew != null">
+                ${ew.sqlSegment}
+            </if>
+        </where>
+    </select>
+    <sql id="Base_Column_Query">
+        <if test="request!=null">
+            <if test="request.taskId!= null and request.taskId!=''">
+                and task_id = #{request.taskId}
+            </if>
+            <if test="request.estimateGridX!= null and request.estimateGridX!=''">
+                and estimate_grid_x = #{request.estimateGridX}
+            </if>
+            <if test="request.estimateGridY!= null and request.estimateGridY!=''">
+                and estimate_grid_y = #{request.estimateGridY}
+            </if>
+            <if test="request.curGridX!= null and request.curGridX!=''">
+                and cur_grid_x = #{request.curGridX}
+            </if>
+            <if test="request.curGridY!= null and request.curGridY!=''">
+                and cur_grid_y = #{request.curGridY}
+            </if>
+        </if>
+    </sql>
+</mapper>
\ No newline at end of file

diff --git a/.gitignore b/.gitignore
index 9e20bc1..f42a9cb 100644
--- a/.gitignore
+++ b/.gitignore
@@ -2,3 +2,4 @@
 **/target/**
 logs/
 *.iml
+*.exe
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java b/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
index c7b5d28..a849528 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
@@ -28,6 +28,7 @@
     String KEY_CUR_TASK_ID = "CUR_TASK_ID_";
     String KEY_CUR_TASK_INIT = "CUR_TASK_INIT_";
 
+    String KEY_COST_MAP = "KEY_COST_MAP";
     /**
      * PSD数据ID
      */
@@ -41,6 +42,7 @@
      */
     String KEY_CUR_XY_POINT = "KEY_CUR_XY_POINT_";
     String KEY_CUR_ESTIMATE_GRID = "KEY_CUR_ESTIMATE_GRID_";
+    String KEY_PLANING_FLAG = "KEY_PLANING_FLAG_";
     /**
      * 机器人巡点栅格
      */
@@ -55,6 +57,7 @@
      * 探测器配置参数缓存
      */
     String KEY_CONFIG_PARAMS_ID = "KEY_CONFIG_PARAMS_ID_";
+    String KEY_CONFIG_COSTMAP_RESOLUTION_SET = "KEY_CONFIG_COSTMAP_RESOLUTION_SET_";
 
     /**
      * 暂停状态缓存信息
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
index c4c905d..3bb475f 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
@@ -1,9 +1,14 @@
 package com.casic.missiles.modular.robot.controller;
 
+import cn.hutool.core.lang.Assert;
 import cn.hutool.core.util.StrUtil;
 import com.casic.missiles.core.base.controller.BaseController;
+import com.casic.missiles.core.cache.CacheUtil;
 import com.casic.missiles.core.util.SpringContextHolder;
+import com.casic.missiles.model.exception.RequestValidException;
+import com.casic.missiles.model.response.ResponseData;
 import com.casic.missiles.model.response.dto.ResponseDataDTO;
+import com.casic.missiles.modular.constants.RobotDictConstants;
 import com.casic.missiles.modular.robot.IBaseRobotService;
 import com.casic.missiles.modular.robot.dto.RobotInfoSetDTO;
 import com.casic.missiles.modular.robot.model.RobotInfo;
@@ -19,10 +24,16 @@
 import io.swagger.annotations.ApiOperation;
 import io.swagger.annotations.ApiParam;
 import lombok.SneakyThrows;
+import lombok.extern.slf4j.Slf4j;
+import org.apache.ibatis.annotations.Param;
+import org.springframework.beans.factory.annotation.Value;
 import org.springframework.web.bind.annotation.*;
+import org.springframework.web.multipart.MultipartFile;
 
 import javax.servlet.http.HttpServletResponse;
+import java.io.File;
 import java.util.List;
+import java.util.Optional;
 
 /**
  * 机器人操作类
@@ -30,6 +41,7 @@
  * @author lwh
  * @date 2023-10-30
  */
+@Slf4j
 @Api(tags = "机器人控制接口")
 @RestController
 @RequestMapping("/robot/opt")
@@ -61,6 +73,28 @@
         return baseRobotService.execCmdHandler(request, InstructCodeEnums.CHANGE_PCD);
     }
 
+    @Value("${casic.file.uploadPath}")
+    private String filePath;
+
+    @SneakyThrows
+    @ApiOperation(value = "PCD上传接口")
+    @PostMapping(value = "/uploadPcd")
+    public ResponseData uploadPcd(@RequestParam("file") MultipartFile file, @ApiParam("机器人ID,url拼接传参") String robotId) {
+        Assert.isFalse(StrUtil.isEmpty(robotId), () -> new RequestValidException("机器ID不能为空", Optional.of(new Throwable().getStackTrace()[2])));
+        RobotInfo info = robotInfoService.getById(robotId);
+        if (file != null && !file.isEmpty()) {
+            File pcdFile = new File(filePath + "finalCloud_" + info.getId() + ".pcd");
+            try {
+                file.transferTo(pcdFile);
+            } catch (Exception e) {
+                log.error(e.getMessage());
+            }
+            return ResponseData.success();
+        } else {
+            return ResponseData.error("附件上传失败!");
+        }
+    }
+
     @SneakyThrows
     @ApiOperation(value = "PCD下载接口")
     @PostMapping(value = "/pcdDownload")
@@ -136,6 +170,10 @@
     @ApiOperation(value = "启动或关闭循迹功能", notes = "返回 1成功 0失败  wait 启动时是否等待，0代表循迹启动即运动，不等待；1代表循迹启动原地等待。若无此字段，默认循迹启动不等待直接运动")
     @PostMapping(value = "/startTrack")
     public ResponseDataDTO<Integer> startTrack(@RequestBody StartTrackRequest request) {
+        request.setIsTask(1);
+        if (request.getStepLen() == null) {
+            request.setStepLen(2);
+        }
         baseRobotService.startTrack(request);
         return ResponseDataDTO.success();
     }
@@ -156,6 +194,13 @@
         MessageRequestDTO<CrlNavRequest> messageDTO = new MessageRequestDTO();
         messageDTO.setMsg(request);
         ResponseDataDTO<Integer> list = baseRobotService.getRobotRouteOptService().robotCmd(info, InstructCodeEnums.CTRL_NAV, messageDTO);
+        if (request.getAction() == 1) {
+            CostMapResolutionRequest costMap = new CostMapResolutionRequest();
+            costMap.setResolution(info.getResolution().doubleValue());
+            request.setRobotId(request.getRobotId());
+            //是否下发分辨率
+            CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CONFIG_COSTMAP_RESOLUTION_SET + request.getRobotId(), costMap);
+        }
         return list;
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java
new file mode 100644
index 0000000..5317651
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java
@@ -0,0 +1,86 @@
+package com.casic.missiles.modular.robot.controller;
+
+import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
+import com.baomidou.mybatisplus.extension.plugins.pagination.Page;
+import com.casic.missiles.core.base.controller.BaseController;
+import com.casic.missiles.core.page.PageFactory;
+import com.casic.missiles.core.page.PageInfoBT;
+import com.casic.missiles.model.form.IdForms;
+import com.casic.missiles.model.response.dto.ResponseDataDTO;
+import com.casic.missiles.modular.robot.model.TaskGridInfo;
+import com.casic.missiles.modular.robot.service.ITaskGridInfoService;
+import com.casic.missiles.modular.robot.vo.TaskGridInfoVO;
+import io.swagger.annotations.Api;
+import io.swagger.annotations.ApiOperation;
+import org.springframework.web.bind.annotation.*;
+
+import java.util.List;
+
+/**
+ * 任务网格信息Controller
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+@Api(tags = "任务网格信息")
+@RestController
+@RequestMapping("/robot/taskGridInfo")
+public class TaskGridInfoController extends BaseController {
+
+    private final ITaskGridInfoService taskGridInfoService;
+
+    public TaskGridInfoController(ITaskGridInfoService taskGridInfoService) {
+        this.taskGridInfoService = taskGridInfoService;
+    }
+
+    @ApiOperation(value = "列表查询")
+    @GetMapping(value = "/list")
+    public ResponseDataDTO<List<TaskGridInfo>> list(TaskGridInfoVO taskGridInfoVO) {
+        QueryWrapper<TaskGridInfo> query = taskGridInfoVO.genQuery();
+        return ResponseDataDTO.success(taskGridInfoService.list(query));
+    }
+
+    @ApiOperation(value = "分页查询")
+    @GetMapping(value = "/listPage")
+    public ResponseDataDTO<PageInfoBT<TaskGridInfo>> listPage(TaskGridInfoVO taskGridInfoVO) {
+        Page<TaskGridInfo> page = PageFactory.defaultPage();
+        QueryWrapper<TaskGridInfo> query = taskGridInfoVO.genQuery();
+        page = taskGridInfoService.page(page, query);
+        return ResponseDataDTO.success(super.packForBT(page));
+    }
+
+
+    @ApiOperation(value = "新增接口")
+    @PostMapping(value = "/add")
+    public ResponseDataDTO add(@RequestBody TaskGridInfo taskGridInfo) {
+        taskGridInfoService.save(taskGridInfo);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "修改接口")
+    @PostMapping(value = "/update")
+    public ResponseDataDTO update(@RequestBody TaskGridInfo taskGridInfo) {
+        taskGridInfoService.updateById(taskGridInfo);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "删除接口")
+    @GetMapping(value = "/delete")
+    public ResponseDataDTO delete(String id) {
+        taskGridInfoService.removeById(id);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation("批量删除")
+    @PostMapping("/batchDelete")
+    public ResponseDataDTO batchDelete(@RequestBody IdForms ids) {
+        taskGridInfoService.removeByIds(ids.getIds());
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "详情查询")
+    @GetMapping(value = "/detail")
+    public ResponseDataDTO<TaskGridInfo> detail(String id) {
+        return ResponseDataDTO.success(taskGridInfoService.getById(id));
+    }
+}
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
index e82a792..6992274 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
@@ -98,7 +98,7 @@
     public ResponseDataDTO<AlgorithmResponse> test() {
         AlgorithmResponse response = new AlgorithmResponse();
         response.setTaskHeatMap(new TaskHeatMap());
-        algorithmService.readAlgorithmResponse("step3.txt",response,123L,null);
+        algorithmService.readAlgorithmResponse("step3.txt",null,response,123L,null);
         sendWebSocket(InstructStatusMsgEnums.HEAT_MAP, "1", response.getTaskHeatMap());
         return ResponseDataDTO.success(response);
     }
@@ -121,7 +121,7 @@
     @GetMapping(value = "/testGrid")
     public ResponseDataDTO<List<RouteInfo>> testGrid(@RequestBody PathInitDTO pathInitDTO) {
         RobotInfo robotInfo = infoService.getById(pathInitDTO.getRobotId());
-        GridPointDTO gridPointDTO = algorithmService.convertSlamToGrid(new BigDecimal(pathInitDTO.getX()), new BigDecimal(10), new BigDecimal(4), robotInfo);
+        GridPointDTO gridPointDTO = algorithmService.convertSlamToGrid(new BigDecimal(pathInitDTO.getXNow()), new BigDecimal(10), new BigDecimal(4), robotInfo);
         return ResponseDataDTO.success(algorithmService.convertGridCenterPoint(gridPointDTO, new BigDecimal(10), robotInfo));
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/TaskGridInfoMapper.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/TaskGridInfoMapper.java
new file mode 100644
index 0000000..5c3a70b
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/TaskGridInfoMapper.java
@@ -0,0 +1,23 @@
+package com.casic.missiles.modular.robot.dao;
+
+
+import com.baomidou.mybatisplus.core.mapper.BaseMapper;
+import com.casic.missiles.modular.robot.model.TaskGridInfo;
+import org.apache.ibatis.annotations.Param;
+import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
+import com.baomidou.mybatisplus.extension.plugins.pagination.Page;
+import java.util.List;
+/**
+ * <p>
+ *  任务网格信息 Mapper 接口
+ * </p>
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+public interface TaskGridInfoMapper extends BaseMapper<TaskGridInfo> {
+/**
+* 任务网格信息 分页检索
+*/
+List<TaskGridInfo> selectTaskGridInfoPage( @Param("page") Page<TaskGridInfo> page,@Param("ew" ) QueryWrapper<TaskGridInfo> query);
+}
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/mapping/TaskGridInfoMapper.xml b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/mapping/TaskGridInfoMapper.xml
new file mode 100644
index 0000000..be573e0
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/mapping/TaskGridInfoMapper.xml
@@ -0,0 +1,80 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!DOCTYPE mapper PUBLIC "-//mybatis.org//DTD Mapper 3.0//EN" "http://mybatis.org/dtd/mybatis-3-mapper.dtd">
+<mapper namespace="com.casic.missiles.modular.robot.dao.TaskGridInfoMapper">
+    <!-- 通用查询映射结果 -->
+    <resultMap id="BaseResultMap" type="com.casic.missiles.modular.robot.model.TaskGridInfo">
+        <result column="id" property="id"/>
+        <result column="create_time" property="createTime"/>
+        <result column="update_time" property="updateTime"/>
+        <result column="task_id" property="taskId"/>
+        <result column="estimate_grid_x" property="estimateGridX"/>
+        <result column="estimate_grid_y" property="estimateGridY"/>
+        <result column="neutron_count" property="neutronCount"/>
+        <result column="cur_grid_x" property="curGridX"/>
+        <result column="cur_grid_y" property="curGridY"/>
+    </resultMap>
+    <!-- 通用查询结果列 -->
+    <sql id="Base_Column_List">
+        id
+        ,
+                    create_time as createTime ,
+                    update_time as updateTime ,
+                    task_id as taskId ,
+                    estimate_grid_x as estimateGridX ,
+                    estimate_grid_y as estimateGridY ,
+                    neutron_count as neutronCount ,
+                    cur_grid_x as curGridX ,
+                    cur_grid_y as curGridY
+    </sql>
+    <sql id="Base_Column_Alias">
+        a
+        .
+        id
+        ,
+            a.create_time
+            as createTime ,
+            a.update_time
+            as updateTime ,
+            a.task_id
+            as taskId ,
+            a.estimate_grid_x
+            as estimateGridX ,
+            a.estimate_grid_y
+            as estimateGridY ,
+            a.neutron_count
+            as neutronCount ,
+            a.cur_grid_x
+            as curGridX ,
+            a.cur_grid_y
+            as curGridY
+    </sql>
+    <select id="selectTaskGridInfoPage" resultType="com.casic.missiles.modular.robot.model.TaskGridInfo">
+        select
+        <include refid="Base_Column_Alias"/>
+        from task_grid_info a
+        <where>
+            <if test="ew != null">
+                ${ew.sqlSegment}
+            </if>
+        </where>
+    </select>
+    <sql id="Base_Column_Query">
+        <if test="request!=null">
+            <if test="request.taskId!= null and request.taskId!=''">
+                and task_id = #{request.taskId}
+            </if>
+            <if test="request.estimateGridX!= null and request.estimateGridX!=''">
+                and estimate_grid_x = #{request.estimateGridX}
+            </if>
+            <if test="request.estimateGridY!= null and request.estimateGridY!=''">
+                and estimate_grid_y = #{request.estimateGridY}
+            </if>
+            <if test="request.curGridX!= null and request.curGridX!=''">
+                and cur_grid_x = #{request.curGridX}
+            </if>
+            <if test="request.curGridY!= null and request.curGridY!=''">
+                and cur_grid_y = #{request.curGridY}
+            </if>
+        </if>
+    </sql>
+</mapper>
\ No newline at end of file
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java
index 749f559..4559074 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java
@@ -13,4 +13,9 @@
      * 规划点位信息
      */
     private PointNextDTO pointNextDTO;
+    /**
+     * 寻源目标地址
+     */
+    private PointNextDTO coordinate;
+    private boolean isEnd;
 }

diff --git a/.gitignore b/.gitignore
index 9e20bc1..f42a9cb 100644
--- a/.gitignore
+++ b/.gitignore
@@ -2,3 +2,4 @@
 **/target/**
 logs/
 *.iml
+*.exe
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java b/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
index c7b5d28..a849528 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
@@ -28,6 +28,7 @@
     String KEY_CUR_TASK_ID = "CUR_TASK_ID_";
     String KEY_CUR_TASK_INIT = "CUR_TASK_INIT_";
 
+    String KEY_COST_MAP = "KEY_COST_MAP";
     /**
      * PSD数据ID
      */
@@ -41,6 +42,7 @@
      */
     String KEY_CUR_XY_POINT = "KEY_CUR_XY_POINT_";
     String KEY_CUR_ESTIMATE_GRID = "KEY_CUR_ESTIMATE_GRID_";
+    String KEY_PLANING_FLAG = "KEY_PLANING_FLAG_";
     /**
      * 机器人巡点栅格
      */
@@ -55,6 +57,7 @@
      * 探测器配置参数缓存
      */
     String KEY_CONFIG_PARAMS_ID = "KEY_CONFIG_PARAMS_ID_";
+    String KEY_CONFIG_COSTMAP_RESOLUTION_SET = "KEY_CONFIG_COSTMAP_RESOLUTION_SET_";
 
     /**
      * 暂停状态缓存信息
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
index c4c905d..3bb475f 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
@@ -1,9 +1,14 @@
 package com.casic.missiles.modular.robot.controller;
 
+import cn.hutool.core.lang.Assert;
 import cn.hutool.core.util.StrUtil;
 import com.casic.missiles.core.base.controller.BaseController;
+import com.casic.missiles.core.cache.CacheUtil;
 import com.casic.missiles.core.util.SpringContextHolder;
+import com.casic.missiles.model.exception.RequestValidException;
+import com.casic.missiles.model.response.ResponseData;
 import com.casic.missiles.model.response.dto.ResponseDataDTO;
+import com.casic.missiles.modular.constants.RobotDictConstants;
 import com.casic.missiles.modular.robot.IBaseRobotService;
 import com.casic.missiles.modular.robot.dto.RobotInfoSetDTO;
 import com.casic.missiles.modular.robot.model.RobotInfo;
@@ -19,10 +24,16 @@
 import io.swagger.annotations.ApiOperation;
 import io.swagger.annotations.ApiParam;
 import lombok.SneakyThrows;
+import lombok.extern.slf4j.Slf4j;
+import org.apache.ibatis.annotations.Param;
+import org.springframework.beans.factory.annotation.Value;
 import org.springframework.web.bind.annotation.*;
+import org.springframework.web.multipart.MultipartFile;
 
 import javax.servlet.http.HttpServletResponse;
+import java.io.File;
 import java.util.List;
+import java.util.Optional;
 
 /**
  * 机器人操作类
@@ -30,6 +41,7 @@
  * @author lwh
  * @date 2023-10-30
  */
+@Slf4j
 @Api(tags = "机器人控制接口")
 @RestController
 @RequestMapping("/robot/opt")
@@ -61,6 +73,28 @@
         return baseRobotService.execCmdHandler(request, InstructCodeEnums.CHANGE_PCD);
     }
 
+    @Value("${casic.file.uploadPath}")
+    private String filePath;
+
+    @SneakyThrows
+    @ApiOperation(value = "PCD上传接口")
+    @PostMapping(value = "/uploadPcd")
+    public ResponseData uploadPcd(@RequestParam("file") MultipartFile file, @ApiParam("机器人ID,url拼接传参") String robotId) {
+        Assert.isFalse(StrUtil.isEmpty(robotId), () -> new RequestValidException("机器ID不能为空", Optional.of(new Throwable().getStackTrace()[2])));
+        RobotInfo info = robotInfoService.getById(robotId);
+        if (file != null && !file.isEmpty()) {
+            File pcdFile = new File(filePath + "finalCloud_" + info.getId() + ".pcd");
+            try {
+                file.transferTo(pcdFile);
+            } catch (Exception e) {
+                log.error(e.getMessage());
+            }
+            return ResponseData.success();
+        } else {
+            return ResponseData.error("附件上传失败!");
+        }
+    }
+
     @SneakyThrows
     @ApiOperation(value = "PCD下载接口")
     @PostMapping(value = "/pcdDownload")
@@ -136,6 +170,10 @@
     @ApiOperation(value = "启动或关闭循迹功能", notes = "返回 1成功 0失败  wait 启动时是否等待，0代表循迹启动即运动，不等待；1代表循迹启动原地等待。若无此字段，默认循迹启动不等待直接运动")
     @PostMapping(value = "/startTrack")
     public ResponseDataDTO<Integer> startTrack(@RequestBody StartTrackRequest request) {
+        request.setIsTask(1);
+        if (request.getStepLen() == null) {
+            request.setStepLen(2);
+        }
         baseRobotService.startTrack(request);
         return ResponseDataDTO.success();
     }
@@ -156,6 +194,13 @@
         MessageRequestDTO<CrlNavRequest> messageDTO = new MessageRequestDTO();
         messageDTO.setMsg(request);
         ResponseDataDTO<Integer> list = baseRobotService.getRobotRouteOptService().robotCmd(info, InstructCodeEnums.CTRL_NAV, messageDTO);
+        if (request.getAction() == 1) {
+            CostMapResolutionRequest costMap = new CostMapResolutionRequest();
+            costMap.setResolution(info.getResolution().doubleValue());
+            request.setRobotId(request.getRobotId());
+            //是否下发分辨率
+            CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CONFIG_COSTMAP_RESOLUTION_SET + request.getRobotId(), costMap);
+        }
         return list;
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java
new file mode 100644
index 0000000..5317651
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java
@@ -0,0 +1,86 @@
+package com.casic.missiles.modular.robot.controller;
+
+import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
+import com.baomidou.mybatisplus.extension.plugins.pagination.Page;
+import com.casic.missiles.core.base.controller.BaseController;
+import com.casic.missiles.core.page.PageFactory;
+import com.casic.missiles.core.page.PageInfoBT;
+import com.casic.missiles.model.form.IdForms;
+import com.casic.missiles.model.response.dto.ResponseDataDTO;
+import com.casic.missiles.modular.robot.model.TaskGridInfo;
+import com.casic.missiles.modular.robot.service.ITaskGridInfoService;
+import com.casic.missiles.modular.robot.vo.TaskGridInfoVO;
+import io.swagger.annotations.Api;
+import io.swagger.annotations.ApiOperation;
+import org.springframework.web.bind.annotation.*;
+
+import java.util.List;
+
+/**
+ * 任务网格信息Controller
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+@Api(tags = "任务网格信息")
+@RestController
+@RequestMapping("/robot/taskGridInfo")
+public class TaskGridInfoController extends BaseController {
+
+    private final ITaskGridInfoService taskGridInfoService;
+
+    public TaskGridInfoController(ITaskGridInfoService taskGridInfoService) {
+        this.taskGridInfoService = taskGridInfoService;
+    }
+
+    @ApiOperation(value = "列表查询")
+    @GetMapping(value = "/list")
+    public ResponseDataDTO<List<TaskGridInfo>> list(TaskGridInfoVO taskGridInfoVO) {
+        QueryWrapper<TaskGridInfo> query = taskGridInfoVO.genQuery();
+        return ResponseDataDTO.success(taskGridInfoService.list(query));
+    }
+
+    @ApiOperation(value = "分页查询")
+    @GetMapping(value = "/listPage")
+    public ResponseDataDTO<PageInfoBT<TaskGridInfo>> listPage(TaskGridInfoVO taskGridInfoVO) {
+        Page<TaskGridInfo> page = PageFactory.defaultPage();
+        QueryWrapper<TaskGridInfo> query = taskGridInfoVO.genQuery();
+        page = taskGridInfoService.page(page, query);
+        return ResponseDataDTO.success(super.packForBT(page));
+    }
+
+
+    @ApiOperation(value = "新增接口")
+    @PostMapping(value = "/add")
+    public ResponseDataDTO add(@RequestBody TaskGridInfo taskGridInfo) {
+        taskGridInfoService.save(taskGridInfo);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "修改接口")
+    @PostMapping(value = "/update")
+    public ResponseDataDTO update(@RequestBody TaskGridInfo taskGridInfo) {
+        taskGridInfoService.updateById(taskGridInfo);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "删除接口")
+    @GetMapping(value = "/delete")
+    public ResponseDataDTO delete(String id) {
+        taskGridInfoService.removeById(id);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation("批量删除")
+    @PostMapping("/batchDelete")
+    public ResponseDataDTO batchDelete(@RequestBody IdForms ids) {
+        taskGridInfoService.removeByIds(ids.getIds());
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "详情查询")
+    @GetMapping(value = "/detail")
+    public ResponseDataDTO<TaskGridInfo> detail(String id) {
+        return ResponseDataDTO.success(taskGridInfoService.getById(id));
+    }
+}
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
index e82a792..6992274 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
@@ -98,7 +98,7 @@
     public ResponseDataDTO<AlgorithmResponse> test() {
         AlgorithmResponse response = new AlgorithmResponse();
         response.setTaskHeatMap(new TaskHeatMap());
-        algorithmService.readAlgorithmResponse("step3.txt",response,123L,null);
+        algorithmService.readAlgorithmResponse("step3.txt",null,response,123L,null);
         sendWebSocket(InstructStatusMsgEnums.HEAT_MAP, "1", response.getTaskHeatMap());
         return ResponseDataDTO.success(response);
     }
@@ -121,7 +121,7 @@
     @GetMapping(value = "/testGrid")
     public ResponseDataDTO<List<RouteInfo>> testGrid(@RequestBody PathInitDTO pathInitDTO) {
         RobotInfo robotInfo = infoService.getById(pathInitDTO.getRobotId());
-        GridPointDTO gridPointDTO = algorithmService.convertSlamToGrid(new BigDecimal(pathInitDTO.getX()), new BigDecimal(10), new BigDecimal(4), robotInfo);
+        GridPointDTO gridPointDTO = algorithmService.convertSlamToGrid(new BigDecimal(pathInitDTO.getXNow()), new BigDecimal(10), new BigDecimal(4), robotInfo);
         return ResponseDataDTO.success(algorithmService.convertGridCenterPoint(gridPointDTO, new BigDecimal(10), robotInfo));
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/TaskGridInfoMapper.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/TaskGridInfoMapper.java
new file mode 100644
index 0000000..5c3a70b
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/TaskGridInfoMapper.java
@@ -0,0 +1,23 @@
+package com.casic.missiles.modular.robot.dao;
+
+
+import com.baomidou.mybatisplus.core.mapper.BaseMapper;
+import com.casic.missiles.modular.robot.model.TaskGridInfo;
+import org.apache.ibatis.annotations.Param;
+import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
+import com.baomidou.mybatisplus.extension.plugins.pagination.Page;
+import java.util.List;
+/**
+ * <p>
+ *  任务网格信息 Mapper 接口
+ * </p>
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+public interface TaskGridInfoMapper extends BaseMapper<TaskGridInfo> {
+/**
+* 任务网格信息 分页检索
+*/
+List<TaskGridInfo> selectTaskGridInfoPage( @Param("page") Page<TaskGridInfo> page,@Param("ew" ) QueryWrapper<TaskGridInfo> query);
+}
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/mapping/TaskGridInfoMapper.xml b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/mapping/TaskGridInfoMapper.xml
new file mode 100644
index 0000000..be573e0
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/mapping/TaskGridInfoMapper.xml
@@ -0,0 +1,80 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!DOCTYPE mapper PUBLIC "-//mybatis.org//DTD Mapper 3.0//EN" "http://mybatis.org/dtd/mybatis-3-mapper.dtd">
+<mapper namespace="com.casic.missiles.modular.robot.dao.TaskGridInfoMapper">
+    <!-- 通用查询映射结果 -->
+    <resultMap id="BaseResultMap" type="com.casic.missiles.modular.robot.model.TaskGridInfo">
+        <result column="id" property="id"/>
+        <result column="create_time" property="createTime"/>
+        <result column="update_time" property="updateTime"/>
+        <result column="task_id" property="taskId"/>
+        <result column="estimate_grid_x" property="estimateGridX"/>
+        <result column="estimate_grid_y" property="estimateGridY"/>
+        <result column="neutron_count" property="neutronCount"/>
+        <result column="cur_grid_x" property="curGridX"/>
+        <result column="cur_grid_y" property="curGridY"/>
+    </resultMap>
+    <!-- 通用查询结果列 -->
+    <sql id="Base_Column_List">
+        id
+        ,
+                    create_time as createTime ,
+                    update_time as updateTime ,
+                    task_id as taskId ,
+                    estimate_grid_x as estimateGridX ,
+                    estimate_grid_y as estimateGridY ,
+                    neutron_count as neutronCount ,
+                    cur_grid_x as curGridX ,
+                    cur_grid_y as curGridY
+    </sql>
+    <sql id="Base_Column_Alias">
+        a
+        .
+        id
+        ,
+            a.create_time
+            as createTime ,
+            a.update_time
+            as updateTime ,
+            a.task_id
+            as taskId ,
+            a.estimate_grid_x
+            as estimateGridX ,
+            a.estimate_grid_y
+            as estimateGridY ,
+            a.neutron_count
+            as neutronCount ,
+            a.cur_grid_x
+            as curGridX ,
+            a.cur_grid_y
+            as curGridY
+    </sql>
+    <select id="selectTaskGridInfoPage" resultType="com.casic.missiles.modular.robot.model.TaskGridInfo">
+        select
+        <include refid="Base_Column_Alias"/>
+        from task_grid_info a
+        <where>
+            <if test="ew != null">
+                ${ew.sqlSegment}
+            </if>
+        </where>
+    </select>
+    <sql id="Base_Column_Query">
+        <if test="request!=null">
+            <if test="request.taskId!= null and request.taskId!=''">
+                and task_id = #{request.taskId}
+            </if>
+            <if test="request.estimateGridX!= null and request.estimateGridX!=''">
+                and estimate_grid_x = #{request.estimateGridX}
+            </if>
+            <if test="request.estimateGridY!= null and request.estimateGridY!=''">
+                and estimate_grid_y = #{request.estimateGridY}
+            </if>
+            <if test="request.curGridX!= null and request.curGridX!=''">
+                and cur_grid_x = #{request.curGridX}
+            </if>
+            <if test="request.curGridY!= null and request.curGridY!=''">
+                and cur_grid_y = #{request.curGridY}
+            </if>
+        </if>
+    </sql>
+</mapper>
\ No newline at end of file
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java
index 749f559..4559074 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java
@@ -13,4 +13,9 @@
      * 规划点位信息
      */
     private PointNextDTO pointNextDTO;
+    /**
+     * 寻源目标地址
+     */
+    private PointNextDTO coordinate;
+    private boolean isEnd;
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java
index 39ff2a2..cef3238 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java
@@ -12,6 +12,9 @@
     private BigDecimal y;
 
     public static GridPointDTO convertGridPoint(PointNextDTO nextDTO) {
+        if(nextDTO==null){
+            return null;
+        }
         GridPointDTO gridPointDTO = new GridPointDTO();
         gridPointDTO.setX(Convert.toBigDecimal(nextDTO.getX()));
         gridPointDTO.setY(Convert.toBigDecimal(nextDTO.getY()));

diff --git a/.gitignore b/.gitignore
index 9e20bc1..f42a9cb 100644
--- a/.gitignore
+++ b/.gitignore
@@ -2,3 +2,4 @@
 **/target/**
 logs/
 *.iml
+*.exe
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java b/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
index c7b5d28..a849528 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
@@ -28,6 +28,7 @@
     String KEY_CUR_TASK_ID = "CUR_TASK_ID_";
     String KEY_CUR_TASK_INIT = "CUR_TASK_INIT_";
 
+    String KEY_COST_MAP = "KEY_COST_MAP";
     /**
      * PSD数据ID
      */
@@ -41,6 +42,7 @@
      */
     String KEY_CUR_XY_POINT = "KEY_CUR_XY_POINT_";
     String KEY_CUR_ESTIMATE_GRID = "KEY_CUR_ESTIMATE_GRID_";
+    String KEY_PLANING_FLAG = "KEY_PLANING_FLAG_";
     /**
      * 机器人巡点栅格
      */
@@ -55,6 +57,7 @@
      * 探测器配置参数缓存
      */
     String KEY_CONFIG_PARAMS_ID = "KEY_CONFIG_PARAMS_ID_";
+    String KEY_CONFIG_COSTMAP_RESOLUTION_SET = "KEY_CONFIG_COSTMAP_RESOLUTION_SET_";
 
     /**
      * 暂停状态缓存信息
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
index c4c905d..3bb475f 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
@@ -1,9 +1,14 @@
 package com.casic.missiles.modular.robot.controller;
 
+import cn.hutool.core.lang.Assert;
 import cn.hutool.core.util.StrUtil;
 import com.casic.missiles.core.base.controller.BaseController;
+import com.casic.missiles.core.cache.CacheUtil;
 import com.casic.missiles.core.util.SpringContextHolder;
+import com.casic.missiles.model.exception.RequestValidException;
+import com.casic.missiles.model.response.ResponseData;
 import com.casic.missiles.model.response.dto.ResponseDataDTO;
+import com.casic.missiles.modular.constants.RobotDictConstants;
 import com.casic.missiles.modular.robot.IBaseRobotService;
 import com.casic.missiles.modular.robot.dto.RobotInfoSetDTO;
 import com.casic.missiles.modular.robot.model.RobotInfo;
@@ -19,10 +24,16 @@
 import io.swagger.annotations.ApiOperation;
 import io.swagger.annotations.ApiParam;
 import lombok.SneakyThrows;
+import lombok.extern.slf4j.Slf4j;
+import org.apache.ibatis.annotations.Param;
+import org.springframework.beans.factory.annotation.Value;
 import org.springframework.web.bind.annotation.*;
+import org.springframework.web.multipart.MultipartFile;
 
 import javax.servlet.http.HttpServletResponse;
+import java.io.File;
 import java.util.List;
+import java.util.Optional;
 
 /**
  * 机器人操作类
@@ -30,6 +41,7 @@
  * @author lwh
  * @date 2023-10-30
  */
+@Slf4j
 @Api(tags = "机器人控制接口")
 @RestController
 @RequestMapping("/robot/opt")
@@ -61,6 +73,28 @@
         return baseRobotService.execCmdHandler(request, InstructCodeEnums.CHANGE_PCD);
     }
 
+    @Value("${casic.file.uploadPath}")
+    private String filePath;
+
+    @SneakyThrows
+    @ApiOperation(value = "PCD上传接口")
+    @PostMapping(value = "/uploadPcd")
+    public ResponseData uploadPcd(@RequestParam("file") MultipartFile file, @ApiParam("机器人ID,url拼接传参") String robotId) {
+        Assert.isFalse(StrUtil.isEmpty(robotId), () -> new RequestValidException("机器ID不能为空", Optional.of(new Throwable().getStackTrace()[2])));
+        RobotInfo info = robotInfoService.getById(robotId);
+        if (file != null && !file.isEmpty()) {
+            File pcdFile = new File(filePath + "finalCloud_" + info.getId() + ".pcd");
+            try {
+                file.transferTo(pcdFile);
+            } catch (Exception e) {
+                log.error(e.getMessage());
+            }
+            return ResponseData.success();
+        } else {
+            return ResponseData.error("附件上传失败!");
+        }
+    }
+
     @SneakyThrows
     @ApiOperation(value = "PCD下载接口")
     @PostMapping(value = "/pcdDownload")
@@ -136,6 +170,10 @@
     @ApiOperation(value = "启动或关闭循迹功能", notes = "返回 1成功 0失败  wait 启动时是否等待，0代表循迹启动即运动，不等待；1代表循迹启动原地等待。若无此字段，默认循迹启动不等待直接运动")
     @PostMapping(value = "/startTrack")
     public ResponseDataDTO<Integer> startTrack(@RequestBody StartTrackRequest request) {
+        request.setIsTask(1);
+        if (request.getStepLen() == null) {
+            request.setStepLen(2);
+        }
         baseRobotService.startTrack(request);
         return ResponseDataDTO.success();
     }
@@ -156,6 +194,13 @@
         MessageRequestDTO<CrlNavRequest> messageDTO = new MessageRequestDTO();
         messageDTO.setMsg(request);
         ResponseDataDTO<Integer> list = baseRobotService.getRobotRouteOptService().robotCmd(info, InstructCodeEnums.CTRL_NAV, messageDTO);
+        if (request.getAction() == 1) {
+            CostMapResolutionRequest costMap = new CostMapResolutionRequest();
+            costMap.setResolution(info.getResolution().doubleValue());
+            request.setRobotId(request.getRobotId());
+            //是否下发分辨率
+            CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CONFIG_COSTMAP_RESOLUTION_SET + request.getRobotId(), costMap);
+        }
         return list;
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java
new file mode 100644
index 0000000..5317651
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java
@@ -0,0 +1,86 @@
+package com.casic.missiles.modular.robot.controller;
+
+import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
+import com.baomidou.mybatisplus.extension.plugins.pagination.Page;
+import com.casic.missiles.core.base.controller.BaseController;
+import com.casic.missiles.core.page.PageFactory;
+import com.casic.missiles.core.page.PageInfoBT;
+import com.casic.missiles.model.form.IdForms;
+import com.casic.missiles.model.response.dto.ResponseDataDTO;
+import com.casic.missiles.modular.robot.model.TaskGridInfo;
+import com.casic.missiles.modular.robot.service.ITaskGridInfoService;
+import com.casic.missiles.modular.robot.vo.TaskGridInfoVO;
+import io.swagger.annotations.Api;
+import io.swagger.annotations.ApiOperation;
+import org.springframework.web.bind.annotation.*;
+
+import java.util.List;
+
+/**
+ * 任务网格信息Controller
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+@Api(tags = "任务网格信息")
+@RestController
+@RequestMapping("/robot/taskGridInfo")
+public class TaskGridInfoController extends BaseController {
+
+    private final ITaskGridInfoService taskGridInfoService;
+
+    public TaskGridInfoController(ITaskGridInfoService taskGridInfoService) {
+        this.taskGridInfoService = taskGridInfoService;
+    }
+
+    @ApiOperation(value = "列表查询")
+    @GetMapping(value = "/list")
+    public ResponseDataDTO<List<TaskGridInfo>> list(TaskGridInfoVO taskGridInfoVO) {
+        QueryWrapper<TaskGridInfo> query = taskGridInfoVO.genQuery();
+        return ResponseDataDTO.success(taskGridInfoService.list(query));
+    }
+
+    @ApiOperation(value = "分页查询")
+    @GetMapping(value = "/listPage")
+    public ResponseDataDTO<PageInfoBT<TaskGridInfo>> listPage(TaskGridInfoVO taskGridInfoVO) {
+        Page<TaskGridInfo> page = PageFactory.defaultPage();
+        QueryWrapper<TaskGridInfo> query = taskGridInfoVO.genQuery();
+        page = taskGridInfoService.page(page, query);
+        return ResponseDataDTO.success(super.packForBT(page));
+    }
+
+
+    @ApiOperation(value = "新增接口")
+    @PostMapping(value = "/add")
+    public ResponseDataDTO add(@RequestBody TaskGridInfo taskGridInfo) {
+        taskGridInfoService.save(taskGridInfo);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "修改接口")
+    @PostMapping(value = "/update")
+    public ResponseDataDTO update(@RequestBody TaskGridInfo taskGridInfo) {
+        taskGridInfoService.updateById(taskGridInfo);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "删除接口")
+    @GetMapping(value = "/delete")
+    public ResponseDataDTO delete(String id) {
+        taskGridInfoService.removeById(id);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation("批量删除")
+    @PostMapping("/batchDelete")
+    public ResponseDataDTO batchDelete(@RequestBody IdForms ids) {
+        taskGridInfoService.removeByIds(ids.getIds());
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "详情查询")
+    @GetMapping(value = "/detail")
+    public ResponseDataDTO<TaskGridInfo> detail(String id) {
+        return ResponseDataDTO.success(taskGridInfoService.getById(id));
+    }
+}
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
index e82a792..6992274 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
@@ -98,7 +98,7 @@
     public ResponseDataDTO<AlgorithmResponse> test() {
         AlgorithmResponse response = new AlgorithmResponse();
         response.setTaskHeatMap(new TaskHeatMap());
-        algorithmService.readAlgorithmResponse("step3.txt",response,123L,null);
+        algorithmService.readAlgorithmResponse("step3.txt",null,response,123L,null);
         sendWebSocket(InstructStatusMsgEnums.HEAT_MAP, "1", response.getTaskHeatMap());
         return ResponseDataDTO.success(response);
     }
@@ -121,7 +121,7 @@
     @GetMapping(value = "/testGrid")
     public ResponseDataDTO<List<RouteInfo>> testGrid(@RequestBody PathInitDTO pathInitDTO) {
         RobotInfo robotInfo = infoService.getById(pathInitDTO.getRobotId());
-        GridPointDTO gridPointDTO = algorithmService.convertSlamToGrid(new BigDecimal(pathInitDTO.getX()), new BigDecimal(10), new BigDecimal(4), robotInfo);
+        GridPointDTO gridPointDTO = algorithmService.convertSlamToGrid(new BigDecimal(pathInitDTO.getXNow()), new BigDecimal(10), new BigDecimal(4), robotInfo);
         return ResponseDataDTO.success(algorithmService.convertGridCenterPoint(gridPointDTO, new BigDecimal(10), robotInfo));
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/TaskGridInfoMapper.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/TaskGridInfoMapper.java
new file mode 100644
index 0000000..5c3a70b
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/TaskGridInfoMapper.java
@@ -0,0 +1,23 @@
+package com.casic.missiles.modular.robot.dao;
+
+
+import com.baomidou.mybatisplus.core.mapper.BaseMapper;
+import com.casic.missiles.modular.robot.model.TaskGridInfo;
+import org.apache.ibatis.annotations.Param;
+import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
+import com.baomidou.mybatisplus.extension.plugins.pagination.Page;
+import java.util.List;
+/**
+ * <p>
+ *  任务网格信息 Mapper 接口
+ * </p>
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+public interface TaskGridInfoMapper extends BaseMapper<TaskGridInfo> {
+/**
+* 任务网格信息 分页检索
+*/
+List<TaskGridInfo> selectTaskGridInfoPage( @Param("page") Page<TaskGridInfo> page,@Param("ew" ) QueryWrapper<TaskGridInfo> query);
+}
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/mapping/TaskGridInfoMapper.xml b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/mapping/TaskGridInfoMapper.xml
new file mode 100644
index 0000000..be573e0
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/mapping/TaskGridInfoMapper.xml
@@ -0,0 +1,80 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!DOCTYPE mapper PUBLIC "-//mybatis.org//DTD Mapper 3.0//EN" "http://mybatis.org/dtd/mybatis-3-mapper.dtd">
+<mapper namespace="com.casic.missiles.modular.robot.dao.TaskGridInfoMapper">
+    <!-- 通用查询映射结果 -->
+    <resultMap id="BaseResultMap" type="com.casic.missiles.modular.robot.model.TaskGridInfo">
+        <result column="id" property="id"/>
+        <result column="create_time" property="createTime"/>
+        <result column="update_time" property="updateTime"/>
+        <result column="task_id" property="taskId"/>
+        <result column="estimate_grid_x" property="estimateGridX"/>
+        <result column="estimate_grid_y" property="estimateGridY"/>
+        <result column="neutron_count" property="neutronCount"/>
+        <result column="cur_grid_x" property="curGridX"/>
+        <result column="cur_grid_y" property="curGridY"/>
+    </resultMap>
+    <!-- 通用查询结果列 -->
+    <sql id="Base_Column_List">
+        id
+        ,
+                    create_time as createTime ,
+                    update_time as updateTime ,
+                    task_id as taskId ,
+                    estimate_grid_x as estimateGridX ,
+                    estimate_grid_y as estimateGridY ,
+                    neutron_count as neutronCount ,
+                    cur_grid_x as curGridX ,
+                    cur_grid_y as curGridY
+    </sql>
+    <sql id="Base_Column_Alias">
+        a
+        .
+        id
+        ,
+            a.create_time
+            as createTime ,
+            a.update_time
+            as updateTime ,
+            a.task_id
+            as taskId ,
+            a.estimate_grid_x
+            as estimateGridX ,
+            a.estimate_grid_y
+            as estimateGridY ,
+            a.neutron_count
+            as neutronCount ,
+            a.cur_grid_x
+            as curGridX ,
+            a.cur_grid_y
+            as curGridY
+    </sql>
+    <select id="selectTaskGridInfoPage" resultType="com.casic.missiles.modular.robot.model.TaskGridInfo">
+        select
+        <include refid="Base_Column_Alias"/>
+        from task_grid_info a
+        <where>
+            <if test="ew != null">
+                ${ew.sqlSegment}
+            </if>
+        </where>
+    </select>
+    <sql id="Base_Column_Query">
+        <if test="request!=null">
+            <if test="request.taskId!= null and request.taskId!=''">
+                and task_id = #{request.taskId}
+            </if>
+            <if test="request.estimateGridX!= null and request.estimateGridX!=''">
+                and estimate_grid_x = #{request.estimateGridX}
+            </if>
+            <if test="request.estimateGridY!= null and request.estimateGridY!=''">
+                and estimate_grid_y = #{request.estimateGridY}
+            </if>
+            <if test="request.curGridX!= null and request.curGridX!=''">
+                and cur_grid_x = #{request.curGridX}
+            </if>
+            <if test="request.curGridY!= null and request.curGridY!=''">
+                and cur_grid_y = #{request.curGridY}
+            </if>
+        </if>
+    </sql>
+</mapper>
\ No newline at end of file
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java
index 749f559..4559074 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java
@@ -13,4 +13,9 @@
      * 规划点位信息
      */
     private PointNextDTO pointNextDTO;
+    /**
+     * 寻源目标地址
+     */
+    private PointNextDTO coordinate;
+    private boolean isEnd;
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java
index 39ff2a2..cef3238 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java
@@ -12,6 +12,9 @@
     private BigDecimal y;
 
     public static GridPointDTO convertGridPoint(PointNextDTO nextDTO) {
+        if(nextDTO==null){
+            return null;
+        }
         GridPointDTO gridPointDTO = new GridPointDTO();
         gridPointDTO.setX(Convert.toBigDecimal(nextDTO.getX()));
         gridPointDTO.setY(Convert.toBigDecimal(nextDTO.getY()));
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java
index 8c88242..33be25d 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java
@@ -12,25 +12,19 @@
 public class PathInitDTO {
     //---------------------算法参数
     /**
-     * x坐标最大值（x方向栅格总数）
+     * 初始位置x坐标(栅格地图下的坐标)
      */
-    private String xMax;
+    private String xNow;
     /**
-     * y坐标最大值（y方向栅格总数）
+     * 初始位置y坐标(栅格地图下的坐标)
      */
-    private String yMax;
+    private String yNow;
     /**
      * 初始位置计数率
      */
-    private String count;
-    /**
-     * 初始位置x坐标(0<=x<x_max)
-     */
-    private String x;
-    /**
-     * 初始位置y坐标(0<=y<y_max)
-     */
-    private String y;
+    private String zNow;
+
+    private Integer stepLen;
 
     //---------------------其他执行参数
     private String robotId;
@@ -38,11 +32,11 @@
 
     public List<String> toArray() {
         List<String> list = new ArrayList<>();
-        list.add(xMax);
-        list.add(yMax);
-        list.add(count);
-        list.add(x);
-        list.add(y);
+        list.add(xNow);
+        list.add(yNow);
+        list.add(stepLen+"");
+        list.add(zNow);
+        list.add(taskId+"");
         return list;
     }
 }

diff --git a/.gitignore b/.gitignore
index 9e20bc1..f42a9cb 100644
--- a/.gitignore
+++ b/.gitignore
@@ -2,3 +2,4 @@
 **/target/**
 logs/
 *.iml
+*.exe
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java b/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
index c7b5d28..a849528 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
@@ -28,6 +28,7 @@
     String KEY_CUR_TASK_ID = "CUR_TASK_ID_";
     String KEY_CUR_TASK_INIT = "CUR_TASK_INIT_";
 
+    String KEY_COST_MAP = "KEY_COST_MAP";
     /**
      * PSD数据ID
      */
@@ -41,6 +42,7 @@
      */
     String KEY_CUR_XY_POINT = "KEY_CUR_XY_POINT_";
     String KEY_CUR_ESTIMATE_GRID = "KEY_CUR_ESTIMATE_GRID_";
+    String KEY_PLANING_FLAG = "KEY_PLANING_FLAG_";
     /**
      * 机器人巡点栅格
      */
@@ -55,6 +57,7 @@
      * 探测器配置参数缓存
      */
     String KEY_CONFIG_PARAMS_ID = "KEY_CONFIG_PARAMS_ID_";
+    String KEY_CONFIG_COSTMAP_RESOLUTION_SET = "KEY_CONFIG_COSTMAP_RESOLUTION_SET_";
 
     /**
      * 暂停状态缓存信息
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
index c4c905d..3bb475f 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
@@ -1,9 +1,14 @@
 package com.casic.missiles.modular.robot.controller;
 
+import cn.hutool.core.lang.Assert;
 import cn.hutool.core.util.StrUtil;
 import com.casic.missiles.core.base.controller.BaseController;
+import com.casic.missiles.core.cache.CacheUtil;
 import com.casic.missiles.core.util.SpringContextHolder;
+import com.casic.missiles.model.exception.RequestValidException;
+import com.casic.missiles.model.response.ResponseData;
 import com.casic.missiles.model.response.dto.ResponseDataDTO;
+import com.casic.missiles.modular.constants.RobotDictConstants;
 import com.casic.missiles.modular.robot.IBaseRobotService;
 import com.casic.missiles.modular.robot.dto.RobotInfoSetDTO;
 import com.casic.missiles.modular.robot.model.RobotInfo;
@@ -19,10 +24,16 @@
 import io.swagger.annotations.ApiOperation;
 import io.swagger.annotations.ApiParam;
 import lombok.SneakyThrows;
+import lombok.extern.slf4j.Slf4j;
+import org.apache.ibatis.annotations.Param;
+import org.springframework.beans.factory.annotation.Value;
 import org.springframework.web.bind.annotation.*;
+import org.springframework.web.multipart.MultipartFile;
 
 import javax.servlet.http.HttpServletResponse;
+import java.io.File;
 import java.util.List;
+import java.util.Optional;
 
 /**
  * 机器人操作类
@@ -30,6 +41,7 @@
  * @author lwh
  * @date 2023-10-30
  */
+@Slf4j
 @Api(tags = "机器人控制接口")
 @RestController
 @RequestMapping("/robot/opt")
@@ -61,6 +73,28 @@
         return baseRobotService.execCmdHandler(request, InstructCodeEnums.CHANGE_PCD);
     }
 
+    @Value("${casic.file.uploadPath}")
+    private String filePath;
+
+    @SneakyThrows
+    @ApiOperation(value = "PCD上传接口")
+    @PostMapping(value = "/uploadPcd")
+    public ResponseData uploadPcd(@RequestParam("file") MultipartFile file, @ApiParam("机器人ID,url拼接传参") String robotId) {
+        Assert.isFalse(StrUtil.isEmpty(robotId), () -> new RequestValidException("机器ID不能为空", Optional.of(new Throwable().getStackTrace()[2])));
+        RobotInfo info = robotInfoService.getById(robotId);
+        if (file != null && !file.isEmpty()) {
+            File pcdFile = new File(filePath + "finalCloud_" + info.getId() + ".pcd");
+            try {
+                file.transferTo(pcdFile);
+            } catch (Exception e) {
+                log.error(e.getMessage());
+            }
+            return ResponseData.success();
+        } else {
+            return ResponseData.error("附件上传失败!");
+        }
+    }
+
     @SneakyThrows
     @ApiOperation(value = "PCD下载接口")
     @PostMapping(value = "/pcdDownload")
@@ -136,6 +170,10 @@
     @ApiOperation(value = "启动或关闭循迹功能", notes = "返回 1成功 0失败  wait 启动时是否等待，0代表循迹启动即运动，不等待；1代表循迹启动原地等待。若无此字段，默认循迹启动不等待直接运动")
     @PostMapping(value = "/startTrack")
     public ResponseDataDTO<Integer> startTrack(@RequestBody StartTrackRequest request) {
+        request.setIsTask(1);
+        if (request.getStepLen() == null) {
+            request.setStepLen(2);
+        }
         baseRobotService.startTrack(request);
         return ResponseDataDTO.success();
     }
@@ -156,6 +194,13 @@
         MessageRequestDTO<CrlNavRequest> messageDTO = new MessageRequestDTO();
         messageDTO.setMsg(request);
         ResponseDataDTO<Integer> list = baseRobotService.getRobotRouteOptService().robotCmd(info, InstructCodeEnums.CTRL_NAV, messageDTO);
+        if (request.getAction() == 1) {
+            CostMapResolutionRequest costMap = new CostMapResolutionRequest();
+            costMap.setResolution(info.getResolution().doubleValue());
+            request.setRobotId(request.getRobotId());
+            //是否下发分辨率
+            CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CONFIG_COSTMAP_RESOLUTION_SET + request.getRobotId(), costMap);
+        }
         return list;
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java
new file mode 100644
index 0000000..5317651
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java
@@ -0,0 +1,86 @@
+package com.casic.missiles.modular.robot.controller;
+
+import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
+import com.baomidou.mybatisplus.extension.plugins.pagination.Page;
+import com.casic.missiles.core.base.controller.BaseController;
+import com.casic.missiles.core.page.PageFactory;
+import com.casic.missiles.core.page.PageInfoBT;
+import com.casic.missiles.model.form.IdForms;
+import com.casic.missiles.model.response.dto.ResponseDataDTO;
+import com.casic.missiles.modular.robot.model.TaskGridInfo;
+import com.casic.missiles.modular.robot.service.ITaskGridInfoService;
+import com.casic.missiles.modular.robot.vo.TaskGridInfoVO;
+import io.swagger.annotations.Api;
+import io.swagger.annotations.ApiOperation;
+import org.springframework.web.bind.annotation.*;
+
+import java.util.List;
+
+/**
+ * 任务网格信息Controller
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+@Api(tags = "任务网格信息")
+@RestController
+@RequestMapping("/robot/taskGridInfo")
+public class TaskGridInfoController extends BaseController {
+
+    private final ITaskGridInfoService taskGridInfoService;
+
+    public TaskGridInfoController(ITaskGridInfoService taskGridInfoService) {
+        this.taskGridInfoService = taskGridInfoService;
+    }
+
+    @ApiOperation(value = "列表查询")
+    @GetMapping(value = "/list")
+    public ResponseDataDTO<List<TaskGridInfo>> list(TaskGridInfoVO taskGridInfoVO) {
+        QueryWrapper<TaskGridInfo> query = taskGridInfoVO.genQuery();
+        return ResponseDataDTO.success(taskGridInfoService.list(query));
+    }
+
+    @ApiOperation(value = "分页查询")
+    @GetMapping(value = "/listPage")
+    public ResponseDataDTO<PageInfoBT<TaskGridInfo>> listPage(TaskGridInfoVO taskGridInfoVO) {
+        Page<TaskGridInfo> page = PageFactory.defaultPage();
+        QueryWrapper<TaskGridInfo> query = taskGridInfoVO.genQuery();
+        page = taskGridInfoService.page(page, query);
+        return ResponseDataDTO.success(super.packForBT(page));
+    }
+
+
+    @ApiOperation(value = "新增接口")
+    @PostMapping(value = "/add")
+    public ResponseDataDTO add(@RequestBody TaskGridInfo taskGridInfo) {
+        taskGridInfoService.save(taskGridInfo);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "修改接口")
+    @PostMapping(value = "/update")
+    public ResponseDataDTO update(@RequestBody TaskGridInfo taskGridInfo) {
+        taskGridInfoService.updateById(taskGridInfo);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "删除接口")
+    @GetMapping(value = "/delete")
+    public ResponseDataDTO delete(String id) {
+        taskGridInfoService.removeById(id);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation("批量删除")
+    @PostMapping("/batchDelete")
+    public ResponseDataDTO batchDelete(@RequestBody IdForms ids) {
+        taskGridInfoService.removeByIds(ids.getIds());
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "详情查询")
+    @GetMapping(value = "/detail")
+    public ResponseDataDTO<TaskGridInfo> detail(String id) {
+        return ResponseDataDTO.success(taskGridInfoService.getById(id));
+    }
+}
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
index e82a792..6992274 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
@@ -98,7 +98,7 @@
     public ResponseDataDTO<AlgorithmResponse> test() {
         AlgorithmResponse response = new AlgorithmResponse();
         response.setTaskHeatMap(new TaskHeatMap());
-        algorithmService.readAlgorithmResponse("step3.txt",response,123L,null);
+        algorithmService.readAlgorithmResponse("step3.txt",null,response,123L,null);
         sendWebSocket(InstructStatusMsgEnums.HEAT_MAP, "1", response.getTaskHeatMap());
         return ResponseDataDTO.success(response);
     }
@@ -121,7 +121,7 @@
     @GetMapping(value = "/testGrid")
     public ResponseDataDTO<List<RouteInfo>> testGrid(@RequestBody PathInitDTO pathInitDTO) {
         RobotInfo robotInfo = infoService.getById(pathInitDTO.getRobotId());
-        GridPointDTO gridPointDTO = algorithmService.convertSlamToGrid(new BigDecimal(pathInitDTO.getX()), new BigDecimal(10), new BigDecimal(4), robotInfo);
+        GridPointDTO gridPointDTO = algorithmService.convertSlamToGrid(new BigDecimal(pathInitDTO.getXNow()), new BigDecimal(10), new BigDecimal(4), robotInfo);
         return ResponseDataDTO.success(algorithmService.convertGridCenterPoint(gridPointDTO, new BigDecimal(10), robotInfo));
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/TaskGridInfoMapper.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/TaskGridInfoMapper.java
new file mode 100644
index 0000000..5c3a70b
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/TaskGridInfoMapper.java
@@ -0,0 +1,23 @@
+package com.casic.missiles.modular.robot.dao;
+
+
+import com.baomidou.mybatisplus.core.mapper.BaseMapper;
+import com.casic.missiles.modular.robot.model.TaskGridInfo;
+import org.apache.ibatis.annotations.Param;
+import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
+import com.baomidou.mybatisplus.extension.plugins.pagination.Page;
+import java.util.List;
+/**
+ * <p>
+ *  任务网格信息 Mapper 接口
+ * </p>
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+public interface TaskGridInfoMapper extends BaseMapper<TaskGridInfo> {
+/**
+* 任务网格信息 分页检索
+*/
+List<TaskGridInfo> selectTaskGridInfoPage( @Param("page") Page<TaskGridInfo> page,@Param("ew" ) QueryWrapper<TaskGridInfo> query);
+}
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/mapping/TaskGridInfoMapper.xml b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/mapping/TaskGridInfoMapper.xml
new file mode 100644
index 0000000..be573e0
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/mapping/TaskGridInfoMapper.xml
@@ -0,0 +1,80 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!DOCTYPE mapper PUBLIC "-//mybatis.org//DTD Mapper 3.0//EN" "http://mybatis.org/dtd/mybatis-3-mapper.dtd">
+<mapper namespace="com.casic.missiles.modular.robot.dao.TaskGridInfoMapper">
+    <!-- 通用查询映射结果 -->
+    <resultMap id="BaseResultMap" type="com.casic.missiles.modular.robot.model.TaskGridInfo">
+        <result column="id" property="id"/>
+        <result column="create_time" property="createTime"/>
+        <result column="update_time" property="updateTime"/>
+        <result column="task_id" property="taskId"/>
+        <result column="estimate_grid_x" property="estimateGridX"/>
+        <result column="estimate_grid_y" property="estimateGridY"/>
+        <result column="neutron_count" property="neutronCount"/>
+        <result column="cur_grid_x" property="curGridX"/>
+        <result column="cur_grid_y" property="curGridY"/>
+    </resultMap>
+    <!-- 通用查询结果列 -->
+    <sql id="Base_Column_List">
+        id
+        ,
+                    create_time as createTime ,
+                    update_time as updateTime ,
+                    task_id as taskId ,
+                    estimate_grid_x as estimateGridX ,
+                    estimate_grid_y as estimateGridY ,
+                    neutron_count as neutronCount ,
+                    cur_grid_x as curGridX ,
+                    cur_grid_y as curGridY
+    </sql>
+    <sql id="Base_Column_Alias">
+        a
+        .
+        id
+        ,
+            a.create_time
+            as createTime ,
+            a.update_time
+            as updateTime ,
+            a.task_id
+            as taskId ,
+            a.estimate_grid_x
+            as estimateGridX ,
+            a.estimate_grid_y
+            as estimateGridY ,
+            a.neutron_count
+            as neutronCount ,
+            a.cur_grid_x
+            as curGridX ,
+            a.cur_grid_y
+            as curGridY
+    </sql>
+    <select id="selectTaskGridInfoPage" resultType="com.casic.missiles.modular.robot.model.TaskGridInfo">
+        select
+        <include refid="Base_Column_Alias"/>
+        from task_grid_info a
+        <where>
+            <if test="ew != null">
+                ${ew.sqlSegment}
+            </if>
+        </where>
+    </select>
+    <sql id="Base_Column_Query">
+        <if test="request!=null">
+            <if test="request.taskId!= null and request.taskId!=''">
+                and task_id = #{request.taskId}
+            </if>
+            <if test="request.estimateGridX!= null and request.estimateGridX!=''">
+                and estimate_grid_x = #{request.estimateGridX}
+            </if>
+            <if test="request.estimateGridY!= null and request.estimateGridY!=''">
+                and estimate_grid_y = #{request.estimateGridY}
+            </if>
+            <if test="request.curGridX!= null and request.curGridX!=''">
+                and cur_grid_x = #{request.curGridX}
+            </if>
+            <if test="request.curGridY!= null and request.curGridY!=''">
+                and cur_grid_y = #{request.curGridY}
+            </if>
+        </if>
+    </sql>
+</mapper>
\ No newline at end of file
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java
index 749f559..4559074 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java
@@ -13,4 +13,9 @@
      * 规划点位信息
      */
     private PointNextDTO pointNextDTO;
+    /**
+     * 寻源目标地址
+     */
+    private PointNextDTO coordinate;
+    private boolean isEnd;
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java
index 39ff2a2..cef3238 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java
@@ -12,6 +12,9 @@
     private BigDecimal y;
 
     public static GridPointDTO convertGridPoint(PointNextDTO nextDTO) {
+        if(nextDTO==null){
+            return null;
+        }
         GridPointDTO gridPointDTO = new GridPointDTO();
         gridPointDTO.setX(Convert.toBigDecimal(nextDTO.getX()));
         gridPointDTO.setY(Convert.toBigDecimal(nextDTO.getY()));
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java
index 8c88242..33be25d 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java
@@ -12,25 +12,19 @@
 public class PathInitDTO {
     //---------------------算法参数
     /**
-     * x坐标最大值（x方向栅格总数）
+     * 初始位置x坐标(栅格地图下的坐标)
      */
-    private String xMax;
+    private String xNow;
     /**
-     * y坐标最大值（y方向栅格总数）
+     * 初始位置y坐标(栅格地图下的坐标)
      */
-    private String yMax;
+    private String yNow;
     /**
      * 初始位置计数率
      */
-    private String count;
-    /**
-     * 初始位置x坐标(0<=x<x_max)
-     */
-    private String x;
-    /**
-     * 初始位置y坐标(0<=y<y_max)
-     */
-    private String y;
+    private String zNow;
+
+    private Integer stepLen;
 
     //---------------------其他执行参数
     private String robotId;
@@ -38,11 +32,11 @@
 
     public List<String> toArray() {
         List<String> list = new ArrayList<>();
-        list.add(xMax);
-        list.add(yMax);
-        list.add(count);
-        list.add(x);
-        list.add(y);
+        list.add(xNow);
+        list.add(yNow);
+        list.add(stepLen+"");
+        list.add(zNow);
+        list.add(taskId+"");
         return list;
     }
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java
index 109e95c..ada6774 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java
@@ -10,21 +10,17 @@
  */
 @Data
 public class PathPlanDTO extends PathInitDTO {
-
     /**
-     * 长度为8的数组，记录(x-1, y-1), (x-1, y), (x-1, y+1), (x, y-1), (x, y+1), (x+1, y-1), (x+1, y), (x+1, y+1)8个坐标的可通行信息。
-     * 若某坐标有障碍物（不可通行行）则记为0，否则记为1
+     * 浮点阈值(阈值，超过该值后结束巡检开始寻源)
      */
-    private String open;
-
+    private String th;
     public List<String> toArray() {
         List<String> list = new ArrayList<>();
-        list.add(getXMax());
-        list.add(getYMax());
-        list.add(open);
-        list.add(getCount());
-        list.add(getX());
-        list.add(getY());
+
+        list.add(getStepLen()+"");
+        list.add(getTh());
+        list.add(getZNow());
+        list.add(getTaskId()+"");
         return list;
     }
 }

diff --git a/.gitignore b/.gitignore
index 9e20bc1..f42a9cb 100644
--- a/.gitignore
+++ b/.gitignore
@@ -2,3 +2,4 @@
 **/target/**
 logs/
 *.iml
+*.exe
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java b/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
index c7b5d28..a849528 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
@@ -28,6 +28,7 @@
     String KEY_CUR_TASK_ID = "CUR_TASK_ID_";
     String KEY_CUR_TASK_INIT = "CUR_TASK_INIT_";
 
+    String KEY_COST_MAP = "KEY_COST_MAP";
     /**
      * PSD数据ID
      */
@@ -41,6 +42,7 @@
      */
     String KEY_CUR_XY_POINT = "KEY_CUR_XY_POINT_";
     String KEY_CUR_ESTIMATE_GRID = "KEY_CUR_ESTIMATE_GRID_";
+    String KEY_PLANING_FLAG = "KEY_PLANING_FLAG_";
     /**
      * 机器人巡点栅格
      */
@@ -55,6 +57,7 @@
      * 探测器配置参数缓存
      */
     String KEY_CONFIG_PARAMS_ID = "KEY_CONFIG_PARAMS_ID_";
+    String KEY_CONFIG_COSTMAP_RESOLUTION_SET = "KEY_CONFIG_COSTMAP_RESOLUTION_SET_";
 
     /**
      * 暂停状态缓存信息
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
index c4c905d..3bb475f 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
@@ -1,9 +1,14 @@
 package com.casic.missiles.modular.robot.controller;
 
+import cn.hutool.core.lang.Assert;
 import cn.hutool.core.util.StrUtil;
 import com.casic.missiles.core.base.controller.BaseController;
+import com.casic.missiles.core.cache.CacheUtil;
 import com.casic.missiles.core.util.SpringContextHolder;
+import com.casic.missiles.model.exception.RequestValidException;
+import com.casic.missiles.model.response.ResponseData;
 import com.casic.missiles.model.response.dto.ResponseDataDTO;
+import com.casic.missiles.modular.constants.RobotDictConstants;
 import com.casic.missiles.modular.robot.IBaseRobotService;
 import com.casic.missiles.modular.robot.dto.RobotInfoSetDTO;
 import com.casic.missiles.modular.robot.model.RobotInfo;
@@ -19,10 +24,16 @@
 import io.swagger.annotations.ApiOperation;
 import io.swagger.annotations.ApiParam;
 import lombok.SneakyThrows;
+import lombok.extern.slf4j.Slf4j;
+import org.apache.ibatis.annotations.Param;
+import org.springframework.beans.factory.annotation.Value;
 import org.springframework.web.bind.annotation.*;
+import org.springframework.web.multipart.MultipartFile;
 
 import javax.servlet.http.HttpServletResponse;
+import java.io.File;
 import java.util.List;
+import java.util.Optional;
 
 /**
  * 机器人操作类
@@ -30,6 +41,7 @@
  * @author lwh
  * @date 2023-10-30
  */
+@Slf4j
 @Api(tags = "机器人控制接口")
 @RestController
 @RequestMapping("/robot/opt")
@@ -61,6 +73,28 @@
         return baseRobotService.execCmdHandler(request, InstructCodeEnums.CHANGE_PCD);
     }
 
+    @Value("${casic.file.uploadPath}")
+    private String filePath;
+
+    @SneakyThrows
+    @ApiOperation(value = "PCD上传接口")
+    @PostMapping(value = "/uploadPcd")
+    public ResponseData uploadPcd(@RequestParam("file") MultipartFile file, @ApiParam("机器人ID,url拼接传参") String robotId) {
+        Assert.isFalse(StrUtil.isEmpty(robotId), () -> new RequestValidException("机器ID不能为空", Optional.of(new Throwable().getStackTrace()[2])));
+        RobotInfo info = robotInfoService.getById(robotId);
+        if (file != null && !file.isEmpty()) {
+            File pcdFile = new File(filePath + "finalCloud_" + info.getId() + ".pcd");
+            try {
+                file.transferTo(pcdFile);
+            } catch (Exception e) {
+                log.error(e.getMessage());
+            }
+            return ResponseData.success();
+        } else {
+            return ResponseData.error("附件上传失败!");
+        }
+    }
+
     @SneakyThrows
     @ApiOperation(value = "PCD下载接口")
     @PostMapping(value = "/pcdDownload")
@@ -136,6 +170,10 @@
     @ApiOperation(value = "启动或关闭循迹功能", notes = "返回 1成功 0失败  wait 启动时是否等待，0代表循迹启动即运动，不等待；1代表循迹启动原地等待。若无此字段，默认循迹启动不等待直接运动")
     @PostMapping(value = "/startTrack")
     public ResponseDataDTO<Integer> startTrack(@RequestBody StartTrackRequest request) {
+        request.setIsTask(1);
+        if (request.getStepLen() == null) {
+            request.setStepLen(2);
+        }
         baseRobotService.startTrack(request);
         return ResponseDataDTO.success();
     }
@@ -156,6 +194,13 @@
         MessageRequestDTO<CrlNavRequest> messageDTO = new MessageRequestDTO();
         messageDTO.setMsg(request);
         ResponseDataDTO<Integer> list = baseRobotService.getRobotRouteOptService().robotCmd(info, InstructCodeEnums.CTRL_NAV, messageDTO);
+        if (request.getAction() == 1) {
+            CostMapResolutionRequest costMap = new CostMapResolutionRequest();
+            costMap.setResolution(info.getResolution().doubleValue());
+            request.setRobotId(request.getRobotId());
+            //是否下发分辨率
+            CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CONFIG_COSTMAP_RESOLUTION_SET + request.getRobotId(), costMap);
+        }
         return list;
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java
new file mode 100644
index 0000000..5317651
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java
@@ -0,0 +1,86 @@
+package com.casic.missiles.modular.robot.controller;
+
+import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
+import com.baomidou.mybatisplus.extension.plugins.pagination.Page;
+import com.casic.missiles.core.base.controller.BaseController;
+import com.casic.missiles.core.page.PageFactory;
+import com.casic.missiles.core.page.PageInfoBT;
+import com.casic.missiles.model.form.IdForms;
+import com.casic.missiles.model.response.dto.ResponseDataDTO;
+import com.casic.missiles.modular.robot.model.TaskGridInfo;
+import com.casic.missiles.modular.robot.service.ITaskGridInfoService;
+import com.casic.missiles.modular.robot.vo.TaskGridInfoVO;
+import io.swagger.annotations.Api;
+import io.swagger.annotations.ApiOperation;
+import org.springframework.web.bind.annotation.*;
+
+import java.util.List;
+
+/**
+ * 任务网格信息Controller
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+@Api(tags = "任务网格信息")
+@RestController
+@RequestMapping("/robot/taskGridInfo")
+public class TaskGridInfoController extends BaseController {
+
+    private final ITaskGridInfoService taskGridInfoService;
+
+    public TaskGridInfoController(ITaskGridInfoService taskGridInfoService) {
+        this.taskGridInfoService = taskGridInfoService;
+    }
+
+    @ApiOperation(value = "列表查询")
+    @GetMapping(value = "/list")
+    public ResponseDataDTO<List<TaskGridInfo>> list(TaskGridInfoVO taskGridInfoVO) {
+        QueryWrapper<TaskGridInfo> query = taskGridInfoVO.genQuery();
+        return ResponseDataDTO.success(taskGridInfoService.list(query));
+    }
+
+    @ApiOperation(value = "分页查询")
+    @GetMapping(value = "/listPage")
+    public ResponseDataDTO<PageInfoBT<TaskGridInfo>> listPage(TaskGridInfoVO taskGridInfoVO) {
+        Page<TaskGridInfo> page = PageFactory.defaultPage();
+        QueryWrapper<TaskGridInfo> query = taskGridInfoVO.genQuery();
+        page = taskGridInfoService.page(page, query);
+        return ResponseDataDTO.success(super.packForBT(page));
+    }
+
+
+    @ApiOperation(value = "新增接口")
+    @PostMapping(value = "/add")
+    public ResponseDataDTO add(@RequestBody TaskGridInfo taskGridInfo) {
+        taskGridInfoService.save(taskGridInfo);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "修改接口")
+    @PostMapping(value = "/update")
+    public ResponseDataDTO update(@RequestBody TaskGridInfo taskGridInfo) {
+        taskGridInfoService.updateById(taskGridInfo);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "删除接口")
+    @GetMapping(value = "/delete")
+    public ResponseDataDTO delete(String id) {
+        taskGridInfoService.removeById(id);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation("批量删除")
+    @PostMapping("/batchDelete")
+    public ResponseDataDTO batchDelete(@RequestBody IdForms ids) {
+        taskGridInfoService.removeByIds(ids.getIds());
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "详情查询")
+    @GetMapping(value = "/detail")
+    public ResponseDataDTO<TaskGridInfo> detail(String id) {
+        return ResponseDataDTO.success(taskGridInfoService.getById(id));
+    }
+}
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
index e82a792..6992274 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
@@ -98,7 +98,7 @@
     public ResponseDataDTO<AlgorithmResponse> test() {
         AlgorithmResponse response = new AlgorithmResponse();
         response.setTaskHeatMap(new TaskHeatMap());
-        algorithmService.readAlgorithmResponse("step3.txt",response,123L,null);
+        algorithmService.readAlgorithmResponse("step3.txt",null,response,123L,null);
         sendWebSocket(InstructStatusMsgEnums.HEAT_MAP, "1", response.getTaskHeatMap());
         return ResponseDataDTO.success(response);
     }
@@ -121,7 +121,7 @@
     @GetMapping(value = "/testGrid")
     public ResponseDataDTO<List<RouteInfo>> testGrid(@RequestBody PathInitDTO pathInitDTO) {
         RobotInfo robotInfo = infoService.getById(pathInitDTO.getRobotId());
-        GridPointDTO gridPointDTO = algorithmService.convertSlamToGrid(new BigDecimal(pathInitDTO.getX()), new BigDecimal(10), new BigDecimal(4), robotInfo);
+        GridPointDTO gridPointDTO = algorithmService.convertSlamToGrid(new BigDecimal(pathInitDTO.getXNow()), new BigDecimal(10), new BigDecimal(4), robotInfo);
         return ResponseDataDTO.success(algorithmService.convertGridCenterPoint(gridPointDTO, new BigDecimal(10), robotInfo));
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/TaskGridInfoMapper.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/TaskGridInfoMapper.java
new file mode 100644
index 0000000..5c3a70b
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/TaskGridInfoMapper.java
@@ -0,0 +1,23 @@
+package com.casic.missiles.modular.robot.dao;
+
+
+import com.baomidou.mybatisplus.core.mapper.BaseMapper;
+import com.casic.missiles.modular.robot.model.TaskGridInfo;
+import org.apache.ibatis.annotations.Param;
+import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
+import com.baomidou.mybatisplus.extension.plugins.pagination.Page;
+import java.util.List;
+/**
+ * <p>
+ *  任务网格信息 Mapper 接口
+ * </p>
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+public interface TaskGridInfoMapper extends BaseMapper<TaskGridInfo> {
+/**
+* 任务网格信息 分页检索
+*/
+List<TaskGridInfo> selectTaskGridInfoPage( @Param("page") Page<TaskGridInfo> page,@Param("ew" ) QueryWrapper<TaskGridInfo> query);
+}
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/mapping/TaskGridInfoMapper.xml b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/mapping/TaskGridInfoMapper.xml
new file mode 100644
index 0000000..be573e0
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/mapping/TaskGridInfoMapper.xml
@@ -0,0 +1,80 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!DOCTYPE mapper PUBLIC "-//mybatis.org//DTD Mapper 3.0//EN" "http://mybatis.org/dtd/mybatis-3-mapper.dtd">
+<mapper namespace="com.casic.missiles.modular.robot.dao.TaskGridInfoMapper">
+    <!-- 通用查询映射结果 -->
+    <resultMap id="BaseResultMap" type="com.casic.missiles.modular.robot.model.TaskGridInfo">
+        <result column="id" property="id"/>
+        <result column="create_time" property="createTime"/>
+        <result column="update_time" property="updateTime"/>
+        <result column="task_id" property="taskId"/>
+        <result column="estimate_grid_x" property="estimateGridX"/>
+        <result column="estimate_grid_y" property="estimateGridY"/>
+        <result column="neutron_count" property="neutronCount"/>
+        <result column="cur_grid_x" property="curGridX"/>
+        <result column="cur_grid_y" property="curGridY"/>
+    </resultMap>
+    <!-- 通用查询结果列 -->
+    <sql id="Base_Column_List">
+        id
+        ,
+                    create_time as createTime ,
+                    update_time as updateTime ,
+                    task_id as taskId ,
+                    estimate_grid_x as estimateGridX ,
+                    estimate_grid_y as estimateGridY ,
+                    neutron_count as neutronCount ,
+                    cur_grid_x as curGridX ,
+                    cur_grid_y as curGridY
+    </sql>
+    <sql id="Base_Column_Alias">
+        a
+        .
+        id
+        ,
+            a.create_time
+            as createTime ,
+            a.update_time
+            as updateTime ,
+            a.task_id
+            as taskId ,
+            a.estimate_grid_x
+            as estimateGridX ,
+            a.estimate_grid_y
+            as estimateGridY ,
+            a.neutron_count
+            as neutronCount ,
+            a.cur_grid_x
+            as curGridX ,
+            a.cur_grid_y
+            as curGridY
+    </sql>
+    <select id="selectTaskGridInfoPage" resultType="com.casic.missiles.modular.robot.model.TaskGridInfo">
+        select
+        <include refid="Base_Column_Alias"/>
+        from task_grid_info a
+        <where>
+            <if test="ew != null">
+                ${ew.sqlSegment}
+            </if>
+        </where>
+    </select>
+    <sql id="Base_Column_Query">
+        <if test="request!=null">
+            <if test="request.taskId!= null and request.taskId!=''">
+                and task_id = #{request.taskId}
+            </if>
+            <if test="request.estimateGridX!= null and request.estimateGridX!=''">
+                and estimate_grid_x = #{request.estimateGridX}
+            </if>
+            <if test="request.estimateGridY!= null and request.estimateGridY!=''">
+                and estimate_grid_y = #{request.estimateGridY}
+            </if>
+            <if test="request.curGridX!= null and request.curGridX!=''">
+                and cur_grid_x = #{request.curGridX}
+            </if>
+            <if test="request.curGridY!= null and request.curGridY!=''">
+                and cur_grid_y = #{request.curGridY}
+            </if>
+        </if>
+    </sql>
+</mapper>
\ No newline at end of file
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java
index 749f559..4559074 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java
@@ -13,4 +13,9 @@
      * 规划点位信息
      */
     private PointNextDTO pointNextDTO;
+    /**
+     * 寻源目标地址
+     */
+    private PointNextDTO coordinate;
+    private boolean isEnd;
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java
index 39ff2a2..cef3238 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java
@@ -12,6 +12,9 @@
     private BigDecimal y;
 
     public static GridPointDTO convertGridPoint(PointNextDTO nextDTO) {
+        if(nextDTO==null){
+            return null;
+        }
         GridPointDTO gridPointDTO = new GridPointDTO();
         gridPointDTO.setX(Convert.toBigDecimal(nextDTO.getX()));
         gridPointDTO.setY(Convert.toBigDecimal(nextDTO.getY()));
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java
index 8c88242..33be25d 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java
@@ -12,25 +12,19 @@
 public class PathInitDTO {
     //---------------------算法参数
     /**
-     * x坐标最大值（x方向栅格总数）
+     * 初始位置x坐标(栅格地图下的坐标)
      */
-    private String xMax;
+    private String xNow;
     /**
-     * y坐标最大值（y方向栅格总数）
+     * 初始位置y坐标(栅格地图下的坐标)
      */
-    private String yMax;
+    private String yNow;
     /**
      * 初始位置计数率
      */
-    private String count;
-    /**
-     * 初始位置x坐标(0<=x<x_max)
-     */
-    private String x;
-    /**
-     * 初始位置y坐标(0<=y<y_max)
-     */
-    private String y;
+    private String zNow;
+
+    private Integer stepLen;
 
     //---------------------其他执行参数
     private String robotId;
@@ -38,11 +32,11 @@
 
     public List<String> toArray() {
         List<String> list = new ArrayList<>();
-        list.add(xMax);
-        list.add(yMax);
-        list.add(count);
-        list.add(x);
-        list.add(y);
+        list.add(xNow);
+        list.add(yNow);
+        list.add(stepLen+"");
+        list.add(zNow);
+        list.add(taskId+"");
         return list;
     }
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java
index 109e95c..ada6774 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java
@@ -10,21 +10,17 @@
  */
 @Data
 public class PathPlanDTO extends PathInitDTO {
-
     /**
-     * 长度为8的数组，记录(x-1, y-1), (x-1, y), (x-1, y+1), (x, y-1), (x, y+1), (x+1, y-1), (x+1, y), (x+1, y+1)8个坐标的可通行信息。
-     * 若某坐标有障碍物（不可通行行）则记为0，否则记为1
+     * 浮点阈值(阈值，超过该值后结束巡检开始寻源)
      */
-    private String open;
-
+    private String th;
     public List<String> toArray() {
         List<String> list = new ArrayList<>();
-        list.add(getXMax());
-        list.add(getYMax());
-        list.add(open);
-        list.add(getCount());
-        list.add(getX());
-        list.add(getY());
+
+        list.add(getStepLen()+"");
+        list.add(getTh());
+        list.add(getZNow());
+        list.add(getTaskId()+"");
         return list;
     }
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskGridInfo.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskGridInfo.java
new file mode 100644
index 0000000..101c10a
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskGridInfo.java
@@ -0,0 +1,83 @@
+package com.casic.missiles.modular.robot.model;
+
+import com.baomidou.mybatisplus.annotation.TableField;
+import com.baomidou.mybatisplus.annotation.TableName;
+import com.baomidou.mybatisplus.extension.activerecord.Model;
+import io.swagger.annotations.ApiModelProperty;
+import lombok.Data;
+
+import java.math.BigDecimal;
+import java.util.Date;
+
+/**
+ * 任务网格信息对象 task_grid_info
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+@Data
+@TableName("task_grid_info")
+public class TaskGridInfo extends Model<TaskGridInfo> {
+    private static final long serialVersionUID = 1L;
+    @TableField(exist = false)
+    private Integer index;
+    @ApiModelProperty(value = "主键", dataType = "Long")
+    @TableField("id")
+    private Long id;
+
+    @ApiModelProperty(value = "创建时间", dataType = "Date")
+    @TableField("create_time")
+    private Date createTime;
+
+    @ApiModelProperty(value = "修改时间", dataType = "Date")
+    @TableField("update_time")
+    private Date updateTime;
+
+    @ApiModelProperty(value = "任务ID", dataType = "Long")
+    @TableField("task_id")
+    private Long taskId;
+
+    @ApiModelProperty(value = "估算栅格x", dataType = "Long")
+    @TableField("estimate_grid_x")
+    private String estimateGridX;
+
+    @ApiModelProperty(value = "估算栅格y", dataType = "Long")
+    @TableField("estimate_grid_y")
+    private String estimateGridY;
+
+    @ApiModelProperty(value = "中子计数率", dataType = "BigDecimal")
+    @TableField("neutron_count")
+    private BigDecimal neutronCount;
+
+    @ApiModelProperty(value = "当前栅格x", dataType = "Long")
+    @TableField("cur_grid_x")
+    private Integer curGridX;
+
+    @ApiModelProperty(value = "当前栅格y", dataType = "Long")
+    @TableField("cur_grid_y")
+    private Integer curGridY;
+
+    public String getCurPoint(){
+        return "("+curGridX+","+curGridY+")";
+    }
+    public String getEstimatePoint(){
+        if(estimateGridX==null&estimateGridY==null){
+            return "结束";
+        }
+        return "("+estimateGridX+","+estimateGridY+")";
+    }
+    @Override
+    public String toString() {
+        return "TaskGridInfo{"
+                + "id=" + id
+                + "createTime=" + createTime
+                + "updateTime=" + updateTime
+                + "taskId=" + taskId
+                + "estimateGridX=" + estimateGridX
+                + "estimateGridY=" + estimateGridY
+                + "neutronCount=" + neutronCount
+                + "curGridX=" + curGridX
+                + "curGridY=" + curGridY
+                + "}";
+    }
+}
\ No newline at end of file

diff --git a/.gitignore b/.gitignore
index 9e20bc1..f42a9cb 100644
--- a/.gitignore
+++ b/.gitignore
@@ -2,3 +2,4 @@
 **/target/**
 logs/
 *.iml
+*.exe
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java b/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
index c7b5d28..a849528 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
@@ -28,6 +28,7 @@
     String KEY_CUR_TASK_ID = "CUR_TASK_ID_";
     String KEY_CUR_TASK_INIT = "CUR_TASK_INIT_";
 
+    String KEY_COST_MAP = "KEY_COST_MAP";
     /**
      * PSD数据ID
      */
@@ -41,6 +42,7 @@
      */
     String KEY_CUR_XY_POINT = "KEY_CUR_XY_POINT_";
     String KEY_CUR_ESTIMATE_GRID = "KEY_CUR_ESTIMATE_GRID_";
+    String KEY_PLANING_FLAG = "KEY_PLANING_FLAG_";
     /**
      * 机器人巡点栅格
      */
@@ -55,6 +57,7 @@
      * 探测器配置参数缓存
      */
     String KEY_CONFIG_PARAMS_ID = "KEY_CONFIG_PARAMS_ID_";
+    String KEY_CONFIG_COSTMAP_RESOLUTION_SET = "KEY_CONFIG_COSTMAP_RESOLUTION_SET_";
 
     /**
      * 暂停状态缓存信息
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
index c4c905d..3bb475f 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
@@ -1,9 +1,14 @@
 package com.casic.missiles.modular.robot.controller;
 
+import cn.hutool.core.lang.Assert;
 import cn.hutool.core.util.StrUtil;
 import com.casic.missiles.core.base.controller.BaseController;
+import com.casic.missiles.core.cache.CacheUtil;
 import com.casic.missiles.core.util.SpringContextHolder;
+import com.casic.missiles.model.exception.RequestValidException;
+import com.casic.missiles.model.response.ResponseData;
 import com.casic.missiles.model.response.dto.ResponseDataDTO;
+import com.casic.missiles.modular.constants.RobotDictConstants;
 import com.casic.missiles.modular.robot.IBaseRobotService;
 import com.casic.missiles.modular.robot.dto.RobotInfoSetDTO;
 import com.casic.missiles.modular.robot.model.RobotInfo;
@@ -19,10 +24,16 @@
 import io.swagger.annotations.ApiOperation;
 import io.swagger.annotations.ApiParam;
 import lombok.SneakyThrows;
+import lombok.extern.slf4j.Slf4j;
+import org.apache.ibatis.annotations.Param;
+import org.springframework.beans.factory.annotation.Value;
 import org.springframework.web.bind.annotation.*;
+import org.springframework.web.multipart.MultipartFile;
 
 import javax.servlet.http.HttpServletResponse;
+import java.io.File;
 import java.util.List;
+import java.util.Optional;
 
 /**
  * 机器人操作类
@@ -30,6 +41,7 @@
  * @author lwh
  * @date 2023-10-30
  */
+@Slf4j
 @Api(tags = "机器人控制接口")
 @RestController
 @RequestMapping("/robot/opt")
@@ -61,6 +73,28 @@
         return baseRobotService.execCmdHandler(request, InstructCodeEnums.CHANGE_PCD);
     }
 
+    @Value("${casic.file.uploadPath}")
+    private String filePath;
+
+    @SneakyThrows
+    @ApiOperation(value = "PCD上传接口")
+    @PostMapping(value = "/uploadPcd")
+    public ResponseData uploadPcd(@RequestParam("file") MultipartFile file, @ApiParam("机器人ID,url拼接传参") String robotId) {
+        Assert.isFalse(StrUtil.isEmpty(robotId), () -> new RequestValidException("机器ID不能为空", Optional.of(new Throwable().getStackTrace()[2])));
+        RobotInfo info = robotInfoService.getById(robotId);
+        if (file != null && !file.isEmpty()) {
+            File pcdFile = new File(filePath + "finalCloud_" + info.getId() + ".pcd");
+            try {
+                file.transferTo(pcdFile);
+            } catch (Exception e) {
+                log.error(e.getMessage());
+            }
+            return ResponseData.success();
+        } else {
+            return ResponseData.error("附件上传失败!");
+        }
+    }
+
     @SneakyThrows
     @ApiOperation(value = "PCD下载接口")
     @PostMapping(value = "/pcdDownload")
@@ -136,6 +170,10 @@
     @ApiOperation(value = "启动或关闭循迹功能", notes = "返回 1成功 0失败  wait 启动时是否等待，0代表循迹启动即运动，不等待；1代表循迹启动原地等待。若无此字段，默认循迹启动不等待直接运动")
     @PostMapping(value = "/startTrack")
     public ResponseDataDTO<Integer> startTrack(@RequestBody StartTrackRequest request) {
+        request.setIsTask(1);
+        if (request.getStepLen() == null) {
+            request.setStepLen(2);
+        }
         baseRobotService.startTrack(request);
         return ResponseDataDTO.success();
     }
@@ -156,6 +194,13 @@
         MessageRequestDTO<CrlNavRequest> messageDTO = new MessageRequestDTO();
         messageDTO.setMsg(request);
         ResponseDataDTO<Integer> list = baseRobotService.getRobotRouteOptService().robotCmd(info, InstructCodeEnums.CTRL_NAV, messageDTO);
+        if (request.getAction() == 1) {
+            CostMapResolutionRequest costMap = new CostMapResolutionRequest();
+            costMap.setResolution(info.getResolution().doubleValue());
+            request.setRobotId(request.getRobotId());
+            //是否下发分辨率
+            CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CONFIG_COSTMAP_RESOLUTION_SET + request.getRobotId(), costMap);
+        }
         return list;
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java
new file mode 100644
index 0000000..5317651
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java
@@ -0,0 +1,86 @@
+package com.casic.missiles.modular.robot.controller;
+
+import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
+import com.baomidou.mybatisplus.extension.plugins.pagination.Page;
+import com.casic.missiles.core.base.controller.BaseController;
+import com.casic.missiles.core.page.PageFactory;
+import com.casic.missiles.core.page.PageInfoBT;
+import com.casic.missiles.model.form.IdForms;
+import com.casic.missiles.model.response.dto.ResponseDataDTO;
+import com.casic.missiles.modular.robot.model.TaskGridInfo;
+import com.casic.missiles.modular.robot.service.ITaskGridInfoService;
+import com.casic.missiles.modular.robot.vo.TaskGridInfoVO;
+import io.swagger.annotations.Api;
+import io.swagger.annotations.ApiOperation;
+import org.springframework.web.bind.annotation.*;
+
+import java.util.List;
+
+/**
+ * 任务网格信息Controller
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+@Api(tags = "任务网格信息")
+@RestController
+@RequestMapping("/robot/taskGridInfo")
+public class TaskGridInfoController extends BaseController {
+
+    private final ITaskGridInfoService taskGridInfoService;
+
+    public TaskGridInfoController(ITaskGridInfoService taskGridInfoService) {
+        this.taskGridInfoService = taskGridInfoService;
+    }
+
+    @ApiOperation(value = "列表查询")
+    @GetMapping(value = "/list")
+    public ResponseDataDTO<List<TaskGridInfo>> list(TaskGridInfoVO taskGridInfoVO) {
+        QueryWrapper<TaskGridInfo> query = taskGridInfoVO.genQuery();
+        return ResponseDataDTO.success(taskGridInfoService.list(query));
+    }
+
+    @ApiOperation(value = "分页查询")
+    @GetMapping(value = "/listPage")
+    public ResponseDataDTO<PageInfoBT<TaskGridInfo>> listPage(TaskGridInfoVO taskGridInfoVO) {
+        Page<TaskGridInfo> page = PageFactory.defaultPage();
+        QueryWrapper<TaskGridInfo> query = taskGridInfoVO.genQuery();
+        page = taskGridInfoService.page(page, query);
+        return ResponseDataDTO.success(super.packForBT(page));
+    }
+
+
+    @ApiOperation(value = "新增接口")
+    @PostMapping(value = "/add")
+    public ResponseDataDTO add(@RequestBody TaskGridInfo taskGridInfo) {
+        taskGridInfoService.save(taskGridInfo);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "修改接口")
+    @PostMapping(value = "/update")
+    public ResponseDataDTO update(@RequestBody TaskGridInfo taskGridInfo) {
+        taskGridInfoService.updateById(taskGridInfo);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "删除接口")
+    @GetMapping(value = "/delete")
+    public ResponseDataDTO delete(String id) {
+        taskGridInfoService.removeById(id);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation("批量删除")
+    @PostMapping("/batchDelete")
+    public ResponseDataDTO batchDelete(@RequestBody IdForms ids) {
+        taskGridInfoService.removeByIds(ids.getIds());
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "详情查询")
+    @GetMapping(value = "/detail")
+    public ResponseDataDTO<TaskGridInfo> detail(String id) {
+        return ResponseDataDTO.success(taskGridInfoService.getById(id));
+    }
+}
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
index e82a792..6992274 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
@@ -98,7 +98,7 @@
     public ResponseDataDTO<AlgorithmResponse> test() {
         AlgorithmResponse response = new AlgorithmResponse();
         response.setTaskHeatMap(new TaskHeatMap());
-        algorithmService.readAlgorithmResponse("step3.txt",response,123L,null);
+        algorithmService.readAlgorithmResponse("step3.txt",null,response,123L,null);
         sendWebSocket(InstructStatusMsgEnums.HEAT_MAP, "1", response.getTaskHeatMap());
         return ResponseDataDTO.success(response);
     }
@@ -121,7 +121,7 @@
     @GetMapping(value = "/testGrid")
     public ResponseDataDTO<List<RouteInfo>> testGrid(@RequestBody PathInitDTO pathInitDTO) {
         RobotInfo robotInfo = infoService.getById(pathInitDTO.getRobotId());
-        GridPointDTO gridPointDTO = algorithmService.convertSlamToGrid(new BigDecimal(pathInitDTO.getX()), new BigDecimal(10), new BigDecimal(4), robotInfo);
+        GridPointDTO gridPointDTO = algorithmService.convertSlamToGrid(new BigDecimal(pathInitDTO.getXNow()), new BigDecimal(10), new BigDecimal(4), robotInfo);
         return ResponseDataDTO.success(algorithmService.convertGridCenterPoint(gridPointDTO, new BigDecimal(10), robotInfo));
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/TaskGridInfoMapper.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/TaskGridInfoMapper.java
new file mode 100644
index 0000000..5c3a70b
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/TaskGridInfoMapper.java
@@ -0,0 +1,23 @@
+package com.casic.missiles.modular.robot.dao;
+
+
+import com.baomidou.mybatisplus.core.mapper.BaseMapper;
+import com.casic.missiles.modular.robot.model.TaskGridInfo;
+import org.apache.ibatis.annotations.Param;
+import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
+import com.baomidou.mybatisplus.extension.plugins.pagination.Page;
+import java.util.List;
+/**
+ * <p>
+ *  任务网格信息 Mapper 接口
+ * </p>
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+public interface TaskGridInfoMapper extends BaseMapper<TaskGridInfo> {
+/**
+* 任务网格信息 分页检索
+*/
+List<TaskGridInfo> selectTaskGridInfoPage( @Param("page") Page<TaskGridInfo> page,@Param("ew" ) QueryWrapper<TaskGridInfo> query);
+}
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/mapping/TaskGridInfoMapper.xml b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/mapping/TaskGridInfoMapper.xml
new file mode 100644
index 0000000..be573e0
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/mapping/TaskGridInfoMapper.xml
@@ -0,0 +1,80 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!DOCTYPE mapper PUBLIC "-//mybatis.org//DTD Mapper 3.0//EN" "http://mybatis.org/dtd/mybatis-3-mapper.dtd">
+<mapper namespace="com.casic.missiles.modular.robot.dao.TaskGridInfoMapper">
+    <!-- 通用查询映射结果 -->
+    <resultMap id="BaseResultMap" type="com.casic.missiles.modular.robot.model.TaskGridInfo">
+        <result column="id" property="id"/>
+        <result column="create_time" property="createTime"/>
+        <result column="update_time" property="updateTime"/>
+        <result column="task_id" property="taskId"/>
+        <result column="estimate_grid_x" property="estimateGridX"/>
+        <result column="estimate_grid_y" property="estimateGridY"/>
+        <result column="neutron_count" property="neutronCount"/>
+        <result column="cur_grid_x" property="curGridX"/>
+        <result column="cur_grid_y" property="curGridY"/>
+    </resultMap>
+    <!-- 通用查询结果列 -->
+    <sql id="Base_Column_List">
+        id
+        ,
+                    create_time as createTime ,
+                    update_time as updateTime ,
+                    task_id as taskId ,
+                    estimate_grid_x as estimateGridX ,
+                    estimate_grid_y as estimateGridY ,
+                    neutron_count as neutronCount ,
+                    cur_grid_x as curGridX ,
+                    cur_grid_y as curGridY
+    </sql>
+    <sql id="Base_Column_Alias">
+        a
+        .
+        id
+        ,
+            a.create_time
+            as createTime ,
+            a.update_time
+            as updateTime ,
+            a.task_id
+            as taskId ,
+            a.estimate_grid_x
+            as estimateGridX ,
+            a.estimate_grid_y
+            as estimateGridY ,
+            a.neutron_count
+            as neutronCount ,
+            a.cur_grid_x
+            as curGridX ,
+            a.cur_grid_y
+            as curGridY
+    </sql>
+    <select id="selectTaskGridInfoPage" resultType="com.casic.missiles.modular.robot.model.TaskGridInfo">
+        select
+        <include refid="Base_Column_Alias"/>
+        from task_grid_info a
+        <where>
+            <if test="ew != null">
+                ${ew.sqlSegment}
+            </if>
+        </where>
+    </select>
+    <sql id="Base_Column_Query">
+        <if test="request!=null">
+            <if test="request.taskId!= null and request.taskId!=''">
+                and task_id = #{request.taskId}
+            </if>
+            <if test="request.estimateGridX!= null and request.estimateGridX!=''">
+                and estimate_grid_x = #{request.estimateGridX}
+            </if>
+            <if test="request.estimateGridY!= null and request.estimateGridY!=''">
+                and estimate_grid_y = #{request.estimateGridY}
+            </if>
+            <if test="request.curGridX!= null and request.curGridX!=''">
+                and cur_grid_x = #{request.curGridX}
+            </if>
+            <if test="request.curGridY!= null and request.curGridY!=''">
+                and cur_grid_y = #{request.curGridY}
+            </if>
+        </if>
+    </sql>
+</mapper>
\ No newline at end of file
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java
index 749f559..4559074 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java
@@ -13,4 +13,9 @@
      * 规划点位信息
      */
     private PointNextDTO pointNextDTO;
+    /**
+     * 寻源目标地址
+     */
+    private PointNextDTO coordinate;
+    private boolean isEnd;
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java
index 39ff2a2..cef3238 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java
@@ -12,6 +12,9 @@
     private BigDecimal y;
 
     public static GridPointDTO convertGridPoint(PointNextDTO nextDTO) {
+        if(nextDTO==null){
+            return null;
+        }
         GridPointDTO gridPointDTO = new GridPointDTO();
         gridPointDTO.setX(Convert.toBigDecimal(nextDTO.getX()));
         gridPointDTO.setY(Convert.toBigDecimal(nextDTO.getY()));
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java
index 8c88242..33be25d 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java
@@ -12,25 +12,19 @@
 public class PathInitDTO {
     //---------------------算法参数
     /**
-     * x坐标最大值（x方向栅格总数）
+     * 初始位置x坐标(栅格地图下的坐标)
      */
-    private String xMax;
+    private String xNow;
     /**
-     * y坐标最大值（y方向栅格总数）
+     * 初始位置y坐标(栅格地图下的坐标)
      */
-    private String yMax;
+    private String yNow;
     /**
      * 初始位置计数率
      */
-    private String count;
-    /**
-     * 初始位置x坐标(0<=x<x_max)
-     */
-    private String x;
-    /**
-     * 初始位置y坐标(0<=y<y_max)
-     */
-    private String y;
+    private String zNow;
+
+    private Integer stepLen;
 
     //---------------------其他执行参数
     private String robotId;
@@ -38,11 +32,11 @@
 
     public List<String> toArray() {
         List<String> list = new ArrayList<>();
-        list.add(xMax);
-        list.add(yMax);
-        list.add(count);
-        list.add(x);
-        list.add(y);
+        list.add(xNow);
+        list.add(yNow);
+        list.add(stepLen+"");
+        list.add(zNow);
+        list.add(taskId+"");
         return list;
     }
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java
index 109e95c..ada6774 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java
@@ -10,21 +10,17 @@
  */
 @Data
 public class PathPlanDTO extends PathInitDTO {
-
     /**
-     * 长度为8的数组，记录(x-1, y-1), (x-1, y), (x-1, y+1), (x, y-1), (x, y+1), (x+1, y-1), (x+1, y), (x+1, y+1)8个坐标的可通行信息。
-     * 若某坐标有障碍物（不可通行行）则记为0，否则记为1
+     * 浮点阈值(阈值，超过该值后结束巡检开始寻源)
      */
-    private String open;
-
+    private String th;
     public List<String> toArray() {
         List<String> list = new ArrayList<>();
-        list.add(getXMax());
-        list.add(getYMax());
-        list.add(open);
-        list.add(getCount());
-        list.add(getX());
-        list.add(getY());
+
+        list.add(getStepLen()+"");
+        list.add(getTh());
+        list.add(getZNow());
+        list.add(getTaskId()+"");
         return list;
     }
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskGridInfo.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskGridInfo.java
new file mode 100644
index 0000000..101c10a
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskGridInfo.java
@@ -0,0 +1,83 @@
+package com.casic.missiles.modular.robot.model;
+
+import com.baomidou.mybatisplus.annotation.TableField;
+import com.baomidou.mybatisplus.annotation.TableName;
+import com.baomidou.mybatisplus.extension.activerecord.Model;
+import io.swagger.annotations.ApiModelProperty;
+import lombok.Data;
+
+import java.math.BigDecimal;
+import java.util.Date;
+
+/**
+ * 任务网格信息对象 task_grid_info
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+@Data
+@TableName("task_grid_info")
+public class TaskGridInfo extends Model<TaskGridInfo> {
+    private static final long serialVersionUID = 1L;
+    @TableField(exist = false)
+    private Integer index;
+    @ApiModelProperty(value = "主键", dataType = "Long")
+    @TableField("id")
+    private Long id;
+
+    @ApiModelProperty(value = "创建时间", dataType = "Date")
+    @TableField("create_time")
+    private Date createTime;
+
+    @ApiModelProperty(value = "修改时间", dataType = "Date")
+    @TableField("update_time")
+    private Date updateTime;
+
+    @ApiModelProperty(value = "任务ID", dataType = "Long")
+    @TableField("task_id")
+    private Long taskId;
+
+    @ApiModelProperty(value = "估算栅格x", dataType = "Long")
+    @TableField("estimate_grid_x")
+    private String estimateGridX;
+
+    @ApiModelProperty(value = "估算栅格y", dataType = "Long")
+    @TableField("estimate_grid_y")
+    private String estimateGridY;
+
+    @ApiModelProperty(value = "中子计数率", dataType = "BigDecimal")
+    @TableField("neutron_count")
+    private BigDecimal neutronCount;
+
+    @ApiModelProperty(value = "当前栅格x", dataType = "Long")
+    @TableField("cur_grid_x")
+    private Integer curGridX;
+
+    @ApiModelProperty(value = "当前栅格y", dataType = "Long")
+    @TableField("cur_grid_y")
+    private Integer curGridY;
+
+    public String getCurPoint(){
+        return "("+curGridX+","+curGridY+")";
+    }
+    public String getEstimatePoint(){
+        if(estimateGridX==null&estimateGridY==null){
+            return "结束";
+        }
+        return "("+estimateGridX+","+estimateGridY+")";
+    }
+    @Override
+    public String toString() {
+        return "TaskGridInfo{"
+                + "id=" + id
+                + "createTime=" + createTime
+                + "updateTime=" + updateTime
+                + "taskId=" + taskId
+                + "estimateGridX=" + estimateGridX
+                + "estimateGridY=" + estimateGridY
+                + "neutronCount=" + neutronCount
+                + "curGridX=" + curGridX
+                + "curGridY=" + curGridY
+                + "}";
+    }
+}
\ No newline at end of file
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java
index edc104e..a8acb2a 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java
@@ -53,6 +53,13 @@
     @TableField("route_id")
     private Long routeId;
 
+    @ApiModelProperty(value = "寻源初始步长", dataType = "Integer")
+    @TableField("step_len")
+    private Integer stepLen;
+
+    @ApiModelProperty(value = "寻源阈值设定", dataType = "BigDecimal")
+    @TableField("max_threshold")
+    private BigDecimal maxThreshold;
     /**
      * 任务类型(1巡检任务)
      */
@@ -62,7 +69,6 @@
     @ApiModelProperty(value = "任务类型字典名称", dataType = "String")
     @TableField(exist = false)
     private String taskTypeName;
-
     @ApiModelProperty(value = "识别类型(discernType 1中子源识别)", dataType = "Integer")
     @TableField("discern_type")
     private Integer discernType;

diff --git a/.gitignore b/.gitignore
index 9e20bc1..f42a9cb 100644
--- a/.gitignore
+++ b/.gitignore
@@ -2,3 +2,4 @@
 **/target/**
 logs/
 *.iml
+*.exe
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java b/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
index c7b5d28..a849528 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
@@ -28,6 +28,7 @@
     String KEY_CUR_TASK_ID = "CUR_TASK_ID_";
     String KEY_CUR_TASK_INIT = "CUR_TASK_INIT_";
 
+    String KEY_COST_MAP = "KEY_COST_MAP";
     /**
      * PSD数据ID
      */
@@ -41,6 +42,7 @@
      */
     String KEY_CUR_XY_POINT = "KEY_CUR_XY_POINT_";
     String KEY_CUR_ESTIMATE_GRID = "KEY_CUR_ESTIMATE_GRID_";
+    String KEY_PLANING_FLAG = "KEY_PLANING_FLAG_";
     /**
      * 机器人巡点栅格
      */
@@ -55,6 +57,7 @@
      * 探测器配置参数缓存
      */
     String KEY_CONFIG_PARAMS_ID = "KEY_CONFIG_PARAMS_ID_";
+    String KEY_CONFIG_COSTMAP_RESOLUTION_SET = "KEY_CONFIG_COSTMAP_RESOLUTION_SET_";
 
     /**
      * 暂停状态缓存信息
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
index c4c905d..3bb475f 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
@@ -1,9 +1,14 @@
 package com.casic.missiles.modular.robot.controller;
 
+import cn.hutool.core.lang.Assert;
 import cn.hutool.core.util.StrUtil;
 import com.casic.missiles.core.base.controller.BaseController;
+import com.casic.missiles.core.cache.CacheUtil;
 import com.casic.missiles.core.util.SpringContextHolder;
+import com.casic.missiles.model.exception.RequestValidException;
+import com.casic.missiles.model.response.ResponseData;
 import com.casic.missiles.model.response.dto.ResponseDataDTO;
+import com.casic.missiles.modular.constants.RobotDictConstants;
 import com.casic.missiles.modular.robot.IBaseRobotService;
 import com.casic.missiles.modular.robot.dto.RobotInfoSetDTO;
 import com.casic.missiles.modular.robot.model.RobotInfo;
@@ -19,10 +24,16 @@
 import io.swagger.annotations.ApiOperation;
 import io.swagger.annotations.ApiParam;
 import lombok.SneakyThrows;
+import lombok.extern.slf4j.Slf4j;
+import org.apache.ibatis.annotations.Param;
+import org.springframework.beans.factory.annotation.Value;
 import org.springframework.web.bind.annotation.*;
+import org.springframework.web.multipart.MultipartFile;
 
 import javax.servlet.http.HttpServletResponse;
+import java.io.File;
 import java.util.List;
+import java.util.Optional;
 
 /**
  * 机器人操作类
@@ -30,6 +41,7 @@
  * @author lwh
  * @date 2023-10-30
  */
+@Slf4j
 @Api(tags = "机器人控制接口")
 @RestController
 @RequestMapping("/robot/opt")
@@ -61,6 +73,28 @@
         return baseRobotService.execCmdHandler(request, InstructCodeEnums.CHANGE_PCD);
     }
 
+    @Value("${casic.file.uploadPath}")
+    private String filePath;
+
+    @SneakyThrows
+    @ApiOperation(value = "PCD上传接口")
+    @PostMapping(value = "/uploadPcd")
+    public ResponseData uploadPcd(@RequestParam("file") MultipartFile file, @ApiParam("机器人ID,url拼接传参") String robotId) {
+        Assert.isFalse(StrUtil.isEmpty(robotId), () -> new RequestValidException("机器ID不能为空", Optional.of(new Throwable().getStackTrace()[2])));
+        RobotInfo info = robotInfoService.getById(robotId);
+        if (file != null && !file.isEmpty()) {
+            File pcdFile = new File(filePath + "finalCloud_" + info.getId() + ".pcd");
+            try {
+                file.transferTo(pcdFile);
+            } catch (Exception e) {
+                log.error(e.getMessage());
+            }
+            return ResponseData.success();
+        } else {
+            return ResponseData.error("附件上传失败!");
+        }
+    }
+
     @SneakyThrows
     @ApiOperation(value = "PCD下载接口")
     @PostMapping(value = "/pcdDownload")
@@ -136,6 +170,10 @@
     @ApiOperation(value = "启动或关闭循迹功能", notes = "返回 1成功 0失败  wait 启动时是否等待，0代表循迹启动即运动，不等待；1代表循迹启动原地等待。若无此字段，默认循迹启动不等待直接运动")
     @PostMapping(value = "/startTrack")
     public ResponseDataDTO<Integer> startTrack(@RequestBody StartTrackRequest request) {
+        request.setIsTask(1);
+        if (request.getStepLen() == null) {
+            request.setStepLen(2);
+        }
         baseRobotService.startTrack(request);
         return ResponseDataDTO.success();
     }
@@ -156,6 +194,13 @@
         MessageRequestDTO<CrlNavRequest> messageDTO = new MessageRequestDTO();
         messageDTO.setMsg(request);
         ResponseDataDTO<Integer> list = baseRobotService.getRobotRouteOptService().robotCmd(info, InstructCodeEnums.CTRL_NAV, messageDTO);
+        if (request.getAction() == 1) {
+            CostMapResolutionRequest costMap = new CostMapResolutionRequest();
+            costMap.setResolution(info.getResolution().doubleValue());
+            request.setRobotId(request.getRobotId());
+            //是否下发分辨率
+            CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CONFIG_COSTMAP_RESOLUTION_SET + request.getRobotId(), costMap);
+        }
         return list;
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java
new file mode 100644
index 0000000..5317651
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java
@@ -0,0 +1,86 @@
+package com.casic.missiles.modular.robot.controller;
+
+import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
+import com.baomidou.mybatisplus.extension.plugins.pagination.Page;
+import com.casic.missiles.core.base.controller.BaseController;
+import com.casic.missiles.core.page.PageFactory;
+import com.casic.missiles.core.page.PageInfoBT;
+import com.casic.missiles.model.form.IdForms;
+import com.casic.missiles.model.response.dto.ResponseDataDTO;
+import com.casic.missiles.modular.robot.model.TaskGridInfo;
+import com.casic.missiles.modular.robot.service.ITaskGridInfoService;
+import com.casic.missiles.modular.robot.vo.TaskGridInfoVO;
+import io.swagger.annotations.Api;
+import io.swagger.annotations.ApiOperation;
+import org.springframework.web.bind.annotation.*;
+
+import java.util.List;
+
+/**
+ * 任务网格信息Controller
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+@Api(tags = "任务网格信息")
+@RestController
+@RequestMapping("/robot/taskGridInfo")
+public class TaskGridInfoController extends BaseController {
+
+    private final ITaskGridInfoService taskGridInfoService;
+
+    public TaskGridInfoController(ITaskGridInfoService taskGridInfoService) {
+        this.taskGridInfoService = taskGridInfoService;
+    }
+
+    @ApiOperation(value = "列表查询")
+    @GetMapping(value = "/list")
+    public ResponseDataDTO<List<TaskGridInfo>> list(TaskGridInfoVO taskGridInfoVO) {
+        QueryWrapper<TaskGridInfo> query = taskGridInfoVO.genQuery();
+        return ResponseDataDTO.success(taskGridInfoService.list(query));
+    }
+
+    @ApiOperation(value = "分页查询")
+    @GetMapping(value = "/listPage")
+    public ResponseDataDTO<PageInfoBT<TaskGridInfo>> listPage(TaskGridInfoVO taskGridInfoVO) {
+        Page<TaskGridInfo> page = PageFactory.defaultPage();
+        QueryWrapper<TaskGridInfo> query = taskGridInfoVO.genQuery();
+        page = taskGridInfoService.page(page, query);
+        return ResponseDataDTO.success(super.packForBT(page));
+    }
+
+
+    @ApiOperation(value = "新增接口")
+    @PostMapping(value = "/add")
+    public ResponseDataDTO add(@RequestBody TaskGridInfo taskGridInfo) {
+        taskGridInfoService.save(taskGridInfo);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "修改接口")
+    @PostMapping(value = "/update")
+    public ResponseDataDTO update(@RequestBody TaskGridInfo taskGridInfo) {
+        taskGridInfoService.updateById(taskGridInfo);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "删除接口")
+    @GetMapping(value = "/delete")
+    public ResponseDataDTO delete(String id) {
+        taskGridInfoService.removeById(id);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation("批量删除")
+    @PostMapping("/batchDelete")
+    public ResponseDataDTO batchDelete(@RequestBody IdForms ids) {
+        taskGridInfoService.removeByIds(ids.getIds());
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "详情查询")
+    @GetMapping(value = "/detail")
+    public ResponseDataDTO<TaskGridInfo> detail(String id) {
+        return ResponseDataDTO.success(taskGridInfoService.getById(id));
+    }
+}
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
index e82a792..6992274 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
@@ -98,7 +98,7 @@
     public ResponseDataDTO<AlgorithmResponse> test() {
         AlgorithmResponse response = new AlgorithmResponse();
         response.setTaskHeatMap(new TaskHeatMap());
-        algorithmService.readAlgorithmResponse("step3.txt",response,123L,null);
+        algorithmService.readAlgorithmResponse("step3.txt",null,response,123L,null);
         sendWebSocket(InstructStatusMsgEnums.HEAT_MAP, "1", response.getTaskHeatMap());
         return ResponseDataDTO.success(response);
     }
@@ -121,7 +121,7 @@
     @GetMapping(value = "/testGrid")
     public ResponseDataDTO<List<RouteInfo>> testGrid(@RequestBody PathInitDTO pathInitDTO) {
         RobotInfo robotInfo = infoService.getById(pathInitDTO.getRobotId());
-        GridPointDTO gridPointDTO = algorithmService.convertSlamToGrid(new BigDecimal(pathInitDTO.getX()), new BigDecimal(10), new BigDecimal(4), robotInfo);
+        GridPointDTO gridPointDTO = algorithmService.convertSlamToGrid(new BigDecimal(pathInitDTO.getXNow()), new BigDecimal(10), new BigDecimal(4), robotInfo);
         return ResponseDataDTO.success(algorithmService.convertGridCenterPoint(gridPointDTO, new BigDecimal(10), robotInfo));
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/TaskGridInfoMapper.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/TaskGridInfoMapper.java
new file mode 100644
index 0000000..5c3a70b
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/TaskGridInfoMapper.java
@@ -0,0 +1,23 @@
+package com.casic.missiles.modular.robot.dao;
+
+
+import com.baomidou.mybatisplus.core.mapper.BaseMapper;
+import com.casic.missiles.modular.robot.model.TaskGridInfo;
+import org.apache.ibatis.annotations.Param;
+import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
+import com.baomidou.mybatisplus.extension.plugins.pagination.Page;
+import java.util.List;
+/**
+ * <p>
+ *  任务网格信息 Mapper 接口
+ * </p>
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+public interface TaskGridInfoMapper extends BaseMapper<TaskGridInfo> {
+/**
+* 任务网格信息 分页检索
+*/
+List<TaskGridInfo> selectTaskGridInfoPage( @Param("page") Page<TaskGridInfo> page,@Param("ew" ) QueryWrapper<TaskGridInfo> query);
+}
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/mapping/TaskGridInfoMapper.xml b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/mapping/TaskGridInfoMapper.xml
new file mode 100644
index 0000000..be573e0
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/mapping/TaskGridInfoMapper.xml
@@ -0,0 +1,80 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!DOCTYPE mapper PUBLIC "-//mybatis.org//DTD Mapper 3.0//EN" "http://mybatis.org/dtd/mybatis-3-mapper.dtd">
+<mapper namespace="com.casic.missiles.modular.robot.dao.TaskGridInfoMapper">
+    <!-- 通用查询映射结果 -->
+    <resultMap id="BaseResultMap" type="com.casic.missiles.modular.robot.model.TaskGridInfo">
+        <result column="id" property="id"/>
+        <result column="create_time" property="createTime"/>
+        <result column="update_time" property="updateTime"/>
+        <result column="task_id" property="taskId"/>
+        <result column="estimate_grid_x" property="estimateGridX"/>
+        <result column="estimate_grid_y" property="estimateGridY"/>
+        <result column="neutron_count" property="neutronCount"/>
+        <result column="cur_grid_x" property="curGridX"/>
+        <result column="cur_grid_y" property="curGridY"/>
+    </resultMap>
+    <!-- 通用查询结果列 -->
+    <sql id="Base_Column_List">
+        id
+        ,
+                    create_time as createTime ,
+                    update_time as updateTime ,
+                    task_id as taskId ,
+                    estimate_grid_x as estimateGridX ,
+                    estimate_grid_y as estimateGridY ,
+                    neutron_count as neutronCount ,
+                    cur_grid_x as curGridX ,
+                    cur_grid_y as curGridY
+    </sql>
+    <sql id="Base_Column_Alias">
+        a
+        .
+        id
+        ,
+            a.create_time
+            as createTime ,
+            a.update_time
+            as updateTime ,
+            a.task_id
+            as taskId ,
+            a.estimate_grid_x
+            as estimateGridX ,
+            a.estimate_grid_y
+            as estimateGridY ,
+            a.neutron_count
+            as neutronCount ,
+            a.cur_grid_x
+            as curGridX ,
+            a.cur_grid_y
+            as curGridY
+    </sql>
+    <select id="selectTaskGridInfoPage" resultType="com.casic.missiles.modular.robot.model.TaskGridInfo">
+        select
+        <include refid="Base_Column_Alias"/>
+        from task_grid_info a
+        <where>
+            <if test="ew != null">
+                ${ew.sqlSegment}
+            </if>
+        </where>
+    </select>
+    <sql id="Base_Column_Query">
+        <if test="request!=null">
+            <if test="request.taskId!= null and request.taskId!=''">
+                and task_id = #{request.taskId}
+            </if>
+            <if test="request.estimateGridX!= null and request.estimateGridX!=''">
+                and estimate_grid_x = #{request.estimateGridX}
+            </if>
+            <if test="request.estimateGridY!= null and request.estimateGridY!=''">
+                and estimate_grid_y = #{request.estimateGridY}
+            </if>
+            <if test="request.curGridX!= null and request.curGridX!=''">
+                and cur_grid_x = #{request.curGridX}
+            </if>
+            <if test="request.curGridY!= null and request.curGridY!=''">
+                and cur_grid_y = #{request.curGridY}
+            </if>
+        </if>
+    </sql>
+</mapper>
\ No newline at end of file
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java
index 749f559..4559074 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java
@@ -13,4 +13,9 @@
      * 规划点位信息
      */
     private PointNextDTO pointNextDTO;
+    /**
+     * 寻源目标地址
+     */
+    private PointNextDTO coordinate;
+    private boolean isEnd;
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java
index 39ff2a2..cef3238 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java
@@ -12,6 +12,9 @@
     private BigDecimal y;
 
     public static GridPointDTO convertGridPoint(PointNextDTO nextDTO) {
+        if(nextDTO==null){
+            return null;
+        }
         GridPointDTO gridPointDTO = new GridPointDTO();
         gridPointDTO.setX(Convert.toBigDecimal(nextDTO.getX()));
         gridPointDTO.setY(Convert.toBigDecimal(nextDTO.getY()));
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java
index 8c88242..33be25d 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java
@@ -12,25 +12,19 @@
 public class PathInitDTO {
     //---------------------算法参数
     /**
-     * x坐标最大值（x方向栅格总数）
+     * 初始位置x坐标(栅格地图下的坐标)
      */
-    private String xMax;
+    private String xNow;
     /**
-     * y坐标最大值（y方向栅格总数）
+     * 初始位置y坐标(栅格地图下的坐标)
      */
-    private String yMax;
+    private String yNow;
     /**
      * 初始位置计数率
      */
-    private String count;
-    /**
-     * 初始位置x坐标(0<=x<x_max)
-     */
-    private String x;
-    /**
-     * 初始位置y坐标(0<=y<y_max)
-     */
-    private String y;
+    private String zNow;
+
+    private Integer stepLen;
 
     //---------------------其他执行参数
     private String robotId;
@@ -38,11 +32,11 @@
 
     public List<String> toArray() {
         List<String> list = new ArrayList<>();
-        list.add(xMax);
-        list.add(yMax);
-        list.add(count);
-        list.add(x);
-        list.add(y);
+        list.add(xNow);
+        list.add(yNow);
+        list.add(stepLen+"");
+        list.add(zNow);
+        list.add(taskId+"");
         return list;
     }
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java
index 109e95c..ada6774 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java
@@ -10,21 +10,17 @@
  */
 @Data
 public class PathPlanDTO extends PathInitDTO {
-
     /**
-     * 长度为8的数组，记录(x-1, y-1), (x-1, y), (x-1, y+1), (x, y-1), (x, y+1), (x+1, y-1), (x+1, y), (x+1, y+1)8个坐标的可通行信息。
-     * 若某坐标有障碍物（不可通行行）则记为0，否则记为1
+     * 浮点阈值(阈值，超过该值后结束巡检开始寻源)
      */
-    private String open;
-
+    private String th;
     public List<String> toArray() {
         List<String> list = new ArrayList<>();
-        list.add(getXMax());
-        list.add(getYMax());
-        list.add(open);
-        list.add(getCount());
-        list.add(getX());
-        list.add(getY());
+
+        list.add(getStepLen()+"");
+        list.add(getTh());
+        list.add(getZNow());
+        list.add(getTaskId()+"");
         return list;
     }
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskGridInfo.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskGridInfo.java
new file mode 100644
index 0000000..101c10a
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskGridInfo.java
@@ -0,0 +1,83 @@
+package com.casic.missiles.modular.robot.model;
+
+import com.baomidou.mybatisplus.annotation.TableField;
+import com.baomidou.mybatisplus.annotation.TableName;
+import com.baomidou.mybatisplus.extension.activerecord.Model;
+import io.swagger.annotations.ApiModelProperty;
+import lombok.Data;
+
+import java.math.BigDecimal;
+import java.util.Date;
+
+/**
+ * 任务网格信息对象 task_grid_info
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+@Data
+@TableName("task_grid_info")
+public class TaskGridInfo extends Model<TaskGridInfo> {
+    private static final long serialVersionUID = 1L;
+    @TableField(exist = false)
+    private Integer index;
+    @ApiModelProperty(value = "主键", dataType = "Long")
+    @TableField("id")
+    private Long id;
+
+    @ApiModelProperty(value = "创建时间", dataType = "Date")
+    @TableField("create_time")
+    private Date createTime;
+
+    @ApiModelProperty(value = "修改时间", dataType = "Date")
+    @TableField("update_time")
+    private Date updateTime;
+
+    @ApiModelProperty(value = "任务ID", dataType = "Long")
+    @TableField("task_id")
+    private Long taskId;
+
+    @ApiModelProperty(value = "估算栅格x", dataType = "Long")
+    @TableField("estimate_grid_x")
+    private String estimateGridX;
+
+    @ApiModelProperty(value = "估算栅格y", dataType = "Long")
+    @TableField("estimate_grid_y")
+    private String estimateGridY;
+
+    @ApiModelProperty(value = "中子计数率", dataType = "BigDecimal")
+    @TableField("neutron_count")
+    private BigDecimal neutronCount;
+
+    @ApiModelProperty(value = "当前栅格x", dataType = "Long")
+    @TableField("cur_grid_x")
+    private Integer curGridX;
+
+    @ApiModelProperty(value = "当前栅格y", dataType = "Long")
+    @TableField("cur_grid_y")
+    private Integer curGridY;
+
+    public String getCurPoint(){
+        return "("+curGridX+","+curGridY+")";
+    }
+    public String getEstimatePoint(){
+        if(estimateGridX==null&estimateGridY==null){
+            return "结束";
+        }
+        return "("+estimateGridX+","+estimateGridY+")";
+    }
+    @Override
+    public String toString() {
+        return "TaskGridInfo{"
+                + "id=" + id
+                + "createTime=" + createTime
+                + "updateTime=" + updateTime
+                + "taskId=" + taskId
+                + "estimateGridX=" + estimateGridX
+                + "estimateGridY=" + estimateGridY
+                + "neutronCount=" + neutronCount
+                + "curGridX=" + curGridX
+                + "curGridY=" + curGridY
+                + "}";
+    }
+}
\ No newline at end of file
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java
index edc104e..a8acb2a 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java
@@ -53,6 +53,13 @@
     @TableField("route_id")
     private Long routeId;
 
+    @ApiModelProperty(value = "寻源初始步长", dataType = "Integer")
+    @TableField("step_len")
+    private Integer stepLen;
+
+    @ApiModelProperty(value = "寻源阈值设定", dataType = "BigDecimal")
+    @TableField("max_threshold")
+    private BigDecimal maxThreshold;
     /**
      * 任务类型(1巡检任务)
      */
@@ -62,7 +69,6 @@
     @ApiModelProperty(value = "任务类型字典名称", dataType = "String")
     @TableField(exist = false)
     private String taskTypeName;
-
     @ApiModelProperty(value = "识别类型(discernType 1中子源识别)", dataType = "Integer")
     @TableField("discern_type")
     private Integer discernType;
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java
index c4bc25d..c6620f3 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java
@@ -53,7 +53,7 @@
     }),
     SET_SPEED("/set_speed" , "/set_speed_response" , "最大运动速度设置" , true, new TypeReference<RobotMsgResponseDTO<Integer>>() {
     }),
-    SET_TARGETPOINT("/set_targetpoint" , "/set_targetpoint_response" , "设置导航目标点" , false, new TypeReference<RobotMsgResponseDTO<Integer>>() {
+    SET_TARGETPOINT("/set_targetpoint" , "/set_targetpoint_response" , "设置导航目标点" , true, new TypeReference<RobotMsgResponseDTO<Integer>>() {
     }),
 
     GET_DATA_LIST("/get_data_list" , "/get_data_list_response" , "获取数据包列表" , true, new TypeReference<RobotMsgResponseDTO<List<String>>>() {

diff --git a/.gitignore b/.gitignore
index 9e20bc1..f42a9cb 100644
--- a/.gitignore
+++ b/.gitignore
@@ -2,3 +2,4 @@
 **/target/**
 logs/
 *.iml
+*.exe
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java b/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
index c7b5d28..a849528 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
@@ -28,6 +28,7 @@
     String KEY_CUR_TASK_ID = "CUR_TASK_ID_";
     String KEY_CUR_TASK_INIT = "CUR_TASK_INIT_";
 
+    String KEY_COST_MAP = "KEY_COST_MAP";
     /**
      * PSD数据ID
      */
@@ -41,6 +42,7 @@
      */
     String KEY_CUR_XY_POINT = "KEY_CUR_XY_POINT_";
     String KEY_CUR_ESTIMATE_GRID = "KEY_CUR_ESTIMATE_GRID_";
+    String KEY_PLANING_FLAG = "KEY_PLANING_FLAG_";
     /**
      * 机器人巡点栅格
      */
@@ -55,6 +57,7 @@
      * 探测器配置参数缓存
      */
     String KEY_CONFIG_PARAMS_ID = "KEY_CONFIG_PARAMS_ID_";
+    String KEY_CONFIG_COSTMAP_RESOLUTION_SET = "KEY_CONFIG_COSTMAP_RESOLUTION_SET_";
 
     /**
      * 暂停状态缓存信息
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
index c4c905d..3bb475f 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
@@ -1,9 +1,14 @@
 package com.casic.missiles.modular.robot.controller;
 
+import cn.hutool.core.lang.Assert;
 import cn.hutool.core.util.StrUtil;
 import com.casic.missiles.core.base.controller.BaseController;
+import com.casic.missiles.core.cache.CacheUtil;
 import com.casic.missiles.core.util.SpringContextHolder;
+import com.casic.missiles.model.exception.RequestValidException;
+import com.casic.missiles.model.response.ResponseData;
 import com.casic.missiles.model.response.dto.ResponseDataDTO;
+import com.casic.missiles.modular.constants.RobotDictConstants;
 import com.casic.missiles.modular.robot.IBaseRobotService;
 import com.casic.missiles.modular.robot.dto.RobotInfoSetDTO;
 import com.casic.missiles.modular.robot.model.RobotInfo;
@@ -19,10 +24,16 @@
 import io.swagger.annotations.ApiOperation;
 import io.swagger.annotations.ApiParam;
 import lombok.SneakyThrows;
+import lombok.extern.slf4j.Slf4j;
+import org.apache.ibatis.annotations.Param;
+import org.springframework.beans.factory.annotation.Value;
 import org.springframework.web.bind.annotation.*;
+import org.springframework.web.multipart.MultipartFile;
 
 import javax.servlet.http.HttpServletResponse;
+import java.io.File;
 import java.util.List;
+import java.util.Optional;
 
 /**
  * 机器人操作类
@@ -30,6 +41,7 @@
  * @author lwh
  * @date 2023-10-30
  */
+@Slf4j
 @Api(tags = "机器人控制接口")
 @RestController
 @RequestMapping("/robot/opt")
@@ -61,6 +73,28 @@
         return baseRobotService.execCmdHandler(request, InstructCodeEnums.CHANGE_PCD);
     }
 
+    @Value("${casic.file.uploadPath}")
+    private String filePath;
+
+    @SneakyThrows
+    @ApiOperation(value = "PCD上传接口")
+    @PostMapping(value = "/uploadPcd")
+    public ResponseData uploadPcd(@RequestParam("file") MultipartFile file, @ApiParam("机器人ID,url拼接传参") String robotId) {
+        Assert.isFalse(StrUtil.isEmpty(robotId), () -> new RequestValidException("机器ID不能为空", Optional.of(new Throwable().getStackTrace()[2])));
+        RobotInfo info = robotInfoService.getById(robotId);
+        if (file != null && !file.isEmpty()) {
+            File pcdFile = new File(filePath + "finalCloud_" + info.getId() + ".pcd");
+            try {
+                file.transferTo(pcdFile);
+            } catch (Exception e) {
+                log.error(e.getMessage());
+            }
+            return ResponseData.success();
+        } else {
+            return ResponseData.error("附件上传失败!");
+        }
+    }
+
     @SneakyThrows
     @ApiOperation(value = "PCD下载接口")
     @PostMapping(value = "/pcdDownload")
@@ -136,6 +170,10 @@
     @ApiOperation(value = "启动或关闭循迹功能", notes = "返回 1成功 0失败  wait 启动时是否等待，0代表循迹启动即运动，不等待；1代表循迹启动原地等待。若无此字段，默认循迹启动不等待直接运动")
     @PostMapping(value = "/startTrack")
     public ResponseDataDTO<Integer> startTrack(@RequestBody StartTrackRequest request) {
+        request.setIsTask(1);
+        if (request.getStepLen() == null) {
+            request.setStepLen(2);
+        }
         baseRobotService.startTrack(request);
         return ResponseDataDTO.success();
     }
@@ -156,6 +194,13 @@
         MessageRequestDTO<CrlNavRequest> messageDTO = new MessageRequestDTO();
         messageDTO.setMsg(request);
         ResponseDataDTO<Integer> list = baseRobotService.getRobotRouteOptService().robotCmd(info, InstructCodeEnums.CTRL_NAV, messageDTO);
+        if (request.getAction() == 1) {
+            CostMapResolutionRequest costMap = new CostMapResolutionRequest();
+            costMap.setResolution(info.getResolution().doubleValue());
+            request.setRobotId(request.getRobotId());
+            //是否下发分辨率
+            CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CONFIG_COSTMAP_RESOLUTION_SET + request.getRobotId(), costMap);
+        }
         return list;
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java
new file mode 100644
index 0000000..5317651
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java
@@ -0,0 +1,86 @@
+package com.casic.missiles.modular.robot.controller;
+
+import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
+import com.baomidou.mybatisplus.extension.plugins.pagination.Page;
+import com.casic.missiles.core.base.controller.BaseController;
+import com.casic.missiles.core.page.PageFactory;
+import com.casic.missiles.core.page.PageInfoBT;
+import com.casic.missiles.model.form.IdForms;
+import com.casic.missiles.model.response.dto.ResponseDataDTO;
+import com.casic.missiles.modular.robot.model.TaskGridInfo;
+import com.casic.missiles.modular.robot.service.ITaskGridInfoService;
+import com.casic.missiles.modular.robot.vo.TaskGridInfoVO;
+import io.swagger.annotations.Api;
+import io.swagger.annotations.ApiOperation;
+import org.springframework.web.bind.annotation.*;
+
+import java.util.List;
+
+/**
+ * 任务网格信息Controller
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+@Api(tags = "任务网格信息")
+@RestController
+@RequestMapping("/robot/taskGridInfo")
+public class TaskGridInfoController extends BaseController {
+
+    private final ITaskGridInfoService taskGridInfoService;
+
+    public TaskGridInfoController(ITaskGridInfoService taskGridInfoService) {
+        this.taskGridInfoService = taskGridInfoService;
+    }
+
+    @ApiOperation(value = "列表查询")
+    @GetMapping(value = "/list")
+    public ResponseDataDTO<List<TaskGridInfo>> list(TaskGridInfoVO taskGridInfoVO) {
+        QueryWrapper<TaskGridInfo> query = taskGridInfoVO.genQuery();
+        return ResponseDataDTO.success(taskGridInfoService.list(query));
+    }
+
+    @ApiOperation(value = "分页查询")
+    @GetMapping(value = "/listPage")
+    public ResponseDataDTO<PageInfoBT<TaskGridInfo>> listPage(TaskGridInfoVO taskGridInfoVO) {
+        Page<TaskGridInfo> page = PageFactory.defaultPage();
+        QueryWrapper<TaskGridInfo> query = taskGridInfoVO.genQuery();
+        page = taskGridInfoService.page(page, query);
+        return ResponseDataDTO.success(super.packForBT(page));
+    }
+
+
+    @ApiOperation(value = "新增接口")
+    @PostMapping(value = "/add")
+    public ResponseDataDTO add(@RequestBody TaskGridInfo taskGridInfo) {
+        taskGridInfoService.save(taskGridInfo);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "修改接口")
+    @PostMapping(value = "/update")
+    public ResponseDataDTO update(@RequestBody TaskGridInfo taskGridInfo) {
+        taskGridInfoService.updateById(taskGridInfo);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "删除接口")
+    @GetMapping(value = "/delete")
+    public ResponseDataDTO delete(String id) {
+        taskGridInfoService.removeById(id);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation("批量删除")
+    @PostMapping("/batchDelete")
+    public ResponseDataDTO batchDelete(@RequestBody IdForms ids) {
+        taskGridInfoService.removeByIds(ids.getIds());
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "详情查询")
+    @GetMapping(value = "/detail")
+    public ResponseDataDTO<TaskGridInfo> detail(String id) {
+        return ResponseDataDTO.success(taskGridInfoService.getById(id));
+    }
+}
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
index e82a792..6992274 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
@@ -98,7 +98,7 @@
     public ResponseDataDTO<AlgorithmResponse> test() {
         AlgorithmResponse response = new AlgorithmResponse();
         response.setTaskHeatMap(new TaskHeatMap());
-        algorithmService.readAlgorithmResponse("step3.txt",response,123L,null);
+        algorithmService.readAlgorithmResponse("step3.txt",null,response,123L,null);
         sendWebSocket(InstructStatusMsgEnums.HEAT_MAP, "1", response.getTaskHeatMap());
         return ResponseDataDTO.success(response);
     }
@@ -121,7 +121,7 @@
     @GetMapping(value = "/testGrid")
     public ResponseDataDTO<List<RouteInfo>> testGrid(@RequestBody PathInitDTO pathInitDTO) {
         RobotInfo robotInfo = infoService.getById(pathInitDTO.getRobotId());
-        GridPointDTO gridPointDTO = algorithmService.convertSlamToGrid(new BigDecimal(pathInitDTO.getX()), new BigDecimal(10), new BigDecimal(4), robotInfo);
+        GridPointDTO gridPointDTO = algorithmService.convertSlamToGrid(new BigDecimal(pathInitDTO.getXNow()), new BigDecimal(10), new BigDecimal(4), robotInfo);
         return ResponseDataDTO.success(algorithmService.convertGridCenterPoint(gridPointDTO, new BigDecimal(10), robotInfo));
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/TaskGridInfoMapper.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/TaskGridInfoMapper.java
new file mode 100644
index 0000000..5c3a70b
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/TaskGridInfoMapper.java
@@ -0,0 +1,23 @@
+package com.casic.missiles.modular.robot.dao;
+
+
+import com.baomidou.mybatisplus.core.mapper.BaseMapper;
+import com.casic.missiles.modular.robot.model.TaskGridInfo;
+import org.apache.ibatis.annotations.Param;
+import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
+import com.baomidou.mybatisplus.extension.plugins.pagination.Page;
+import java.util.List;
+/**
+ * <p>
+ *  任务网格信息 Mapper 接口
+ * </p>
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+public interface TaskGridInfoMapper extends BaseMapper<TaskGridInfo> {
+/**
+* 任务网格信息 分页检索
+*/
+List<TaskGridInfo> selectTaskGridInfoPage( @Param("page") Page<TaskGridInfo> page,@Param("ew" ) QueryWrapper<TaskGridInfo> query);
+}
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/mapping/TaskGridInfoMapper.xml b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/mapping/TaskGridInfoMapper.xml
new file mode 100644
index 0000000..be573e0
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/mapping/TaskGridInfoMapper.xml
@@ -0,0 +1,80 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!DOCTYPE mapper PUBLIC "-//mybatis.org//DTD Mapper 3.0//EN" "http://mybatis.org/dtd/mybatis-3-mapper.dtd">
+<mapper namespace="com.casic.missiles.modular.robot.dao.TaskGridInfoMapper">
+    <!-- 通用查询映射结果 -->
+    <resultMap id="BaseResultMap" type="com.casic.missiles.modular.robot.model.TaskGridInfo">
+        <result column="id" property="id"/>
+        <result column="create_time" property="createTime"/>
+        <result column="update_time" property="updateTime"/>
+        <result column="task_id" property="taskId"/>
+        <result column="estimate_grid_x" property="estimateGridX"/>
+        <result column="estimate_grid_y" property="estimateGridY"/>
+        <result column="neutron_count" property="neutronCount"/>
+        <result column="cur_grid_x" property="curGridX"/>
+        <result column="cur_grid_y" property="curGridY"/>
+    </resultMap>
+    <!-- 通用查询结果列 -->
+    <sql id="Base_Column_List">
+        id
+        ,
+                    create_time as createTime ,
+                    update_time as updateTime ,
+                    task_id as taskId ,
+                    estimate_grid_x as estimateGridX ,
+                    estimate_grid_y as estimateGridY ,
+                    neutron_count as neutronCount ,
+                    cur_grid_x as curGridX ,
+                    cur_grid_y as curGridY
+    </sql>
+    <sql id="Base_Column_Alias">
+        a
+        .
+        id
+        ,
+            a.create_time
+            as createTime ,
+            a.update_time
+            as updateTime ,
+            a.task_id
+            as taskId ,
+            a.estimate_grid_x
+            as estimateGridX ,
+            a.estimate_grid_y
+            as estimateGridY ,
+            a.neutron_count
+            as neutronCount ,
+            a.cur_grid_x
+            as curGridX ,
+            a.cur_grid_y
+            as curGridY
+    </sql>
+    <select id="selectTaskGridInfoPage" resultType="com.casic.missiles.modular.robot.model.TaskGridInfo">
+        select
+        <include refid="Base_Column_Alias"/>
+        from task_grid_info a
+        <where>
+            <if test="ew != null">
+                ${ew.sqlSegment}
+            </if>
+        </where>
+    </select>
+    <sql id="Base_Column_Query">
+        <if test="request!=null">
+            <if test="request.taskId!= null and request.taskId!=''">
+                and task_id = #{request.taskId}
+            </if>
+            <if test="request.estimateGridX!= null and request.estimateGridX!=''">
+                and estimate_grid_x = #{request.estimateGridX}
+            </if>
+            <if test="request.estimateGridY!= null and request.estimateGridY!=''">
+                and estimate_grid_y = #{request.estimateGridY}
+            </if>
+            <if test="request.curGridX!= null and request.curGridX!=''">
+                and cur_grid_x = #{request.curGridX}
+            </if>
+            <if test="request.curGridY!= null and request.curGridY!=''">
+                and cur_grid_y = #{request.curGridY}
+            </if>
+        </if>
+    </sql>
+</mapper>
\ No newline at end of file
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java
index 749f559..4559074 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java
@@ -13,4 +13,9 @@
      * 规划点位信息
      */
     private PointNextDTO pointNextDTO;
+    /**
+     * 寻源目标地址
+     */
+    private PointNextDTO coordinate;
+    private boolean isEnd;
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java
index 39ff2a2..cef3238 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java
@@ -12,6 +12,9 @@
     private BigDecimal y;
 
     public static GridPointDTO convertGridPoint(PointNextDTO nextDTO) {
+        if(nextDTO==null){
+            return null;
+        }
         GridPointDTO gridPointDTO = new GridPointDTO();
         gridPointDTO.setX(Convert.toBigDecimal(nextDTO.getX()));
         gridPointDTO.setY(Convert.toBigDecimal(nextDTO.getY()));
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java
index 8c88242..33be25d 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java
@@ -12,25 +12,19 @@
 public class PathInitDTO {
     //---------------------算法参数
     /**
-     * x坐标最大值（x方向栅格总数）
+     * 初始位置x坐标(栅格地图下的坐标)
      */
-    private String xMax;
+    private String xNow;
     /**
-     * y坐标最大值（y方向栅格总数）
+     * 初始位置y坐标(栅格地图下的坐标)
      */
-    private String yMax;
+    private String yNow;
     /**
      * 初始位置计数率
      */
-    private String count;
-    /**
-     * 初始位置x坐标(0<=x<x_max)
-     */
-    private String x;
-    /**
-     * 初始位置y坐标(0<=y<y_max)
-     */
-    private String y;
+    private String zNow;
+
+    private Integer stepLen;
 
     //---------------------其他执行参数
     private String robotId;
@@ -38,11 +32,11 @@
 
     public List<String> toArray() {
         List<String> list = new ArrayList<>();
-        list.add(xMax);
-        list.add(yMax);
-        list.add(count);
-        list.add(x);
-        list.add(y);
+        list.add(xNow);
+        list.add(yNow);
+        list.add(stepLen+"");
+        list.add(zNow);
+        list.add(taskId+"");
         return list;
     }
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java
index 109e95c..ada6774 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java
@@ -10,21 +10,17 @@
  */
 @Data
 public class PathPlanDTO extends PathInitDTO {
-
     /**
-     * 长度为8的数组，记录(x-1, y-1), (x-1, y), (x-1, y+1), (x, y-1), (x, y+1), (x+1, y-1), (x+1, y), (x+1, y+1)8个坐标的可通行信息。
-     * 若某坐标有障碍物（不可通行行）则记为0，否则记为1
+     * 浮点阈值(阈值，超过该值后结束巡检开始寻源)
      */
-    private String open;
-
+    private String th;
     public List<String> toArray() {
         List<String> list = new ArrayList<>();
-        list.add(getXMax());
-        list.add(getYMax());
-        list.add(open);
-        list.add(getCount());
-        list.add(getX());
-        list.add(getY());
+
+        list.add(getStepLen()+"");
+        list.add(getTh());
+        list.add(getZNow());
+        list.add(getTaskId()+"");
         return list;
     }
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskGridInfo.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskGridInfo.java
new file mode 100644
index 0000000..101c10a
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskGridInfo.java
@@ -0,0 +1,83 @@
+package com.casic.missiles.modular.robot.model;
+
+import com.baomidou.mybatisplus.annotation.TableField;
+import com.baomidou.mybatisplus.annotation.TableName;
+import com.baomidou.mybatisplus.extension.activerecord.Model;
+import io.swagger.annotations.ApiModelProperty;
+import lombok.Data;
+
+import java.math.BigDecimal;
+import java.util.Date;
+
+/**
+ * 任务网格信息对象 task_grid_info
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+@Data
+@TableName("task_grid_info")
+public class TaskGridInfo extends Model<TaskGridInfo> {
+    private static final long serialVersionUID = 1L;
+    @TableField(exist = false)
+    private Integer index;
+    @ApiModelProperty(value = "主键", dataType = "Long")
+    @TableField("id")
+    private Long id;
+
+    @ApiModelProperty(value = "创建时间", dataType = "Date")
+    @TableField("create_time")
+    private Date createTime;
+
+    @ApiModelProperty(value = "修改时间", dataType = "Date")
+    @TableField("update_time")
+    private Date updateTime;
+
+    @ApiModelProperty(value = "任务ID", dataType = "Long")
+    @TableField("task_id")
+    private Long taskId;
+
+    @ApiModelProperty(value = "估算栅格x", dataType = "Long")
+    @TableField("estimate_grid_x")
+    private String estimateGridX;
+
+    @ApiModelProperty(value = "估算栅格y", dataType = "Long")
+    @TableField("estimate_grid_y")
+    private String estimateGridY;
+
+    @ApiModelProperty(value = "中子计数率", dataType = "BigDecimal")
+    @TableField("neutron_count")
+    private BigDecimal neutronCount;
+
+    @ApiModelProperty(value = "当前栅格x", dataType = "Long")
+    @TableField("cur_grid_x")
+    private Integer curGridX;
+
+    @ApiModelProperty(value = "当前栅格y", dataType = "Long")
+    @TableField("cur_grid_y")
+    private Integer curGridY;
+
+    public String getCurPoint(){
+        return "("+curGridX+","+curGridY+")";
+    }
+    public String getEstimatePoint(){
+        if(estimateGridX==null&estimateGridY==null){
+            return "结束";
+        }
+        return "("+estimateGridX+","+estimateGridY+")";
+    }
+    @Override
+    public String toString() {
+        return "TaskGridInfo{"
+                + "id=" + id
+                + "createTime=" + createTime
+                + "updateTime=" + updateTime
+                + "taskId=" + taskId
+                + "estimateGridX=" + estimateGridX
+                + "estimateGridY=" + estimateGridY
+                + "neutronCount=" + neutronCount
+                + "curGridX=" + curGridX
+                + "curGridY=" + curGridY
+                + "}";
+    }
+}
\ No newline at end of file
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java
index edc104e..a8acb2a 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java
@@ -53,6 +53,13 @@
     @TableField("route_id")
     private Long routeId;
 
+    @ApiModelProperty(value = "寻源初始步长", dataType = "Integer")
+    @TableField("step_len")
+    private Integer stepLen;
+
+    @ApiModelProperty(value = "寻源阈值设定", dataType = "BigDecimal")
+    @TableField("max_threshold")
+    private BigDecimal maxThreshold;
     /**
      * 任务类型(1巡检任务)
      */
@@ -62,7 +69,6 @@
     @ApiModelProperty(value = "任务类型字典名称", dataType = "String")
     @TableField(exist = false)
     private String taskTypeName;
-
     @ApiModelProperty(value = "识别类型(discernType 1中子源识别)", dataType = "Integer")
     @TableField("discern_type")
     private Integer discernType;
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java
index c4bc25d..c6620f3 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java
@@ -53,7 +53,7 @@
     }),
     SET_SPEED("/set_speed" , "/set_speed_response" , "最大运动速度设置" , true, new TypeReference<RobotMsgResponseDTO<Integer>>() {
     }),
-    SET_TARGETPOINT("/set_targetpoint" , "/set_targetpoint_response" , "设置导航目标点" , false, new TypeReference<RobotMsgResponseDTO<Integer>>() {
+    SET_TARGETPOINT("/set_targetpoint" , "/set_targetpoint_response" , "设置导航目标点" , true, new TypeReference<RobotMsgResponseDTO<Integer>>() {
     }),
 
     GET_DATA_LIST("/get_data_list" , "/get_data_list_response" , "获取数据包列表" , true, new TypeReference<RobotMsgResponseDTO<List<String>>>() {
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/AbstractSocketHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/AbstractSocketHandler.java
index 83f4cb2..e47e343 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/AbstractSocketHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/AbstractSocketHandler.java
@@ -49,7 +49,7 @@
                 wsMsgBaseDTO.setMsgKey(enums.getMsgKey());
                 wsMsgBaseDTO.setData(jsonObject.get("msg"));
                 String msg = JSON.toJSONString(wsMsgBaseDTO);
-                log.info("推送{}消息：{}", enums.getRemarks(), msg);
+//                log.info("推送{}消息：{}", enums.getRemarks(), msg);
                 webSocket.sendOneMessage(robotId, msg);
             }
         } catch (Exception e) {

diff --git a/.gitignore b/.gitignore
index 9e20bc1..f42a9cb 100644
--- a/.gitignore
+++ b/.gitignore
@@ -2,3 +2,4 @@
 **/target/**
 logs/
 *.iml
+*.exe
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java b/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
index c7b5d28..a849528 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
@@ -28,6 +28,7 @@
     String KEY_CUR_TASK_ID = "CUR_TASK_ID_";
     String KEY_CUR_TASK_INIT = "CUR_TASK_INIT_";
 
+    String KEY_COST_MAP = "KEY_COST_MAP";
     /**
      * PSD数据ID
      */
@@ -41,6 +42,7 @@
      */
     String KEY_CUR_XY_POINT = "KEY_CUR_XY_POINT_";
     String KEY_CUR_ESTIMATE_GRID = "KEY_CUR_ESTIMATE_GRID_";
+    String KEY_PLANING_FLAG = "KEY_PLANING_FLAG_";
     /**
      * 机器人巡点栅格
      */
@@ -55,6 +57,7 @@
      * 探测器配置参数缓存
      */
     String KEY_CONFIG_PARAMS_ID = "KEY_CONFIG_PARAMS_ID_";
+    String KEY_CONFIG_COSTMAP_RESOLUTION_SET = "KEY_CONFIG_COSTMAP_RESOLUTION_SET_";
 
     /**
      * 暂停状态缓存信息
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
index c4c905d..3bb475f 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
@@ -1,9 +1,14 @@
 package com.casic.missiles.modular.robot.controller;
 
+import cn.hutool.core.lang.Assert;
 import cn.hutool.core.util.StrUtil;
 import com.casic.missiles.core.base.controller.BaseController;
+import com.casic.missiles.core.cache.CacheUtil;
 import com.casic.missiles.core.util.SpringContextHolder;
+import com.casic.missiles.model.exception.RequestValidException;
+import com.casic.missiles.model.response.ResponseData;
 import com.casic.missiles.model.response.dto.ResponseDataDTO;
+import com.casic.missiles.modular.constants.RobotDictConstants;
 import com.casic.missiles.modular.robot.IBaseRobotService;
 import com.casic.missiles.modular.robot.dto.RobotInfoSetDTO;
 import com.casic.missiles.modular.robot.model.RobotInfo;
@@ -19,10 +24,16 @@
 import io.swagger.annotations.ApiOperation;
 import io.swagger.annotations.ApiParam;
 import lombok.SneakyThrows;
+import lombok.extern.slf4j.Slf4j;
+import org.apache.ibatis.annotations.Param;
+import org.springframework.beans.factory.annotation.Value;
 import org.springframework.web.bind.annotation.*;
+import org.springframework.web.multipart.MultipartFile;
 
 import javax.servlet.http.HttpServletResponse;
+import java.io.File;
 import java.util.List;
+import java.util.Optional;
 
 /**
  * 机器人操作类
@@ -30,6 +41,7 @@
  * @author lwh
  * @date 2023-10-30
  */
+@Slf4j
 @Api(tags = "机器人控制接口")
 @RestController
 @RequestMapping("/robot/opt")
@@ -61,6 +73,28 @@
         return baseRobotService.execCmdHandler(request, InstructCodeEnums.CHANGE_PCD);
     }
 
+    @Value("${casic.file.uploadPath}")
+    private String filePath;
+
+    @SneakyThrows
+    @ApiOperation(value = "PCD上传接口")
+    @PostMapping(value = "/uploadPcd")
+    public ResponseData uploadPcd(@RequestParam("file") MultipartFile file, @ApiParam("机器人ID,url拼接传参") String robotId) {
+        Assert.isFalse(StrUtil.isEmpty(robotId), () -> new RequestValidException("机器ID不能为空", Optional.of(new Throwable().getStackTrace()[2])));
+        RobotInfo info = robotInfoService.getById(robotId);
+        if (file != null && !file.isEmpty()) {
+            File pcdFile = new File(filePath + "finalCloud_" + info.getId() + ".pcd");
+            try {
+                file.transferTo(pcdFile);
+            } catch (Exception e) {
+                log.error(e.getMessage());
+            }
+            return ResponseData.success();
+        } else {
+            return ResponseData.error("附件上传失败!");
+        }
+    }
+
     @SneakyThrows
     @ApiOperation(value = "PCD下载接口")
     @PostMapping(value = "/pcdDownload")
@@ -136,6 +170,10 @@
     @ApiOperation(value = "启动或关闭循迹功能", notes = "返回 1成功 0失败  wait 启动时是否等待，0代表循迹启动即运动，不等待；1代表循迹启动原地等待。若无此字段，默认循迹启动不等待直接运动")
     @PostMapping(value = "/startTrack")
     public ResponseDataDTO<Integer> startTrack(@RequestBody StartTrackRequest request) {
+        request.setIsTask(1);
+        if (request.getStepLen() == null) {
+            request.setStepLen(2);
+        }
         baseRobotService.startTrack(request);
         return ResponseDataDTO.success();
     }
@@ -156,6 +194,13 @@
         MessageRequestDTO<CrlNavRequest> messageDTO = new MessageRequestDTO();
         messageDTO.setMsg(request);
         ResponseDataDTO<Integer> list = baseRobotService.getRobotRouteOptService().robotCmd(info, InstructCodeEnums.CTRL_NAV, messageDTO);
+        if (request.getAction() == 1) {
+            CostMapResolutionRequest costMap = new CostMapResolutionRequest();
+            costMap.setResolution(info.getResolution().doubleValue());
+            request.setRobotId(request.getRobotId());
+            //是否下发分辨率
+            CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CONFIG_COSTMAP_RESOLUTION_SET + request.getRobotId(), costMap);
+        }
         return list;
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java
new file mode 100644
index 0000000..5317651
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java
@@ -0,0 +1,86 @@
+package com.casic.missiles.modular.robot.controller;
+
+import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
+import com.baomidou.mybatisplus.extension.plugins.pagination.Page;
+import com.casic.missiles.core.base.controller.BaseController;
+import com.casic.missiles.core.page.PageFactory;
+import com.casic.missiles.core.page.PageInfoBT;
+import com.casic.missiles.model.form.IdForms;
+import com.casic.missiles.model.response.dto.ResponseDataDTO;
+import com.casic.missiles.modular.robot.model.TaskGridInfo;
+import com.casic.missiles.modular.robot.service.ITaskGridInfoService;
+import com.casic.missiles.modular.robot.vo.TaskGridInfoVO;
+import io.swagger.annotations.Api;
+import io.swagger.annotations.ApiOperation;
+import org.springframework.web.bind.annotation.*;
+
+import java.util.List;
+
+/**
+ * 任务网格信息Controller
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+@Api(tags = "任务网格信息")
+@RestController
+@RequestMapping("/robot/taskGridInfo")
+public class TaskGridInfoController extends BaseController {
+
+    private final ITaskGridInfoService taskGridInfoService;
+
+    public TaskGridInfoController(ITaskGridInfoService taskGridInfoService) {
+        this.taskGridInfoService = taskGridInfoService;
+    }
+
+    @ApiOperation(value = "列表查询")
+    @GetMapping(value = "/list")
+    public ResponseDataDTO<List<TaskGridInfo>> list(TaskGridInfoVO taskGridInfoVO) {
+        QueryWrapper<TaskGridInfo> query = taskGridInfoVO.genQuery();
+        return ResponseDataDTO.success(taskGridInfoService.list(query));
+    }
+
+    @ApiOperation(value = "分页查询")
+    @GetMapping(value = "/listPage")
+    public ResponseDataDTO<PageInfoBT<TaskGridInfo>> listPage(TaskGridInfoVO taskGridInfoVO) {
+        Page<TaskGridInfo> page = PageFactory.defaultPage();
+        QueryWrapper<TaskGridInfo> query = taskGridInfoVO.genQuery();
+        page = taskGridInfoService.page(page, query);
+        return ResponseDataDTO.success(super.packForBT(page));
+    }
+
+
+    @ApiOperation(value = "新增接口")
+    @PostMapping(value = "/add")
+    public ResponseDataDTO add(@RequestBody TaskGridInfo taskGridInfo) {
+        taskGridInfoService.save(taskGridInfo);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "修改接口")
+    @PostMapping(value = "/update")
+    public ResponseDataDTO update(@RequestBody TaskGridInfo taskGridInfo) {
+        taskGridInfoService.updateById(taskGridInfo);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "删除接口")
+    @GetMapping(value = "/delete")
+    public ResponseDataDTO delete(String id) {
+        taskGridInfoService.removeById(id);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation("批量删除")
+    @PostMapping("/batchDelete")
+    public ResponseDataDTO batchDelete(@RequestBody IdForms ids) {
+        taskGridInfoService.removeByIds(ids.getIds());
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "详情查询")
+    @GetMapping(value = "/detail")
+    public ResponseDataDTO<TaskGridInfo> detail(String id) {
+        return ResponseDataDTO.success(taskGridInfoService.getById(id));
+    }
+}
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
index e82a792..6992274 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
@@ -98,7 +98,7 @@
     public ResponseDataDTO<AlgorithmResponse> test() {
         AlgorithmResponse response = new AlgorithmResponse();
         response.setTaskHeatMap(new TaskHeatMap());
-        algorithmService.readAlgorithmResponse("step3.txt",response,123L,null);
+        algorithmService.readAlgorithmResponse("step3.txt",null,response,123L,null);
         sendWebSocket(InstructStatusMsgEnums.HEAT_MAP, "1", response.getTaskHeatMap());
         return ResponseDataDTO.success(response);
     }
@@ -121,7 +121,7 @@
     @GetMapping(value = "/testGrid")
     public ResponseDataDTO<List<RouteInfo>> testGrid(@RequestBody PathInitDTO pathInitDTO) {
         RobotInfo robotInfo = infoService.getById(pathInitDTO.getRobotId());
-        GridPointDTO gridPointDTO = algorithmService.convertSlamToGrid(new BigDecimal(pathInitDTO.getX()), new BigDecimal(10), new BigDecimal(4), robotInfo);
+        GridPointDTO gridPointDTO = algorithmService.convertSlamToGrid(new BigDecimal(pathInitDTO.getXNow()), new BigDecimal(10), new BigDecimal(4), robotInfo);
         return ResponseDataDTO.success(algorithmService.convertGridCenterPoint(gridPointDTO, new BigDecimal(10), robotInfo));
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/TaskGridInfoMapper.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/TaskGridInfoMapper.java
new file mode 100644
index 0000000..5c3a70b
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/TaskGridInfoMapper.java
@@ -0,0 +1,23 @@
+package com.casic.missiles.modular.robot.dao;
+
+
+import com.baomidou.mybatisplus.core.mapper.BaseMapper;
+import com.casic.missiles.modular.robot.model.TaskGridInfo;
+import org.apache.ibatis.annotations.Param;
+import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
+import com.baomidou.mybatisplus.extension.plugins.pagination.Page;
+import java.util.List;
+/**
+ * <p>
+ *  任务网格信息 Mapper 接口
+ * </p>
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+public interface TaskGridInfoMapper extends BaseMapper<TaskGridInfo> {
+/**
+* 任务网格信息 分页检索
+*/
+List<TaskGridInfo> selectTaskGridInfoPage( @Param("page") Page<TaskGridInfo> page,@Param("ew" ) QueryWrapper<TaskGridInfo> query);
+}
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/mapping/TaskGridInfoMapper.xml b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/mapping/TaskGridInfoMapper.xml
new file mode 100644
index 0000000..be573e0
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/mapping/TaskGridInfoMapper.xml
@@ -0,0 +1,80 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!DOCTYPE mapper PUBLIC "-//mybatis.org//DTD Mapper 3.0//EN" "http://mybatis.org/dtd/mybatis-3-mapper.dtd">
+<mapper namespace="com.casic.missiles.modular.robot.dao.TaskGridInfoMapper">
+    <!-- 通用查询映射结果 -->
+    <resultMap id="BaseResultMap" type="com.casic.missiles.modular.robot.model.TaskGridInfo">
+        <result column="id" property="id"/>
+        <result column="create_time" property="createTime"/>
+        <result column="update_time" property="updateTime"/>
+        <result column="task_id" property="taskId"/>
+        <result column="estimate_grid_x" property="estimateGridX"/>
+        <result column="estimate_grid_y" property="estimateGridY"/>
+        <result column="neutron_count" property="neutronCount"/>
+        <result column="cur_grid_x" property="curGridX"/>
+        <result column="cur_grid_y" property="curGridY"/>
+    </resultMap>
+    <!-- 通用查询结果列 -->
+    <sql id="Base_Column_List">
+        id
+        ,
+                    create_time as createTime ,
+                    update_time as updateTime ,
+                    task_id as taskId ,
+                    estimate_grid_x as estimateGridX ,
+                    estimate_grid_y as estimateGridY ,
+                    neutron_count as neutronCount ,
+                    cur_grid_x as curGridX ,
+                    cur_grid_y as curGridY
+    </sql>
+    <sql id="Base_Column_Alias">
+        a
+        .
+        id
+        ,
+            a.create_time
+            as createTime ,
+            a.update_time
+            as updateTime ,
+            a.task_id
+            as taskId ,
+            a.estimate_grid_x
+            as estimateGridX ,
+            a.estimate_grid_y
+            as estimateGridY ,
+            a.neutron_count
+            as neutronCount ,
+            a.cur_grid_x
+            as curGridX ,
+            a.cur_grid_y
+            as curGridY
+    </sql>
+    <select id="selectTaskGridInfoPage" resultType="com.casic.missiles.modular.robot.model.TaskGridInfo">
+        select
+        <include refid="Base_Column_Alias"/>
+        from task_grid_info a
+        <where>
+            <if test="ew != null">
+                ${ew.sqlSegment}
+            </if>
+        </where>
+    </select>
+    <sql id="Base_Column_Query">
+        <if test="request!=null">
+            <if test="request.taskId!= null and request.taskId!=''">
+                and task_id = #{request.taskId}
+            </if>
+            <if test="request.estimateGridX!= null and request.estimateGridX!=''">
+                and estimate_grid_x = #{request.estimateGridX}
+            </if>
+            <if test="request.estimateGridY!= null and request.estimateGridY!=''">
+                and estimate_grid_y = #{request.estimateGridY}
+            </if>
+            <if test="request.curGridX!= null and request.curGridX!=''">
+                and cur_grid_x = #{request.curGridX}
+            </if>
+            <if test="request.curGridY!= null and request.curGridY!=''">
+                and cur_grid_y = #{request.curGridY}
+            </if>
+        </if>
+    </sql>
+</mapper>
\ No newline at end of file
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java
index 749f559..4559074 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java
@@ -13,4 +13,9 @@
      * 规划点位信息
      */
     private PointNextDTO pointNextDTO;
+    /**
+     * 寻源目标地址
+     */
+    private PointNextDTO coordinate;
+    private boolean isEnd;
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java
index 39ff2a2..cef3238 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java
@@ -12,6 +12,9 @@
     private BigDecimal y;
 
     public static GridPointDTO convertGridPoint(PointNextDTO nextDTO) {
+        if(nextDTO==null){
+            return null;
+        }
         GridPointDTO gridPointDTO = new GridPointDTO();
         gridPointDTO.setX(Convert.toBigDecimal(nextDTO.getX()));
         gridPointDTO.setY(Convert.toBigDecimal(nextDTO.getY()));
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java
index 8c88242..33be25d 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java
@@ -12,25 +12,19 @@
 public class PathInitDTO {
     //---------------------算法参数
     /**
-     * x坐标最大值（x方向栅格总数）
+     * 初始位置x坐标(栅格地图下的坐标)
      */
-    private String xMax;
+    private String xNow;
     /**
-     * y坐标最大值（y方向栅格总数）
+     * 初始位置y坐标(栅格地图下的坐标)
      */
-    private String yMax;
+    private String yNow;
     /**
      * 初始位置计数率
      */
-    private String count;
-    /**
-     * 初始位置x坐标(0<=x<x_max)
-     */
-    private String x;
-    /**
-     * 初始位置y坐标(0<=y<y_max)
-     */
-    private String y;
+    private String zNow;
+
+    private Integer stepLen;
 
     //---------------------其他执行参数
     private String robotId;
@@ -38,11 +32,11 @@
 
     public List<String> toArray() {
         List<String> list = new ArrayList<>();
-        list.add(xMax);
-        list.add(yMax);
-        list.add(count);
-        list.add(x);
-        list.add(y);
+        list.add(xNow);
+        list.add(yNow);
+        list.add(stepLen+"");
+        list.add(zNow);
+        list.add(taskId+"");
         return list;
     }
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java
index 109e95c..ada6774 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java
@@ -10,21 +10,17 @@
  */
 @Data
 public class PathPlanDTO extends PathInitDTO {
-
     /**
-     * 长度为8的数组，记录(x-1, y-1), (x-1, y), (x-1, y+1), (x, y-1), (x, y+1), (x+1, y-1), (x+1, y), (x+1, y+1)8个坐标的可通行信息。
-     * 若某坐标有障碍物（不可通行行）则记为0，否则记为1
+     * 浮点阈值(阈值，超过该值后结束巡检开始寻源)
      */
-    private String open;
-
+    private String th;
     public List<String> toArray() {
         List<String> list = new ArrayList<>();
-        list.add(getXMax());
-        list.add(getYMax());
-        list.add(open);
-        list.add(getCount());
-        list.add(getX());
-        list.add(getY());
+
+        list.add(getStepLen()+"");
+        list.add(getTh());
+        list.add(getZNow());
+        list.add(getTaskId()+"");
         return list;
     }
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskGridInfo.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskGridInfo.java
new file mode 100644
index 0000000..101c10a
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskGridInfo.java
@@ -0,0 +1,83 @@
+package com.casic.missiles.modular.robot.model;
+
+import com.baomidou.mybatisplus.annotation.TableField;
+import com.baomidou.mybatisplus.annotation.TableName;
+import com.baomidou.mybatisplus.extension.activerecord.Model;
+import io.swagger.annotations.ApiModelProperty;
+import lombok.Data;
+
+import java.math.BigDecimal;
+import java.util.Date;
+
+/**
+ * 任务网格信息对象 task_grid_info
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+@Data
+@TableName("task_grid_info")
+public class TaskGridInfo extends Model<TaskGridInfo> {
+    private static final long serialVersionUID = 1L;
+    @TableField(exist = false)
+    private Integer index;
+    @ApiModelProperty(value = "主键", dataType = "Long")
+    @TableField("id")
+    private Long id;
+
+    @ApiModelProperty(value = "创建时间", dataType = "Date")
+    @TableField("create_time")
+    private Date createTime;
+
+    @ApiModelProperty(value = "修改时间", dataType = "Date")
+    @TableField("update_time")
+    private Date updateTime;
+
+    @ApiModelProperty(value = "任务ID", dataType = "Long")
+    @TableField("task_id")
+    private Long taskId;
+
+    @ApiModelProperty(value = "估算栅格x", dataType = "Long")
+    @TableField("estimate_grid_x")
+    private String estimateGridX;
+
+    @ApiModelProperty(value = "估算栅格y", dataType = "Long")
+    @TableField("estimate_grid_y")
+    private String estimateGridY;
+
+    @ApiModelProperty(value = "中子计数率", dataType = "BigDecimal")
+    @TableField("neutron_count")
+    private BigDecimal neutronCount;
+
+    @ApiModelProperty(value = "当前栅格x", dataType = "Long")
+    @TableField("cur_grid_x")
+    private Integer curGridX;
+
+    @ApiModelProperty(value = "当前栅格y", dataType = "Long")
+    @TableField("cur_grid_y")
+    private Integer curGridY;
+
+    public String getCurPoint(){
+        return "("+curGridX+","+curGridY+")";
+    }
+    public String getEstimatePoint(){
+        if(estimateGridX==null&estimateGridY==null){
+            return "结束";
+        }
+        return "("+estimateGridX+","+estimateGridY+")";
+    }
+    @Override
+    public String toString() {
+        return "TaskGridInfo{"
+                + "id=" + id
+                + "createTime=" + createTime
+                + "updateTime=" + updateTime
+                + "taskId=" + taskId
+                + "estimateGridX=" + estimateGridX
+                + "estimateGridY=" + estimateGridY
+                + "neutronCount=" + neutronCount
+                + "curGridX=" + curGridX
+                + "curGridY=" + curGridY
+                + "}";
+    }
+}
\ No newline at end of file
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java
index edc104e..a8acb2a 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java
@@ -53,6 +53,13 @@
     @TableField("route_id")
     private Long routeId;
 
+    @ApiModelProperty(value = "寻源初始步长", dataType = "Integer")
+    @TableField("step_len")
+    private Integer stepLen;
+
+    @ApiModelProperty(value = "寻源阈值设定", dataType = "BigDecimal")
+    @TableField("max_threshold")
+    private BigDecimal maxThreshold;
     /**
      * 任务类型(1巡检任务)
      */
@@ -62,7 +69,6 @@
     @ApiModelProperty(value = "任务类型字典名称", dataType = "String")
     @TableField(exist = false)
     private String taskTypeName;
-
     @ApiModelProperty(value = "识别类型(discernType 1中子源识别)", dataType = "Integer")
     @TableField("discern_type")
     private Integer discernType;
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java
index c4bc25d..c6620f3 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java
@@ -53,7 +53,7 @@
     }),
     SET_SPEED("/set_speed" , "/set_speed_response" , "最大运动速度设置" , true, new TypeReference<RobotMsgResponseDTO<Integer>>() {
     }),
-    SET_TARGETPOINT("/set_targetpoint" , "/set_targetpoint_response" , "设置导航目标点" , false, new TypeReference<RobotMsgResponseDTO<Integer>>() {
+    SET_TARGETPOINT("/set_targetpoint" , "/set_targetpoint_response" , "设置导航目标点" , true, new TypeReference<RobotMsgResponseDTO<Integer>>() {
     }),
 
     GET_DATA_LIST("/get_data_list" , "/get_data_list_response" , "获取数据包列表" , true, new TypeReference<RobotMsgResponseDTO<List<String>>>() {
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/AbstractSocketHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/AbstractSocketHandler.java
index 83f4cb2..e47e343 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/AbstractSocketHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/AbstractSocketHandler.java
@@ -49,7 +49,7 @@
                 wsMsgBaseDTO.setMsgKey(enums.getMsgKey());
                 wsMsgBaseDTO.setData(jsonObject.get("msg"));
                 String msg = JSON.toJSONString(wsMsgBaseDTO);
-                log.info("推送{}消息：{}", enums.getRemarks(), msg);
+//                log.info("推送{}消息：{}", enums.getRemarks(), msg);
                 webSocket.sendOneMessage(robotId, msg);
             }
         } catch (Exception e) {
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/cmd/TargetPointCmdHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/cmd/TargetPointCmdHandler.java
index c1c09fa..43b4460 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/cmd/TargetPointCmdHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/cmd/TargetPointCmdHandler.java
@@ -34,7 +34,11 @@
     @Override
     public ResponseDataDTO senderCmd(TargetPointRequest request, InstructCodeEnums enums) {
         MessageRequestDTO<TargetPointRequest> messageDTO = new MessageRequestDTO();
-        //读取默认关键点信息
+        //读取默认关键点信息 {"x":"1.715708819927424","y":"-0.9100042544250901","z":"0.017993879527185876","theta":"-0.039278239414312105"}
+//        request.setX("1.715708819927424");
+//        request.setY("-0.9100042544250901");
+//        request.setZ("0.017993879527185876");
+//        request.setTheta("-0.039278239414312105");
         messageDTO.setMsg(request);
         ResponseDataDTO<Integer> list = optService.robotCmd(getRobotInfo(), enums, messageDTO);
         return list;

diff --git a/.gitignore b/.gitignore
index 9e20bc1..f42a9cb 100644
--- a/.gitignore
+++ b/.gitignore
@@ -2,3 +2,4 @@
 **/target/**
 logs/
 *.iml
+*.exe
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java b/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
index c7b5d28..a849528 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
@@ -28,6 +28,7 @@
     String KEY_CUR_TASK_ID = "CUR_TASK_ID_";
     String KEY_CUR_TASK_INIT = "CUR_TASK_INIT_";
 
+    String KEY_COST_MAP = "KEY_COST_MAP";
     /**
      * PSD数据ID
      */
@@ -41,6 +42,7 @@
      */
     String KEY_CUR_XY_POINT = "KEY_CUR_XY_POINT_";
     String KEY_CUR_ESTIMATE_GRID = "KEY_CUR_ESTIMATE_GRID_";
+    String KEY_PLANING_FLAG = "KEY_PLANING_FLAG_";
     /**
      * 机器人巡点栅格
      */
@@ -55,6 +57,7 @@
      * 探测器配置参数缓存
      */
     String KEY_CONFIG_PARAMS_ID = "KEY_CONFIG_PARAMS_ID_";
+    String KEY_CONFIG_COSTMAP_RESOLUTION_SET = "KEY_CONFIG_COSTMAP_RESOLUTION_SET_";
 
     /**
      * 暂停状态缓存信息
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
index c4c905d..3bb475f 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
@@ -1,9 +1,14 @@
 package com.casic.missiles.modular.robot.controller;
 
+import cn.hutool.core.lang.Assert;
 import cn.hutool.core.util.StrUtil;
 import com.casic.missiles.core.base.controller.BaseController;
+import com.casic.missiles.core.cache.CacheUtil;
 import com.casic.missiles.core.util.SpringContextHolder;
+import com.casic.missiles.model.exception.RequestValidException;
+import com.casic.missiles.model.response.ResponseData;
 import com.casic.missiles.model.response.dto.ResponseDataDTO;
+import com.casic.missiles.modular.constants.RobotDictConstants;
 import com.casic.missiles.modular.robot.IBaseRobotService;
 import com.casic.missiles.modular.robot.dto.RobotInfoSetDTO;
 import com.casic.missiles.modular.robot.model.RobotInfo;
@@ -19,10 +24,16 @@
 import io.swagger.annotations.ApiOperation;
 import io.swagger.annotations.ApiParam;
 import lombok.SneakyThrows;
+import lombok.extern.slf4j.Slf4j;
+import org.apache.ibatis.annotations.Param;
+import org.springframework.beans.factory.annotation.Value;
 import org.springframework.web.bind.annotation.*;
+import org.springframework.web.multipart.MultipartFile;
 
 import javax.servlet.http.HttpServletResponse;
+import java.io.File;
 import java.util.List;
+import java.util.Optional;
 
 /**
  * 机器人操作类
@@ -30,6 +41,7 @@
  * @author lwh
  * @date 2023-10-30
  */
+@Slf4j
 @Api(tags = "机器人控制接口")
 @RestController
 @RequestMapping("/robot/opt")
@@ -61,6 +73,28 @@
         return baseRobotService.execCmdHandler(request, InstructCodeEnums.CHANGE_PCD);
     }
 
+    @Value("${casic.file.uploadPath}")
+    private String filePath;
+
+    @SneakyThrows
+    @ApiOperation(value = "PCD上传接口")
+    @PostMapping(value = "/uploadPcd")
+    public ResponseData uploadPcd(@RequestParam("file") MultipartFile file, @ApiParam("机器人ID,url拼接传参") String robotId) {
+        Assert.isFalse(StrUtil.isEmpty(robotId), () -> new RequestValidException("机器ID不能为空", Optional.of(new Throwable().getStackTrace()[2])));
+        RobotInfo info = robotInfoService.getById(robotId);
+        if (file != null && !file.isEmpty()) {
+            File pcdFile = new File(filePath + "finalCloud_" + info.getId() + ".pcd");
+            try {
+                file.transferTo(pcdFile);
+            } catch (Exception e) {
+                log.error(e.getMessage());
+            }
+            return ResponseData.success();
+        } else {
+            return ResponseData.error("附件上传失败!");
+        }
+    }
+
     @SneakyThrows
     @ApiOperation(value = "PCD下载接口")
     @PostMapping(value = "/pcdDownload")
@@ -136,6 +170,10 @@
     @ApiOperation(value = "启动或关闭循迹功能", notes = "返回 1成功 0失败  wait 启动时是否等待，0代表循迹启动即运动，不等待；1代表循迹启动原地等待。若无此字段，默认循迹启动不等待直接运动")
     @PostMapping(value = "/startTrack")
     public ResponseDataDTO<Integer> startTrack(@RequestBody StartTrackRequest request) {
+        request.setIsTask(1);
+        if (request.getStepLen() == null) {
+            request.setStepLen(2);
+        }
         baseRobotService.startTrack(request);
         return ResponseDataDTO.success();
     }
@@ -156,6 +194,13 @@
         MessageRequestDTO<CrlNavRequest> messageDTO = new MessageRequestDTO();
         messageDTO.setMsg(request);
         ResponseDataDTO<Integer> list = baseRobotService.getRobotRouteOptService().robotCmd(info, InstructCodeEnums.CTRL_NAV, messageDTO);
+        if (request.getAction() == 1) {
+            CostMapResolutionRequest costMap = new CostMapResolutionRequest();
+            costMap.setResolution(info.getResolution().doubleValue());
+            request.setRobotId(request.getRobotId());
+            //是否下发分辨率
+            CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CONFIG_COSTMAP_RESOLUTION_SET + request.getRobotId(), costMap);
+        }
         return list;
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java
new file mode 100644
index 0000000..5317651
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java
@@ -0,0 +1,86 @@
+package com.casic.missiles.modular.robot.controller;
+
+import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
+import com.baomidou.mybatisplus.extension.plugins.pagination.Page;
+import com.casic.missiles.core.base.controller.BaseController;
+import com.casic.missiles.core.page.PageFactory;
+import com.casic.missiles.core.page.PageInfoBT;
+import com.casic.missiles.model.form.IdForms;
+import com.casic.missiles.model.response.dto.ResponseDataDTO;
+import com.casic.missiles.modular.robot.model.TaskGridInfo;
+import com.casic.missiles.modular.robot.service.ITaskGridInfoService;
+import com.casic.missiles.modular.robot.vo.TaskGridInfoVO;
+import io.swagger.annotations.Api;
+import io.swagger.annotations.ApiOperation;
+import org.springframework.web.bind.annotation.*;
+
+import java.util.List;
+
+/**
+ * 任务网格信息Controller
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+@Api(tags = "任务网格信息")
+@RestController
+@RequestMapping("/robot/taskGridInfo")
+public class TaskGridInfoController extends BaseController {
+
+    private final ITaskGridInfoService taskGridInfoService;
+
+    public TaskGridInfoController(ITaskGridInfoService taskGridInfoService) {
+        this.taskGridInfoService = taskGridInfoService;
+    }
+
+    @ApiOperation(value = "列表查询")
+    @GetMapping(value = "/list")
+    public ResponseDataDTO<List<TaskGridInfo>> list(TaskGridInfoVO taskGridInfoVO) {
+        QueryWrapper<TaskGridInfo> query = taskGridInfoVO.genQuery();
+        return ResponseDataDTO.success(taskGridInfoService.list(query));
+    }
+
+    @ApiOperation(value = "分页查询")
+    @GetMapping(value = "/listPage")
+    public ResponseDataDTO<PageInfoBT<TaskGridInfo>> listPage(TaskGridInfoVO taskGridInfoVO) {
+        Page<TaskGridInfo> page = PageFactory.defaultPage();
+        QueryWrapper<TaskGridInfo> query = taskGridInfoVO.genQuery();
+        page = taskGridInfoService.page(page, query);
+        return ResponseDataDTO.success(super.packForBT(page));
+    }
+
+
+    @ApiOperation(value = "新增接口")
+    @PostMapping(value = "/add")
+    public ResponseDataDTO add(@RequestBody TaskGridInfo taskGridInfo) {
+        taskGridInfoService.save(taskGridInfo);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "修改接口")
+    @PostMapping(value = "/update")
+    public ResponseDataDTO update(@RequestBody TaskGridInfo taskGridInfo) {
+        taskGridInfoService.updateById(taskGridInfo);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "删除接口")
+    @GetMapping(value = "/delete")
+    public ResponseDataDTO delete(String id) {
+        taskGridInfoService.removeById(id);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation("批量删除")
+    @PostMapping("/batchDelete")
+    public ResponseDataDTO batchDelete(@RequestBody IdForms ids) {
+        taskGridInfoService.removeByIds(ids.getIds());
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "详情查询")
+    @GetMapping(value = "/detail")
+    public ResponseDataDTO<TaskGridInfo> detail(String id) {
+        return ResponseDataDTO.success(taskGridInfoService.getById(id));
+    }
+}
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
index e82a792..6992274 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
@@ -98,7 +98,7 @@
     public ResponseDataDTO<AlgorithmResponse> test() {
         AlgorithmResponse response = new AlgorithmResponse();
         response.setTaskHeatMap(new TaskHeatMap());
-        algorithmService.readAlgorithmResponse("step3.txt",response,123L,null);
+        algorithmService.readAlgorithmResponse("step3.txt",null,response,123L,null);
         sendWebSocket(InstructStatusMsgEnums.HEAT_MAP, "1", response.getTaskHeatMap());
         return ResponseDataDTO.success(response);
     }
@@ -121,7 +121,7 @@
     @GetMapping(value = "/testGrid")
     public ResponseDataDTO<List<RouteInfo>> testGrid(@RequestBody PathInitDTO pathInitDTO) {
         RobotInfo robotInfo = infoService.getById(pathInitDTO.getRobotId());
-        GridPointDTO gridPointDTO = algorithmService.convertSlamToGrid(new BigDecimal(pathInitDTO.getX()), new BigDecimal(10), new BigDecimal(4), robotInfo);
+        GridPointDTO gridPointDTO = algorithmService.convertSlamToGrid(new BigDecimal(pathInitDTO.getXNow()), new BigDecimal(10), new BigDecimal(4), robotInfo);
         return ResponseDataDTO.success(algorithmService.convertGridCenterPoint(gridPointDTO, new BigDecimal(10), robotInfo));
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/TaskGridInfoMapper.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/TaskGridInfoMapper.java
new file mode 100644
index 0000000..5c3a70b
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/TaskGridInfoMapper.java
@@ -0,0 +1,23 @@
+package com.casic.missiles.modular.robot.dao;
+
+
+import com.baomidou.mybatisplus.core.mapper.BaseMapper;
+import com.casic.missiles.modular.robot.model.TaskGridInfo;
+import org.apache.ibatis.annotations.Param;
+import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
+import com.baomidou.mybatisplus.extension.plugins.pagination.Page;
+import java.util.List;
+/**
+ * <p>
+ *  任务网格信息 Mapper 接口
+ * </p>
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+public interface TaskGridInfoMapper extends BaseMapper<TaskGridInfo> {
+/**
+* 任务网格信息 分页检索
+*/
+List<TaskGridInfo> selectTaskGridInfoPage( @Param("page") Page<TaskGridInfo> page,@Param("ew" ) QueryWrapper<TaskGridInfo> query);
+}
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/mapping/TaskGridInfoMapper.xml b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/mapping/TaskGridInfoMapper.xml
new file mode 100644
index 0000000..be573e0
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/mapping/TaskGridInfoMapper.xml
@@ -0,0 +1,80 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!DOCTYPE mapper PUBLIC "-//mybatis.org//DTD Mapper 3.0//EN" "http://mybatis.org/dtd/mybatis-3-mapper.dtd">
+<mapper namespace="com.casic.missiles.modular.robot.dao.TaskGridInfoMapper">
+    <!-- 通用查询映射结果 -->
+    <resultMap id="BaseResultMap" type="com.casic.missiles.modular.robot.model.TaskGridInfo">
+        <result column="id" property="id"/>
+        <result column="create_time" property="createTime"/>
+        <result column="update_time" property="updateTime"/>
+        <result column="task_id" property="taskId"/>
+        <result column="estimate_grid_x" property="estimateGridX"/>
+        <result column="estimate_grid_y" property="estimateGridY"/>
+        <result column="neutron_count" property="neutronCount"/>
+        <result column="cur_grid_x" property="curGridX"/>
+        <result column="cur_grid_y" property="curGridY"/>
+    </resultMap>
+    <!-- 通用查询结果列 -->
+    <sql id="Base_Column_List">
+        id
+        ,
+                    create_time as createTime ,
+                    update_time as updateTime ,
+                    task_id as taskId ,
+                    estimate_grid_x as estimateGridX ,
+                    estimate_grid_y as estimateGridY ,
+                    neutron_count as neutronCount ,
+                    cur_grid_x as curGridX ,
+                    cur_grid_y as curGridY
+    </sql>
+    <sql id="Base_Column_Alias">
+        a
+        .
+        id
+        ,
+            a.create_time
+            as createTime ,
+            a.update_time
+            as updateTime ,
+            a.task_id
+            as taskId ,
+            a.estimate_grid_x
+            as estimateGridX ,
+            a.estimate_grid_y
+            as estimateGridY ,
+            a.neutron_count
+            as neutronCount ,
+            a.cur_grid_x
+            as curGridX ,
+            a.cur_grid_y
+            as curGridY
+    </sql>
+    <select id="selectTaskGridInfoPage" resultType="com.casic.missiles.modular.robot.model.TaskGridInfo">
+        select
+        <include refid="Base_Column_Alias"/>
+        from task_grid_info a
+        <where>
+            <if test="ew != null">
+                ${ew.sqlSegment}
+            </if>
+        </where>
+    </select>
+    <sql id="Base_Column_Query">
+        <if test="request!=null">
+            <if test="request.taskId!= null and request.taskId!=''">
+                and task_id = #{request.taskId}
+            </if>
+            <if test="request.estimateGridX!= null and request.estimateGridX!=''">
+                and estimate_grid_x = #{request.estimateGridX}
+            </if>
+            <if test="request.estimateGridY!= null and request.estimateGridY!=''">
+                and estimate_grid_y = #{request.estimateGridY}
+            </if>
+            <if test="request.curGridX!= null and request.curGridX!=''">
+                and cur_grid_x = #{request.curGridX}
+            </if>
+            <if test="request.curGridY!= null and request.curGridY!=''">
+                and cur_grid_y = #{request.curGridY}
+            </if>
+        </if>
+    </sql>
+</mapper>
\ No newline at end of file
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java
index 749f559..4559074 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java
@@ -13,4 +13,9 @@
      * 规划点位信息
      */
     private PointNextDTO pointNextDTO;
+    /**
+     * 寻源目标地址
+     */
+    private PointNextDTO coordinate;
+    private boolean isEnd;
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java
index 39ff2a2..cef3238 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java
@@ -12,6 +12,9 @@
     private BigDecimal y;
 
     public static GridPointDTO convertGridPoint(PointNextDTO nextDTO) {
+        if(nextDTO==null){
+            return null;
+        }
         GridPointDTO gridPointDTO = new GridPointDTO();
         gridPointDTO.setX(Convert.toBigDecimal(nextDTO.getX()));
         gridPointDTO.setY(Convert.toBigDecimal(nextDTO.getY()));
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java
index 8c88242..33be25d 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java
@@ -12,25 +12,19 @@
 public class PathInitDTO {
     //---------------------算法参数
     /**
-     * x坐标最大值（x方向栅格总数）
+     * 初始位置x坐标(栅格地图下的坐标)
      */
-    private String xMax;
+    private String xNow;
     /**
-     * y坐标最大值（y方向栅格总数）
+     * 初始位置y坐标(栅格地图下的坐标)
      */
-    private String yMax;
+    private String yNow;
     /**
      * 初始位置计数率
      */
-    private String count;
-    /**
-     * 初始位置x坐标(0<=x<x_max)
-     */
-    private String x;
-    /**
-     * 初始位置y坐标(0<=y<y_max)
-     */
-    private String y;
+    private String zNow;
+
+    private Integer stepLen;
 
     //---------------------其他执行参数
     private String robotId;
@@ -38,11 +32,11 @@
 
     public List<String> toArray() {
         List<String> list = new ArrayList<>();
-        list.add(xMax);
-        list.add(yMax);
-        list.add(count);
-        list.add(x);
-        list.add(y);
+        list.add(xNow);
+        list.add(yNow);
+        list.add(stepLen+"");
+        list.add(zNow);
+        list.add(taskId+"");
         return list;
     }
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java
index 109e95c..ada6774 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java
@@ -10,21 +10,17 @@
  */
 @Data
 public class PathPlanDTO extends PathInitDTO {
-
     /**
-     * 长度为8的数组，记录(x-1, y-1), (x-1, y), (x-1, y+1), (x, y-1), (x, y+1), (x+1, y-1), (x+1, y), (x+1, y+1)8个坐标的可通行信息。
-     * 若某坐标有障碍物（不可通行行）则记为0，否则记为1
+     * 浮点阈值(阈值，超过该值后结束巡检开始寻源)
      */
-    private String open;
-
+    private String th;
     public List<String> toArray() {
         List<String> list = new ArrayList<>();
-        list.add(getXMax());
-        list.add(getYMax());
-        list.add(open);
-        list.add(getCount());
-        list.add(getX());
-        list.add(getY());
+
+        list.add(getStepLen()+"");
+        list.add(getTh());
+        list.add(getZNow());
+        list.add(getTaskId()+"");
         return list;
     }
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskGridInfo.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskGridInfo.java
new file mode 100644
index 0000000..101c10a
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskGridInfo.java
@@ -0,0 +1,83 @@
+package com.casic.missiles.modular.robot.model;
+
+import com.baomidou.mybatisplus.annotation.TableField;
+import com.baomidou.mybatisplus.annotation.TableName;
+import com.baomidou.mybatisplus.extension.activerecord.Model;
+import io.swagger.annotations.ApiModelProperty;
+import lombok.Data;
+
+import java.math.BigDecimal;
+import java.util.Date;
+
+/**
+ * 任务网格信息对象 task_grid_info
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+@Data
+@TableName("task_grid_info")
+public class TaskGridInfo extends Model<TaskGridInfo> {
+    private static final long serialVersionUID = 1L;
+    @TableField(exist = false)
+    private Integer index;
+    @ApiModelProperty(value = "主键", dataType = "Long")
+    @TableField("id")
+    private Long id;
+
+    @ApiModelProperty(value = "创建时间", dataType = "Date")
+    @TableField("create_time")
+    private Date createTime;
+
+    @ApiModelProperty(value = "修改时间", dataType = "Date")
+    @TableField("update_time")
+    private Date updateTime;
+
+    @ApiModelProperty(value = "任务ID", dataType = "Long")
+    @TableField("task_id")
+    private Long taskId;
+
+    @ApiModelProperty(value = "估算栅格x", dataType = "Long")
+    @TableField("estimate_grid_x")
+    private String estimateGridX;
+
+    @ApiModelProperty(value = "估算栅格y", dataType = "Long")
+    @TableField("estimate_grid_y")
+    private String estimateGridY;
+
+    @ApiModelProperty(value = "中子计数率", dataType = "BigDecimal")
+    @TableField("neutron_count")
+    private BigDecimal neutronCount;
+
+    @ApiModelProperty(value = "当前栅格x", dataType = "Long")
+    @TableField("cur_grid_x")
+    private Integer curGridX;
+
+    @ApiModelProperty(value = "当前栅格y", dataType = "Long")
+    @TableField("cur_grid_y")
+    private Integer curGridY;
+
+    public String getCurPoint(){
+        return "("+curGridX+","+curGridY+")";
+    }
+    public String getEstimatePoint(){
+        if(estimateGridX==null&estimateGridY==null){
+            return "结束";
+        }
+        return "("+estimateGridX+","+estimateGridY+")";
+    }
+    @Override
+    public String toString() {
+        return "TaskGridInfo{"
+                + "id=" + id
+                + "createTime=" + createTime
+                + "updateTime=" + updateTime
+                + "taskId=" + taskId
+                + "estimateGridX=" + estimateGridX
+                + "estimateGridY=" + estimateGridY
+                + "neutronCount=" + neutronCount
+                + "curGridX=" + curGridX
+                + "curGridY=" + curGridY
+                + "}";
+    }
+}
\ No newline at end of file
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java
index edc104e..a8acb2a 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java
@@ -53,6 +53,13 @@
     @TableField("route_id")
     private Long routeId;
 
+    @ApiModelProperty(value = "寻源初始步长", dataType = "Integer")
+    @TableField("step_len")
+    private Integer stepLen;
+
+    @ApiModelProperty(value = "寻源阈值设定", dataType = "BigDecimal")
+    @TableField("max_threshold")
+    private BigDecimal maxThreshold;
     /**
      * 任务类型(1巡检任务)
      */
@@ -62,7 +69,6 @@
     @ApiModelProperty(value = "任务类型字典名称", dataType = "String")
     @TableField(exist = false)
     private String taskTypeName;
-
     @ApiModelProperty(value = "识别类型(discernType 1中子源识别)", dataType = "Integer")
     @TableField("discern_type")
     private Integer discernType;
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java
index c4bc25d..c6620f3 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java
@@ -53,7 +53,7 @@
     }),
     SET_SPEED("/set_speed" , "/set_speed_response" , "最大运动速度设置" , true, new TypeReference<RobotMsgResponseDTO<Integer>>() {
     }),
-    SET_TARGETPOINT("/set_targetpoint" , "/set_targetpoint_response" , "设置导航目标点" , false, new TypeReference<RobotMsgResponseDTO<Integer>>() {
+    SET_TARGETPOINT("/set_targetpoint" , "/set_targetpoint_response" , "设置导航目标点" , true, new TypeReference<RobotMsgResponseDTO<Integer>>() {
     }),
 
     GET_DATA_LIST("/get_data_list" , "/get_data_list_response" , "获取数据包列表" , true, new TypeReference<RobotMsgResponseDTO<List<String>>>() {
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/AbstractSocketHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/AbstractSocketHandler.java
index 83f4cb2..e47e343 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/AbstractSocketHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/AbstractSocketHandler.java
@@ -49,7 +49,7 @@
                 wsMsgBaseDTO.setMsgKey(enums.getMsgKey());
                 wsMsgBaseDTO.setData(jsonObject.get("msg"));
                 String msg = JSON.toJSONString(wsMsgBaseDTO);
-                log.info("推送{}消息：{}", enums.getRemarks(), msg);
+//                log.info("推送{}消息：{}", enums.getRemarks(), msg);
                 webSocket.sendOneMessage(robotId, msg);
             }
         } catch (Exception e) {
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/cmd/TargetPointCmdHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/cmd/TargetPointCmdHandler.java
index c1c09fa..43b4460 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/cmd/TargetPointCmdHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/cmd/TargetPointCmdHandler.java
@@ -34,7 +34,11 @@
     @Override
     public ResponseDataDTO senderCmd(TargetPointRequest request, InstructCodeEnums enums) {
         MessageRequestDTO<TargetPointRequest> messageDTO = new MessageRequestDTO();
-        //读取默认关键点信息
+        //读取默认关键点信息 {"x":"1.715708819927424","y":"-0.9100042544250901","z":"0.017993879527185876","theta":"-0.039278239414312105"}
+//        request.setX("1.715708819927424");
+//        request.setY("-0.9100042544250901");
+//        request.setZ("0.017993879527185876");
+//        request.setTheta("-0.039278239414312105");
         messageDTO.setMsg(request);
         ResponseDataDTO<Integer> list = optService.robotCmd(getRobotInfo(), enums, messageDTO);
         return list;
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/AppCostMapHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/AppCostMapHandler.java
index 0335693..88c9f79 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/AppCostMapHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/AppCostMapHandler.java
@@ -1,6 +1,11 @@
 package com.casic.missiles.modular.robot.opt.handler.msg;
 
+import com.casic.missiles.core.cache.CacheUtil;
+import com.casic.missiles.modular.constants.RobotDictConstants;
+import com.casic.missiles.modular.robot.IBaseRobotService;
+import com.casic.missiles.modular.robot.opt.enums.InstructCodeEnums;
 import com.casic.missiles.modular.robot.opt.handler.AbstractSocketHandler;
+import com.casic.missiles.modular.robot.opt.instruct.dto.CostMapResolutionRequest;
 import com.casic.missiles.modular.robot.service.IRobotStatusInfoService;
 import lombok.extern.slf4j.Slf4j;
 import org.eclipse.paho.client.mqttv3.MqttMessage;
@@ -16,14 +21,27 @@
 @Component("/app_costmap_sender")
 public class AppCostMapHandler extends AbstractSocketHandler {
     private final IRobotStatusInfoService statusInfoService;
+    private final IBaseRobotService baseRobotService;
     private static Map<String, Integer> index = new ConcurrentHashMap<>();
 
-    public AppCostMapHandler(IRobotStatusInfoService statusInfoService) {
+    public AppCostMapHandler(IRobotStatusInfoService statusInfoService, IBaseRobotService baseRobotService) {
         this.statusInfoService = statusInfoService;
+        this.baseRobotService = baseRobotService;
     }
 
     @Override
     public void process(String robotId, String topic, MqttMessage message) {
         statusInfoService.saveCostMap(robotId, message);
+
+        CostMapResolutionRequest costMapSet = CacheUtil.get(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CONFIG_COSTMAP_RESOLUTION_SET +robotId);
+        if(costMapSet != null){
+            CacheUtil.remove(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CONFIG_COSTMAP_RESOLUTION_SET +robotId);
+            new Thread(() -> {
+                costMapSet.setRobotId(Long.valueOf(robotId));
+                baseRobotService.execCmdHandler(costMapSet, InstructCodeEnums.COSTMAP_RESOLUTION);
+            }).start();
+
+        }
+
     }
 }

diff --git a/.gitignore b/.gitignore
index 9e20bc1..f42a9cb 100644
--- a/.gitignore
+++ b/.gitignore
@@ -2,3 +2,4 @@
 **/target/**
 logs/
 *.iml
+*.exe
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java b/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
index c7b5d28..a849528 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
@@ -28,6 +28,7 @@
     String KEY_CUR_TASK_ID = "CUR_TASK_ID_";
     String KEY_CUR_TASK_INIT = "CUR_TASK_INIT_";
 
+    String KEY_COST_MAP = "KEY_COST_MAP";
     /**
      * PSD数据ID
      */
@@ -41,6 +42,7 @@
      */
     String KEY_CUR_XY_POINT = "KEY_CUR_XY_POINT_";
     String KEY_CUR_ESTIMATE_GRID = "KEY_CUR_ESTIMATE_GRID_";
+    String KEY_PLANING_FLAG = "KEY_PLANING_FLAG_";
     /**
      * 机器人巡点栅格
      */
@@ -55,6 +57,7 @@
      * 探测器配置参数缓存
      */
     String KEY_CONFIG_PARAMS_ID = "KEY_CONFIG_PARAMS_ID_";
+    String KEY_CONFIG_COSTMAP_RESOLUTION_SET = "KEY_CONFIG_COSTMAP_RESOLUTION_SET_";
 
     /**
      * 暂停状态缓存信息
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
index c4c905d..3bb475f 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
@@ -1,9 +1,14 @@
 package com.casic.missiles.modular.robot.controller;
 
+import cn.hutool.core.lang.Assert;
 import cn.hutool.core.util.StrUtil;
 import com.casic.missiles.core.base.controller.BaseController;
+import com.casic.missiles.core.cache.CacheUtil;
 import com.casic.missiles.core.util.SpringContextHolder;
+import com.casic.missiles.model.exception.RequestValidException;
+import com.casic.missiles.model.response.ResponseData;
 import com.casic.missiles.model.response.dto.ResponseDataDTO;
+import com.casic.missiles.modular.constants.RobotDictConstants;
 import com.casic.missiles.modular.robot.IBaseRobotService;
 import com.casic.missiles.modular.robot.dto.RobotInfoSetDTO;
 import com.casic.missiles.modular.robot.model.RobotInfo;
@@ -19,10 +24,16 @@
 import io.swagger.annotations.ApiOperation;
 import io.swagger.annotations.ApiParam;
 import lombok.SneakyThrows;
+import lombok.extern.slf4j.Slf4j;
+import org.apache.ibatis.annotations.Param;
+import org.springframework.beans.factory.annotation.Value;
 import org.springframework.web.bind.annotation.*;
+import org.springframework.web.multipart.MultipartFile;
 
 import javax.servlet.http.HttpServletResponse;
+import java.io.File;
 import java.util.List;
+import java.util.Optional;
 
 /**
  * 机器人操作类
@@ -30,6 +41,7 @@
  * @author lwh
  * @date 2023-10-30
  */
+@Slf4j
 @Api(tags = "机器人控制接口")
 @RestController
 @RequestMapping("/robot/opt")
@@ -61,6 +73,28 @@
         return baseRobotService.execCmdHandler(request, InstructCodeEnums.CHANGE_PCD);
     }
 
+    @Value("${casic.file.uploadPath}")
+    private String filePath;
+
+    @SneakyThrows
+    @ApiOperation(value = "PCD上传接口")
+    @PostMapping(value = "/uploadPcd")
+    public ResponseData uploadPcd(@RequestParam("file") MultipartFile file, @ApiParam("机器人ID,url拼接传参") String robotId) {
+        Assert.isFalse(StrUtil.isEmpty(robotId), () -> new RequestValidException("机器ID不能为空", Optional.of(new Throwable().getStackTrace()[2])));
+        RobotInfo info = robotInfoService.getById(robotId);
+        if (file != null && !file.isEmpty()) {
+            File pcdFile = new File(filePath + "finalCloud_" + info.getId() + ".pcd");
+            try {
+                file.transferTo(pcdFile);
+            } catch (Exception e) {
+                log.error(e.getMessage());
+            }
+            return ResponseData.success();
+        } else {
+            return ResponseData.error("附件上传失败!");
+        }
+    }
+
     @SneakyThrows
     @ApiOperation(value = "PCD下载接口")
     @PostMapping(value = "/pcdDownload")
@@ -136,6 +170,10 @@
     @ApiOperation(value = "启动或关闭循迹功能", notes = "返回 1成功 0失败  wait 启动时是否等待，0代表循迹启动即运动，不等待；1代表循迹启动原地等待。若无此字段，默认循迹启动不等待直接运动")
     @PostMapping(value = "/startTrack")
     public ResponseDataDTO<Integer> startTrack(@RequestBody StartTrackRequest request) {
+        request.setIsTask(1);
+        if (request.getStepLen() == null) {
+            request.setStepLen(2);
+        }
         baseRobotService.startTrack(request);
         return ResponseDataDTO.success();
     }
@@ -156,6 +194,13 @@
         MessageRequestDTO<CrlNavRequest> messageDTO = new MessageRequestDTO();
         messageDTO.setMsg(request);
         ResponseDataDTO<Integer> list = baseRobotService.getRobotRouteOptService().robotCmd(info, InstructCodeEnums.CTRL_NAV, messageDTO);
+        if (request.getAction() == 1) {
+            CostMapResolutionRequest costMap = new CostMapResolutionRequest();
+            costMap.setResolution(info.getResolution().doubleValue());
+            request.setRobotId(request.getRobotId());
+            //是否下发分辨率
+            CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CONFIG_COSTMAP_RESOLUTION_SET + request.getRobotId(), costMap);
+        }
         return list;
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java
new file mode 100644
index 0000000..5317651
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java
@@ -0,0 +1,86 @@
+package com.casic.missiles.modular.robot.controller;
+
+import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
+import com.baomidou.mybatisplus.extension.plugins.pagination.Page;
+import com.casic.missiles.core.base.controller.BaseController;
+import com.casic.missiles.core.page.PageFactory;
+import com.casic.missiles.core.page.PageInfoBT;
+import com.casic.missiles.model.form.IdForms;
+import com.casic.missiles.model.response.dto.ResponseDataDTO;
+import com.casic.missiles.modular.robot.model.TaskGridInfo;
+import com.casic.missiles.modular.robot.service.ITaskGridInfoService;
+import com.casic.missiles.modular.robot.vo.TaskGridInfoVO;
+import io.swagger.annotations.Api;
+import io.swagger.annotations.ApiOperation;
+import org.springframework.web.bind.annotation.*;
+
+import java.util.List;
+
+/**
+ * 任务网格信息Controller
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+@Api(tags = "任务网格信息")
+@RestController
+@RequestMapping("/robot/taskGridInfo")
+public class TaskGridInfoController extends BaseController {
+
+    private final ITaskGridInfoService taskGridInfoService;
+
+    public TaskGridInfoController(ITaskGridInfoService taskGridInfoService) {
+        this.taskGridInfoService = taskGridInfoService;
+    }
+
+    @ApiOperation(value = "列表查询")
+    @GetMapping(value = "/list")
+    public ResponseDataDTO<List<TaskGridInfo>> list(TaskGridInfoVO taskGridInfoVO) {
+        QueryWrapper<TaskGridInfo> query = taskGridInfoVO.genQuery();
+        return ResponseDataDTO.success(taskGridInfoService.list(query));
+    }
+
+    @ApiOperation(value = "分页查询")
+    @GetMapping(value = "/listPage")
+    public ResponseDataDTO<PageInfoBT<TaskGridInfo>> listPage(TaskGridInfoVO taskGridInfoVO) {
+        Page<TaskGridInfo> page = PageFactory.defaultPage();
+        QueryWrapper<TaskGridInfo> query = taskGridInfoVO.genQuery();
+        page = taskGridInfoService.page(page, query);
+        return ResponseDataDTO.success(super.packForBT(page));
+    }
+
+
+    @ApiOperation(value = "新增接口")
+    @PostMapping(value = "/add")
+    public ResponseDataDTO add(@RequestBody TaskGridInfo taskGridInfo) {
+        taskGridInfoService.save(taskGridInfo);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "修改接口")
+    @PostMapping(value = "/update")
+    public ResponseDataDTO update(@RequestBody TaskGridInfo taskGridInfo) {
+        taskGridInfoService.updateById(taskGridInfo);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "删除接口")
+    @GetMapping(value = "/delete")
+    public ResponseDataDTO delete(String id) {
+        taskGridInfoService.removeById(id);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation("批量删除")
+    @PostMapping("/batchDelete")
+    public ResponseDataDTO batchDelete(@RequestBody IdForms ids) {
+        taskGridInfoService.removeByIds(ids.getIds());
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "详情查询")
+    @GetMapping(value = "/detail")
+    public ResponseDataDTO<TaskGridInfo> detail(String id) {
+        return ResponseDataDTO.success(taskGridInfoService.getById(id));
+    }
+}
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
index e82a792..6992274 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
@@ -98,7 +98,7 @@
     public ResponseDataDTO<AlgorithmResponse> test() {
         AlgorithmResponse response = new AlgorithmResponse();
         response.setTaskHeatMap(new TaskHeatMap());
-        algorithmService.readAlgorithmResponse("step3.txt",response,123L,null);
+        algorithmService.readAlgorithmResponse("step3.txt",null,response,123L,null);
         sendWebSocket(InstructStatusMsgEnums.HEAT_MAP, "1", response.getTaskHeatMap());
         return ResponseDataDTO.success(response);
     }
@@ -121,7 +121,7 @@
     @GetMapping(value = "/testGrid")
     public ResponseDataDTO<List<RouteInfo>> testGrid(@RequestBody PathInitDTO pathInitDTO) {
         RobotInfo robotInfo = infoService.getById(pathInitDTO.getRobotId());
-        GridPointDTO gridPointDTO = algorithmService.convertSlamToGrid(new BigDecimal(pathInitDTO.getX()), new BigDecimal(10), new BigDecimal(4), robotInfo);
+        GridPointDTO gridPointDTO = algorithmService.convertSlamToGrid(new BigDecimal(pathInitDTO.getXNow()), new BigDecimal(10), new BigDecimal(4), robotInfo);
         return ResponseDataDTO.success(algorithmService.convertGridCenterPoint(gridPointDTO, new BigDecimal(10), robotInfo));
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/TaskGridInfoMapper.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/TaskGridInfoMapper.java
new file mode 100644
index 0000000..5c3a70b
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/TaskGridInfoMapper.java
@@ -0,0 +1,23 @@
+package com.casic.missiles.modular.robot.dao;
+
+
+import com.baomidou.mybatisplus.core.mapper.BaseMapper;
+import com.casic.missiles.modular.robot.model.TaskGridInfo;
+import org.apache.ibatis.annotations.Param;
+import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
+import com.baomidou.mybatisplus.extension.plugins.pagination.Page;
+import java.util.List;
+/**
+ * <p>
+ *  任务网格信息 Mapper 接口
+ * </p>
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+public interface TaskGridInfoMapper extends BaseMapper<TaskGridInfo> {
+/**
+* 任务网格信息 分页检索
+*/
+List<TaskGridInfo> selectTaskGridInfoPage( @Param("page") Page<TaskGridInfo> page,@Param("ew" ) QueryWrapper<TaskGridInfo> query);
+}
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/mapping/TaskGridInfoMapper.xml b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/mapping/TaskGridInfoMapper.xml
new file mode 100644
index 0000000..be573e0
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/mapping/TaskGridInfoMapper.xml
@@ -0,0 +1,80 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!DOCTYPE mapper PUBLIC "-//mybatis.org//DTD Mapper 3.0//EN" "http://mybatis.org/dtd/mybatis-3-mapper.dtd">
+<mapper namespace="com.casic.missiles.modular.robot.dao.TaskGridInfoMapper">
+    <!-- 通用查询映射结果 -->
+    <resultMap id="BaseResultMap" type="com.casic.missiles.modular.robot.model.TaskGridInfo">
+        <result column="id" property="id"/>
+        <result column="create_time" property="createTime"/>
+        <result column="update_time" property="updateTime"/>
+        <result column="task_id" property="taskId"/>
+        <result column="estimate_grid_x" property="estimateGridX"/>
+        <result column="estimate_grid_y" property="estimateGridY"/>
+        <result column="neutron_count" property="neutronCount"/>
+        <result column="cur_grid_x" property="curGridX"/>
+        <result column="cur_grid_y" property="curGridY"/>
+    </resultMap>
+    <!-- 通用查询结果列 -->
+    <sql id="Base_Column_List">
+        id
+        ,
+                    create_time as createTime ,
+                    update_time as updateTime ,
+                    task_id as taskId ,
+                    estimate_grid_x as estimateGridX ,
+                    estimate_grid_y as estimateGridY ,
+                    neutron_count as neutronCount ,
+                    cur_grid_x as curGridX ,
+                    cur_grid_y as curGridY
+    </sql>
+    <sql id="Base_Column_Alias">
+        a
+        .
+        id
+        ,
+            a.create_time
+            as createTime ,
+            a.update_time
+            as updateTime ,
+            a.task_id
+            as taskId ,
+            a.estimate_grid_x
+            as estimateGridX ,
+            a.estimate_grid_y
+            as estimateGridY ,
+            a.neutron_count
+            as neutronCount ,
+            a.cur_grid_x
+            as curGridX ,
+            a.cur_grid_y
+            as curGridY
+    </sql>
+    <select id="selectTaskGridInfoPage" resultType="com.casic.missiles.modular.robot.model.TaskGridInfo">
+        select
+        <include refid="Base_Column_Alias"/>
+        from task_grid_info a
+        <where>
+            <if test="ew != null">
+                ${ew.sqlSegment}
+            </if>
+        </where>
+    </select>
+    <sql id="Base_Column_Query">
+        <if test="request!=null">
+            <if test="request.taskId!= null and request.taskId!=''">
+                and task_id = #{request.taskId}
+            </if>
+            <if test="request.estimateGridX!= null and request.estimateGridX!=''">
+                and estimate_grid_x = #{request.estimateGridX}
+            </if>
+            <if test="request.estimateGridY!= null and request.estimateGridY!=''">
+                and estimate_grid_y = #{request.estimateGridY}
+            </if>
+            <if test="request.curGridX!= null and request.curGridX!=''">
+                and cur_grid_x = #{request.curGridX}
+            </if>
+            <if test="request.curGridY!= null and request.curGridY!=''">
+                and cur_grid_y = #{request.curGridY}
+            </if>
+        </if>
+    </sql>
+</mapper>
\ No newline at end of file
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java
index 749f559..4559074 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java
@@ -13,4 +13,9 @@
      * 规划点位信息
      */
     private PointNextDTO pointNextDTO;
+    /**
+     * 寻源目标地址
+     */
+    private PointNextDTO coordinate;
+    private boolean isEnd;
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java
index 39ff2a2..cef3238 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java
@@ -12,6 +12,9 @@
     private BigDecimal y;
 
     public static GridPointDTO convertGridPoint(PointNextDTO nextDTO) {
+        if(nextDTO==null){
+            return null;
+        }
         GridPointDTO gridPointDTO = new GridPointDTO();
         gridPointDTO.setX(Convert.toBigDecimal(nextDTO.getX()));
         gridPointDTO.setY(Convert.toBigDecimal(nextDTO.getY()));
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java
index 8c88242..33be25d 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java
@@ -12,25 +12,19 @@
 public class PathInitDTO {
     //---------------------算法参数
     /**
-     * x坐标最大值（x方向栅格总数）
+     * 初始位置x坐标(栅格地图下的坐标)
      */
-    private String xMax;
+    private String xNow;
     /**
-     * y坐标最大值（y方向栅格总数）
+     * 初始位置y坐标(栅格地图下的坐标)
      */
-    private String yMax;
+    private String yNow;
     /**
      * 初始位置计数率
      */
-    private String count;
-    /**
-     * 初始位置x坐标(0<=x<x_max)
-     */
-    private String x;
-    /**
-     * 初始位置y坐标(0<=y<y_max)
-     */
-    private String y;
+    private String zNow;
+
+    private Integer stepLen;
 
     //---------------------其他执行参数
     private String robotId;
@@ -38,11 +32,11 @@
 
     public List<String> toArray() {
         List<String> list = new ArrayList<>();
-        list.add(xMax);
-        list.add(yMax);
-        list.add(count);
-        list.add(x);
-        list.add(y);
+        list.add(xNow);
+        list.add(yNow);
+        list.add(stepLen+"");
+        list.add(zNow);
+        list.add(taskId+"");
         return list;
     }
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java
index 109e95c..ada6774 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java
@@ -10,21 +10,17 @@
  */
 @Data
 public class PathPlanDTO extends PathInitDTO {
-
     /**
-     * 长度为8的数组，记录(x-1, y-1), (x-1, y), (x-1, y+1), (x, y-1), (x, y+1), (x+1, y-1), (x+1, y), (x+1, y+1)8个坐标的可通行信息。
-     * 若某坐标有障碍物（不可通行行）则记为0，否则记为1
+     * 浮点阈值(阈值，超过该值后结束巡检开始寻源)
      */
-    private String open;
-
+    private String th;
     public List<String> toArray() {
         List<String> list = new ArrayList<>();
-        list.add(getXMax());
-        list.add(getYMax());
-        list.add(open);
-        list.add(getCount());
-        list.add(getX());
-        list.add(getY());
+
+        list.add(getStepLen()+"");
+        list.add(getTh());
+        list.add(getZNow());
+        list.add(getTaskId()+"");
         return list;
     }
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskGridInfo.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskGridInfo.java
new file mode 100644
index 0000000..101c10a
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskGridInfo.java
@@ -0,0 +1,83 @@
+package com.casic.missiles.modular.robot.model;
+
+import com.baomidou.mybatisplus.annotation.TableField;
+import com.baomidou.mybatisplus.annotation.TableName;
+import com.baomidou.mybatisplus.extension.activerecord.Model;
+import io.swagger.annotations.ApiModelProperty;
+import lombok.Data;
+
+import java.math.BigDecimal;
+import java.util.Date;
+
+/**
+ * 任务网格信息对象 task_grid_info
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+@Data
+@TableName("task_grid_info")
+public class TaskGridInfo extends Model<TaskGridInfo> {
+    private static final long serialVersionUID = 1L;
+    @TableField(exist = false)
+    private Integer index;
+    @ApiModelProperty(value = "主键", dataType = "Long")
+    @TableField("id")
+    private Long id;
+
+    @ApiModelProperty(value = "创建时间", dataType = "Date")
+    @TableField("create_time")
+    private Date createTime;
+
+    @ApiModelProperty(value = "修改时间", dataType = "Date")
+    @TableField("update_time")
+    private Date updateTime;
+
+    @ApiModelProperty(value = "任务ID", dataType = "Long")
+    @TableField("task_id")
+    private Long taskId;
+
+    @ApiModelProperty(value = "估算栅格x", dataType = "Long")
+    @TableField("estimate_grid_x")
+    private String estimateGridX;
+
+    @ApiModelProperty(value = "估算栅格y", dataType = "Long")
+    @TableField("estimate_grid_y")
+    private String estimateGridY;
+
+    @ApiModelProperty(value = "中子计数率", dataType = "BigDecimal")
+    @TableField("neutron_count")
+    private BigDecimal neutronCount;
+
+    @ApiModelProperty(value = "当前栅格x", dataType = "Long")
+    @TableField("cur_grid_x")
+    private Integer curGridX;
+
+    @ApiModelProperty(value = "当前栅格y", dataType = "Long")
+    @TableField("cur_grid_y")
+    private Integer curGridY;
+
+    public String getCurPoint(){
+        return "("+curGridX+","+curGridY+")";
+    }
+    public String getEstimatePoint(){
+        if(estimateGridX==null&estimateGridY==null){
+            return "结束";
+        }
+        return "("+estimateGridX+","+estimateGridY+")";
+    }
+    @Override
+    public String toString() {
+        return "TaskGridInfo{"
+                + "id=" + id
+                + "createTime=" + createTime
+                + "updateTime=" + updateTime
+                + "taskId=" + taskId
+                + "estimateGridX=" + estimateGridX
+                + "estimateGridY=" + estimateGridY
+                + "neutronCount=" + neutronCount
+                + "curGridX=" + curGridX
+                + "curGridY=" + curGridY
+                + "}";
+    }
+}
\ No newline at end of file
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java
index edc104e..a8acb2a 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java
@@ -53,6 +53,13 @@
     @TableField("route_id")
     private Long routeId;
 
+    @ApiModelProperty(value = "寻源初始步长", dataType = "Integer")
+    @TableField("step_len")
+    private Integer stepLen;
+
+    @ApiModelProperty(value = "寻源阈值设定", dataType = "BigDecimal")
+    @TableField("max_threshold")
+    private BigDecimal maxThreshold;
     /**
      * 任务类型(1巡检任务)
      */
@@ -62,7 +69,6 @@
     @ApiModelProperty(value = "任务类型字典名称", dataType = "String")
     @TableField(exist = false)
     private String taskTypeName;
-
     @ApiModelProperty(value = "识别类型(discernType 1中子源识别)", dataType = "Integer")
     @TableField("discern_type")
     private Integer discernType;
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java
index c4bc25d..c6620f3 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java
@@ -53,7 +53,7 @@
     }),
     SET_SPEED("/set_speed" , "/set_speed_response" , "最大运动速度设置" , true, new TypeReference<RobotMsgResponseDTO<Integer>>() {
     }),
-    SET_TARGETPOINT("/set_targetpoint" , "/set_targetpoint_response" , "设置导航目标点" , false, new TypeReference<RobotMsgResponseDTO<Integer>>() {
+    SET_TARGETPOINT("/set_targetpoint" , "/set_targetpoint_response" , "设置导航目标点" , true, new TypeReference<RobotMsgResponseDTO<Integer>>() {
     }),
 
     GET_DATA_LIST("/get_data_list" , "/get_data_list_response" , "获取数据包列表" , true, new TypeReference<RobotMsgResponseDTO<List<String>>>() {
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/AbstractSocketHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/AbstractSocketHandler.java
index 83f4cb2..e47e343 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/AbstractSocketHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/AbstractSocketHandler.java
@@ -49,7 +49,7 @@
                 wsMsgBaseDTO.setMsgKey(enums.getMsgKey());
                 wsMsgBaseDTO.setData(jsonObject.get("msg"));
                 String msg = JSON.toJSONString(wsMsgBaseDTO);
-                log.info("推送{}消息：{}", enums.getRemarks(), msg);
+//                log.info("推送{}消息：{}", enums.getRemarks(), msg);
                 webSocket.sendOneMessage(robotId, msg);
             }
         } catch (Exception e) {
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/cmd/TargetPointCmdHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/cmd/TargetPointCmdHandler.java
index c1c09fa..43b4460 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/cmd/TargetPointCmdHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/cmd/TargetPointCmdHandler.java
@@ -34,7 +34,11 @@
     @Override
     public ResponseDataDTO senderCmd(TargetPointRequest request, InstructCodeEnums enums) {
         MessageRequestDTO<TargetPointRequest> messageDTO = new MessageRequestDTO();
-        //读取默认关键点信息
+        //读取默认关键点信息 {"x":"1.715708819927424","y":"-0.9100042544250901","z":"0.017993879527185876","theta":"-0.039278239414312105"}
+//        request.setX("1.715708819927424");
+//        request.setY("-0.9100042544250901");
+//        request.setZ("0.017993879527185876");
+//        request.setTheta("-0.039278239414312105");
         messageDTO.setMsg(request);
         ResponseDataDTO<Integer> list = optService.robotCmd(getRobotInfo(), enums, messageDTO);
         return list;
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/AppCostMapHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/AppCostMapHandler.java
index 0335693..88c9f79 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/AppCostMapHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/AppCostMapHandler.java
@@ -1,6 +1,11 @@
 package com.casic.missiles.modular.robot.opt.handler.msg;
 
+import com.casic.missiles.core.cache.CacheUtil;
+import com.casic.missiles.modular.constants.RobotDictConstants;
+import com.casic.missiles.modular.robot.IBaseRobotService;
+import com.casic.missiles.modular.robot.opt.enums.InstructCodeEnums;
 import com.casic.missiles.modular.robot.opt.handler.AbstractSocketHandler;
+import com.casic.missiles.modular.robot.opt.instruct.dto.CostMapResolutionRequest;
 import com.casic.missiles.modular.robot.service.IRobotStatusInfoService;
 import lombok.extern.slf4j.Slf4j;
 import org.eclipse.paho.client.mqttv3.MqttMessage;
@@ -16,14 +21,27 @@
 @Component("/app_costmap_sender")
 public class AppCostMapHandler extends AbstractSocketHandler {
     private final IRobotStatusInfoService statusInfoService;
+    private final IBaseRobotService baseRobotService;
     private static Map<String, Integer> index = new ConcurrentHashMap<>();
 
-    public AppCostMapHandler(IRobotStatusInfoService statusInfoService) {
+    public AppCostMapHandler(IRobotStatusInfoService statusInfoService, IBaseRobotService baseRobotService) {
         this.statusInfoService = statusInfoService;
+        this.baseRobotService = baseRobotService;
     }
 
     @Override
     public void process(String robotId, String topic, MqttMessage message) {
         statusInfoService.saveCostMap(robotId, message);
+
+        CostMapResolutionRequest costMapSet = CacheUtil.get(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CONFIG_COSTMAP_RESOLUTION_SET +robotId);
+        if(costMapSet != null){
+            CacheUtil.remove(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CONFIG_COSTMAP_RESOLUTION_SET +robotId);
+            new Thread(() -> {
+                costMapSet.setRobotId(Long.valueOf(robotId));
+                baseRobotService.execCmdHandler(costMapSet, InstructCodeEnums.COSTMAP_RESOLUTION);
+            }).start();
+
+        }
+
     }
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/PoseMessageMsgHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/PoseMessageMsgHandler.java
index 29f8919..6c3c3bd 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/PoseMessageMsgHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/PoseMessageMsgHandler.java
@@ -17,11 +17,9 @@
 @Component("/pose_message")
 public class PoseMessageMsgHandler extends AbstractSocketHandler {
 
-
     private final EventPublisher publisher;
 
     public PoseMessageMsgHandler(EventPublisher publisher) {
-
         this.publisher = publisher;
     }
 

diff --git a/.gitignore b/.gitignore
index 9e20bc1..f42a9cb 100644
--- a/.gitignore
+++ b/.gitignore
@@ -2,3 +2,4 @@
 **/target/**
 logs/
 *.iml
+*.exe
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java b/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
index c7b5d28..a849528 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
@@ -28,6 +28,7 @@
     String KEY_CUR_TASK_ID = "CUR_TASK_ID_";
     String KEY_CUR_TASK_INIT = "CUR_TASK_INIT_";
 
+    String KEY_COST_MAP = "KEY_COST_MAP";
     /**
      * PSD数据ID
      */
@@ -41,6 +42,7 @@
      */
     String KEY_CUR_XY_POINT = "KEY_CUR_XY_POINT_";
     String KEY_CUR_ESTIMATE_GRID = "KEY_CUR_ESTIMATE_GRID_";
+    String KEY_PLANING_FLAG = "KEY_PLANING_FLAG_";
     /**
      * 机器人巡点栅格
      */
@@ -55,6 +57,7 @@
      * 探测器配置参数缓存
      */
     String KEY_CONFIG_PARAMS_ID = "KEY_CONFIG_PARAMS_ID_";
+    String KEY_CONFIG_COSTMAP_RESOLUTION_SET = "KEY_CONFIG_COSTMAP_RESOLUTION_SET_";
 
     /**
      * 暂停状态缓存信息
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
index c4c905d..3bb475f 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
@@ -1,9 +1,14 @@
 package com.casic.missiles.modular.robot.controller;
 
+import cn.hutool.core.lang.Assert;
 import cn.hutool.core.util.StrUtil;
 import com.casic.missiles.core.base.controller.BaseController;
+import com.casic.missiles.core.cache.CacheUtil;
 import com.casic.missiles.core.util.SpringContextHolder;
+import com.casic.missiles.model.exception.RequestValidException;
+import com.casic.missiles.model.response.ResponseData;
 import com.casic.missiles.model.response.dto.ResponseDataDTO;
+import com.casic.missiles.modular.constants.RobotDictConstants;
 import com.casic.missiles.modular.robot.IBaseRobotService;
 import com.casic.missiles.modular.robot.dto.RobotInfoSetDTO;
 import com.casic.missiles.modular.robot.model.RobotInfo;
@@ -19,10 +24,16 @@
 import io.swagger.annotations.ApiOperation;
 import io.swagger.annotations.ApiParam;
 import lombok.SneakyThrows;
+import lombok.extern.slf4j.Slf4j;
+import org.apache.ibatis.annotations.Param;
+import org.springframework.beans.factory.annotation.Value;
 import org.springframework.web.bind.annotation.*;
+import org.springframework.web.multipart.MultipartFile;
 
 import javax.servlet.http.HttpServletResponse;
+import java.io.File;
 import java.util.List;
+import java.util.Optional;
 
 /**
  * 机器人操作类
@@ -30,6 +41,7 @@
  * @author lwh
  * @date 2023-10-30
  */
+@Slf4j
 @Api(tags = "机器人控制接口")
 @RestController
 @RequestMapping("/robot/opt")
@@ -61,6 +73,28 @@
         return baseRobotService.execCmdHandler(request, InstructCodeEnums.CHANGE_PCD);
     }
 
+    @Value("${casic.file.uploadPath}")
+    private String filePath;
+
+    @SneakyThrows
+    @ApiOperation(value = "PCD上传接口")
+    @PostMapping(value = "/uploadPcd")
+    public ResponseData uploadPcd(@RequestParam("file") MultipartFile file, @ApiParam("机器人ID,url拼接传参") String robotId) {
+        Assert.isFalse(StrUtil.isEmpty(robotId), () -> new RequestValidException("机器ID不能为空", Optional.of(new Throwable().getStackTrace()[2])));
+        RobotInfo info = robotInfoService.getById(robotId);
+        if (file != null && !file.isEmpty()) {
+            File pcdFile = new File(filePath + "finalCloud_" + info.getId() + ".pcd");
+            try {
+                file.transferTo(pcdFile);
+            } catch (Exception e) {
+                log.error(e.getMessage());
+            }
+            return ResponseData.success();
+        } else {
+            return ResponseData.error("附件上传失败!");
+        }
+    }
+
     @SneakyThrows
     @ApiOperation(value = "PCD下载接口")
     @PostMapping(value = "/pcdDownload")
@@ -136,6 +170,10 @@
     @ApiOperation(value = "启动或关闭循迹功能", notes = "返回 1成功 0失败  wait 启动时是否等待，0代表循迹启动即运动，不等待；1代表循迹启动原地等待。若无此字段，默认循迹启动不等待直接运动")
     @PostMapping(value = "/startTrack")
     public ResponseDataDTO<Integer> startTrack(@RequestBody StartTrackRequest request) {
+        request.setIsTask(1);
+        if (request.getStepLen() == null) {
+            request.setStepLen(2);
+        }
         baseRobotService.startTrack(request);
         return ResponseDataDTO.success();
     }
@@ -156,6 +194,13 @@
         MessageRequestDTO<CrlNavRequest> messageDTO = new MessageRequestDTO();
         messageDTO.setMsg(request);
         ResponseDataDTO<Integer> list = baseRobotService.getRobotRouteOptService().robotCmd(info, InstructCodeEnums.CTRL_NAV, messageDTO);
+        if (request.getAction() == 1) {
+            CostMapResolutionRequest costMap = new CostMapResolutionRequest();
+            costMap.setResolution(info.getResolution().doubleValue());
+            request.setRobotId(request.getRobotId());
+            //是否下发分辨率
+            CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CONFIG_COSTMAP_RESOLUTION_SET + request.getRobotId(), costMap);
+        }
         return list;
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java
new file mode 100644
index 0000000..5317651
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java
@@ -0,0 +1,86 @@
+package com.casic.missiles.modular.robot.controller;
+
+import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
+import com.baomidou.mybatisplus.extension.plugins.pagination.Page;
+import com.casic.missiles.core.base.controller.BaseController;
+import com.casic.missiles.core.page.PageFactory;
+import com.casic.missiles.core.page.PageInfoBT;
+import com.casic.missiles.model.form.IdForms;
+import com.casic.missiles.model.response.dto.ResponseDataDTO;
+import com.casic.missiles.modular.robot.model.TaskGridInfo;
+import com.casic.missiles.modular.robot.service.ITaskGridInfoService;
+import com.casic.missiles.modular.robot.vo.TaskGridInfoVO;
+import io.swagger.annotations.Api;
+import io.swagger.annotations.ApiOperation;
+import org.springframework.web.bind.annotation.*;
+
+import java.util.List;
+
+/**
+ * 任务网格信息Controller
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+@Api(tags = "任务网格信息")
+@RestController
+@RequestMapping("/robot/taskGridInfo")
+public class TaskGridInfoController extends BaseController {
+
+    private final ITaskGridInfoService taskGridInfoService;
+
+    public TaskGridInfoController(ITaskGridInfoService taskGridInfoService) {
+        this.taskGridInfoService = taskGridInfoService;
+    }
+
+    @ApiOperation(value = "列表查询")
+    @GetMapping(value = "/list")
+    public ResponseDataDTO<List<TaskGridInfo>> list(TaskGridInfoVO taskGridInfoVO) {
+        QueryWrapper<TaskGridInfo> query = taskGridInfoVO.genQuery();
+        return ResponseDataDTO.success(taskGridInfoService.list(query));
+    }
+
+    @ApiOperation(value = "分页查询")
+    @GetMapping(value = "/listPage")
+    public ResponseDataDTO<PageInfoBT<TaskGridInfo>> listPage(TaskGridInfoVO taskGridInfoVO) {
+        Page<TaskGridInfo> page = PageFactory.defaultPage();
+        QueryWrapper<TaskGridInfo> query = taskGridInfoVO.genQuery();
+        page = taskGridInfoService.page(page, query);
+        return ResponseDataDTO.success(super.packForBT(page));
+    }
+
+
+    @ApiOperation(value = "新增接口")
+    @PostMapping(value = "/add")
+    public ResponseDataDTO add(@RequestBody TaskGridInfo taskGridInfo) {
+        taskGridInfoService.save(taskGridInfo);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "修改接口")
+    @PostMapping(value = "/update")
+    public ResponseDataDTO update(@RequestBody TaskGridInfo taskGridInfo) {
+        taskGridInfoService.updateById(taskGridInfo);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "删除接口")
+    @GetMapping(value = "/delete")
+    public ResponseDataDTO delete(String id) {
+        taskGridInfoService.removeById(id);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation("批量删除")
+    @PostMapping("/batchDelete")
+    public ResponseDataDTO batchDelete(@RequestBody IdForms ids) {
+        taskGridInfoService.removeByIds(ids.getIds());
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "详情查询")
+    @GetMapping(value = "/detail")
+    public ResponseDataDTO<TaskGridInfo> detail(String id) {
+        return ResponseDataDTO.success(taskGridInfoService.getById(id));
+    }
+}
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
index e82a792..6992274 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
@@ -98,7 +98,7 @@
     public ResponseDataDTO<AlgorithmResponse> test() {
         AlgorithmResponse response = new AlgorithmResponse();
         response.setTaskHeatMap(new TaskHeatMap());
-        algorithmService.readAlgorithmResponse("step3.txt",response,123L,null);
+        algorithmService.readAlgorithmResponse("step3.txt",null,response,123L,null);
         sendWebSocket(InstructStatusMsgEnums.HEAT_MAP, "1", response.getTaskHeatMap());
         return ResponseDataDTO.success(response);
     }
@@ -121,7 +121,7 @@
     @GetMapping(value = "/testGrid")
     public ResponseDataDTO<List<RouteInfo>> testGrid(@RequestBody PathInitDTO pathInitDTO) {
         RobotInfo robotInfo = infoService.getById(pathInitDTO.getRobotId());
-        GridPointDTO gridPointDTO = algorithmService.convertSlamToGrid(new BigDecimal(pathInitDTO.getX()), new BigDecimal(10), new BigDecimal(4), robotInfo);
+        GridPointDTO gridPointDTO = algorithmService.convertSlamToGrid(new BigDecimal(pathInitDTO.getXNow()), new BigDecimal(10), new BigDecimal(4), robotInfo);
         return ResponseDataDTO.success(algorithmService.convertGridCenterPoint(gridPointDTO, new BigDecimal(10), robotInfo));
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/TaskGridInfoMapper.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/TaskGridInfoMapper.java
new file mode 100644
index 0000000..5c3a70b
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/TaskGridInfoMapper.java
@@ -0,0 +1,23 @@
+package com.casic.missiles.modular.robot.dao;
+
+
+import com.baomidou.mybatisplus.core.mapper.BaseMapper;
+import com.casic.missiles.modular.robot.model.TaskGridInfo;
+import org.apache.ibatis.annotations.Param;
+import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
+import com.baomidou.mybatisplus.extension.plugins.pagination.Page;
+import java.util.List;
+/**
+ * <p>
+ *  任务网格信息 Mapper 接口
+ * </p>
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+public interface TaskGridInfoMapper extends BaseMapper<TaskGridInfo> {
+/**
+* 任务网格信息 分页检索
+*/
+List<TaskGridInfo> selectTaskGridInfoPage( @Param("page") Page<TaskGridInfo> page,@Param("ew" ) QueryWrapper<TaskGridInfo> query);
+}
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/mapping/TaskGridInfoMapper.xml b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/mapping/TaskGridInfoMapper.xml
new file mode 100644
index 0000000..be573e0
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/mapping/TaskGridInfoMapper.xml
@@ -0,0 +1,80 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!DOCTYPE mapper PUBLIC "-//mybatis.org//DTD Mapper 3.0//EN" "http://mybatis.org/dtd/mybatis-3-mapper.dtd">
+<mapper namespace="com.casic.missiles.modular.robot.dao.TaskGridInfoMapper">
+    <!-- 通用查询映射结果 -->
+    <resultMap id="BaseResultMap" type="com.casic.missiles.modular.robot.model.TaskGridInfo">
+        <result column="id" property="id"/>
+        <result column="create_time" property="createTime"/>
+        <result column="update_time" property="updateTime"/>
+        <result column="task_id" property="taskId"/>
+        <result column="estimate_grid_x" property="estimateGridX"/>
+        <result column="estimate_grid_y" property="estimateGridY"/>
+        <result column="neutron_count" property="neutronCount"/>
+        <result column="cur_grid_x" property="curGridX"/>
+        <result column="cur_grid_y" property="curGridY"/>
+    </resultMap>
+    <!-- 通用查询结果列 -->
+    <sql id="Base_Column_List">
+        id
+        ,
+                    create_time as createTime ,
+                    update_time as updateTime ,
+                    task_id as taskId ,
+                    estimate_grid_x as estimateGridX ,
+                    estimate_grid_y as estimateGridY ,
+                    neutron_count as neutronCount ,
+                    cur_grid_x as curGridX ,
+                    cur_grid_y as curGridY
+    </sql>
+    <sql id="Base_Column_Alias">
+        a
+        .
+        id
+        ,
+            a.create_time
+            as createTime ,
+            a.update_time
+            as updateTime ,
+            a.task_id
+            as taskId ,
+            a.estimate_grid_x
+            as estimateGridX ,
+            a.estimate_grid_y
+            as estimateGridY ,
+            a.neutron_count
+            as neutronCount ,
+            a.cur_grid_x
+            as curGridX ,
+            a.cur_grid_y
+            as curGridY
+    </sql>
+    <select id="selectTaskGridInfoPage" resultType="com.casic.missiles.modular.robot.model.TaskGridInfo">
+        select
+        <include refid="Base_Column_Alias"/>
+        from task_grid_info a
+        <where>
+            <if test="ew != null">
+                ${ew.sqlSegment}
+            </if>
+        </where>
+    </select>
+    <sql id="Base_Column_Query">
+        <if test="request!=null">
+            <if test="request.taskId!= null and request.taskId!=''">
+                and task_id = #{request.taskId}
+            </if>
+            <if test="request.estimateGridX!= null and request.estimateGridX!=''">
+                and estimate_grid_x = #{request.estimateGridX}
+            </if>
+            <if test="request.estimateGridY!= null and request.estimateGridY!=''">
+                and estimate_grid_y = #{request.estimateGridY}
+            </if>
+            <if test="request.curGridX!= null and request.curGridX!=''">
+                and cur_grid_x = #{request.curGridX}
+            </if>
+            <if test="request.curGridY!= null and request.curGridY!=''">
+                and cur_grid_y = #{request.curGridY}
+            </if>
+        </if>
+    </sql>
+</mapper>
\ No newline at end of file
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java
index 749f559..4559074 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java
@@ -13,4 +13,9 @@
      * 规划点位信息
      */
     private PointNextDTO pointNextDTO;
+    /**
+     * 寻源目标地址
+     */
+    private PointNextDTO coordinate;
+    private boolean isEnd;
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java
index 39ff2a2..cef3238 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java
@@ -12,6 +12,9 @@
     private BigDecimal y;
 
     public static GridPointDTO convertGridPoint(PointNextDTO nextDTO) {
+        if(nextDTO==null){
+            return null;
+        }
         GridPointDTO gridPointDTO = new GridPointDTO();
         gridPointDTO.setX(Convert.toBigDecimal(nextDTO.getX()));
         gridPointDTO.setY(Convert.toBigDecimal(nextDTO.getY()));
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java
index 8c88242..33be25d 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java
@@ -12,25 +12,19 @@
 public class PathInitDTO {
     //---------------------算法参数
     /**
-     * x坐标最大值（x方向栅格总数）
+     * 初始位置x坐标(栅格地图下的坐标)
      */
-    private String xMax;
+    private String xNow;
     /**
-     * y坐标最大值（y方向栅格总数）
+     * 初始位置y坐标(栅格地图下的坐标)
      */
-    private String yMax;
+    private String yNow;
     /**
      * 初始位置计数率
      */
-    private String count;
-    /**
-     * 初始位置x坐标(0<=x<x_max)
-     */
-    private String x;
-    /**
-     * 初始位置y坐标(0<=y<y_max)
-     */
-    private String y;
+    private String zNow;
+
+    private Integer stepLen;
 
     //---------------------其他执行参数
     private String robotId;
@@ -38,11 +32,11 @@
 
     public List<String> toArray() {
         List<String> list = new ArrayList<>();
-        list.add(xMax);
-        list.add(yMax);
-        list.add(count);
-        list.add(x);
-        list.add(y);
+        list.add(xNow);
+        list.add(yNow);
+        list.add(stepLen+"");
+        list.add(zNow);
+        list.add(taskId+"");
         return list;
     }
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java
index 109e95c..ada6774 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java
@@ -10,21 +10,17 @@
  */
 @Data
 public class PathPlanDTO extends PathInitDTO {
-
     /**
-     * 长度为8的数组，记录(x-1, y-1), (x-1, y), (x-1, y+1), (x, y-1), (x, y+1), (x+1, y-1), (x+1, y), (x+1, y+1)8个坐标的可通行信息。
-     * 若某坐标有障碍物（不可通行行）则记为0，否则记为1
+     * 浮点阈值(阈值，超过该值后结束巡检开始寻源)
      */
-    private String open;
-
+    private String th;
     public List<String> toArray() {
         List<String> list = new ArrayList<>();
-        list.add(getXMax());
-        list.add(getYMax());
-        list.add(open);
-        list.add(getCount());
-        list.add(getX());
-        list.add(getY());
+
+        list.add(getStepLen()+"");
+        list.add(getTh());
+        list.add(getZNow());
+        list.add(getTaskId()+"");
         return list;
     }
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskGridInfo.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskGridInfo.java
new file mode 100644
index 0000000..101c10a
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskGridInfo.java
@@ -0,0 +1,83 @@
+package com.casic.missiles.modular.robot.model;
+
+import com.baomidou.mybatisplus.annotation.TableField;
+import com.baomidou.mybatisplus.annotation.TableName;
+import com.baomidou.mybatisplus.extension.activerecord.Model;
+import io.swagger.annotations.ApiModelProperty;
+import lombok.Data;
+
+import java.math.BigDecimal;
+import java.util.Date;
+
+/**
+ * 任务网格信息对象 task_grid_info
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+@Data
+@TableName("task_grid_info")
+public class TaskGridInfo extends Model<TaskGridInfo> {
+    private static final long serialVersionUID = 1L;
+    @TableField(exist = false)
+    private Integer index;
+    @ApiModelProperty(value = "主键", dataType = "Long")
+    @TableField("id")
+    private Long id;
+
+    @ApiModelProperty(value = "创建时间", dataType = "Date")
+    @TableField("create_time")
+    private Date createTime;
+
+    @ApiModelProperty(value = "修改时间", dataType = "Date")
+    @TableField("update_time")
+    private Date updateTime;
+
+    @ApiModelProperty(value = "任务ID", dataType = "Long")
+    @TableField("task_id")
+    private Long taskId;
+
+    @ApiModelProperty(value = "估算栅格x", dataType = "Long")
+    @TableField("estimate_grid_x")
+    private String estimateGridX;
+
+    @ApiModelProperty(value = "估算栅格y", dataType = "Long")
+    @TableField("estimate_grid_y")
+    private String estimateGridY;
+
+    @ApiModelProperty(value = "中子计数率", dataType = "BigDecimal")
+    @TableField("neutron_count")
+    private BigDecimal neutronCount;
+
+    @ApiModelProperty(value = "当前栅格x", dataType = "Long")
+    @TableField("cur_grid_x")
+    private Integer curGridX;
+
+    @ApiModelProperty(value = "当前栅格y", dataType = "Long")
+    @TableField("cur_grid_y")
+    private Integer curGridY;
+
+    public String getCurPoint(){
+        return "("+curGridX+","+curGridY+")";
+    }
+    public String getEstimatePoint(){
+        if(estimateGridX==null&estimateGridY==null){
+            return "结束";
+        }
+        return "("+estimateGridX+","+estimateGridY+")";
+    }
+    @Override
+    public String toString() {
+        return "TaskGridInfo{"
+                + "id=" + id
+                + "createTime=" + createTime
+                + "updateTime=" + updateTime
+                + "taskId=" + taskId
+                + "estimateGridX=" + estimateGridX
+                + "estimateGridY=" + estimateGridY
+                + "neutronCount=" + neutronCount
+                + "curGridX=" + curGridX
+                + "curGridY=" + curGridY
+                + "}";
+    }
+}
\ No newline at end of file
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java
index edc104e..a8acb2a 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java
@@ -53,6 +53,13 @@
     @TableField("route_id")
     private Long routeId;
 
+    @ApiModelProperty(value = "寻源初始步长", dataType = "Integer")
+    @TableField("step_len")
+    private Integer stepLen;
+
+    @ApiModelProperty(value = "寻源阈值设定", dataType = "BigDecimal")
+    @TableField("max_threshold")
+    private BigDecimal maxThreshold;
     /**
      * 任务类型(1巡检任务)
      */
@@ -62,7 +69,6 @@
     @ApiModelProperty(value = "任务类型字典名称", dataType = "String")
     @TableField(exist = false)
     private String taskTypeName;
-
     @ApiModelProperty(value = "识别类型(discernType 1中子源识别)", dataType = "Integer")
     @TableField("discern_type")
     private Integer discernType;
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java
index c4bc25d..c6620f3 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java
@@ -53,7 +53,7 @@
     }),
     SET_SPEED("/set_speed" , "/set_speed_response" , "最大运动速度设置" , true, new TypeReference<RobotMsgResponseDTO<Integer>>() {
     }),
-    SET_TARGETPOINT("/set_targetpoint" , "/set_targetpoint_response" , "设置导航目标点" , false, new TypeReference<RobotMsgResponseDTO<Integer>>() {
+    SET_TARGETPOINT("/set_targetpoint" , "/set_targetpoint_response" , "设置导航目标点" , true, new TypeReference<RobotMsgResponseDTO<Integer>>() {
     }),
 
     GET_DATA_LIST("/get_data_list" , "/get_data_list_response" , "获取数据包列表" , true, new TypeReference<RobotMsgResponseDTO<List<String>>>() {
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/AbstractSocketHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/AbstractSocketHandler.java
index 83f4cb2..e47e343 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/AbstractSocketHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/AbstractSocketHandler.java
@@ -49,7 +49,7 @@
                 wsMsgBaseDTO.setMsgKey(enums.getMsgKey());
                 wsMsgBaseDTO.setData(jsonObject.get("msg"));
                 String msg = JSON.toJSONString(wsMsgBaseDTO);
-                log.info("推送{}消息：{}", enums.getRemarks(), msg);
+//                log.info("推送{}消息：{}", enums.getRemarks(), msg);
                 webSocket.sendOneMessage(robotId, msg);
             }
         } catch (Exception e) {
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/cmd/TargetPointCmdHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/cmd/TargetPointCmdHandler.java
index c1c09fa..43b4460 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/cmd/TargetPointCmdHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/cmd/TargetPointCmdHandler.java
@@ -34,7 +34,11 @@
     @Override
     public ResponseDataDTO senderCmd(TargetPointRequest request, InstructCodeEnums enums) {
         MessageRequestDTO<TargetPointRequest> messageDTO = new MessageRequestDTO();
-        //读取默认关键点信息
+        //读取默认关键点信息 {"x":"1.715708819927424","y":"-0.9100042544250901","z":"0.017993879527185876","theta":"-0.039278239414312105"}
+//        request.setX("1.715708819927424");
+//        request.setY("-0.9100042544250901");
+//        request.setZ("0.017993879527185876");
+//        request.setTheta("-0.039278239414312105");
         messageDTO.setMsg(request);
         ResponseDataDTO<Integer> list = optService.robotCmd(getRobotInfo(), enums, messageDTO);
         return list;
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/AppCostMapHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/AppCostMapHandler.java
index 0335693..88c9f79 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/AppCostMapHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/AppCostMapHandler.java
@@ -1,6 +1,11 @@
 package com.casic.missiles.modular.robot.opt.handler.msg;
 
+import com.casic.missiles.core.cache.CacheUtil;
+import com.casic.missiles.modular.constants.RobotDictConstants;
+import com.casic.missiles.modular.robot.IBaseRobotService;
+import com.casic.missiles.modular.robot.opt.enums.InstructCodeEnums;
 import com.casic.missiles.modular.robot.opt.handler.AbstractSocketHandler;
+import com.casic.missiles.modular.robot.opt.instruct.dto.CostMapResolutionRequest;
 import com.casic.missiles.modular.robot.service.IRobotStatusInfoService;
 import lombok.extern.slf4j.Slf4j;
 import org.eclipse.paho.client.mqttv3.MqttMessage;
@@ -16,14 +21,27 @@
 @Component("/app_costmap_sender")
 public class AppCostMapHandler extends AbstractSocketHandler {
     private final IRobotStatusInfoService statusInfoService;
+    private final IBaseRobotService baseRobotService;
     private static Map<String, Integer> index = new ConcurrentHashMap<>();
 
-    public AppCostMapHandler(IRobotStatusInfoService statusInfoService) {
+    public AppCostMapHandler(IRobotStatusInfoService statusInfoService, IBaseRobotService baseRobotService) {
         this.statusInfoService = statusInfoService;
+        this.baseRobotService = baseRobotService;
     }
 
     @Override
     public void process(String robotId, String topic, MqttMessage message) {
         statusInfoService.saveCostMap(robotId, message);
+
+        CostMapResolutionRequest costMapSet = CacheUtil.get(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CONFIG_COSTMAP_RESOLUTION_SET +robotId);
+        if(costMapSet != null){
+            CacheUtil.remove(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CONFIG_COSTMAP_RESOLUTION_SET +robotId);
+            new Thread(() -> {
+                costMapSet.setRobotId(Long.valueOf(robotId));
+                baseRobotService.execCmdHandler(costMapSet, InstructCodeEnums.COSTMAP_RESOLUTION);
+            }).start();
+
+        }
+
     }
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/PoseMessageMsgHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/PoseMessageMsgHandler.java
index 29f8919..6c3c3bd 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/PoseMessageMsgHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/PoseMessageMsgHandler.java
@@ -17,11 +17,9 @@
 @Component("/pose_message")
 public class PoseMessageMsgHandler extends AbstractSocketHandler {
 
-
     private final EventPublisher publisher;
 
     public PoseMessageMsgHandler(EventPublisher publisher) {
-
         this.publisher = publisher;
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/StopStateMsgHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/StopStateMsgHandler.java
index 8950815..8a71e0b 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/StopStateMsgHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/StopStateMsgHandler.java
@@ -30,7 +30,7 @@
     public void process(String robotId, String topic, MqttMessage message) {
 
         index.put(index_key, index.get("index") + 1);
-        if (index.get(index_key)>50) {
+        if (index.get(index_key)>30) {
             index.put(index_key, 0);
             log.info("主键：{},停障状态topic:{},消息内容:{}", robotId, topic, message.toString());
 

diff --git a/.gitignore b/.gitignore
index 9e20bc1..f42a9cb 100644
--- a/.gitignore
+++ b/.gitignore
@@ -2,3 +2,4 @@
 **/target/**
 logs/
 *.iml
+*.exe
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java b/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
index c7b5d28..a849528 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
@@ -28,6 +28,7 @@
     String KEY_CUR_TASK_ID = "CUR_TASK_ID_";
     String KEY_CUR_TASK_INIT = "CUR_TASK_INIT_";
 
+    String KEY_COST_MAP = "KEY_COST_MAP";
     /**
      * PSD数据ID
      */
@@ -41,6 +42,7 @@
      */
     String KEY_CUR_XY_POINT = "KEY_CUR_XY_POINT_";
     String KEY_CUR_ESTIMATE_GRID = "KEY_CUR_ESTIMATE_GRID_";
+    String KEY_PLANING_FLAG = "KEY_PLANING_FLAG_";
     /**
      * 机器人巡点栅格
      */
@@ -55,6 +57,7 @@
      * 探测器配置参数缓存
      */
     String KEY_CONFIG_PARAMS_ID = "KEY_CONFIG_PARAMS_ID_";
+    String KEY_CONFIG_COSTMAP_RESOLUTION_SET = "KEY_CONFIG_COSTMAP_RESOLUTION_SET_";
 
     /**
      * 暂停状态缓存信息
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
index c4c905d..3bb475f 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
@@ -1,9 +1,14 @@
 package com.casic.missiles.modular.robot.controller;
 
+import cn.hutool.core.lang.Assert;
 import cn.hutool.core.util.StrUtil;
 import com.casic.missiles.core.base.controller.BaseController;
+import com.casic.missiles.core.cache.CacheUtil;
 import com.casic.missiles.core.util.SpringContextHolder;
+import com.casic.missiles.model.exception.RequestValidException;
+import com.casic.missiles.model.response.ResponseData;
 import com.casic.missiles.model.response.dto.ResponseDataDTO;
+import com.casic.missiles.modular.constants.RobotDictConstants;
 import com.casic.missiles.modular.robot.IBaseRobotService;
 import com.casic.missiles.modular.robot.dto.RobotInfoSetDTO;
 import com.casic.missiles.modular.robot.model.RobotInfo;
@@ -19,10 +24,16 @@
 import io.swagger.annotations.ApiOperation;
 import io.swagger.annotations.ApiParam;
 import lombok.SneakyThrows;
+import lombok.extern.slf4j.Slf4j;
+import org.apache.ibatis.annotations.Param;
+import org.springframework.beans.factory.annotation.Value;
 import org.springframework.web.bind.annotation.*;
+import org.springframework.web.multipart.MultipartFile;
 
 import javax.servlet.http.HttpServletResponse;
+import java.io.File;
 import java.util.List;
+import java.util.Optional;
 
 /**
  * 机器人操作类
@@ -30,6 +41,7 @@
  * @author lwh
  * @date 2023-10-30
  */
+@Slf4j
 @Api(tags = "机器人控制接口")
 @RestController
 @RequestMapping("/robot/opt")
@@ -61,6 +73,28 @@
         return baseRobotService.execCmdHandler(request, InstructCodeEnums.CHANGE_PCD);
     }
 
+    @Value("${casic.file.uploadPath}")
+    private String filePath;
+
+    @SneakyThrows
+    @ApiOperation(value = "PCD上传接口")
+    @PostMapping(value = "/uploadPcd")
+    public ResponseData uploadPcd(@RequestParam("file") MultipartFile file, @ApiParam("机器人ID,url拼接传参") String robotId) {
+        Assert.isFalse(StrUtil.isEmpty(robotId), () -> new RequestValidException("机器ID不能为空", Optional.of(new Throwable().getStackTrace()[2])));
+        RobotInfo info = robotInfoService.getById(robotId);
+        if (file != null && !file.isEmpty()) {
+            File pcdFile = new File(filePath + "finalCloud_" + info.getId() + ".pcd");
+            try {
+                file.transferTo(pcdFile);
+            } catch (Exception e) {
+                log.error(e.getMessage());
+            }
+            return ResponseData.success();
+        } else {
+            return ResponseData.error("附件上传失败!");
+        }
+    }
+
     @SneakyThrows
     @ApiOperation(value = "PCD下载接口")
     @PostMapping(value = "/pcdDownload")
@@ -136,6 +170,10 @@
     @ApiOperation(value = "启动或关闭循迹功能", notes = "返回 1成功 0失败  wait 启动时是否等待，0代表循迹启动即运动，不等待；1代表循迹启动原地等待。若无此字段，默认循迹启动不等待直接运动")
     @PostMapping(value = "/startTrack")
     public ResponseDataDTO<Integer> startTrack(@RequestBody StartTrackRequest request) {
+        request.setIsTask(1);
+        if (request.getStepLen() == null) {
+            request.setStepLen(2);
+        }
         baseRobotService.startTrack(request);
         return ResponseDataDTO.success();
     }
@@ -156,6 +194,13 @@
         MessageRequestDTO<CrlNavRequest> messageDTO = new MessageRequestDTO();
         messageDTO.setMsg(request);
         ResponseDataDTO<Integer> list = baseRobotService.getRobotRouteOptService().robotCmd(info, InstructCodeEnums.CTRL_NAV, messageDTO);
+        if (request.getAction() == 1) {
+            CostMapResolutionRequest costMap = new CostMapResolutionRequest();
+            costMap.setResolution(info.getResolution().doubleValue());
+            request.setRobotId(request.getRobotId());
+            //是否下发分辨率
+            CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CONFIG_COSTMAP_RESOLUTION_SET + request.getRobotId(), costMap);
+        }
         return list;
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java
new file mode 100644
index 0000000..5317651
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java
@@ -0,0 +1,86 @@
+package com.casic.missiles.modular.robot.controller;
+
+import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
+import com.baomidou.mybatisplus.extension.plugins.pagination.Page;
+import com.casic.missiles.core.base.controller.BaseController;
+import com.casic.missiles.core.page.PageFactory;
+import com.casic.missiles.core.page.PageInfoBT;
+import com.casic.missiles.model.form.IdForms;
+import com.casic.missiles.model.response.dto.ResponseDataDTO;
+import com.casic.missiles.modular.robot.model.TaskGridInfo;
+import com.casic.missiles.modular.robot.service.ITaskGridInfoService;
+import com.casic.missiles.modular.robot.vo.TaskGridInfoVO;
+import io.swagger.annotations.Api;
+import io.swagger.annotations.ApiOperation;
+import org.springframework.web.bind.annotation.*;
+
+import java.util.List;
+
+/**
+ * 任务网格信息Controller
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+@Api(tags = "任务网格信息")
+@RestController
+@RequestMapping("/robot/taskGridInfo")
+public class TaskGridInfoController extends BaseController {
+
+    private final ITaskGridInfoService taskGridInfoService;
+
+    public TaskGridInfoController(ITaskGridInfoService taskGridInfoService) {
+        this.taskGridInfoService = taskGridInfoService;
+    }
+
+    @ApiOperation(value = "列表查询")
+    @GetMapping(value = "/list")
+    public ResponseDataDTO<List<TaskGridInfo>> list(TaskGridInfoVO taskGridInfoVO) {
+        QueryWrapper<TaskGridInfo> query = taskGridInfoVO.genQuery();
+        return ResponseDataDTO.success(taskGridInfoService.list(query));
+    }
+
+    @ApiOperation(value = "分页查询")
+    @GetMapping(value = "/listPage")
+    public ResponseDataDTO<PageInfoBT<TaskGridInfo>> listPage(TaskGridInfoVO taskGridInfoVO) {
+        Page<TaskGridInfo> page = PageFactory.defaultPage();
+        QueryWrapper<TaskGridInfo> query = taskGridInfoVO.genQuery();
+        page = taskGridInfoService.page(page, query);
+        return ResponseDataDTO.success(super.packForBT(page));
+    }
+
+
+    @ApiOperation(value = "新增接口")
+    @PostMapping(value = "/add")
+    public ResponseDataDTO add(@RequestBody TaskGridInfo taskGridInfo) {
+        taskGridInfoService.save(taskGridInfo);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "修改接口")
+    @PostMapping(value = "/update")
+    public ResponseDataDTO update(@RequestBody TaskGridInfo taskGridInfo) {
+        taskGridInfoService.updateById(taskGridInfo);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "删除接口")
+    @GetMapping(value = "/delete")
+    public ResponseDataDTO delete(String id) {
+        taskGridInfoService.removeById(id);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation("批量删除")
+    @PostMapping("/batchDelete")
+    public ResponseDataDTO batchDelete(@RequestBody IdForms ids) {
+        taskGridInfoService.removeByIds(ids.getIds());
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "详情查询")
+    @GetMapping(value = "/detail")
+    public ResponseDataDTO<TaskGridInfo> detail(String id) {
+        return ResponseDataDTO.success(taskGridInfoService.getById(id));
+    }
+}
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
index e82a792..6992274 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
@@ -98,7 +98,7 @@
     public ResponseDataDTO<AlgorithmResponse> test() {
         AlgorithmResponse response = new AlgorithmResponse();
         response.setTaskHeatMap(new TaskHeatMap());
-        algorithmService.readAlgorithmResponse("step3.txt",response,123L,null);
+        algorithmService.readAlgorithmResponse("step3.txt",null,response,123L,null);
         sendWebSocket(InstructStatusMsgEnums.HEAT_MAP, "1", response.getTaskHeatMap());
         return ResponseDataDTO.success(response);
     }
@@ -121,7 +121,7 @@
     @GetMapping(value = "/testGrid")
     public ResponseDataDTO<List<RouteInfo>> testGrid(@RequestBody PathInitDTO pathInitDTO) {
         RobotInfo robotInfo = infoService.getById(pathInitDTO.getRobotId());
-        GridPointDTO gridPointDTO = algorithmService.convertSlamToGrid(new BigDecimal(pathInitDTO.getX()), new BigDecimal(10), new BigDecimal(4), robotInfo);
+        GridPointDTO gridPointDTO = algorithmService.convertSlamToGrid(new BigDecimal(pathInitDTO.getXNow()), new BigDecimal(10), new BigDecimal(4), robotInfo);
         return ResponseDataDTO.success(algorithmService.convertGridCenterPoint(gridPointDTO, new BigDecimal(10), robotInfo));
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/TaskGridInfoMapper.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/TaskGridInfoMapper.java
new file mode 100644
index 0000000..5c3a70b
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/TaskGridInfoMapper.java
@@ -0,0 +1,23 @@
+package com.casic.missiles.modular.robot.dao;
+
+
+import com.baomidou.mybatisplus.core.mapper.BaseMapper;
+import com.casic.missiles.modular.robot.model.TaskGridInfo;
+import org.apache.ibatis.annotations.Param;
+import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
+import com.baomidou.mybatisplus.extension.plugins.pagination.Page;
+import java.util.List;
+/**
+ * <p>
+ *  任务网格信息 Mapper 接口
+ * </p>
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+public interface TaskGridInfoMapper extends BaseMapper<TaskGridInfo> {
+/**
+* 任务网格信息 分页检索
+*/
+List<TaskGridInfo> selectTaskGridInfoPage( @Param("page") Page<TaskGridInfo> page,@Param("ew" ) QueryWrapper<TaskGridInfo> query);
+}
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/mapping/TaskGridInfoMapper.xml b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/mapping/TaskGridInfoMapper.xml
new file mode 100644
index 0000000..be573e0
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/mapping/TaskGridInfoMapper.xml
@@ -0,0 +1,80 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!DOCTYPE mapper PUBLIC "-//mybatis.org//DTD Mapper 3.0//EN" "http://mybatis.org/dtd/mybatis-3-mapper.dtd">
+<mapper namespace="com.casic.missiles.modular.robot.dao.TaskGridInfoMapper">
+    <!-- 通用查询映射结果 -->
+    <resultMap id="BaseResultMap" type="com.casic.missiles.modular.robot.model.TaskGridInfo">
+        <result column="id" property="id"/>
+        <result column="create_time" property="createTime"/>
+        <result column="update_time" property="updateTime"/>
+        <result column="task_id" property="taskId"/>
+        <result column="estimate_grid_x" property="estimateGridX"/>
+        <result column="estimate_grid_y" property="estimateGridY"/>
+        <result column="neutron_count" property="neutronCount"/>
+        <result column="cur_grid_x" property="curGridX"/>
+        <result column="cur_grid_y" property="curGridY"/>
+    </resultMap>
+    <!-- 通用查询结果列 -->
+    <sql id="Base_Column_List">
+        id
+        ,
+                    create_time as createTime ,
+                    update_time as updateTime ,
+                    task_id as taskId ,
+                    estimate_grid_x as estimateGridX ,
+                    estimate_grid_y as estimateGridY ,
+                    neutron_count as neutronCount ,
+                    cur_grid_x as curGridX ,
+                    cur_grid_y as curGridY
+    </sql>
+    <sql id="Base_Column_Alias">
+        a
+        .
+        id
+        ,
+            a.create_time
+            as createTime ,
+            a.update_time
+            as updateTime ,
+            a.task_id
+            as taskId ,
+            a.estimate_grid_x
+            as estimateGridX ,
+            a.estimate_grid_y
+            as estimateGridY ,
+            a.neutron_count
+            as neutronCount ,
+            a.cur_grid_x
+            as curGridX ,
+            a.cur_grid_y
+            as curGridY
+    </sql>
+    <select id="selectTaskGridInfoPage" resultType="com.casic.missiles.modular.robot.model.TaskGridInfo">
+        select
+        <include refid="Base_Column_Alias"/>
+        from task_grid_info a
+        <where>
+            <if test="ew != null">
+                ${ew.sqlSegment}
+            </if>
+        </where>
+    </select>
+    <sql id="Base_Column_Query">
+        <if test="request!=null">
+            <if test="request.taskId!= null and request.taskId!=''">
+                and task_id = #{request.taskId}
+            </if>
+            <if test="request.estimateGridX!= null and request.estimateGridX!=''">
+                and estimate_grid_x = #{request.estimateGridX}
+            </if>
+            <if test="request.estimateGridY!= null and request.estimateGridY!=''">
+                and estimate_grid_y = #{request.estimateGridY}
+            </if>
+            <if test="request.curGridX!= null and request.curGridX!=''">
+                and cur_grid_x = #{request.curGridX}
+            </if>
+            <if test="request.curGridY!= null and request.curGridY!=''">
+                and cur_grid_y = #{request.curGridY}
+            </if>
+        </if>
+    </sql>
+</mapper>
\ No newline at end of file
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java
index 749f559..4559074 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java
@@ -13,4 +13,9 @@
      * 规划点位信息
      */
     private PointNextDTO pointNextDTO;
+    /**
+     * 寻源目标地址
+     */
+    private PointNextDTO coordinate;
+    private boolean isEnd;
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java
index 39ff2a2..cef3238 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java
@@ -12,6 +12,9 @@
     private BigDecimal y;
 
     public static GridPointDTO convertGridPoint(PointNextDTO nextDTO) {
+        if(nextDTO==null){
+            return null;
+        }
         GridPointDTO gridPointDTO = new GridPointDTO();
         gridPointDTO.setX(Convert.toBigDecimal(nextDTO.getX()));
         gridPointDTO.setY(Convert.toBigDecimal(nextDTO.getY()));
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java
index 8c88242..33be25d 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java
@@ -12,25 +12,19 @@
 public class PathInitDTO {
     //---------------------算法参数
     /**
-     * x坐标最大值（x方向栅格总数）
+     * 初始位置x坐标(栅格地图下的坐标)
      */
-    private String xMax;
+    private String xNow;
     /**
-     * y坐标最大值（y方向栅格总数）
+     * 初始位置y坐标(栅格地图下的坐标)
      */
-    private String yMax;
+    private String yNow;
     /**
      * 初始位置计数率
      */
-    private String count;
-    /**
-     * 初始位置x坐标(0<=x<x_max)
-     */
-    private String x;
-    /**
-     * 初始位置y坐标(0<=y<y_max)
-     */
-    private String y;
+    private String zNow;
+
+    private Integer stepLen;
 
     //---------------------其他执行参数
     private String robotId;
@@ -38,11 +32,11 @@
 
     public List<String> toArray() {
         List<String> list = new ArrayList<>();
-        list.add(xMax);
-        list.add(yMax);
-        list.add(count);
-        list.add(x);
-        list.add(y);
+        list.add(xNow);
+        list.add(yNow);
+        list.add(stepLen+"");
+        list.add(zNow);
+        list.add(taskId+"");
         return list;
     }
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java
index 109e95c..ada6774 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java
@@ -10,21 +10,17 @@
  */
 @Data
 public class PathPlanDTO extends PathInitDTO {
-
     /**
-     * 长度为8的数组，记录(x-1, y-1), (x-1, y), (x-1, y+1), (x, y-1), (x, y+1), (x+1, y-1), (x+1, y), (x+1, y+1)8个坐标的可通行信息。
-     * 若某坐标有障碍物（不可通行行）则记为0，否则记为1
+     * 浮点阈值(阈值，超过该值后结束巡检开始寻源)
      */
-    private String open;
-
+    private String th;
     public List<String> toArray() {
         List<String> list = new ArrayList<>();
-        list.add(getXMax());
-        list.add(getYMax());
-        list.add(open);
-        list.add(getCount());
-        list.add(getX());
-        list.add(getY());
+
+        list.add(getStepLen()+"");
+        list.add(getTh());
+        list.add(getZNow());
+        list.add(getTaskId()+"");
         return list;
     }
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskGridInfo.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskGridInfo.java
new file mode 100644
index 0000000..101c10a
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskGridInfo.java
@@ -0,0 +1,83 @@
+package com.casic.missiles.modular.robot.model;
+
+import com.baomidou.mybatisplus.annotation.TableField;
+import com.baomidou.mybatisplus.annotation.TableName;
+import com.baomidou.mybatisplus.extension.activerecord.Model;
+import io.swagger.annotations.ApiModelProperty;
+import lombok.Data;
+
+import java.math.BigDecimal;
+import java.util.Date;
+
+/**
+ * 任务网格信息对象 task_grid_info
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+@Data
+@TableName("task_grid_info")
+public class TaskGridInfo extends Model<TaskGridInfo> {
+    private static final long serialVersionUID = 1L;
+    @TableField(exist = false)
+    private Integer index;
+    @ApiModelProperty(value = "主键", dataType = "Long")
+    @TableField("id")
+    private Long id;
+
+    @ApiModelProperty(value = "创建时间", dataType = "Date")
+    @TableField("create_time")
+    private Date createTime;
+
+    @ApiModelProperty(value = "修改时间", dataType = "Date")
+    @TableField("update_time")
+    private Date updateTime;
+
+    @ApiModelProperty(value = "任务ID", dataType = "Long")
+    @TableField("task_id")
+    private Long taskId;
+
+    @ApiModelProperty(value = "估算栅格x", dataType = "Long")
+    @TableField("estimate_grid_x")
+    private String estimateGridX;
+
+    @ApiModelProperty(value = "估算栅格y", dataType = "Long")
+    @TableField("estimate_grid_y")
+    private String estimateGridY;
+
+    @ApiModelProperty(value = "中子计数率", dataType = "BigDecimal")
+    @TableField("neutron_count")
+    private BigDecimal neutronCount;
+
+    @ApiModelProperty(value = "当前栅格x", dataType = "Long")
+    @TableField("cur_grid_x")
+    private Integer curGridX;
+
+    @ApiModelProperty(value = "当前栅格y", dataType = "Long")
+    @TableField("cur_grid_y")
+    private Integer curGridY;
+
+    public String getCurPoint(){
+        return "("+curGridX+","+curGridY+")";
+    }
+    public String getEstimatePoint(){
+        if(estimateGridX==null&estimateGridY==null){
+            return "结束";
+        }
+        return "("+estimateGridX+","+estimateGridY+")";
+    }
+    @Override
+    public String toString() {
+        return "TaskGridInfo{"
+                + "id=" + id
+                + "createTime=" + createTime
+                + "updateTime=" + updateTime
+                + "taskId=" + taskId
+                + "estimateGridX=" + estimateGridX
+                + "estimateGridY=" + estimateGridY
+                + "neutronCount=" + neutronCount
+                + "curGridX=" + curGridX
+                + "curGridY=" + curGridY
+                + "}";
+    }
+}
\ No newline at end of file
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java
index edc104e..a8acb2a 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java
@@ -53,6 +53,13 @@
     @TableField("route_id")
     private Long routeId;
 
+    @ApiModelProperty(value = "寻源初始步长", dataType = "Integer")
+    @TableField("step_len")
+    private Integer stepLen;
+
+    @ApiModelProperty(value = "寻源阈值设定", dataType = "BigDecimal")
+    @TableField("max_threshold")
+    private BigDecimal maxThreshold;
     /**
      * 任务类型(1巡检任务)
      */
@@ -62,7 +69,6 @@
     @ApiModelProperty(value = "任务类型字典名称", dataType = "String")
     @TableField(exist = false)
     private String taskTypeName;
-
     @ApiModelProperty(value = "识别类型(discernType 1中子源识别)", dataType = "Integer")
     @TableField("discern_type")
     private Integer discernType;
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java
index c4bc25d..c6620f3 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java
@@ -53,7 +53,7 @@
     }),
     SET_SPEED("/set_speed" , "/set_speed_response" , "最大运动速度设置" , true, new TypeReference<RobotMsgResponseDTO<Integer>>() {
     }),
-    SET_TARGETPOINT("/set_targetpoint" , "/set_targetpoint_response" , "设置导航目标点" , false, new TypeReference<RobotMsgResponseDTO<Integer>>() {
+    SET_TARGETPOINT("/set_targetpoint" , "/set_targetpoint_response" , "设置导航目标点" , true, new TypeReference<RobotMsgResponseDTO<Integer>>() {
     }),
 
     GET_DATA_LIST("/get_data_list" , "/get_data_list_response" , "获取数据包列表" , true, new TypeReference<RobotMsgResponseDTO<List<String>>>() {
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/AbstractSocketHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/AbstractSocketHandler.java
index 83f4cb2..e47e343 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/AbstractSocketHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/AbstractSocketHandler.java
@@ -49,7 +49,7 @@
                 wsMsgBaseDTO.setMsgKey(enums.getMsgKey());
                 wsMsgBaseDTO.setData(jsonObject.get("msg"));
                 String msg = JSON.toJSONString(wsMsgBaseDTO);
-                log.info("推送{}消息：{}", enums.getRemarks(), msg);
+//                log.info("推送{}消息：{}", enums.getRemarks(), msg);
                 webSocket.sendOneMessage(robotId, msg);
             }
         } catch (Exception e) {
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/cmd/TargetPointCmdHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/cmd/TargetPointCmdHandler.java
index c1c09fa..43b4460 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/cmd/TargetPointCmdHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/cmd/TargetPointCmdHandler.java
@@ -34,7 +34,11 @@
     @Override
     public ResponseDataDTO senderCmd(TargetPointRequest request, InstructCodeEnums enums) {
         MessageRequestDTO<TargetPointRequest> messageDTO = new MessageRequestDTO();
-        //读取默认关键点信息
+        //读取默认关键点信息 {"x":"1.715708819927424","y":"-0.9100042544250901","z":"0.017993879527185876","theta":"-0.039278239414312105"}
+//        request.setX("1.715708819927424");
+//        request.setY("-0.9100042544250901");
+//        request.setZ("0.017993879527185876");
+//        request.setTheta("-0.039278239414312105");
         messageDTO.setMsg(request);
         ResponseDataDTO<Integer> list = optService.robotCmd(getRobotInfo(), enums, messageDTO);
         return list;
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/AppCostMapHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/AppCostMapHandler.java
index 0335693..88c9f79 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/AppCostMapHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/AppCostMapHandler.java
@@ -1,6 +1,11 @@
 package com.casic.missiles.modular.robot.opt.handler.msg;
 
+import com.casic.missiles.core.cache.CacheUtil;
+import com.casic.missiles.modular.constants.RobotDictConstants;
+import com.casic.missiles.modular.robot.IBaseRobotService;
+import com.casic.missiles.modular.robot.opt.enums.InstructCodeEnums;
 import com.casic.missiles.modular.robot.opt.handler.AbstractSocketHandler;
+import com.casic.missiles.modular.robot.opt.instruct.dto.CostMapResolutionRequest;
 import com.casic.missiles.modular.robot.service.IRobotStatusInfoService;
 import lombok.extern.slf4j.Slf4j;
 import org.eclipse.paho.client.mqttv3.MqttMessage;
@@ -16,14 +21,27 @@
 @Component("/app_costmap_sender")
 public class AppCostMapHandler extends AbstractSocketHandler {
     private final IRobotStatusInfoService statusInfoService;
+    private final IBaseRobotService baseRobotService;
     private static Map<String, Integer> index = new ConcurrentHashMap<>();
 
-    public AppCostMapHandler(IRobotStatusInfoService statusInfoService) {
+    public AppCostMapHandler(IRobotStatusInfoService statusInfoService, IBaseRobotService baseRobotService) {
         this.statusInfoService = statusInfoService;
+        this.baseRobotService = baseRobotService;
     }
 
     @Override
     public void process(String robotId, String topic, MqttMessage message) {
         statusInfoService.saveCostMap(robotId, message);
+
+        CostMapResolutionRequest costMapSet = CacheUtil.get(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CONFIG_COSTMAP_RESOLUTION_SET +robotId);
+        if(costMapSet != null){
+            CacheUtil.remove(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CONFIG_COSTMAP_RESOLUTION_SET +robotId);
+            new Thread(() -> {
+                costMapSet.setRobotId(Long.valueOf(robotId));
+                baseRobotService.execCmdHandler(costMapSet, InstructCodeEnums.COSTMAP_RESOLUTION);
+            }).start();
+
+        }
+
     }
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/PoseMessageMsgHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/PoseMessageMsgHandler.java
index 29f8919..6c3c3bd 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/PoseMessageMsgHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/PoseMessageMsgHandler.java
@@ -17,11 +17,9 @@
 @Component("/pose_message")
 public class PoseMessageMsgHandler extends AbstractSocketHandler {
 
-
     private final EventPublisher publisher;
 
     public PoseMessageMsgHandler(EventPublisher publisher) {
-
         this.publisher = publisher;
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/StopStateMsgHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/StopStateMsgHandler.java
index 8950815..8a71e0b 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/StopStateMsgHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/StopStateMsgHandler.java
@@ -30,7 +30,7 @@
     public void process(String robotId, String topic, MqttMessage message) {
 
         index.put(index_key, index.get("index") + 1);
-        if (index.get(index_key)>50) {
+        if (index.get(index_key)>30) {
             index.put(index_key, 0);
             log.info("主键：{},停障状态topic:{},消息内容:{}", robotId, topic, message.toString());
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/impl/RobotOptServiceImpl.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/impl/RobotOptServiceImpl.java
index 8471e7a..7940a24 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/impl/RobotOptServiceImpl.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/impl/RobotOptServiceImpl.java
@@ -146,7 +146,7 @@
     public void startTrack(StartTrackRequest request) {
         //增加循迹任务记录
         taskInfoService.reloadTask(request, robotInfoService.getById(request.getRobotId()));
-        this.execCmdHandler(request, InstructCodeEnums.START_TRACK);
+//        this.execCmdHandler(request, InstructCodeEnums.START_TRACK);
     }
 
     public IRobotRouteOptService getRobotRouteOptService() {

diff --git a/.gitignore b/.gitignore
index 9e20bc1..f42a9cb 100644
--- a/.gitignore
+++ b/.gitignore
@@ -2,3 +2,4 @@
 **/target/**
 logs/
 *.iml
+*.exe
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java b/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
index c7b5d28..a849528 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
@@ -28,6 +28,7 @@
     String KEY_CUR_TASK_ID = "CUR_TASK_ID_";
     String KEY_CUR_TASK_INIT = "CUR_TASK_INIT_";
 
+    String KEY_COST_MAP = "KEY_COST_MAP";
     /**
      * PSD数据ID
      */
@@ -41,6 +42,7 @@
      */
     String KEY_CUR_XY_POINT = "KEY_CUR_XY_POINT_";
     String KEY_CUR_ESTIMATE_GRID = "KEY_CUR_ESTIMATE_GRID_";
+    String KEY_PLANING_FLAG = "KEY_PLANING_FLAG_";
     /**
      * 机器人巡点栅格
      */
@@ -55,6 +57,7 @@
      * 探测器配置参数缓存
      */
     String KEY_CONFIG_PARAMS_ID = "KEY_CONFIG_PARAMS_ID_";
+    String KEY_CONFIG_COSTMAP_RESOLUTION_SET = "KEY_CONFIG_COSTMAP_RESOLUTION_SET_";
 
     /**
      * 暂停状态缓存信息
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
index c4c905d..3bb475f 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
@@ -1,9 +1,14 @@
 package com.casic.missiles.modular.robot.controller;
 
+import cn.hutool.core.lang.Assert;
 import cn.hutool.core.util.StrUtil;
 import com.casic.missiles.core.base.controller.BaseController;
+import com.casic.missiles.core.cache.CacheUtil;
 import com.casic.missiles.core.util.SpringContextHolder;
+import com.casic.missiles.model.exception.RequestValidException;
+import com.casic.missiles.model.response.ResponseData;
 import com.casic.missiles.model.response.dto.ResponseDataDTO;
+import com.casic.missiles.modular.constants.RobotDictConstants;
 import com.casic.missiles.modular.robot.IBaseRobotService;
 import com.casic.missiles.modular.robot.dto.RobotInfoSetDTO;
 import com.casic.missiles.modular.robot.model.RobotInfo;
@@ -19,10 +24,16 @@
 import io.swagger.annotations.ApiOperation;
 import io.swagger.annotations.ApiParam;
 import lombok.SneakyThrows;
+import lombok.extern.slf4j.Slf4j;
+import org.apache.ibatis.annotations.Param;
+import org.springframework.beans.factory.annotation.Value;
 import org.springframework.web.bind.annotation.*;
+import org.springframework.web.multipart.MultipartFile;
 
 import javax.servlet.http.HttpServletResponse;
+import java.io.File;
 import java.util.List;
+import java.util.Optional;
 
 /**
  * 机器人操作类
@@ -30,6 +41,7 @@
  * @author lwh
  * @date 2023-10-30
  */
+@Slf4j
 @Api(tags = "机器人控制接口")
 @RestController
 @RequestMapping("/robot/opt")
@@ -61,6 +73,28 @@
         return baseRobotService.execCmdHandler(request, InstructCodeEnums.CHANGE_PCD);
     }
 
+    @Value("${casic.file.uploadPath}")
+    private String filePath;
+
+    @SneakyThrows
+    @ApiOperation(value = "PCD上传接口")
+    @PostMapping(value = "/uploadPcd")
+    public ResponseData uploadPcd(@RequestParam("file") MultipartFile file, @ApiParam("机器人ID,url拼接传参") String robotId) {
+        Assert.isFalse(StrUtil.isEmpty(robotId), () -> new RequestValidException("机器ID不能为空", Optional.of(new Throwable().getStackTrace()[2])));
+        RobotInfo info = robotInfoService.getById(robotId);
+        if (file != null && !file.isEmpty()) {
+            File pcdFile = new File(filePath + "finalCloud_" + info.getId() + ".pcd");
+            try {
+                file.transferTo(pcdFile);
+            } catch (Exception e) {
+                log.error(e.getMessage());
+            }
+            return ResponseData.success();
+        } else {
+            return ResponseData.error("附件上传失败!");
+        }
+    }
+
     @SneakyThrows
     @ApiOperation(value = "PCD下载接口")
     @PostMapping(value = "/pcdDownload")
@@ -136,6 +170,10 @@
     @ApiOperation(value = "启动或关闭循迹功能", notes = "返回 1成功 0失败  wait 启动时是否等待，0代表循迹启动即运动，不等待；1代表循迹启动原地等待。若无此字段，默认循迹启动不等待直接运动")
     @PostMapping(value = "/startTrack")
     public ResponseDataDTO<Integer> startTrack(@RequestBody StartTrackRequest request) {
+        request.setIsTask(1);
+        if (request.getStepLen() == null) {
+            request.setStepLen(2);
+        }
         baseRobotService.startTrack(request);
         return ResponseDataDTO.success();
     }
@@ -156,6 +194,13 @@
         MessageRequestDTO<CrlNavRequest> messageDTO = new MessageRequestDTO();
         messageDTO.setMsg(request);
         ResponseDataDTO<Integer> list = baseRobotService.getRobotRouteOptService().robotCmd(info, InstructCodeEnums.CTRL_NAV, messageDTO);
+        if (request.getAction() == 1) {
+            CostMapResolutionRequest costMap = new CostMapResolutionRequest();
+            costMap.setResolution(info.getResolution().doubleValue());
+            request.setRobotId(request.getRobotId());
+            //是否下发分辨率
+            CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CONFIG_COSTMAP_RESOLUTION_SET + request.getRobotId(), costMap);
+        }
         return list;
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java
new file mode 100644
index 0000000..5317651
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java
@@ -0,0 +1,86 @@
+package com.casic.missiles.modular.robot.controller;
+
+import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
+import com.baomidou.mybatisplus.extension.plugins.pagination.Page;
+import com.casic.missiles.core.base.controller.BaseController;
+import com.casic.missiles.core.page.PageFactory;
+import com.casic.missiles.core.page.PageInfoBT;
+import com.casic.missiles.model.form.IdForms;
+import com.casic.missiles.model.response.dto.ResponseDataDTO;
+import com.casic.missiles.modular.robot.model.TaskGridInfo;
+import com.casic.missiles.modular.robot.service.ITaskGridInfoService;
+import com.casic.missiles.modular.robot.vo.TaskGridInfoVO;
+import io.swagger.annotations.Api;
+import io.swagger.annotations.ApiOperation;
+import org.springframework.web.bind.annotation.*;
+
+import java.util.List;
+
+/**
+ * 任务网格信息Controller
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+@Api(tags = "任务网格信息")
+@RestController
+@RequestMapping("/robot/taskGridInfo")
+public class TaskGridInfoController extends BaseController {
+
+    private final ITaskGridInfoService taskGridInfoService;
+
+    public TaskGridInfoController(ITaskGridInfoService taskGridInfoService) {
+        this.taskGridInfoService = taskGridInfoService;
+    }
+
+    @ApiOperation(value = "列表查询")
+    @GetMapping(value = "/list")
+    public ResponseDataDTO<List<TaskGridInfo>> list(TaskGridInfoVO taskGridInfoVO) {
+        QueryWrapper<TaskGridInfo> query = taskGridInfoVO.genQuery();
+        return ResponseDataDTO.success(taskGridInfoService.list(query));
+    }
+
+    @ApiOperation(value = "分页查询")
+    @GetMapping(value = "/listPage")
+    public ResponseDataDTO<PageInfoBT<TaskGridInfo>> listPage(TaskGridInfoVO taskGridInfoVO) {
+        Page<TaskGridInfo> page = PageFactory.defaultPage();
+        QueryWrapper<TaskGridInfo> query = taskGridInfoVO.genQuery();
+        page = taskGridInfoService.page(page, query);
+        return ResponseDataDTO.success(super.packForBT(page));
+    }
+
+
+    @ApiOperation(value = "新增接口")
+    @PostMapping(value = "/add")
+    public ResponseDataDTO add(@RequestBody TaskGridInfo taskGridInfo) {
+        taskGridInfoService.save(taskGridInfo);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "修改接口")
+    @PostMapping(value = "/update")
+    public ResponseDataDTO update(@RequestBody TaskGridInfo taskGridInfo) {
+        taskGridInfoService.updateById(taskGridInfo);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "删除接口")
+    @GetMapping(value = "/delete")
+    public ResponseDataDTO delete(String id) {
+        taskGridInfoService.removeById(id);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation("批量删除")
+    @PostMapping("/batchDelete")
+    public ResponseDataDTO batchDelete(@RequestBody IdForms ids) {
+        taskGridInfoService.removeByIds(ids.getIds());
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "详情查询")
+    @GetMapping(value = "/detail")
+    public ResponseDataDTO<TaskGridInfo> detail(String id) {
+        return ResponseDataDTO.success(taskGridInfoService.getById(id));
+    }
+}
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
index e82a792..6992274 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
@@ -98,7 +98,7 @@
     public ResponseDataDTO<AlgorithmResponse> test() {
         AlgorithmResponse response = new AlgorithmResponse();
         response.setTaskHeatMap(new TaskHeatMap());
-        algorithmService.readAlgorithmResponse("step3.txt",response,123L,null);
+        algorithmService.readAlgorithmResponse("step3.txt",null,response,123L,null);
         sendWebSocket(InstructStatusMsgEnums.HEAT_MAP, "1", response.getTaskHeatMap());
         return ResponseDataDTO.success(response);
     }
@@ -121,7 +121,7 @@
     @GetMapping(value = "/testGrid")
     public ResponseDataDTO<List<RouteInfo>> testGrid(@RequestBody PathInitDTO pathInitDTO) {
         RobotInfo robotInfo = infoService.getById(pathInitDTO.getRobotId());
-        GridPointDTO gridPointDTO = algorithmService.convertSlamToGrid(new BigDecimal(pathInitDTO.getX()), new BigDecimal(10), new BigDecimal(4), robotInfo);
+        GridPointDTO gridPointDTO = algorithmService.convertSlamToGrid(new BigDecimal(pathInitDTO.getXNow()), new BigDecimal(10), new BigDecimal(4), robotInfo);
         return ResponseDataDTO.success(algorithmService.convertGridCenterPoint(gridPointDTO, new BigDecimal(10), robotInfo));
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/TaskGridInfoMapper.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/TaskGridInfoMapper.java
new file mode 100644
index 0000000..5c3a70b
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/TaskGridInfoMapper.java
@@ -0,0 +1,23 @@
+package com.casic.missiles.modular.robot.dao;
+
+
+import com.baomidou.mybatisplus.core.mapper.BaseMapper;
+import com.casic.missiles.modular.robot.model.TaskGridInfo;
+import org.apache.ibatis.annotations.Param;
+import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
+import com.baomidou.mybatisplus.extension.plugins.pagination.Page;
+import java.util.List;
+/**
+ * <p>
+ *  任务网格信息 Mapper 接口
+ * </p>
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+public interface TaskGridInfoMapper extends BaseMapper<TaskGridInfo> {
+/**
+* 任务网格信息 分页检索
+*/
+List<TaskGridInfo> selectTaskGridInfoPage( @Param("page") Page<TaskGridInfo> page,@Param("ew" ) QueryWrapper<TaskGridInfo> query);
+}
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/mapping/TaskGridInfoMapper.xml b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/mapping/TaskGridInfoMapper.xml
new file mode 100644
index 0000000..be573e0
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/mapping/TaskGridInfoMapper.xml
@@ -0,0 +1,80 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!DOCTYPE mapper PUBLIC "-//mybatis.org//DTD Mapper 3.0//EN" "http://mybatis.org/dtd/mybatis-3-mapper.dtd">
+<mapper namespace="com.casic.missiles.modular.robot.dao.TaskGridInfoMapper">
+    <!-- 通用查询映射结果 -->
+    <resultMap id="BaseResultMap" type="com.casic.missiles.modular.robot.model.TaskGridInfo">
+        <result column="id" property="id"/>
+        <result column="create_time" property="createTime"/>
+        <result column="update_time" property="updateTime"/>
+        <result column="task_id" property="taskId"/>
+        <result column="estimate_grid_x" property="estimateGridX"/>
+        <result column="estimate_grid_y" property="estimateGridY"/>
+        <result column="neutron_count" property="neutronCount"/>
+        <result column="cur_grid_x" property="curGridX"/>
+        <result column="cur_grid_y" property="curGridY"/>
+    </resultMap>
+    <!-- 通用查询结果列 -->
+    <sql id="Base_Column_List">
+        id
+        ,
+                    create_time as createTime ,
+                    update_time as updateTime ,
+                    task_id as taskId ,
+                    estimate_grid_x as estimateGridX ,
+                    estimate_grid_y as estimateGridY ,
+                    neutron_count as neutronCount ,
+                    cur_grid_x as curGridX ,
+                    cur_grid_y as curGridY
+    </sql>
+    <sql id="Base_Column_Alias">
+        a
+        .
+        id
+        ,
+            a.create_time
+            as createTime ,
+            a.update_time
+            as updateTime ,
+            a.task_id
+            as taskId ,
+            a.estimate_grid_x
+            as estimateGridX ,
+            a.estimate_grid_y
+            as estimateGridY ,
+            a.neutron_count
+            as neutronCount ,
+            a.cur_grid_x
+            as curGridX ,
+            a.cur_grid_y
+            as curGridY
+    </sql>
+    <select id="selectTaskGridInfoPage" resultType="com.casic.missiles.modular.robot.model.TaskGridInfo">
+        select
+        <include refid="Base_Column_Alias"/>
+        from task_grid_info a
+        <where>
+            <if test="ew != null">
+                ${ew.sqlSegment}
+            </if>
+        </where>
+    </select>
+    <sql id="Base_Column_Query">
+        <if test="request!=null">
+            <if test="request.taskId!= null and request.taskId!=''">
+                and task_id = #{request.taskId}
+            </if>
+            <if test="request.estimateGridX!= null and request.estimateGridX!=''">
+                and estimate_grid_x = #{request.estimateGridX}
+            </if>
+            <if test="request.estimateGridY!= null and request.estimateGridY!=''">
+                and estimate_grid_y = #{request.estimateGridY}
+            </if>
+            <if test="request.curGridX!= null and request.curGridX!=''">
+                and cur_grid_x = #{request.curGridX}
+            </if>
+            <if test="request.curGridY!= null and request.curGridY!=''">
+                and cur_grid_y = #{request.curGridY}
+            </if>
+        </if>
+    </sql>
+</mapper>
\ No newline at end of file
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java
index 749f559..4559074 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java
@@ -13,4 +13,9 @@
      * 规划点位信息
      */
     private PointNextDTO pointNextDTO;
+    /**
+     * 寻源目标地址
+     */
+    private PointNextDTO coordinate;
+    private boolean isEnd;
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java
index 39ff2a2..cef3238 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java
@@ -12,6 +12,9 @@
     private BigDecimal y;
 
     public static GridPointDTO convertGridPoint(PointNextDTO nextDTO) {
+        if(nextDTO==null){
+            return null;
+        }
         GridPointDTO gridPointDTO = new GridPointDTO();
         gridPointDTO.setX(Convert.toBigDecimal(nextDTO.getX()));
         gridPointDTO.setY(Convert.toBigDecimal(nextDTO.getY()));
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java
index 8c88242..33be25d 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java
@@ -12,25 +12,19 @@
 public class PathInitDTO {
     //---------------------算法参数
     /**
-     * x坐标最大值（x方向栅格总数）
+     * 初始位置x坐标(栅格地图下的坐标)
      */
-    private String xMax;
+    private String xNow;
     /**
-     * y坐标最大值（y方向栅格总数）
+     * 初始位置y坐标(栅格地图下的坐标)
      */
-    private String yMax;
+    private String yNow;
     /**
      * 初始位置计数率
      */
-    private String count;
-    /**
-     * 初始位置x坐标(0<=x<x_max)
-     */
-    private String x;
-    /**
-     * 初始位置y坐标(0<=y<y_max)
-     */
-    private String y;
+    private String zNow;
+
+    private Integer stepLen;
 
     //---------------------其他执行参数
     private String robotId;
@@ -38,11 +32,11 @@
 
     public List<String> toArray() {
         List<String> list = new ArrayList<>();
-        list.add(xMax);
-        list.add(yMax);
-        list.add(count);
-        list.add(x);
-        list.add(y);
+        list.add(xNow);
+        list.add(yNow);
+        list.add(stepLen+"");
+        list.add(zNow);
+        list.add(taskId+"");
         return list;
     }
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java
index 109e95c..ada6774 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java
@@ -10,21 +10,17 @@
  */
 @Data
 public class PathPlanDTO extends PathInitDTO {
-
     /**
-     * 长度为8的数组，记录(x-1, y-1), (x-1, y), (x-1, y+1), (x, y-1), (x, y+1), (x+1, y-1), (x+1, y), (x+1, y+1)8个坐标的可通行信息。
-     * 若某坐标有障碍物（不可通行行）则记为0，否则记为1
+     * 浮点阈值(阈值，超过该值后结束巡检开始寻源)
      */
-    private String open;
-
+    private String th;
     public List<String> toArray() {
         List<String> list = new ArrayList<>();
-        list.add(getXMax());
-        list.add(getYMax());
-        list.add(open);
-        list.add(getCount());
-        list.add(getX());
-        list.add(getY());
+
+        list.add(getStepLen()+"");
+        list.add(getTh());
+        list.add(getZNow());
+        list.add(getTaskId()+"");
         return list;
     }
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskGridInfo.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskGridInfo.java
new file mode 100644
index 0000000..101c10a
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskGridInfo.java
@@ -0,0 +1,83 @@
+package com.casic.missiles.modular.robot.model;
+
+import com.baomidou.mybatisplus.annotation.TableField;
+import com.baomidou.mybatisplus.annotation.TableName;
+import com.baomidou.mybatisplus.extension.activerecord.Model;
+import io.swagger.annotations.ApiModelProperty;
+import lombok.Data;
+
+import java.math.BigDecimal;
+import java.util.Date;
+
+/**
+ * 任务网格信息对象 task_grid_info
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+@Data
+@TableName("task_grid_info")
+public class TaskGridInfo extends Model<TaskGridInfo> {
+    private static final long serialVersionUID = 1L;
+    @TableField(exist = false)
+    private Integer index;
+    @ApiModelProperty(value = "主键", dataType = "Long")
+    @TableField("id")
+    private Long id;
+
+    @ApiModelProperty(value = "创建时间", dataType = "Date")
+    @TableField("create_time")
+    private Date createTime;
+
+    @ApiModelProperty(value = "修改时间", dataType = "Date")
+    @TableField("update_time")
+    private Date updateTime;
+
+    @ApiModelProperty(value = "任务ID", dataType = "Long")
+    @TableField("task_id")
+    private Long taskId;
+
+    @ApiModelProperty(value = "估算栅格x", dataType = "Long")
+    @TableField("estimate_grid_x")
+    private String estimateGridX;
+
+    @ApiModelProperty(value = "估算栅格y", dataType = "Long")
+    @TableField("estimate_grid_y")
+    private String estimateGridY;
+
+    @ApiModelProperty(value = "中子计数率", dataType = "BigDecimal")
+    @TableField("neutron_count")
+    private BigDecimal neutronCount;
+
+    @ApiModelProperty(value = "当前栅格x", dataType = "Long")
+    @TableField("cur_grid_x")
+    private Integer curGridX;
+
+    @ApiModelProperty(value = "当前栅格y", dataType = "Long")
+    @TableField("cur_grid_y")
+    private Integer curGridY;
+
+    public String getCurPoint(){
+        return "("+curGridX+","+curGridY+")";
+    }
+    public String getEstimatePoint(){
+        if(estimateGridX==null&estimateGridY==null){
+            return "结束";
+        }
+        return "("+estimateGridX+","+estimateGridY+")";
+    }
+    @Override
+    public String toString() {
+        return "TaskGridInfo{"
+                + "id=" + id
+                + "createTime=" + createTime
+                + "updateTime=" + updateTime
+                + "taskId=" + taskId
+                + "estimateGridX=" + estimateGridX
+                + "estimateGridY=" + estimateGridY
+                + "neutronCount=" + neutronCount
+                + "curGridX=" + curGridX
+                + "curGridY=" + curGridY
+                + "}";
+    }
+}
\ No newline at end of file
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java
index edc104e..a8acb2a 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java
@@ -53,6 +53,13 @@
     @TableField("route_id")
     private Long routeId;
 
+    @ApiModelProperty(value = "寻源初始步长", dataType = "Integer")
+    @TableField("step_len")
+    private Integer stepLen;
+
+    @ApiModelProperty(value = "寻源阈值设定", dataType = "BigDecimal")
+    @TableField("max_threshold")
+    private BigDecimal maxThreshold;
     /**
      * 任务类型(1巡检任务)
      */
@@ -62,7 +69,6 @@
     @ApiModelProperty(value = "任务类型字典名称", dataType = "String")
     @TableField(exist = false)
     private String taskTypeName;
-
     @ApiModelProperty(value = "识别类型(discernType 1中子源识别)", dataType = "Integer")
     @TableField("discern_type")
     private Integer discernType;
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java
index c4bc25d..c6620f3 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java
@@ -53,7 +53,7 @@
     }),
     SET_SPEED("/set_speed" , "/set_speed_response" , "最大运动速度设置" , true, new TypeReference<RobotMsgResponseDTO<Integer>>() {
     }),
-    SET_TARGETPOINT("/set_targetpoint" , "/set_targetpoint_response" , "设置导航目标点" , false, new TypeReference<RobotMsgResponseDTO<Integer>>() {
+    SET_TARGETPOINT("/set_targetpoint" , "/set_targetpoint_response" , "设置导航目标点" , true, new TypeReference<RobotMsgResponseDTO<Integer>>() {
     }),
 
     GET_DATA_LIST("/get_data_list" , "/get_data_list_response" , "获取数据包列表" , true, new TypeReference<RobotMsgResponseDTO<List<String>>>() {
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/AbstractSocketHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/AbstractSocketHandler.java
index 83f4cb2..e47e343 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/AbstractSocketHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/AbstractSocketHandler.java
@@ -49,7 +49,7 @@
                 wsMsgBaseDTO.setMsgKey(enums.getMsgKey());
                 wsMsgBaseDTO.setData(jsonObject.get("msg"));
                 String msg = JSON.toJSONString(wsMsgBaseDTO);
-                log.info("推送{}消息：{}", enums.getRemarks(), msg);
+//                log.info("推送{}消息：{}", enums.getRemarks(), msg);
                 webSocket.sendOneMessage(robotId, msg);
             }
         } catch (Exception e) {
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/cmd/TargetPointCmdHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/cmd/TargetPointCmdHandler.java
index c1c09fa..43b4460 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/cmd/TargetPointCmdHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/cmd/TargetPointCmdHandler.java
@@ -34,7 +34,11 @@
     @Override
     public ResponseDataDTO senderCmd(TargetPointRequest request, InstructCodeEnums enums) {
         MessageRequestDTO<TargetPointRequest> messageDTO = new MessageRequestDTO();
-        //读取默认关键点信息
+        //读取默认关键点信息 {"x":"1.715708819927424","y":"-0.9100042544250901","z":"0.017993879527185876","theta":"-0.039278239414312105"}
+//        request.setX("1.715708819927424");
+//        request.setY("-0.9100042544250901");
+//        request.setZ("0.017993879527185876");
+//        request.setTheta("-0.039278239414312105");
         messageDTO.setMsg(request);
         ResponseDataDTO<Integer> list = optService.robotCmd(getRobotInfo(), enums, messageDTO);
         return list;
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/AppCostMapHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/AppCostMapHandler.java
index 0335693..88c9f79 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/AppCostMapHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/AppCostMapHandler.java
@@ -1,6 +1,11 @@
 package com.casic.missiles.modular.robot.opt.handler.msg;
 
+import com.casic.missiles.core.cache.CacheUtil;
+import com.casic.missiles.modular.constants.RobotDictConstants;
+import com.casic.missiles.modular.robot.IBaseRobotService;
+import com.casic.missiles.modular.robot.opt.enums.InstructCodeEnums;
 import com.casic.missiles.modular.robot.opt.handler.AbstractSocketHandler;
+import com.casic.missiles.modular.robot.opt.instruct.dto.CostMapResolutionRequest;
 import com.casic.missiles.modular.robot.service.IRobotStatusInfoService;
 import lombok.extern.slf4j.Slf4j;
 import org.eclipse.paho.client.mqttv3.MqttMessage;
@@ -16,14 +21,27 @@
 @Component("/app_costmap_sender")
 public class AppCostMapHandler extends AbstractSocketHandler {
     private final IRobotStatusInfoService statusInfoService;
+    private final IBaseRobotService baseRobotService;
     private static Map<String, Integer> index = new ConcurrentHashMap<>();
 
-    public AppCostMapHandler(IRobotStatusInfoService statusInfoService) {
+    public AppCostMapHandler(IRobotStatusInfoService statusInfoService, IBaseRobotService baseRobotService) {
         this.statusInfoService = statusInfoService;
+        this.baseRobotService = baseRobotService;
     }
 
     @Override
     public void process(String robotId, String topic, MqttMessage message) {
         statusInfoService.saveCostMap(robotId, message);
+
+        CostMapResolutionRequest costMapSet = CacheUtil.get(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CONFIG_COSTMAP_RESOLUTION_SET +robotId);
+        if(costMapSet != null){
+            CacheUtil.remove(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CONFIG_COSTMAP_RESOLUTION_SET +robotId);
+            new Thread(() -> {
+                costMapSet.setRobotId(Long.valueOf(robotId));
+                baseRobotService.execCmdHandler(costMapSet, InstructCodeEnums.COSTMAP_RESOLUTION);
+            }).start();
+
+        }
+
     }
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/PoseMessageMsgHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/PoseMessageMsgHandler.java
index 29f8919..6c3c3bd 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/PoseMessageMsgHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/PoseMessageMsgHandler.java
@@ -17,11 +17,9 @@
 @Component("/pose_message")
 public class PoseMessageMsgHandler extends AbstractSocketHandler {
 
-
     private final EventPublisher publisher;
 
     public PoseMessageMsgHandler(EventPublisher publisher) {
-
         this.publisher = publisher;
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/StopStateMsgHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/StopStateMsgHandler.java
index 8950815..8a71e0b 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/StopStateMsgHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/StopStateMsgHandler.java
@@ -30,7 +30,7 @@
     public void process(String robotId, String topic, MqttMessage message) {
 
         index.put(index_key, index.get("index") + 1);
-        if (index.get(index_key)>50) {
+        if (index.get(index_key)>30) {
             index.put(index_key, 0);
             log.info("主键：{},停障状态topic:{},消息内容:{}", robotId, topic, message.toString());
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/impl/RobotOptServiceImpl.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/impl/RobotOptServiceImpl.java
index 8471e7a..7940a24 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/impl/RobotOptServiceImpl.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/impl/RobotOptServiceImpl.java
@@ -146,7 +146,7 @@
     public void startTrack(StartTrackRequest request) {
         //增加循迹任务记录
         taskInfoService.reloadTask(request, robotInfoService.getById(request.getRobotId()));
-        this.execCmdHandler(request, InstructCodeEnums.START_TRACK);
+//        this.execCmdHandler(request, InstructCodeEnums.START_TRACK);
     }
 
     public IRobotRouteOptService getRobotRouteOptService() {
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/BaseRobotCmdDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/BaseRobotCmdDTO.java
index 5608f2b..06b53de 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/BaseRobotCmdDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/BaseRobotCmdDTO.java
@@ -3,11 +3,13 @@
 import io.swagger.annotations.ApiModelProperty;
 import lombok.Data;
 
+import java.io.Serializable;
+
 /**
  * 机器人指令基类
  */
 @Data
-public class BaseRobotCmdDTO {
+public class BaseRobotCmdDTO implements Serializable {
     @ApiModelProperty(value = "机器人主键", dataType = "String")
     protected Long robotId;
 }

diff --git a/.gitignore b/.gitignore
index 9e20bc1..f42a9cb 100644
--- a/.gitignore
+++ b/.gitignore
@@ -2,3 +2,4 @@
 **/target/**
 logs/
 *.iml
+*.exe
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java b/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
index c7b5d28..a849528 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
@@ -28,6 +28,7 @@
     String KEY_CUR_TASK_ID = "CUR_TASK_ID_";
     String KEY_CUR_TASK_INIT = "CUR_TASK_INIT_";
 
+    String KEY_COST_MAP = "KEY_COST_MAP";
     /**
      * PSD数据ID
      */
@@ -41,6 +42,7 @@
      */
     String KEY_CUR_XY_POINT = "KEY_CUR_XY_POINT_";
     String KEY_CUR_ESTIMATE_GRID = "KEY_CUR_ESTIMATE_GRID_";
+    String KEY_PLANING_FLAG = "KEY_PLANING_FLAG_";
     /**
      * 机器人巡点栅格
      */
@@ -55,6 +57,7 @@
      * 探测器配置参数缓存
      */
     String KEY_CONFIG_PARAMS_ID = "KEY_CONFIG_PARAMS_ID_";
+    String KEY_CONFIG_COSTMAP_RESOLUTION_SET = "KEY_CONFIG_COSTMAP_RESOLUTION_SET_";
 
     /**
      * 暂停状态缓存信息
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
index c4c905d..3bb475f 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
@@ -1,9 +1,14 @@
 package com.casic.missiles.modular.robot.controller;
 
+import cn.hutool.core.lang.Assert;
 import cn.hutool.core.util.StrUtil;
 import com.casic.missiles.core.base.controller.BaseController;
+import com.casic.missiles.core.cache.CacheUtil;
 import com.casic.missiles.core.util.SpringContextHolder;
+import com.casic.missiles.model.exception.RequestValidException;
+import com.casic.missiles.model.response.ResponseData;
 import com.casic.missiles.model.response.dto.ResponseDataDTO;
+import com.casic.missiles.modular.constants.RobotDictConstants;
 import com.casic.missiles.modular.robot.IBaseRobotService;
 import com.casic.missiles.modular.robot.dto.RobotInfoSetDTO;
 import com.casic.missiles.modular.robot.model.RobotInfo;
@@ -19,10 +24,16 @@
 import io.swagger.annotations.ApiOperation;
 import io.swagger.annotations.ApiParam;
 import lombok.SneakyThrows;
+import lombok.extern.slf4j.Slf4j;
+import org.apache.ibatis.annotations.Param;
+import org.springframework.beans.factory.annotation.Value;
 import org.springframework.web.bind.annotation.*;
+import org.springframework.web.multipart.MultipartFile;
 
 import javax.servlet.http.HttpServletResponse;
+import java.io.File;
 import java.util.List;
+import java.util.Optional;
 
 /**
  * 机器人操作类
@@ -30,6 +41,7 @@
  * @author lwh
  * @date 2023-10-30
  */
+@Slf4j
 @Api(tags = "机器人控制接口")
 @RestController
 @RequestMapping("/robot/opt")
@@ -61,6 +73,28 @@
         return baseRobotService.execCmdHandler(request, InstructCodeEnums.CHANGE_PCD);
     }
 
+    @Value("${casic.file.uploadPath}")
+    private String filePath;
+
+    @SneakyThrows
+    @ApiOperation(value = "PCD上传接口")
+    @PostMapping(value = "/uploadPcd")
+    public ResponseData uploadPcd(@RequestParam("file") MultipartFile file, @ApiParam("机器人ID,url拼接传参") String robotId) {
+        Assert.isFalse(StrUtil.isEmpty(robotId), () -> new RequestValidException("机器ID不能为空", Optional.of(new Throwable().getStackTrace()[2])));
+        RobotInfo info = robotInfoService.getById(robotId);
+        if (file != null && !file.isEmpty()) {
+            File pcdFile = new File(filePath + "finalCloud_" + info.getId() + ".pcd");
+            try {
+                file.transferTo(pcdFile);
+            } catch (Exception e) {
+                log.error(e.getMessage());
+            }
+            return ResponseData.success();
+        } else {
+            return ResponseData.error("附件上传失败!");
+        }
+    }
+
     @SneakyThrows
     @ApiOperation(value = "PCD下载接口")
     @PostMapping(value = "/pcdDownload")
@@ -136,6 +170,10 @@
     @ApiOperation(value = "启动或关闭循迹功能", notes = "返回 1成功 0失败  wait 启动时是否等待，0代表循迹启动即运动，不等待；1代表循迹启动原地等待。若无此字段，默认循迹启动不等待直接运动")
     @PostMapping(value = "/startTrack")
     public ResponseDataDTO<Integer> startTrack(@RequestBody StartTrackRequest request) {
+        request.setIsTask(1);
+        if (request.getStepLen() == null) {
+            request.setStepLen(2);
+        }
         baseRobotService.startTrack(request);
         return ResponseDataDTO.success();
     }
@@ -156,6 +194,13 @@
         MessageRequestDTO<CrlNavRequest> messageDTO = new MessageRequestDTO();
         messageDTO.setMsg(request);
         ResponseDataDTO<Integer> list = baseRobotService.getRobotRouteOptService().robotCmd(info, InstructCodeEnums.CTRL_NAV, messageDTO);
+        if (request.getAction() == 1) {
+            CostMapResolutionRequest costMap = new CostMapResolutionRequest();
+            costMap.setResolution(info.getResolution().doubleValue());
+            request.setRobotId(request.getRobotId());
+            //是否下发分辨率
+            CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CONFIG_COSTMAP_RESOLUTION_SET + request.getRobotId(), costMap);
+        }
         return list;
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java
new file mode 100644
index 0000000..5317651
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java
@@ -0,0 +1,86 @@
+package com.casic.missiles.modular.robot.controller;
+
+import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
+import com.baomidou.mybatisplus.extension.plugins.pagination.Page;
+import com.casic.missiles.core.base.controller.BaseController;
+import com.casic.missiles.core.page.PageFactory;
+import com.casic.missiles.core.page.PageInfoBT;
+import com.casic.missiles.model.form.IdForms;
+import com.casic.missiles.model.response.dto.ResponseDataDTO;
+import com.casic.missiles.modular.robot.model.TaskGridInfo;
+import com.casic.missiles.modular.robot.service.ITaskGridInfoService;
+import com.casic.missiles.modular.robot.vo.TaskGridInfoVO;
+import io.swagger.annotations.Api;
+import io.swagger.annotations.ApiOperation;
+import org.springframework.web.bind.annotation.*;
+
+import java.util.List;
+
+/**
+ * 任务网格信息Controller
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+@Api(tags = "任务网格信息")
+@RestController
+@RequestMapping("/robot/taskGridInfo")
+public class TaskGridInfoController extends BaseController {
+
+    private final ITaskGridInfoService taskGridInfoService;
+
+    public TaskGridInfoController(ITaskGridInfoService taskGridInfoService) {
+        this.taskGridInfoService = taskGridInfoService;
+    }
+
+    @ApiOperation(value = "列表查询")
+    @GetMapping(value = "/list")
+    public ResponseDataDTO<List<TaskGridInfo>> list(TaskGridInfoVO taskGridInfoVO) {
+        QueryWrapper<TaskGridInfo> query = taskGridInfoVO.genQuery();
+        return ResponseDataDTO.success(taskGridInfoService.list(query));
+    }
+
+    @ApiOperation(value = "分页查询")
+    @GetMapping(value = "/listPage")
+    public ResponseDataDTO<PageInfoBT<TaskGridInfo>> listPage(TaskGridInfoVO taskGridInfoVO) {
+        Page<TaskGridInfo> page = PageFactory.defaultPage();
+        QueryWrapper<TaskGridInfo> query = taskGridInfoVO.genQuery();
+        page = taskGridInfoService.page(page, query);
+        return ResponseDataDTO.success(super.packForBT(page));
+    }
+
+
+    @ApiOperation(value = "新增接口")
+    @PostMapping(value = "/add")
+    public ResponseDataDTO add(@RequestBody TaskGridInfo taskGridInfo) {
+        taskGridInfoService.save(taskGridInfo);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "修改接口")
+    @PostMapping(value = "/update")
+    public ResponseDataDTO update(@RequestBody TaskGridInfo taskGridInfo) {
+        taskGridInfoService.updateById(taskGridInfo);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "删除接口")
+    @GetMapping(value = "/delete")
+    public ResponseDataDTO delete(String id) {
+        taskGridInfoService.removeById(id);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation("批量删除")
+    @PostMapping("/batchDelete")
+    public ResponseDataDTO batchDelete(@RequestBody IdForms ids) {
+        taskGridInfoService.removeByIds(ids.getIds());
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "详情查询")
+    @GetMapping(value = "/detail")
+    public ResponseDataDTO<TaskGridInfo> detail(String id) {
+        return ResponseDataDTO.success(taskGridInfoService.getById(id));
+    }
+}
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
index e82a792..6992274 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
@@ -98,7 +98,7 @@
     public ResponseDataDTO<AlgorithmResponse> test() {
         AlgorithmResponse response = new AlgorithmResponse();
         response.setTaskHeatMap(new TaskHeatMap());
-        algorithmService.readAlgorithmResponse("step3.txt",response,123L,null);
+        algorithmService.readAlgorithmResponse("step3.txt",null,response,123L,null);
         sendWebSocket(InstructStatusMsgEnums.HEAT_MAP, "1", response.getTaskHeatMap());
         return ResponseDataDTO.success(response);
     }
@@ -121,7 +121,7 @@
     @GetMapping(value = "/testGrid")
     public ResponseDataDTO<List<RouteInfo>> testGrid(@RequestBody PathInitDTO pathInitDTO) {
         RobotInfo robotInfo = infoService.getById(pathInitDTO.getRobotId());
-        GridPointDTO gridPointDTO = algorithmService.convertSlamToGrid(new BigDecimal(pathInitDTO.getX()), new BigDecimal(10), new BigDecimal(4), robotInfo);
+        GridPointDTO gridPointDTO = algorithmService.convertSlamToGrid(new BigDecimal(pathInitDTO.getXNow()), new BigDecimal(10), new BigDecimal(4), robotInfo);
         return ResponseDataDTO.success(algorithmService.convertGridCenterPoint(gridPointDTO, new BigDecimal(10), robotInfo));
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/TaskGridInfoMapper.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/TaskGridInfoMapper.java
new file mode 100644
index 0000000..5c3a70b
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/TaskGridInfoMapper.java
@@ -0,0 +1,23 @@
+package com.casic.missiles.modular.robot.dao;
+
+
+import com.baomidou.mybatisplus.core.mapper.BaseMapper;
+import com.casic.missiles.modular.robot.model.TaskGridInfo;
+import org.apache.ibatis.annotations.Param;
+import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
+import com.baomidou.mybatisplus.extension.plugins.pagination.Page;
+import java.util.List;
+/**
+ * <p>
+ *  任务网格信息 Mapper 接口
+ * </p>
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+public interface TaskGridInfoMapper extends BaseMapper<TaskGridInfo> {
+/**
+* 任务网格信息 分页检索
+*/
+List<TaskGridInfo> selectTaskGridInfoPage( @Param("page") Page<TaskGridInfo> page,@Param("ew" ) QueryWrapper<TaskGridInfo> query);
+}
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/mapping/TaskGridInfoMapper.xml b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/mapping/TaskGridInfoMapper.xml
new file mode 100644
index 0000000..be573e0
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/mapping/TaskGridInfoMapper.xml
@@ -0,0 +1,80 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!DOCTYPE mapper PUBLIC "-//mybatis.org//DTD Mapper 3.0//EN" "http://mybatis.org/dtd/mybatis-3-mapper.dtd">
+<mapper namespace="com.casic.missiles.modular.robot.dao.TaskGridInfoMapper">
+    <!-- 通用查询映射结果 -->
+    <resultMap id="BaseResultMap" type="com.casic.missiles.modular.robot.model.TaskGridInfo">
+        <result column="id" property="id"/>
+        <result column="create_time" property="createTime"/>
+        <result column="update_time" property="updateTime"/>
+        <result column="task_id" property="taskId"/>
+        <result column="estimate_grid_x" property="estimateGridX"/>
+        <result column="estimate_grid_y" property="estimateGridY"/>
+        <result column="neutron_count" property="neutronCount"/>
+        <result column="cur_grid_x" property="curGridX"/>
+        <result column="cur_grid_y" property="curGridY"/>
+    </resultMap>
+    <!-- 通用查询结果列 -->
+    <sql id="Base_Column_List">
+        id
+        ,
+                    create_time as createTime ,
+                    update_time as updateTime ,
+                    task_id as taskId ,
+                    estimate_grid_x as estimateGridX ,
+                    estimate_grid_y as estimateGridY ,
+                    neutron_count as neutronCount ,
+                    cur_grid_x as curGridX ,
+                    cur_grid_y as curGridY
+    </sql>
+    <sql id="Base_Column_Alias">
+        a
+        .
+        id
+        ,
+            a.create_time
+            as createTime ,
+            a.update_time
+            as updateTime ,
+            a.task_id
+            as taskId ,
+            a.estimate_grid_x
+            as estimateGridX ,
+            a.estimate_grid_y
+            as estimateGridY ,
+            a.neutron_count
+            as neutronCount ,
+            a.cur_grid_x
+            as curGridX ,
+            a.cur_grid_y
+            as curGridY
+    </sql>
+    <select id="selectTaskGridInfoPage" resultType="com.casic.missiles.modular.robot.model.TaskGridInfo">
+        select
+        <include refid="Base_Column_Alias"/>
+        from task_grid_info a
+        <where>
+            <if test="ew != null">
+                ${ew.sqlSegment}
+            </if>
+        </where>
+    </select>
+    <sql id="Base_Column_Query">
+        <if test="request!=null">
+            <if test="request.taskId!= null and request.taskId!=''">
+                and task_id = #{request.taskId}
+            </if>
+            <if test="request.estimateGridX!= null and request.estimateGridX!=''">
+                and estimate_grid_x = #{request.estimateGridX}
+            </if>
+            <if test="request.estimateGridY!= null and request.estimateGridY!=''">
+                and estimate_grid_y = #{request.estimateGridY}
+            </if>
+            <if test="request.curGridX!= null and request.curGridX!=''">
+                and cur_grid_x = #{request.curGridX}
+            </if>
+            <if test="request.curGridY!= null and request.curGridY!=''">
+                and cur_grid_y = #{request.curGridY}
+            </if>
+        </if>
+    </sql>
+</mapper>
\ No newline at end of file
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java
index 749f559..4559074 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java
@@ -13,4 +13,9 @@
      * 规划点位信息
      */
     private PointNextDTO pointNextDTO;
+    /**
+     * 寻源目标地址
+     */
+    private PointNextDTO coordinate;
+    private boolean isEnd;
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java
index 39ff2a2..cef3238 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java
@@ -12,6 +12,9 @@
     private BigDecimal y;
 
     public static GridPointDTO convertGridPoint(PointNextDTO nextDTO) {
+        if(nextDTO==null){
+            return null;
+        }
         GridPointDTO gridPointDTO = new GridPointDTO();
         gridPointDTO.setX(Convert.toBigDecimal(nextDTO.getX()));
         gridPointDTO.setY(Convert.toBigDecimal(nextDTO.getY()));
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java
index 8c88242..33be25d 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java
@@ -12,25 +12,19 @@
 public class PathInitDTO {
     //---------------------算法参数
     /**
-     * x坐标最大值（x方向栅格总数）
+     * 初始位置x坐标(栅格地图下的坐标)
      */
-    private String xMax;
+    private String xNow;
     /**
-     * y坐标最大值（y方向栅格总数）
+     * 初始位置y坐标(栅格地图下的坐标)
      */
-    private String yMax;
+    private String yNow;
     /**
      * 初始位置计数率
      */
-    private String count;
-    /**
-     * 初始位置x坐标(0<=x<x_max)
-     */
-    private String x;
-    /**
-     * 初始位置y坐标(0<=y<y_max)
-     */
-    private String y;
+    private String zNow;
+
+    private Integer stepLen;
 
     //---------------------其他执行参数
     private String robotId;
@@ -38,11 +32,11 @@
 
     public List<String> toArray() {
         List<String> list = new ArrayList<>();
-        list.add(xMax);
-        list.add(yMax);
-        list.add(count);
-        list.add(x);
-        list.add(y);
+        list.add(xNow);
+        list.add(yNow);
+        list.add(stepLen+"");
+        list.add(zNow);
+        list.add(taskId+"");
         return list;
     }
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java
index 109e95c..ada6774 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java
@@ -10,21 +10,17 @@
  */
 @Data
 public class PathPlanDTO extends PathInitDTO {
-
     /**
-     * 长度为8的数组，记录(x-1, y-1), (x-1, y), (x-1, y+1), (x, y-1), (x, y+1), (x+1, y-1), (x+1, y), (x+1, y+1)8个坐标的可通行信息。
-     * 若某坐标有障碍物（不可通行行）则记为0，否则记为1
+     * 浮点阈值(阈值，超过该值后结束巡检开始寻源)
      */
-    private String open;
-
+    private String th;
     public List<String> toArray() {
         List<String> list = new ArrayList<>();
-        list.add(getXMax());
-        list.add(getYMax());
-        list.add(open);
-        list.add(getCount());
-        list.add(getX());
-        list.add(getY());
+
+        list.add(getStepLen()+"");
+        list.add(getTh());
+        list.add(getZNow());
+        list.add(getTaskId()+"");
         return list;
     }
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskGridInfo.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskGridInfo.java
new file mode 100644
index 0000000..101c10a
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskGridInfo.java
@@ -0,0 +1,83 @@
+package com.casic.missiles.modular.robot.model;
+
+import com.baomidou.mybatisplus.annotation.TableField;
+import com.baomidou.mybatisplus.annotation.TableName;
+import com.baomidou.mybatisplus.extension.activerecord.Model;
+import io.swagger.annotations.ApiModelProperty;
+import lombok.Data;
+
+import java.math.BigDecimal;
+import java.util.Date;
+
+/**
+ * 任务网格信息对象 task_grid_info
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+@Data
+@TableName("task_grid_info")
+public class TaskGridInfo extends Model<TaskGridInfo> {
+    private static final long serialVersionUID = 1L;
+    @TableField(exist = false)
+    private Integer index;
+    @ApiModelProperty(value = "主键", dataType = "Long")
+    @TableField("id")
+    private Long id;
+
+    @ApiModelProperty(value = "创建时间", dataType = "Date")
+    @TableField("create_time")
+    private Date createTime;
+
+    @ApiModelProperty(value = "修改时间", dataType = "Date")
+    @TableField("update_time")
+    private Date updateTime;
+
+    @ApiModelProperty(value = "任务ID", dataType = "Long")
+    @TableField("task_id")
+    private Long taskId;
+
+    @ApiModelProperty(value = "估算栅格x", dataType = "Long")
+    @TableField("estimate_grid_x")
+    private String estimateGridX;
+
+    @ApiModelProperty(value = "估算栅格y", dataType = "Long")
+    @TableField("estimate_grid_y")
+    private String estimateGridY;
+
+    @ApiModelProperty(value = "中子计数率", dataType = "BigDecimal")
+    @TableField("neutron_count")
+    private BigDecimal neutronCount;
+
+    @ApiModelProperty(value = "当前栅格x", dataType = "Long")
+    @TableField("cur_grid_x")
+    private Integer curGridX;
+
+    @ApiModelProperty(value = "当前栅格y", dataType = "Long")
+    @TableField("cur_grid_y")
+    private Integer curGridY;
+
+    public String getCurPoint(){
+        return "("+curGridX+","+curGridY+")";
+    }
+    public String getEstimatePoint(){
+        if(estimateGridX==null&estimateGridY==null){
+            return "结束";
+        }
+        return "("+estimateGridX+","+estimateGridY+")";
+    }
+    @Override
+    public String toString() {
+        return "TaskGridInfo{"
+                + "id=" + id
+                + "createTime=" + createTime
+                + "updateTime=" + updateTime
+                + "taskId=" + taskId
+                + "estimateGridX=" + estimateGridX
+                + "estimateGridY=" + estimateGridY
+                + "neutronCount=" + neutronCount
+                + "curGridX=" + curGridX
+                + "curGridY=" + curGridY
+                + "}";
+    }
+}
\ No newline at end of file
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java
index edc104e..a8acb2a 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java
@@ -53,6 +53,13 @@
     @TableField("route_id")
     private Long routeId;
 
+    @ApiModelProperty(value = "寻源初始步长", dataType = "Integer")
+    @TableField("step_len")
+    private Integer stepLen;
+
+    @ApiModelProperty(value = "寻源阈值设定", dataType = "BigDecimal")
+    @TableField("max_threshold")
+    private BigDecimal maxThreshold;
     /**
      * 任务类型(1巡检任务)
      */
@@ -62,7 +69,6 @@
     @ApiModelProperty(value = "任务类型字典名称", dataType = "String")
     @TableField(exist = false)
     private String taskTypeName;
-
     @ApiModelProperty(value = "识别类型(discernType 1中子源识别)", dataType = "Integer")
     @TableField("discern_type")
     private Integer discernType;
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java
index c4bc25d..c6620f3 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java
@@ -53,7 +53,7 @@
     }),
     SET_SPEED("/set_speed" , "/set_speed_response" , "最大运动速度设置" , true, new TypeReference<RobotMsgResponseDTO<Integer>>() {
     }),
-    SET_TARGETPOINT("/set_targetpoint" , "/set_targetpoint_response" , "设置导航目标点" , false, new TypeReference<RobotMsgResponseDTO<Integer>>() {
+    SET_TARGETPOINT("/set_targetpoint" , "/set_targetpoint_response" , "设置导航目标点" , true, new TypeReference<RobotMsgResponseDTO<Integer>>() {
     }),
 
     GET_DATA_LIST("/get_data_list" , "/get_data_list_response" , "获取数据包列表" , true, new TypeReference<RobotMsgResponseDTO<List<String>>>() {
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/AbstractSocketHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/AbstractSocketHandler.java
index 83f4cb2..e47e343 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/AbstractSocketHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/AbstractSocketHandler.java
@@ -49,7 +49,7 @@
                 wsMsgBaseDTO.setMsgKey(enums.getMsgKey());
                 wsMsgBaseDTO.setData(jsonObject.get("msg"));
                 String msg = JSON.toJSONString(wsMsgBaseDTO);
-                log.info("推送{}消息：{}", enums.getRemarks(), msg);
+//                log.info("推送{}消息：{}", enums.getRemarks(), msg);
                 webSocket.sendOneMessage(robotId, msg);
             }
         } catch (Exception e) {
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/cmd/TargetPointCmdHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/cmd/TargetPointCmdHandler.java
index c1c09fa..43b4460 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/cmd/TargetPointCmdHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/cmd/TargetPointCmdHandler.java
@@ -34,7 +34,11 @@
     @Override
     public ResponseDataDTO senderCmd(TargetPointRequest request, InstructCodeEnums enums) {
         MessageRequestDTO<TargetPointRequest> messageDTO = new MessageRequestDTO();
-        //读取默认关键点信息
+        //读取默认关键点信息 {"x":"1.715708819927424","y":"-0.9100042544250901","z":"0.017993879527185876","theta":"-0.039278239414312105"}
+//        request.setX("1.715708819927424");
+//        request.setY("-0.9100042544250901");
+//        request.setZ("0.017993879527185876");
+//        request.setTheta("-0.039278239414312105");
         messageDTO.setMsg(request);
         ResponseDataDTO<Integer> list = optService.robotCmd(getRobotInfo(), enums, messageDTO);
         return list;
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/AppCostMapHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/AppCostMapHandler.java
index 0335693..88c9f79 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/AppCostMapHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/AppCostMapHandler.java
@@ -1,6 +1,11 @@
 package com.casic.missiles.modular.robot.opt.handler.msg;
 
+import com.casic.missiles.core.cache.CacheUtil;
+import com.casic.missiles.modular.constants.RobotDictConstants;
+import com.casic.missiles.modular.robot.IBaseRobotService;
+import com.casic.missiles.modular.robot.opt.enums.InstructCodeEnums;
 import com.casic.missiles.modular.robot.opt.handler.AbstractSocketHandler;
+import com.casic.missiles.modular.robot.opt.instruct.dto.CostMapResolutionRequest;
 import com.casic.missiles.modular.robot.service.IRobotStatusInfoService;
 import lombok.extern.slf4j.Slf4j;
 import org.eclipse.paho.client.mqttv3.MqttMessage;
@@ -16,14 +21,27 @@
 @Component("/app_costmap_sender")
 public class AppCostMapHandler extends AbstractSocketHandler {
     private final IRobotStatusInfoService statusInfoService;
+    private final IBaseRobotService baseRobotService;
     private static Map<String, Integer> index = new ConcurrentHashMap<>();
 
-    public AppCostMapHandler(IRobotStatusInfoService statusInfoService) {
+    public AppCostMapHandler(IRobotStatusInfoService statusInfoService, IBaseRobotService baseRobotService) {
         this.statusInfoService = statusInfoService;
+        this.baseRobotService = baseRobotService;
     }
 
     @Override
     public void process(String robotId, String topic, MqttMessage message) {
         statusInfoService.saveCostMap(robotId, message);
+
+        CostMapResolutionRequest costMapSet = CacheUtil.get(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CONFIG_COSTMAP_RESOLUTION_SET +robotId);
+        if(costMapSet != null){
+            CacheUtil.remove(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CONFIG_COSTMAP_RESOLUTION_SET +robotId);
+            new Thread(() -> {
+                costMapSet.setRobotId(Long.valueOf(robotId));
+                baseRobotService.execCmdHandler(costMapSet, InstructCodeEnums.COSTMAP_RESOLUTION);
+            }).start();
+
+        }
+
     }
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/PoseMessageMsgHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/PoseMessageMsgHandler.java
index 29f8919..6c3c3bd 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/PoseMessageMsgHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/PoseMessageMsgHandler.java
@@ -17,11 +17,9 @@
 @Component("/pose_message")
 public class PoseMessageMsgHandler extends AbstractSocketHandler {
 
-
     private final EventPublisher publisher;
 
     public PoseMessageMsgHandler(EventPublisher publisher) {
-
         this.publisher = publisher;
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/StopStateMsgHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/StopStateMsgHandler.java
index 8950815..8a71e0b 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/StopStateMsgHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/StopStateMsgHandler.java
@@ -30,7 +30,7 @@
     public void process(String robotId, String topic, MqttMessage message) {
 
         index.put(index_key, index.get("index") + 1);
-        if (index.get(index_key)>50) {
+        if (index.get(index_key)>30) {
             index.put(index_key, 0);
             log.info("主键：{},停障状态topic:{},消息内容:{}", robotId, topic, message.toString());
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/impl/RobotOptServiceImpl.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/impl/RobotOptServiceImpl.java
index 8471e7a..7940a24 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/impl/RobotOptServiceImpl.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/impl/RobotOptServiceImpl.java
@@ -146,7 +146,7 @@
     public void startTrack(StartTrackRequest request) {
         //增加循迹任务记录
         taskInfoService.reloadTask(request, robotInfoService.getById(request.getRobotId()));
-        this.execCmdHandler(request, InstructCodeEnums.START_TRACK);
+//        this.execCmdHandler(request, InstructCodeEnums.START_TRACK);
     }
 
     public IRobotRouteOptService getRobotRouteOptService() {
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/BaseRobotCmdDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/BaseRobotCmdDTO.java
index 5608f2b..06b53de 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/BaseRobotCmdDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/BaseRobotCmdDTO.java
@@ -3,11 +3,13 @@
 import io.swagger.annotations.ApiModelProperty;
 import lombok.Data;
 
+import java.io.Serializable;
+
 /**
  * 机器人指令基类
  */
 @Data
-public class BaseRobotCmdDTO {
+public class BaseRobotCmdDTO implements Serializable {
     @ApiModelProperty(value = "机器人主键", dataType = "String")
     protected Long robotId;
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/StartTrackRequest.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/StartTrackRequest.java
index a18321b..85546a7 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/StartTrackRequest.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/StartTrackRequest.java
@@ -3,6 +3,8 @@
 import io.swagger.annotations.ApiModelProperty;
 import lombok.Data;
 
+import java.math.BigDecimal;
+
 /**
  * 导航下发指令
  */
@@ -10,18 +12,19 @@
 public class StartTrackRequest extends BaseRobotCmdDTO {
     @ApiModelProperty(value = "sn编码(可不填 默认机器ID)", dataType = "String")
     private String sn;
-
     @ApiModelProperty(value = "下发状态 1:开启 0:关闭", dataType = "String")
     private Integer action;
     @ApiModelProperty(value = "等待状态 1:等待 0:不等待", dataType = "String")
     private Integer wait;
     @ApiModelProperty(value = "等待状态 0:循迹 1:任务", dataType = "isTask")
     private Integer isTask;
-    @ApiModelProperty(value = "轨迹名称", dataType = "String")
+    @ApiModelProperty(value = "轨迹名称(废弃)", dataType = "String")
     private String track_name;
     @ApiModelProperty(value = "关键点名称", dataType = "String", hidden = true)
     private String taskpoint_name;
-
+    @ApiModelProperty(value = "初始寻源步长", dataType = "Integer")
+    private Integer stepLen;
+    @ApiModelProperty(value = "寻源阈值", dataType = "BigDecimal")
+    private BigDecimal maxThreshold;
     private String obs_mode="1";
-
 }

diff --git a/.gitignore b/.gitignore
index 9e20bc1..f42a9cb 100644
--- a/.gitignore
+++ b/.gitignore
@@ -2,3 +2,4 @@
 **/target/**
 logs/
 *.iml
+*.exe
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java b/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
index c7b5d28..a849528 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
@@ -28,6 +28,7 @@
     String KEY_CUR_TASK_ID = "CUR_TASK_ID_";
     String KEY_CUR_TASK_INIT = "CUR_TASK_INIT_";
 
+    String KEY_COST_MAP = "KEY_COST_MAP";
     /**
      * PSD数据ID
      */
@@ -41,6 +42,7 @@
      */
     String KEY_CUR_XY_POINT = "KEY_CUR_XY_POINT_";
     String KEY_CUR_ESTIMATE_GRID = "KEY_CUR_ESTIMATE_GRID_";
+    String KEY_PLANING_FLAG = "KEY_PLANING_FLAG_";
     /**
      * 机器人巡点栅格
      */
@@ -55,6 +57,7 @@
      * 探测器配置参数缓存
      */
     String KEY_CONFIG_PARAMS_ID = "KEY_CONFIG_PARAMS_ID_";
+    String KEY_CONFIG_COSTMAP_RESOLUTION_SET = "KEY_CONFIG_COSTMAP_RESOLUTION_SET_";
 
     /**
      * 暂停状态缓存信息
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
index c4c905d..3bb475f 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
@@ -1,9 +1,14 @@
 package com.casic.missiles.modular.robot.controller;
 
+import cn.hutool.core.lang.Assert;
 import cn.hutool.core.util.StrUtil;
 import com.casic.missiles.core.base.controller.BaseController;
+import com.casic.missiles.core.cache.CacheUtil;
 import com.casic.missiles.core.util.SpringContextHolder;
+import com.casic.missiles.model.exception.RequestValidException;
+import com.casic.missiles.model.response.ResponseData;
 import com.casic.missiles.model.response.dto.ResponseDataDTO;
+import com.casic.missiles.modular.constants.RobotDictConstants;
 import com.casic.missiles.modular.robot.IBaseRobotService;
 import com.casic.missiles.modular.robot.dto.RobotInfoSetDTO;
 import com.casic.missiles.modular.robot.model.RobotInfo;
@@ -19,10 +24,16 @@
 import io.swagger.annotations.ApiOperation;
 import io.swagger.annotations.ApiParam;
 import lombok.SneakyThrows;
+import lombok.extern.slf4j.Slf4j;
+import org.apache.ibatis.annotations.Param;
+import org.springframework.beans.factory.annotation.Value;
 import org.springframework.web.bind.annotation.*;
+import org.springframework.web.multipart.MultipartFile;
 
 import javax.servlet.http.HttpServletResponse;
+import java.io.File;
 import java.util.List;
+import java.util.Optional;
 
 /**
  * 机器人操作类
@@ -30,6 +41,7 @@
  * @author lwh
  * @date 2023-10-30
  */
+@Slf4j
 @Api(tags = "机器人控制接口")
 @RestController
 @RequestMapping("/robot/opt")
@@ -61,6 +73,28 @@
         return baseRobotService.execCmdHandler(request, InstructCodeEnums.CHANGE_PCD);
     }
 
+    @Value("${casic.file.uploadPath}")
+    private String filePath;
+
+    @SneakyThrows
+    @ApiOperation(value = "PCD上传接口")
+    @PostMapping(value = "/uploadPcd")
+    public ResponseData uploadPcd(@RequestParam("file") MultipartFile file, @ApiParam("机器人ID,url拼接传参") String robotId) {
+        Assert.isFalse(StrUtil.isEmpty(robotId), () -> new RequestValidException("机器ID不能为空", Optional.of(new Throwable().getStackTrace()[2])));
+        RobotInfo info = robotInfoService.getById(robotId);
+        if (file != null && !file.isEmpty()) {
+            File pcdFile = new File(filePath + "finalCloud_" + info.getId() + ".pcd");
+            try {
+                file.transferTo(pcdFile);
+            } catch (Exception e) {
+                log.error(e.getMessage());
+            }
+            return ResponseData.success();
+        } else {
+            return ResponseData.error("附件上传失败!");
+        }
+    }
+
     @SneakyThrows
     @ApiOperation(value = "PCD下载接口")
     @PostMapping(value = "/pcdDownload")
@@ -136,6 +170,10 @@
     @ApiOperation(value = "启动或关闭循迹功能", notes = "返回 1成功 0失败  wait 启动时是否等待，0代表循迹启动即运动，不等待；1代表循迹启动原地等待。若无此字段，默认循迹启动不等待直接运动")
     @PostMapping(value = "/startTrack")
     public ResponseDataDTO<Integer> startTrack(@RequestBody StartTrackRequest request) {
+        request.setIsTask(1);
+        if (request.getStepLen() == null) {
+            request.setStepLen(2);
+        }
         baseRobotService.startTrack(request);
         return ResponseDataDTO.success();
     }
@@ -156,6 +194,13 @@
         MessageRequestDTO<CrlNavRequest> messageDTO = new MessageRequestDTO();
         messageDTO.setMsg(request);
         ResponseDataDTO<Integer> list = baseRobotService.getRobotRouteOptService().robotCmd(info, InstructCodeEnums.CTRL_NAV, messageDTO);
+        if (request.getAction() == 1) {
+            CostMapResolutionRequest costMap = new CostMapResolutionRequest();
+            costMap.setResolution(info.getResolution().doubleValue());
+            request.setRobotId(request.getRobotId());
+            //是否下发分辨率
+            CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CONFIG_COSTMAP_RESOLUTION_SET + request.getRobotId(), costMap);
+        }
         return list;
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java
new file mode 100644
index 0000000..5317651
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java
@@ -0,0 +1,86 @@
+package com.casic.missiles.modular.robot.controller;
+
+import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
+import com.baomidou.mybatisplus.extension.plugins.pagination.Page;
+import com.casic.missiles.core.base.controller.BaseController;
+import com.casic.missiles.core.page.PageFactory;
+import com.casic.missiles.core.page.PageInfoBT;
+import com.casic.missiles.model.form.IdForms;
+import com.casic.missiles.model.response.dto.ResponseDataDTO;
+import com.casic.missiles.modular.robot.model.TaskGridInfo;
+import com.casic.missiles.modular.robot.service.ITaskGridInfoService;
+import com.casic.missiles.modular.robot.vo.TaskGridInfoVO;
+import io.swagger.annotations.Api;
+import io.swagger.annotations.ApiOperation;
+import org.springframework.web.bind.annotation.*;
+
+import java.util.List;
+
+/**
+ * 任务网格信息Controller
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+@Api(tags = "任务网格信息")
+@RestController
+@RequestMapping("/robot/taskGridInfo")
+public class TaskGridInfoController extends BaseController {
+
+    private final ITaskGridInfoService taskGridInfoService;
+
+    public TaskGridInfoController(ITaskGridInfoService taskGridInfoService) {
+        this.taskGridInfoService = taskGridInfoService;
+    }
+
+    @ApiOperation(value = "列表查询")
+    @GetMapping(value = "/list")
+    public ResponseDataDTO<List<TaskGridInfo>> list(TaskGridInfoVO taskGridInfoVO) {
+        QueryWrapper<TaskGridInfo> query = taskGridInfoVO.genQuery();
+        return ResponseDataDTO.success(taskGridInfoService.list(query));
+    }
+
+    @ApiOperation(value = "分页查询")
+    @GetMapping(value = "/listPage")
+    public ResponseDataDTO<PageInfoBT<TaskGridInfo>> listPage(TaskGridInfoVO taskGridInfoVO) {
+        Page<TaskGridInfo> page = PageFactory.defaultPage();
+        QueryWrapper<TaskGridInfo> query = taskGridInfoVO.genQuery();
+        page = taskGridInfoService.page(page, query);
+        return ResponseDataDTO.success(super.packForBT(page));
+    }
+
+
+    @ApiOperation(value = "新增接口")
+    @PostMapping(value = "/add")
+    public ResponseDataDTO add(@RequestBody TaskGridInfo taskGridInfo) {
+        taskGridInfoService.save(taskGridInfo);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "修改接口")
+    @PostMapping(value = "/update")
+    public ResponseDataDTO update(@RequestBody TaskGridInfo taskGridInfo) {
+        taskGridInfoService.updateById(taskGridInfo);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "删除接口")
+    @GetMapping(value = "/delete")
+    public ResponseDataDTO delete(String id) {
+        taskGridInfoService.removeById(id);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation("批量删除")
+    @PostMapping("/batchDelete")
+    public ResponseDataDTO batchDelete(@RequestBody IdForms ids) {
+        taskGridInfoService.removeByIds(ids.getIds());
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "详情查询")
+    @GetMapping(value = "/detail")
+    public ResponseDataDTO<TaskGridInfo> detail(String id) {
+        return ResponseDataDTO.success(taskGridInfoService.getById(id));
+    }
+}
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
index e82a792..6992274 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
@@ -98,7 +98,7 @@
     public ResponseDataDTO<AlgorithmResponse> test() {
         AlgorithmResponse response = new AlgorithmResponse();
         response.setTaskHeatMap(new TaskHeatMap());
-        algorithmService.readAlgorithmResponse("step3.txt",response,123L,null);
+        algorithmService.readAlgorithmResponse("step3.txt",null,response,123L,null);
         sendWebSocket(InstructStatusMsgEnums.HEAT_MAP, "1", response.getTaskHeatMap());
         return ResponseDataDTO.success(response);
     }
@@ -121,7 +121,7 @@
     @GetMapping(value = "/testGrid")
     public ResponseDataDTO<List<RouteInfo>> testGrid(@RequestBody PathInitDTO pathInitDTO) {
         RobotInfo robotInfo = infoService.getById(pathInitDTO.getRobotId());
-        GridPointDTO gridPointDTO = algorithmService.convertSlamToGrid(new BigDecimal(pathInitDTO.getX()), new BigDecimal(10), new BigDecimal(4), robotInfo);
+        GridPointDTO gridPointDTO = algorithmService.convertSlamToGrid(new BigDecimal(pathInitDTO.getXNow()), new BigDecimal(10), new BigDecimal(4), robotInfo);
         return ResponseDataDTO.success(algorithmService.convertGridCenterPoint(gridPointDTO, new BigDecimal(10), robotInfo));
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/TaskGridInfoMapper.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/TaskGridInfoMapper.java
new file mode 100644
index 0000000..5c3a70b
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/TaskGridInfoMapper.java
@@ -0,0 +1,23 @@
+package com.casic.missiles.modular.robot.dao;
+
+
+import com.baomidou.mybatisplus.core.mapper.BaseMapper;
+import com.casic.missiles.modular.robot.model.TaskGridInfo;
+import org.apache.ibatis.annotations.Param;
+import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
+import com.baomidou.mybatisplus.extension.plugins.pagination.Page;
+import java.util.List;
+/**
+ * <p>
+ *  任务网格信息 Mapper 接口
+ * </p>
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+public interface TaskGridInfoMapper extends BaseMapper<TaskGridInfo> {
+/**
+* 任务网格信息 分页检索
+*/
+List<TaskGridInfo> selectTaskGridInfoPage( @Param("page") Page<TaskGridInfo> page,@Param("ew" ) QueryWrapper<TaskGridInfo> query);
+}
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/mapping/TaskGridInfoMapper.xml b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/mapping/TaskGridInfoMapper.xml
new file mode 100644
index 0000000..be573e0
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/mapping/TaskGridInfoMapper.xml
@@ -0,0 +1,80 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!DOCTYPE mapper PUBLIC "-//mybatis.org//DTD Mapper 3.0//EN" "http://mybatis.org/dtd/mybatis-3-mapper.dtd">
+<mapper namespace="com.casic.missiles.modular.robot.dao.TaskGridInfoMapper">
+    <!-- 通用查询映射结果 -->
+    <resultMap id="BaseResultMap" type="com.casic.missiles.modular.robot.model.TaskGridInfo">
+        <result column="id" property="id"/>
+        <result column="create_time" property="createTime"/>
+        <result column="update_time" property="updateTime"/>
+        <result column="task_id" property="taskId"/>
+        <result column="estimate_grid_x" property="estimateGridX"/>
+        <result column="estimate_grid_y" property="estimateGridY"/>
+        <result column="neutron_count" property="neutronCount"/>
+        <result column="cur_grid_x" property="curGridX"/>
+        <result column="cur_grid_y" property="curGridY"/>
+    </resultMap>
+    <!-- 通用查询结果列 -->
+    <sql id="Base_Column_List">
+        id
+        ,
+                    create_time as createTime ,
+                    update_time as updateTime ,
+                    task_id as taskId ,
+                    estimate_grid_x as estimateGridX ,
+                    estimate_grid_y as estimateGridY ,
+                    neutron_count as neutronCount ,
+                    cur_grid_x as curGridX ,
+                    cur_grid_y as curGridY
+    </sql>
+    <sql id="Base_Column_Alias">
+        a
+        .
+        id
+        ,
+            a.create_time
+            as createTime ,
+            a.update_time
+            as updateTime ,
+            a.task_id
+            as taskId ,
+            a.estimate_grid_x
+            as estimateGridX ,
+            a.estimate_grid_y
+            as estimateGridY ,
+            a.neutron_count
+            as neutronCount ,
+            a.cur_grid_x
+            as curGridX ,
+            a.cur_grid_y
+            as curGridY
+    </sql>
+    <select id="selectTaskGridInfoPage" resultType="com.casic.missiles.modular.robot.model.TaskGridInfo">
+        select
+        <include refid="Base_Column_Alias"/>
+        from task_grid_info a
+        <where>
+            <if test="ew != null">
+                ${ew.sqlSegment}
+            </if>
+        </where>
+    </select>
+    <sql id="Base_Column_Query">
+        <if test="request!=null">
+            <if test="request.taskId!= null and request.taskId!=''">
+                and task_id = #{request.taskId}
+            </if>
+            <if test="request.estimateGridX!= null and request.estimateGridX!=''">
+                and estimate_grid_x = #{request.estimateGridX}
+            </if>
+            <if test="request.estimateGridY!= null and request.estimateGridY!=''">
+                and estimate_grid_y = #{request.estimateGridY}
+            </if>
+            <if test="request.curGridX!= null and request.curGridX!=''">
+                and cur_grid_x = #{request.curGridX}
+            </if>
+            <if test="request.curGridY!= null and request.curGridY!=''">
+                and cur_grid_y = #{request.curGridY}
+            </if>
+        </if>
+    </sql>
+</mapper>
\ No newline at end of file
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java
index 749f559..4559074 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java
@@ -13,4 +13,9 @@
      * 规划点位信息
      */
     private PointNextDTO pointNextDTO;
+    /**
+     * 寻源目标地址
+     */
+    private PointNextDTO coordinate;
+    private boolean isEnd;
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java
index 39ff2a2..cef3238 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java
@@ -12,6 +12,9 @@
     private BigDecimal y;
 
     public static GridPointDTO convertGridPoint(PointNextDTO nextDTO) {
+        if(nextDTO==null){
+            return null;
+        }
         GridPointDTO gridPointDTO = new GridPointDTO();
         gridPointDTO.setX(Convert.toBigDecimal(nextDTO.getX()));
         gridPointDTO.setY(Convert.toBigDecimal(nextDTO.getY()));
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java
index 8c88242..33be25d 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java
@@ -12,25 +12,19 @@
 public class PathInitDTO {
     //---------------------算法参数
     /**
-     * x坐标最大值（x方向栅格总数）
+     * 初始位置x坐标(栅格地图下的坐标)
      */
-    private String xMax;
+    private String xNow;
     /**
-     * y坐标最大值（y方向栅格总数）
+     * 初始位置y坐标(栅格地图下的坐标)
      */
-    private String yMax;
+    private String yNow;
     /**
      * 初始位置计数率
      */
-    private String count;
-    /**
-     * 初始位置x坐标(0<=x<x_max)
-     */
-    private String x;
-    /**
-     * 初始位置y坐标(0<=y<y_max)
-     */
-    private String y;
+    private String zNow;
+
+    private Integer stepLen;
 
     //---------------------其他执行参数
     private String robotId;
@@ -38,11 +32,11 @@
 
     public List<String> toArray() {
         List<String> list = new ArrayList<>();
-        list.add(xMax);
-        list.add(yMax);
-        list.add(count);
-        list.add(x);
-        list.add(y);
+        list.add(xNow);
+        list.add(yNow);
+        list.add(stepLen+"");
+        list.add(zNow);
+        list.add(taskId+"");
         return list;
     }
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java
index 109e95c..ada6774 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java
@@ -10,21 +10,17 @@
  */
 @Data
 public class PathPlanDTO extends PathInitDTO {
-
     /**
-     * 长度为8的数组，记录(x-1, y-1), (x-1, y), (x-1, y+1), (x, y-1), (x, y+1), (x+1, y-1), (x+1, y), (x+1, y+1)8个坐标的可通行信息。
-     * 若某坐标有障碍物（不可通行行）则记为0，否则记为1
+     * 浮点阈值(阈值，超过该值后结束巡检开始寻源)
      */
-    private String open;
-
+    private String th;
     public List<String> toArray() {
         List<String> list = new ArrayList<>();
-        list.add(getXMax());
-        list.add(getYMax());
-        list.add(open);
-        list.add(getCount());
-        list.add(getX());
-        list.add(getY());
+
+        list.add(getStepLen()+"");
+        list.add(getTh());
+        list.add(getZNow());
+        list.add(getTaskId()+"");
         return list;
     }
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskGridInfo.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskGridInfo.java
new file mode 100644
index 0000000..101c10a
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskGridInfo.java
@@ -0,0 +1,83 @@
+package com.casic.missiles.modular.robot.model;
+
+import com.baomidou.mybatisplus.annotation.TableField;
+import com.baomidou.mybatisplus.annotation.TableName;
+import com.baomidou.mybatisplus.extension.activerecord.Model;
+import io.swagger.annotations.ApiModelProperty;
+import lombok.Data;
+
+import java.math.BigDecimal;
+import java.util.Date;
+
+/**
+ * 任务网格信息对象 task_grid_info
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+@Data
+@TableName("task_grid_info")
+public class TaskGridInfo extends Model<TaskGridInfo> {
+    private static final long serialVersionUID = 1L;
+    @TableField(exist = false)
+    private Integer index;
+    @ApiModelProperty(value = "主键", dataType = "Long")
+    @TableField("id")
+    private Long id;
+
+    @ApiModelProperty(value = "创建时间", dataType = "Date")
+    @TableField("create_time")
+    private Date createTime;
+
+    @ApiModelProperty(value = "修改时间", dataType = "Date")
+    @TableField("update_time")
+    private Date updateTime;
+
+    @ApiModelProperty(value = "任务ID", dataType = "Long")
+    @TableField("task_id")
+    private Long taskId;
+
+    @ApiModelProperty(value = "估算栅格x", dataType = "Long")
+    @TableField("estimate_grid_x")
+    private String estimateGridX;
+
+    @ApiModelProperty(value = "估算栅格y", dataType = "Long")
+    @TableField("estimate_grid_y")
+    private String estimateGridY;
+
+    @ApiModelProperty(value = "中子计数率", dataType = "BigDecimal")
+    @TableField("neutron_count")
+    private BigDecimal neutronCount;
+
+    @ApiModelProperty(value = "当前栅格x", dataType = "Long")
+    @TableField("cur_grid_x")
+    private Integer curGridX;
+
+    @ApiModelProperty(value = "当前栅格y", dataType = "Long")
+    @TableField("cur_grid_y")
+    private Integer curGridY;
+
+    public String getCurPoint(){
+        return "("+curGridX+","+curGridY+")";
+    }
+    public String getEstimatePoint(){
+        if(estimateGridX==null&estimateGridY==null){
+            return "结束";
+        }
+        return "("+estimateGridX+","+estimateGridY+")";
+    }
+    @Override
+    public String toString() {
+        return "TaskGridInfo{"
+                + "id=" + id
+                + "createTime=" + createTime
+                + "updateTime=" + updateTime
+                + "taskId=" + taskId
+                + "estimateGridX=" + estimateGridX
+                + "estimateGridY=" + estimateGridY
+                + "neutronCount=" + neutronCount
+                + "curGridX=" + curGridX
+                + "curGridY=" + curGridY
+                + "}";
+    }
+}
\ No newline at end of file
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java
index edc104e..a8acb2a 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java
@@ -53,6 +53,13 @@
     @TableField("route_id")
     private Long routeId;
 
+    @ApiModelProperty(value = "寻源初始步长", dataType = "Integer")
+    @TableField("step_len")
+    private Integer stepLen;
+
+    @ApiModelProperty(value = "寻源阈值设定", dataType = "BigDecimal")
+    @TableField("max_threshold")
+    private BigDecimal maxThreshold;
     /**
      * 任务类型(1巡检任务)
      */
@@ -62,7 +69,6 @@
     @ApiModelProperty(value = "任务类型字典名称", dataType = "String")
     @TableField(exist = false)
     private String taskTypeName;
-
     @ApiModelProperty(value = "识别类型(discernType 1中子源识别)", dataType = "Integer")
     @TableField("discern_type")
     private Integer discernType;
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java
index c4bc25d..c6620f3 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java
@@ -53,7 +53,7 @@
     }),
     SET_SPEED("/set_speed" , "/set_speed_response" , "最大运动速度设置" , true, new TypeReference<RobotMsgResponseDTO<Integer>>() {
     }),
-    SET_TARGETPOINT("/set_targetpoint" , "/set_targetpoint_response" , "设置导航目标点" , false, new TypeReference<RobotMsgResponseDTO<Integer>>() {
+    SET_TARGETPOINT("/set_targetpoint" , "/set_targetpoint_response" , "设置导航目标点" , true, new TypeReference<RobotMsgResponseDTO<Integer>>() {
     }),
 
     GET_DATA_LIST("/get_data_list" , "/get_data_list_response" , "获取数据包列表" , true, new TypeReference<RobotMsgResponseDTO<List<String>>>() {
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/AbstractSocketHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/AbstractSocketHandler.java
index 83f4cb2..e47e343 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/AbstractSocketHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/AbstractSocketHandler.java
@@ -49,7 +49,7 @@
                 wsMsgBaseDTO.setMsgKey(enums.getMsgKey());
                 wsMsgBaseDTO.setData(jsonObject.get("msg"));
                 String msg = JSON.toJSONString(wsMsgBaseDTO);
-                log.info("推送{}消息：{}", enums.getRemarks(), msg);
+//                log.info("推送{}消息：{}", enums.getRemarks(), msg);
                 webSocket.sendOneMessage(robotId, msg);
             }
         } catch (Exception e) {
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/cmd/TargetPointCmdHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/cmd/TargetPointCmdHandler.java
index c1c09fa..43b4460 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/cmd/TargetPointCmdHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/cmd/TargetPointCmdHandler.java
@@ -34,7 +34,11 @@
     @Override
     public ResponseDataDTO senderCmd(TargetPointRequest request, InstructCodeEnums enums) {
         MessageRequestDTO<TargetPointRequest> messageDTO = new MessageRequestDTO();
-        //读取默认关键点信息
+        //读取默认关键点信息 {"x":"1.715708819927424","y":"-0.9100042544250901","z":"0.017993879527185876","theta":"-0.039278239414312105"}
+//        request.setX("1.715708819927424");
+//        request.setY("-0.9100042544250901");
+//        request.setZ("0.017993879527185876");
+//        request.setTheta("-0.039278239414312105");
         messageDTO.setMsg(request);
         ResponseDataDTO<Integer> list = optService.robotCmd(getRobotInfo(), enums, messageDTO);
         return list;
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/AppCostMapHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/AppCostMapHandler.java
index 0335693..88c9f79 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/AppCostMapHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/AppCostMapHandler.java
@@ -1,6 +1,11 @@
 package com.casic.missiles.modular.robot.opt.handler.msg;
 
+import com.casic.missiles.core.cache.CacheUtil;
+import com.casic.missiles.modular.constants.RobotDictConstants;
+import com.casic.missiles.modular.robot.IBaseRobotService;
+import com.casic.missiles.modular.robot.opt.enums.InstructCodeEnums;
 import com.casic.missiles.modular.robot.opt.handler.AbstractSocketHandler;
+import com.casic.missiles.modular.robot.opt.instruct.dto.CostMapResolutionRequest;
 import com.casic.missiles.modular.robot.service.IRobotStatusInfoService;
 import lombok.extern.slf4j.Slf4j;
 import org.eclipse.paho.client.mqttv3.MqttMessage;
@@ -16,14 +21,27 @@
 @Component("/app_costmap_sender")
 public class AppCostMapHandler extends AbstractSocketHandler {
     private final IRobotStatusInfoService statusInfoService;
+    private final IBaseRobotService baseRobotService;
     private static Map<String, Integer> index = new ConcurrentHashMap<>();
 
-    public AppCostMapHandler(IRobotStatusInfoService statusInfoService) {
+    public AppCostMapHandler(IRobotStatusInfoService statusInfoService, IBaseRobotService baseRobotService) {
         this.statusInfoService = statusInfoService;
+        this.baseRobotService = baseRobotService;
     }
 
     @Override
     public void process(String robotId, String topic, MqttMessage message) {
         statusInfoService.saveCostMap(robotId, message);
+
+        CostMapResolutionRequest costMapSet = CacheUtil.get(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CONFIG_COSTMAP_RESOLUTION_SET +robotId);
+        if(costMapSet != null){
+            CacheUtil.remove(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CONFIG_COSTMAP_RESOLUTION_SET +robotId);
+            new Thread(() -> {
+                costMapSet.setRobotId(Long.valueOf(robotId));
+                baseRobotService.execCmdHandler(costMapSet, InstructCodeEnums.COSTMAP_RESOLUTION);
+            }).start();
+
+        }
+
     }
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/PoseMessageMsgHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/PoseMessageMsgHandler.java
index 29f8919..6c3c3bd 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/PoseMessageMsgHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/PoseMessageMsgHandler.java
@@ -17,11 +17,9 @@
 @Component("/pose_message")
 public class PoseMessageMsgHandler extends AbstractSocketHandler {
 
-
     private final EventPublisher publisher;
 
     public PoseMessageMsgHandler(EventPublisher publisher) {
-
         this.publisher = publisher;
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/StopStateMsgHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/StopStateMsgHandler.java
index 8950815..8a71e0b 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/StopStateMsgHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/StopStateMsgHandler.java
@@ -30,7 +30,7 @@
     public void process(String robotId, String topic, MqttMessage message) {
 
         index.put(index_key, index.get("index") + 1);
-        if (index.get(index_key)>50) {
+        if (index.get(index_key)>30) {
             index.put(index_key, 0);
             log.info("主键：{},停障状态topic:{},消息内容:{}", robotId, topic, message.toString());
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/impl/RobotOptServiceImpl.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/impl/RobotOptServiceImpl.java
index 8471e7a..7940a24 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/impl/RobotOptServiceImpl.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/impl/RobotOptServiceImpl.java
@@ -146,7 +146,7 @@
     public void startTrack(StartTrackRequest request) {
         //增加循迹任务记录
         taskInfoService.reloadTask(request, robotInfoService.getById(request.getRobotId()));
-        this.execCmdHandler(request, InstructCodeEnums.START_TRACK);
+//        this.execCmdHandler(request, InstructCodeEnums.START_TRACK);
     }
 
     public IRobotRouteOptService getRobotRouteOptService() {
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/BaseRobotCmdDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/BaseRobotCmdDTO.java
index 5608f2b..06b53de 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/BaseRobotCmdDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/BaseRobotCmdDTO.java
@@ -3,11 +3,13 @@
 import io.swagger.annotations.ApiModelProperty;
 import lombok.Data;
 
+import java.io.Serializable;
+
 /**
  * 机器人指令基类
  */
 @Data
-public class BaseRobotCmdDTO {
+public class BaseRobotCmdDTO implements Serializable {
     @ApiModelProperty(value = "机器人主键", dataType = "String")
     protected Long robotId;
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/StartTrackRequest.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/StartTrackRequest.java
index a18321b..85546a7 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/StartTrackRequest.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/StartTrackRequest.java
@@ -3,6 +3,8 @@
 import io.swagger.annotations.ApiModelProperty;
 import lombok.Data;
 
+import java.math.BigDecimal;
+
 /**
  * 导航下发指令
  */
@@ -10,18 +12,19 @@
 public class StartTrackRequest extends BaseRobotCmdDTO {
     @ApiModelProperty(value = "sn编码(可不填 默认机器ID)", dataType = "String")
     private String sn;
-
     @ApiModelProperty(value = "下发状态 1:开启 0:关闭", dataType = "String")
     private Integer action;
     @ApiModelProperty(value = "等待状态 1:等待 0:不等待", dataType = "String")
     private Integer wait;
     @ApiModelProperty(value = "等待状态 0:循迹 1:任务", dataType = "isTask")
     private Integer isTask;
-    @ApiModelProperty(value = "轨迹名称", dataType = "String")
+    @ApiModelProperty(value = "轨迹名称(废弃)", dataType = "String")
     private String track_name;
     @ApiModelProperty(value = "关键点名称", dataType = "String", hidden = true)
     private String taskpoint_name;
-
+    @ApiModelProperty(value = "初始寻源步长", dataType = "Integer")
+    private Integer stepLen;
+    @ApiModelProperty(value = "寻源阈值", dataType = "BigDecimal")
+    private BigDecimal maxThreshold;
     private String obs_mode="1";
-
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/response/StopStateResponseDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/response/StopStateResponseDTO.java
index eb514bf..e0679d7 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/response/StopStateResponseDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/response/StopStateResponseDTO.java
@@ -3,11 +3,13 @@
 import io.swagger.annotations.ApiModelProperty;
 import lombok.Data;
 
+import java.io.Serializable;
+
 /**
  * 电压信息返回
  */
 @Data
-public class StopStateResponseDTO {
+public class StopStateResponseDTO implements Serializable {
     @ApiModelProperty(value = "软件急停(0：无触发 1：触发)" , dataType = "Integer")
     private Integer soft;
 

diff --git a/.gitignore b/.gitignore
index 9e20bc1..f42a9cb 100644
--- a/.gitignore
+++ b/.gitignore
@@ -2,3 +2,4 @@
 **/target/**
 logs/
 *.iml
+*.exe
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java b/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
index c7b5d28..a849528 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
@@ -28,6 +28,7 @@
     String KEY_CUR_TASK_ID = "CUR_TASK_ID_";
     String KEY_CUR_TASK_INIT = "CUR_TASK_INIT_";
 
+    String KEY_COST_MAP = "KEY_COST_MAP";
     /**
      * PSD数据ID
      */
@@ -41,6 +42,7 @@
      */
     String KEY_CUR_XY_POINT = "KEY_CUR_XY_POINT_";
     String KEY_CUR_ESTIMATE_GRID = "KEY_CUR_ESTIMATE_GRID_";
+    String KEY_PLANING_FLAG = "KEY_PLANING_FLAG_";
     /**
      * 机器人巡点栅格
      */
@@ -55,6 +57,7 @@
      * 探测器配置参数缓存
      */
     String KEY_CONFIG_PARAMS_ID = "KEY_CONFIG_PARAMS_ID_";
+    String KEY_CONFIG_COSTMAP_RESOLUTION_SET = "KEY_CONFIG_COSTMAP_RESOLUTION_SET_";
 
     /**
      * 暂停状态缓存信息
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
index c4c905d..3bb475f 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
@@ -1,9 +1,14 @@
 package com.casic.missiles.modular.robot.controller;
 
+import cn.hutool.core.lang.Assert;
 import cn.hutool.core.util.StrUtil;
 import com.casic.missiles.core.base.controller.BaseController;
+import com.casic.missiles.core.cache.CacheUtil;
 import com.casic.missiles.core.util.SpringContextHolder;
+import com.casic.missiles.model.exception.RequestValidException;
+import com.casic.missiles.model.response.ResponseData;
 import com.casic.missiles.model.response.dto.ResponseDataDTO;
+import com.casic.missiles.modular.constants.RobotDictConstants;
 import com.casic.missiles.modular.robot.IBaseRobotService;
 import com.casic.missiles.modular.robot.dto.RobotInfoSetDTO;
 import com.casic.missiles.modular.robot.model.RobotInfo;
@@ -19,10 +24,16 @@
 import io.swagger.annotations.ApiOperation;
 import io.swagger.annotations.ApiParam;
 import lombok.SneakyThrows;
+import lombok.extern.slf4j.Slf4j;
+import org.apache.ibatis.annotations.Param;
+import org.springframework.beans.factory.annotation.Value;
 import org.springframework.web.bind.annotation.*;
+import org.springframework.web.multipart.MultipartFile;
 
 import javax.servlet.http.HttpServletResponse;
+import java.io.File;
 import java.util.List;
+import java.util.Optional;
 
 /**
  * 机器人操作类
@@ -30,6 +41,7 @@
  * @author lwh
  * @date 2023-10-30
  */
+@Slf4j
 @Api(tags = "机器人控制接口")
 @RestController
 @RequestMapping("/robot/opt")
@@ -61,6 +73,28 @@
         return baseRobotService.execCmdHandler(request, InstructCodeEnums.CHANGE_PCD);
     }
 
+    @Value("${casic.file.uploadPath}")
+    private String filePath;
+
+    @SneakyThrows
+    @ApiOperation(value = "PCD上传接口")
+    @PostMapping(value = "/uploadPcd")
+    public ResponseData uploadPcd(@RequestParam("file") MultipartFile file, @ApiParam("机器人ID,url拼接传参") String robotId) {
+        Assert.isFalse(StrUtil.isEmpty(robotId), () -> new RequestValidException("机器ID不能为空", Optional.of(new Throwable().getStackTrace()[2])));
+        RobotInfo info = robotInfoService.getById(robotId);
+        if (file != null && !file.isEmpty()) {
+            File pcdFile = new File(filePath + "finalCloud_" + info.getId() + ".pcd");
+            try {
+                file.transferTo(pcdFile);
+            } catch (Exception e) {
+                log.error(e.getMessage());
+            }
+            return ResponseData.success();
+        } else {
+            return ResponseData.error("附件上传失败!");
+        }
+    }
+
     @SneakyThrows
     @ApiOperation(value = "PCD下载接口")
     @PostMapping(value = "/pcdDownload")
@@ -136,6 +170,10 @@
     @ApiOperation(value = "启动或关闭循迹功能", notes = "返回 1成功 0失败  wait 启动时是否等待，0代表循迹启动即运动，不等待；1代表循迹启动原地等待。若无此字段，默认循迹启动不等待直接运动")
     @PostMapping(value = "/startTrack")
     public ResponseDataDTO<Integer> startTrack(@RequestBody StartTrackRequest request) {
+        request.setIsTask(1);
+        if (request.getStepLen() == null) {
+            request.setStepLen(2);
+        }
         baseRobotService.startTrack(request);
         return ResponseDataDTO.success();
     }
@@ -156,6 +194,13 @@
         MessageRequestDTO<CrlNavRequest> messageDTO = new MessageRequestDTO();
         messageDTO.setMsg(request);
         ResponseDataDTO<Integer> list = baseRobotService.getRobotRouteOptService().robotCmd(info, InstructCodeEnums.CTRL_NAV, messageDTO);
+        if (request.getAction() == 1) {
+            CostMapResolutionRequest costMap = new CostMapResolutionRequest();
+            costMap.setResolution(info.getResolution().doubleValue());
+            request.setRobotId(request.getRobotId());
+            //是否下发分辨率
+            CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CONFIG_COSTMAP_RESOLUTION_SET + request.getRobotId(), costMap);
+        }
         return list;
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java
new file mode 100644
index 0000000..5317651
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java
@@ -0,0 +1,86 @@
+package com.casic.missiles.modular.robot.controller;
+
+import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
+import com.baomidou.mybatisplus.extension.plugins.pagination.Page;
+import com.casic.missiles.core.base.controller.BaseController;
+import com.casic.missiles.core.page.PageFactory;
+import com.casic.missiles.core.page.PageInfoBT;
+import com.casic.missiles.model.form.IdForms;
+import com.casic.missiles.model.response.dto.ResponseDataDTO;
+import com.casic.missiles.modular.robot.model.TaskGridInfo;
+import com.casic.missiles.modular.robot.service.ITaskGridInfoService;
+import com.casic.missiles.modular.robot.vo.TaskGridInfoVO;
+import io.swagger.annotations.Api;
+import io.swagger.annotations.ApiOperation;
+import org.springframework.web.bind.annotation.*;
+
+import java.util.List;
+
+/**
+ * 任务网格信息Controller
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+@Api(tags = "任务网格信息")
+@RestController
+@RequestMapping("/robot/taskGridInfo")
+public class TaskGridInfoController extends BaseController {
+
+    private final ITaskGridInfoService taskGridInfoService;
+
+    public TaskGridInfoController(ITaskGridInfoService taskGridInfoService) {
+        this.taskGridInfoService = taskGridInfoService;
+    }
+
+    @ApiOperation(value = "列表查询")
+    @GetMapping(value = "/list")
+    public ResponseDataDTO<List<TaskGridInfo>> list(TaskGridInfoVO taskGridInfoVO) {
+        QueryWrapper<TaskGridInfo> query = taskGridInfoVO.genQuery();
+        return ResponseDataDTO.success(taskGridInfoService.list(query));
+    }
+
+    @ApiOperation(value = "分页查询")
+    @GetMapping(value = "/listPage")
+    public ResponseDataDTO<PageInfoBT<TaskGridInfo>> listPage(TaskGridInfoVO taskGridInfoVO) {
+        Page<TaskGridInfo> page = PageFactory.defaultPage();
+        QueryWrapper<TaskGridInfo> query = taskGridInfoVO.genQuery();
+        page = taskGridInfoService.page(page, query);
+        return ResponseDataDTO.success(super.packForBT(page));
+    }
+
+
+    @ApiOperation(value = "新增接口")
+    @PostMapping(value = "/add")
+    public ResponseDataDTO add(@RequestBody TaskGridInfo taskGridInfo) {
+        taskGridInfoService.save(taskGridInfo);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "修改接口")
+    @PostMapping(value = "/update")
+    public ResponseDataDTO update(@RequestBody TaskGridInfo taskGridInfo) {
+        taskGridInfoService.updateById(taskGridInfo);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "删除接口")
+    @GetMapping(value = "/delete")
+    public ResponseDataDTO delete(String id) {
+        taskGridInfoService.removeById(id);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation("批量删除")
+    @PostMapping("/batchDelete")
+    public ResponseDataDTO batchDelete(@RequestBody IdForms ids) {
+        taskGridInfoService.removeByIds(ids.getIds());
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "详情查询")
+    @GetMapping(value = "/detail")
+    public ResponseDataDTO<TaskGridInfo> detail(String id) {
+        return ResponseDataDTO.success(taskGridInfoService.getById(id));
+    }
+}
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
index e82a792..6992274 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
@@ -98,7 +98,7 @@
     public ResponseDataDTO<AlgorithmResponse> test() {
         AlgorithmResponse response = new AlgorithmResponse();
         response.setTaskHeatMap(new TaskHeatMap());
-        algorithmService.readAlgorithmResponse("step3.txt",response,123L,null);
+        algorithmService.readAlgorithmResponse("step3.txt",null,response,123L,null);
         sendWebSocket(InstructStatusMsgEnums.HEAT_MAP, "1", response.getTaskHeatMap());
         return ResponseDataDTO.success(response);
     }
@@ -121,7 +121,7 @@
     @GetMapping(value = "/testGrid")
     public ResponseDataDTO<List<RouteInfo>> testGrid(@RequestBody PathInitDTO pathInitDTO) {
         RobotInfo robotInfo = infoService.getById(pathInitDTO.getRobotId());
-        GridPointDTO gridPointDTO = algorithmService.convertSlamToGrid(new BigDecimal(pathInitDTO.getX()), new BigDecimal(10), new BigDecimal(4), robotInfo);
+        GridPointDTO gridPointDTO = algorithmService.convertSlamToGrid(new BigDecimal(pathInitDTO.getXNow()), new BigDecimal(10), new BigDecimal(4), robotInfo);
         return ResponseDataDTO.success(algorithmService.convertGridCenterPoint(gridPointDTO, new BigDecimal(10), robotInfo));
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/TaskGridInfoMapper.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/TaskGridInfoMapper.java
new file mode 100644
index 0000000..5c3a70b
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/TaskGridInfoMapper.java
@@ -0,0 +1,23 @@
+package com.casic.missiles.modular.robot.dao;
+
+
+import com.baomidou.mybatisplus.core.mapper.BaseMapper;
+import com.casic.missiles.modular.robot.model.TaskGridInfo;
+import org.apache.ibatis.annotations.Param;
+import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
+import com.baomidou.mybatisplus.extension.plugins.pagination.Page;
+import java.util.List;
+/**
+ * <p>
+ *  任务网格信息 Mapper 接口
+ * </p>
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+public interface TaskGridInfoMapper extends BaseMapper<TaskGridInfo> {
+/**
+* 任务网格信息 分页检索
+*/
+List<TaskGridInfo> selectTaskGridInfoPage( @Param("page") Page<TaskGridInfo> page,@Param("ew" ) QueryWrapper<TaskGridInfo> query);
+}
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/mapping/TaskGridInfoMapper.xml b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/mapping/TaskGridInfoMapper.xml
new file mode 100644
index 0000000..be573e0
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/mapping/TaskGridInfoMapper.xml
@@ -0,0 +1,80 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!DOCTYPE mapper PUBLIC "-//mybatis.org//DTD Mapper 3.0//EN" "http://mybatis.org/dtd/mybatis-3-mapper.dtd">
+<mapper namespace="com.casic.missiles.modular.robot.dao.TaskGridInfoMapper">
+    <!-- 通用查询映射结果 -->
+    <resultMap id="BaseResultMap" type="com.casic.missiles.modular.robot.model.TaskGridInfo">
+        <result column="id" property="id"/>
+        <result column="create_time" property="createTime"/>
+        <result column="update_time" property="updateTime"/>
+        <result column="task_id" property="taskId"/>
+        <result column="estimate_grid_x" property="estimateGridX"/>
+        <result column="estimate_grid_y" property="estimateGridY"/>
+        <result column="neutron_count" property="neutronCount"/>
+        <result column="cur_grid_x" property="curGridX"/>
+        <result column="cur_grid_y" property="curGridY"/>
+    </resultMap>
+    <!-- 通用查询结果列 -->
+    <sql id="Base_Column_List">
+        id
+        ,
+                    create_time as createTime ,
+                    update_time as updateTime ,
+                    task_id as taskId ,
+                    estimate_grid_x as estimateGridX ,
+                    estimate_grid_y as estimateGridY ,
+                    neutron_count as neutronCount ,
+                    cur_grid_x as curGridX ,
+                    cur_grid_y as curGridY
+    </sql>
+    <sql id="Base_Column_Alias">
+        a
+        .
+        id
+        ,
+            a.create_time
+            as createTime ,
+            a.update_time
+            as updateTime ,
+            a.task_id
+            as taskId ,
+            a.estimate_grid_x
+            as estimateGridX ,
+            a.estimate_grid_y
+            as estimateGridY ,
+            a.neutron_count
+            as neutronCount ,
+            a.cur_grid_x
+            as curGridX ,
+            a.cur_grid_y
+            as curGridY
+    </sql>
+    <select id="selectTaskGridInfoPage" resultType="com.casic.missiles.modular.robot.model.TaskGridInfo">
+        select
+        <include refid="Base_Column_Alias"/>
+        from task_grid_info a
+        <where>
+            <if test="ew != null">
+                ${ew.sqlSegment}
+            </if>
+        </where>
+    </select>
+    <sql id="Base_Column_Query">
+        <if test="request!=null">
+            <if test="request.taskId!= null and request.taskId!=''">
+                and task_id = #{request.taskId}
+            </if>
+            <if test="request.estimateGridX!= null and request.estimateGridX!=''">
+                and estimate_grid_x = #{request.estimateGridX}
+            </if>
+            <if test="request.estimateGridY!= null and request.estimateGridY!=''">
+                and estimate_grid_y = #{request.estimateGridY}
+            </if>
+            <if test="request.curGridX!= null and request.curGridX!=''">
+                and cur_grid_x = #{request.curGridX}
+            </if>
+            <if test="request.curGridY!= null and request.curGridY!=''">
+                and cur_grid_y = #{request.curGridY}
+            </if>
+        </if>
+    </sql>
+</mapper>
\ No newline at end of file
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java
index 749f559..4559074 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java
@@ -13,4 +13,9 @@
      * 规划点位信息
      */
     private PointNextDTO pointNextDTO;
+    /**
+     * 寻源目标地址
+     */
+    private PointNextDTO coordinate;
+    private boolean isEnd;
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java
index 39ff2a2..cef3238 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java
@@ -12,6 +12,9 @@
     private BigDecimal y;
 
     public static GridPointDTO convertGridPoint(PointNextDTO nextDTO) {
+        if(nextDTO==null){
+            return null;
+        }
         GridPointDTO gridPointDTO = new GridPointDTO();
         gridPointDTO.setX(Convert.toBigDecimal(nextDTO.getX()));
         gridPointDTO.setY(Convert.toBigDecimal(nextDTO.getY()));
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java
index 8c88242..33be25d 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java
@@ -12,25 +12,19 @@
 public class PathInitDTO {
     //---------------------算法参数
     /**
-     * x坐标最大值（x方向栅格总数）
+     * 初始位置x坐标(栅格地图下的坐标)
      */
-    private String xMax;
+    private String xNow;
     /**
-     * y坐标最大值（y方向栅格总数）
+     * 初始位置y坐标(栅格地图下的坐标)
      */
-    private String yMax;
+    private String yNow;
     /**
      * 初始位置计数率
      */
-    private String count;
-    /**
-     * 初始位置x坐标(0<=x<x_max)
-     */
-    private String x;
-    /**
-     * 初始位置y坐标(0<=y<y_max)
-     */
-    private String y;
+    private String zNow;
+
+    private Integer stepLen;
 
     //---------------------其他执行参数
     private String robotId;
@@ -38,11 +32,11 @@
 
     public List<String> toArray() {
         List<String> list = new ArrayList<>();
-        list.add(xMax);
-        list.add(yMax);
-        list.add(count);
-        list.add(x);
-        list.add(y);
+        list.add(xNow);
+        list.add(yNow);
+        list.add(stepLen+"");
+        list.add(zNow);
+        list.add(taskId+"");
         return list;
     }
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java
index 109e95c..ada6774 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java
@@ -10,21 +10,17 @@
  */
 @Data
 public class PathPlanDTO extends PathInitDTO {
-
     /**
-     * 长度为8的数组，记录(x-1, y-1), (x-1, y), (x-1, y+1), (x, y-1), (x, y+1), (x+1, y-1), (x+1, y), (x+1, y+1)8个坐标的可通行信息。
-     * 若某坐标有障碍物（不可通行行）则记为0，否则记为1
+     * 浮点阈值(阈值，超过该值后结束巡检开始寻源)
      */
-    private String open;
-
+    private String th;
     public List<String> toArray() {
         List<String> list = new ArrayList<>();
-        list.add(getXMax());
-        list.add(getYMax());
-        list.add(open);
-        list.add(getCount());
-        list.add(getX());
-        list.add(getY());
+
+        list.add(getStepLen()+"");
+        list.add(getTh());
+        list.add(getZNow());
+        list.add(getTaskId()+"");
         return list;
     }
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskGridInfo.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskGridInfo.java
new file mode 100644
index 0000000..101c10a
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskGridInfo.java
@@ -0,0 +1,83 @@
+package com.casic.missiles.modular.robot.model;
+
+import com.baomidou.mybatisplus.annotation.TableField;
+import com.baomidou.mybatisplus.annotation.TableName;
+import com.baomidou.mybatisplus.extension.activerecord.Model;
+import io.swagger.annotations.ApiModelProperty;
+import lombok.Data;
+
+import java.math.BigDecimal;
+import java.util.Date;
+
+/**
+ * 任务网格信息对象 task_grid_info
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+@Data
+@TableName("task_grid_info")
+public class TaskGridInfo extends Model<TaskGridInfo> {
+    private static final long serialVersionUID = 1L;
+    @TableField(exist = false)
+    private Integer index;
+    @ApiModelProperty(value = "主键", dataType = "Long")
+    @TableField("id")
+    private Long id;
+
+    @ApiModelProperty(value = "创建时间", dataType = "Date")
+    @TableField("create_time")
+    private Date createTime;
+
+    @ApiModelProperty(value = "修改时间", dataType = "Date")
+    @TableField("update_time")
+    private Date updateTime;
+
+    @ApiModelProperty(value = "任务ID", dataType = "Long")
+    @TableField("task_id")
+    private Long taskId;
+
+    @ApiModelProperty(value = "估算栅格x", dataType = "Long")
+    @TableField("estimate_grid_x")
+    private String estimateGridX;
+
+    @ApiModelProperty(value = "估算栅格y", dataType = "Long")
+    @TableField("estimate_grid_y")
+    private String estimateGridY;
+
+    @ApiModelProperty(value = "中子计数率", dataType = "BigDecimal")
+    @TableField("neutron_count")
+    private BigDecimal neutronCount;
+
+    @ApiModelProperty(value = "当前栅格x", dataType = "Long")
+    @TableField("cur_grid_x")
+    private Integer curGridX;
+
+    @ApiModelProperty(value = "当前栅格y", dataType = "Long")
+    @TableField("cur_grid_y")
+    private Integer curGridY;
+
+    public String getCurPoint(){
+        return "("+curGridX+","+curGridY+")";
+    }
+    public String getEstimatePoint(){
+        if(estimateGridX==null&estimateGridY==null){
+            return "结束";
+        }
+        return "("+estimateGridX+","+estimateGridY+")";
+    }
+    @Override
+    public String toString() {
+        return "TaskGridInfo{"
+                + "id=" + id
+                + "createTime=" + createTime
+                + "updateTime=" + updateTime
+                + "taskId=" + taskId
+                + "estimateGridX=" + estimateGridX
+                + "estimateGridY=" + estimateGridY
+                + "neutronCount=" + neutronCount
+                + "curGridX=" + curGridX
+                + "curGridY=" + curGridY
+                + "}";
+    }
+}
\ No newline at end of file
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java
index edc104e..a8acb2a 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java
@@ -53,6 +53,13 @@
     @TableField("route_id")
     private Long routeId;
 
+    @ApiModelProperty(value = "寻源初始步长", dataType = "Integer")
+    @TableField("step_len")
+    private Integer stepLen;
+
+    @ApiModelProperty(value = "寻源阈值设定", dataType = "BigDecimal")
+    @TableField("max_threshold")
+    private BigDecimal maxThreshold;
     /**
      * 任务类型(1巡检任务)
      */
@@ -62,7 +69,6 @@
     @ApiModelProperty(value = "任务类型字典名称", dataType = "String")
     @TableField(exist = false)
     private String taskTypeName;
-
     @ApiModelProperty(value = "识别类型(discernType 1中子源识别)", dataType = "Integer")
     @TableField("discern_type")
     private Integer discernType;
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java
index c4bc25d..c6620f3 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java
@@ -53,7 +53,7 @@
     }),
     SET_SPEED("/set_speed" , "/set_speed_response" , "最大运动速度设置" , true, new TypeReference<RobotMsgResponseDTO<Integer>>() {
     }),
-    SET_TARGETPOINT("/set_targetpoint" , "/set_targetpoint_response" , "设置导航目标点" , false, new TypeReference<RobotMsgResponseDTO<Integer>>() {
+    SET_TARGETPOINT("/set_targetpoint" , "/set_targetpoint_response" , "设置导航目标点" , true, new TypeReference<RobotMsgResponseDTO<Integer>>() {
     }),
 
     GET_DATA_LIST("/get_data_list" , "/get_data_list_response" , "获取数据包列表" , true, new TypeReference<RobotMsgResponseDTO<List<String>>>() {
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/AbstractSocketHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/AbstractSocketHandler.java
index 83f4cb2..e47e343 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/AbstractSocketHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/AbstractSocketHandler.java
@@ -49,7 +49,7 @@
                 wsMsgBaseDTO.setMsgKey(enums.getMsgKey());
                 wsMsgBaseDTO.setData(jsonObject.get("msg"));
                 String msg = JSON.toJSONString(wsMsgBaseDTO);
-                log.info("推送{}消息：{}", enums.getRemarks(), msg);
+//                log.info("推送{}消息：{}", enums.getRemarks(), msg);
                 webSocket.sendOneMessage(robotId, msg);
             }
         } catch (Exception e) {
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/cmd/TargetPointCmdHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/cmd/TargetPointCmdHandler.java
index c1c09fa..43b4460 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/cmd/TargetPointCmdHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/cmd/TargetPointCmdHandler.java
@@ -34,7 +34,11 @@
     @Override
     public ResponseDataDTO senderCmd(TargetPointRequest request, InstructCodeEnums enums) {
         MessageRequestDTO<TargetPointRequest> messageDTO = new MessageRequestDTO();
-        //读取默认关键点信息
+        //读取默认关键点信息 {"x":"1.715708819927424","y":"-0.9100042544250901","z":"0.017993879527185876","theta":"-0.039278239414312105"}
+//        request.setX("1.715708819927424");
+//        request.setY("-0.9100042544250901");
+//        request.setZ("0.017993879527185876");
+//        request.setTheta("-0.039278239414312105");
         messageDTO.setMsg(request);
         ResponseDataDTO<Integer> list = optService.robotCmd(getRobotInfo(), enums, messageDTO);
         return list;
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/AppCostMapHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/AppCostMapHandler.java
index 0335693..88c9f79 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/AppCostMapHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/AppCostMapHandler.java
@@ -1,6 +1,11 @@
 package com.casic.missiles.modular.robot.opt.handler.msg;
 
+import com.casic.missiles.core.cache.CacheUtil;
+import com.casic.missiles.modular.constants.RobotDictConstants;
+import com.casic.missiles.modular.robot.IBaseRobotService;
+import com.casic.missiles.modular.robot.opt.enums.InstructCodeEnums;
 import com.casic.missiles.modular.robot.opt.handler.AbstractSocketHandler;
+import com.casic.missiles.modular.robot.opt.instruct.dto.CostMapResolutionRequest;
 import com.casic.missiles.modular.robot.service.IRobotStatusInfoService;
 import lombok.extern.slf4j.Slf4j;
 import org.eclipse.paho.client.mqttv3.MqttMessage;
@@ -16,14 +21,27 @@
 @Component("/app_costmap_sender")
 public class AppCostMapHandler extends AbstractSocketHandler {
     private final IRobotStatusInfoService statusInfoService;
+    private final IBaseRobotService baseRobotService;
     private static Map<String, Integer> index = new ConcurrentHashMap<>();
 
-    public AppCostMapHandler(IRobotStatusInfoService statusInfoService) {
+    public AppCostMapHandler(IRobotStatusInfoService statusInfoService, IBaseRobotService baseRobotService) {
         this.statusInfoService = statusInfoService;
+        this.baseRobotService = baseRobotService;
     }
 
     @Override
     public void process(String robotId, String topic, MqttMessage message) {
         statusInfoService.saveCostMap(robotId, message);
+
+        CostMapResolutionRequest costMapSet = CacheUtil.get(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CONFIG_COSTMAP_RESOLUTION_SET +robotId);
+        if(costMapSet != null){
+            CacheUtil.remove(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CONFIG_COSTMAP_RESOLUTION_SET +robotId);
+            new Thread(() -> {
+                costMapSet.setRobotId(Long.valueOf(robotId));
+                baseRobotService.execCmdHandler(costMapSet, InstructCodeEnums.COSTMAP_RESOLUTION);
+            }).start();
+
+        }
+
     }
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/PoseMessageMsgHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/PoseMessageMsgHandler.java
index 29f8919..6c3c3bd 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/PoseMessageMsgHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/PoseMessageMsgHandler.java
@@ -17,11 +17,9 @@
 @Component("/pose_message")
 public class PoseMessageMsgHandler extends AbstractSocketHandler {
 
-
     private final EventPublisher publisher;
 
     public PoseMessageMsgHandler(EventPublisher publisher) {
-
         this.publisher = publisher;
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/StopStateMsgHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/StopStateMsgHandler.java
index 8950815..8a71e0b 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/StopStateMsgHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/StopStateMsgHandler.java
@@ -30,7 +30,7 @@
     public void process(String robotId, String topic, MqttMessage message) {
 
         index.put(index_key, index.get("index") + 1);
-        if (index.get(index_key)>50) {
+        if (index.get(index_key)>30) {
             index.put(index_key, 0);
             log.info("主键：{},停障状态topic:{},消息内容:{}", robotId, topic, message.toString());
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/impl/RobotOptServiceImpl.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/impl/RobotOptServiceImpl.java
index 8471e7a..7940a24 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/impl/RobotOptServiceImpl.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/impl/RobotOptServiceImpl.java
@@ -146,7 +146,7 @@
     public void startTrack(StartTrackRequest request) {
         //增加循迹任务记录
         taskInfoService.reloadTask(request, robotInfoService.getById(request.getRobotId()));
-        this.execCmdHandler(request, InstructCodeEnums.START_TRACK);
+//        this.execCmdHandler(request, InstructCodeEnums.START_TRACK);
     }
 
     public IRobotRouteOptService getRobotRouteOptService() {
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/BaseRobotCmdDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/BaseRobotCmdDTO.java
index 5608f2b..06b53de 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/BaseRobotCmdDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/BaseRobotCmdDTO.java
@@ -3,11 +3,13 @@
 import io.swagger.annotations.ApiModelProperty;
 import lombok.Data;
 
+import java.io.Serializable;
+
 /**
  * 机器人指令基类
  */
 @Data
-public class BaseRobotCmdDTO {
+public class BaseRobotCmdDTO implements Serializable {
     @ApiModelProperty(value = "机器人主键", dataType = "String")
     protected Long robotId;
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/StartTrackRequest.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/StartTrackRequest.java
index a18321b..85546a7 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/StartTrackRequest.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/StartTrackRequest.java
@@ -3,6 +3,8 @@
 import io.swagger.annotations.ApiModelProperty;
 import lombok.Data;
 
+import java.math.BigDecimal;
+
 /**
  * 导航下发指令
  */
@@ -10,18 +12,19 @@
 public class StartTrackRequest extends BaseRobotCmdDTO {
     @ApiModelProperty(value = "sn编码(可不填 默认机器ID)", dataType = "String")
     private String sn;
-
     @ApiModelProperty(value = "下发状态 1:开启 0:关闭", dataType = "String")
     private Integer action;
     @ApiModelProperty(value = "等待状态 1:等待 0:不等待", dataType = "String")
     private Integer wait;
     @ApiModelProperty(value = "等待状态 0:循迹 1:任务", dataType = "isTask")
     private Integer isTask;
-    @ApiModelProperty(value = "轨迹名称", dataType = "String")
+    @ApiModelProperty(value = "轨迹名称(废弃)", dataType = "String")
     private String track_name;
     @ApiModelProperty(value = "关键点名称", dataType = "String", hidden = true)
     private String taskpoint_name;
-
+    @ApiModelProperty(value = "初始寻源步长", dataType = "Integer")
+    private Integer stepLen;
+    @ApiModelProperty(value = "寻源阈值", dataType = "BigDecimal")
+    private BigDecimal maxThreshold;
     private String obs_mode="1";
-
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/response/StopStateResponseDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/response/StopStateResponseDTO.java
index eb514bf..e0679d7 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/response/StopStateResponseDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/response/StopStateResponseDTO.java
@@ -3,11 +3,13 @@
 import io.swagger.annotations.ApiModelProperty;
 import lombok.Data;
 
+import java.io.Serializable;
+
 /**
  * 电压信息返回
  */
 @Data
-public class StopStateResponseDTO {
+public class StopStateResponseDTO implements Serializable {
     @ApiModelProperty(value = "软件急停(0：无触发 1：触发)" , dataType = "Integer")
     private Integer soft;
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IAlgorithmService.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IAlgorithmService.java
index 56b3e1d..f62523b 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IAlgorithmService.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IAlgorithmService.java
@@ -6,6 +6,7 @@
 import com.casic.missiles.modular.robot.dto.PathPlanDTO;
 import com.casic.missiles.modular.robot.model.RobotInfo;
 
+import java.io.File;
 import java.math.BigDecimal;
 import java.util.List;
 
@@ -38,5 +39,5 @@
      * @return
      */
     GridPointDTO convertGridCenterPoint(GridPointDTO pointDTO, BigDecimal resolution, RobotInfo robotInfo);
-    void readAlgorithmResponse(String fileName, AlgorithmResponse response, Long taskId, List<String> res);
+    void readAlgorithmResponse(String fileName, File repetitionFile, AlgorithmResponse response, Long taskId, List<String> res);
 }
\ No newline at end of file

diff --git a/.gitignore b/.gitignore
index 9e20bc1..f42a9cb 100644
--- a/.gitignore
+++ b/.gitignore
@@ -2,3 +2,4 @@
 **/target/**
 logs/
 *.iml
+*.exe
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java b/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
index c7b5d28..a849528 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
@@ -28,6 +28,7 @@
     String KEY_CUR_TASK_ID = "CUR_TASK_ID_";
     String KEY_CUR_TASK_INIT = "CUR_TASK_INIT_";
 
+    String KEY_COST_MAP = "KEY_COST_MAP";
     /**
      * PSD数据ID
      */
@@ -41,6 +42,7 @@
      */
     String KEY_CUR_XY_POINT = "KEY_CUR_XY_POINT_";
     String KEY_CUR_ESTIMATE_GRID = "KEY_CUR_ESTIMATE_GRID_";
+    String KEY_PLANING_FLAG = "KEY_PLANING_FLAG_";
     /**
      * 机器人巡点栅格
      */
@@ -55,6 +57,7 @@
      * 探测器配置参数缓存
      */
     String KEY_CONFIG_PARAMS_ID = "KEY_CONFIG_PARAMS_ID_";
+    String KEY_CONFIG_COSTMAP_RESOLUTION_SET = "KEY_CONFIG_COSTMAP_RESOLUTION_SET_";
 
     /**
      * 暂停状态缓存信息
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
index c4c905d..3bb475f 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
@@ -1,9 +1,14 @@
 package com.casic.missiles.modular.robot.controller;
 
+import cn.hutool.core.lang.Assert;
 import cn.hutool.core.util.StrUtil;
 import com.casic.missiles.core.base.controller.BaseController;
+import com.casic.missiles.core.cache.CacheUtil;
 import com.casic.missiles.core.util.SpringContextHolder;
+import com.casic.missiles.model.exception.RequestValidException;
+import com.casic.missiles.model.response.ResponseData;
 import com.casic.missiles.model.response.dto.ResponseDataDTO;
+import com.casic.missiles.modular.constants.RobotDictConstants;
 import com.casic.missiles.modular.robot.IBaseRobotService;
 import com.casic.missiles.modular.robot.dto.RobotInfoSetDTO;
 import com.casic.missiles.modular.robot.model.RobotInfo;
@@ -19,10 +24,16 @@
 import io.swagger.annotations.ApiOperation;
 import io.swagger.annotations.ApiParam;
 import lombok.SneakyThrows;
+import lombok.extern.slf4j.Slf4j;
+import org.apache.ibatis.annotations.Param;
+import org.springframework.beans.factory.annotation.Value;
 import org.springframework.web.bind.annotation.*;
+import org.springframework.web.multipart.MultipartFile;
 
 import javax.servlet.http.HttpServletResponse;
+import java.io.File;
 import java.util.List;
+import java.util.Optional;
 
 /**
  * 机器人操作类
@@ -30,6 +41,7 @@
  * @author lwh
  * @date 2023-10-30
  */
+@Slf4j
 @Api(tags = "机器人控制接口")
 @RestController
 @RequestMapping("/robot/opt")
@@ -61,6 +73,28 @@
         return baseRobotService.execCmdHandler(request, InstructCodeEnums.CHANGE_PCD);
     }
 
+    @Value("${casic.file.uploadPath}")
+    private String filePath;
+
+    @SneakyThrows
+    @ApiOperation(value = "PCD上传接口")
+    @PostMapping(value = "/uploadPcd")
+    public ResponseData uploadPcd(@RequestParam("file") MultipartFile file, @ApiParam("机器人ID,url拼接传参") String robotId) {
+        Assert.isFalse(StrUtil.isEmpty(robotId), () -> new RequestValidException("机器ID不能为空", Optional.of(new Throwable().getStackTrace()[2])));
+        RobotInfo info = robotInfoService.getById(robotId);
+        if (file != null && !file.isEmpty()) {
+            File pcdFile = new File(filePath + "finalCloud_" + info.getId() + ".pcd");
+            try {
+                file.transferTo(pcdFile);
+            } catch (Exception e) {
+                log.error(e.getMessage());
+            }
+            return ResponseData.success();
+        } else {
+            return ResponseData.error("附件上传失败!");
+        }
+    }
+
     @SneakyThrows
     @ApiOperation(value = "PCD下载接口")
     @PostMapping(value = "/pcdDownload")
@@ -136,6 +170,10 @@
     @ApiOperation(value = "启动或关闭循迹功能", notes = "返回 1成功 0失败  wait 启动时是否等待，0代表循迹启动即运动，不等待；1代表循迹启动原地等待。若无此字段，默认循迹启动不等待直接运动")
     @PostMapping(value = "/startTrack")
     public ResponseDataDTO<Integer> startTrack(@RequestBody StartTrackRequest request) {
+        request.setIsTask(1);
+        if (request.getStepLen() == null) {
+            request.setStepLen(2);
+        }
         baseRobotService.startTrack(request);
         return ResponseDataDTO.success();
     }
@@ -156,6 +194,13 @@
         MessageRequestDTO<CrlNavRequest> messageDTO = new MessageRequestDTO();
         messageDTO.setMsg(request);
         ResponseDataDTO<Integer> list = baseRobotService.getRobotRouteOptService().robotCmd(info, InstructCodeEnums.CTRL_NAV, messageDTO);
+        if (request.getAction() == 1) {
+            CostMapResolutionRequest costMap = new CostMapResolutionRequest();
+            costMap.setResolution(info.getResolution().doubleValue());
+            request.setRobotId(request.getRobotId());
+            //是否下发分辨率
+            CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CONFIG_COSTMAP_RESOLUTION_SET + request.getRobotId(), costMap);
+        }
         return list;
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java
new file mode 100644
index 0000000..5317651
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java
@@ -0,0 +1,86 @@
+package com.casic.missiles.modular.robot.controller;
+
+import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
+import com.baomidou.mybatisplus.extension.plugins.pagination.Page;
+import com.casic.missiles.core.base.controller.BaseController;
+import com.casic.missiles.core.page.PageFactory;
+import com.casic.missiles.core.page.PageInfoBT;
+import com.casic.missiles.model.form.IdForms;
+import com.casic.missiles.model.response.dto.ResponseDataDTO;
+import com.casic.missiles.modular.robot.model.TaskGridInfo;
+import com.casic.missiles.modular.robot.service.ITaskGridInfoService;
+import com.casic.missiles.modular.robot.vo.TaskGridInfoVO;
+import io.swagger.annotations.Api;
+import io.swagger.annotations.ApiOperation;
+import org.springframework.web.bind.annotation.*;
+
+import java.util.List;
+
+/**
+ * 任务网格信息Controller
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+@Api(tags = "任务网格信息")
+@RestController
+@RequestMapping("/robot/taskGridInfo")
+public class TaskGridInfoController extends BaseController {
+
+    private final ITaskGridInfoService taskGridInfoService;
+
+    public TaskGridInfoController(ITaskGridInfoService taskGridInfoService) {
+        this.taskGridInfoService = taskGridInfoService;
+    }
+
+    @ApiOperation(value = "列表查询")
+    @GetMapping(value = "/list")
+    public ResponseDataDTO<List<TaskGridInfo>> list(TaskGridInfoVO taskGridInfoVO) {
+        QueryWrapper<TaskGridInfo> query = taskGridInfoVO.genQuery();
+        return ResponseDataDTO.success(taskGridInfoService.list(query));
+    }
+
+    @ApiOperation(value = "分页查询")
+    @GetMapping(value = "/listPage")
+    public ResponseDataDTO<PageInfoBT<TaskGridInfo>> listPage(TaskGridInfoVO taskGridInfoVO) {
+        Page<TaskGridInfo> page = PageFactory.defaultPage();
+        QueryWrapper<TaskGridInfo> query = taskGridInfoVO.genQuery();
+        page = taskGridInfoService.page(page, query);
+        return ResponseDataDTO.success(super.packForBT(page));
+    }
+
+
+    @ApiOperation(value = "新增接口")
+    @PostMapping(value = "/add")
+    public ResponseDataDTO add(@RequestBody TaskGridInfo taskGridInfo) {
+        taskGridInfoService.save(taskGridInfo);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "修改接口")
+    @PostMapping(value = "/update")
+    public ResponseDataDTO update(@RequestBody TaskGridInfo taskGridInfo) {
+        taskGridInfoService.updateById(taskGridInfo);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "删除接口")
+    @GetMapping(value = "/delete")
+    public ResponseDataDTO delete(String id) {
+        taskGridInfoService.removeById(id);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation("批量删除")
+    @PostMapping("/batchDelete")
+    public ResponseDataDTO batchDelete(@RequestBody IdForms ids) {
+        taskGridInfoService.removeByIds(ids.getIds());
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "详情查询")
+    @GetMapping(value = "/detail")
+    public ResponseDataDTO<TaskGridInfo> detail(String id) {
+        return ResponseDataDTO.success(taskGridInfoService.getById(id));
+    }
+}
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
index e82a792..6992274 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
@@ -98,7 +98,7 @@
     public ResponseDataDTO<AlgorithmResponse> test() {
         AlgorithmResponse response = new AlgorithmResponse();
         response.setTaskHeatMap(new TaskHeatMap());
-        algorithmService.readAlgorithmResponse("step3.txt",response,123L,null);
+        algorithmService.readAlgorithmResponse("step3.txt",null,response,123L,null);
         sendWebSocket(InstructStatusMsgEnums.HEAT_MAP, "1", response.getTaskHeatMap());
         return ResponseDataDTO.success(response);
     }
@@ -121,7 +121,7 @@
     @GetMapping(value = "/testGrid")
     public ResponseDataDTO<List<RouteInfo>> testGrid(@RequestBody PathInitDTO pathInitDTO) {
         RobotInfo robotInfo = infoService.getById(pathInitDTO.getRobotId());
-        GridPointDTO gridPointDTO = algorithmService.convertSlamToGrid(new BigDecimal(pathInitDTO.getX()), new BigDecimal(10), new BigDecimal(4), robotInfo);
+        GridPointDTO gridPointDTO = algorithmService.convertSlamToGrid(new BigDecimal(pathInitDTO.getXNow()), new BigDecimal(10), new BigDecimal(4), robotInfo);
         return ResponseDataDTO.success(algorithmService.convertGridCenterPoint(gridPointDTO, new BigDecimal(10), robotInfo));
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/TaskGridInfoMapper.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/TaskGridInfoMapper.java
new file mode 100644
index 0000000..5c3a70b
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/TaskGridInfoMapper.java
@@ -0,0 +1,23 @@
+package com.casic.missiles.modular.robot.dao;
+
+
+import com.baomidou.mybatisplus.core.mapper.BaseMapper;
+import com.casic.missiles.modular.robot.model.TaskGridInfo;
+import org.apache.ibatis.annotations.Param;
+import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
+import com.baomidou.mybatisplus.extension.plugins.pagination.Page;
+import java.util.List;
+/**
+ * <p>
+ *  任务网格信息 Mapper 接口
+ * </p>
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+public interface TaskGridInfoMapper extends BaseMapper<TaskGridInfo> {
+/**
+* 任务网格信息 分页检索
+*/
+List<TaskGridInfo> selectTaskGridInfoPage( @Param("page") Page<TaskGridInfo> page,@Param("ew" ) QueryWrapper<TaskGridInfo> query);
+}
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/mapping/TaskGridInfoMapper.xml b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/mapping/TaskGridInfoMapper.xml
new file mode 100644
index 0000000..be573e0
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/mapping/TaskGridInfoMapper.xml
@@ -0,0 +1,80 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!DOCTYPE mapper PUBLIC "-//mybatis.org//DTD Mapper 3.0//EN" "http://mybatis.org/dtd/mybatis-3-mapper.dtd">
+<mapper namespace="com.casic.missiles.modular.robot.dao.TaskGridInfoMapper">
+    <!-- 通用查询映射结果 -->
+    <resultMap id="BaseResultMap" type="com.casic.missiles.modular.robot.model.TaskGridInfo">
+        <result column="id" property="id"/>
+        <result column="create_time" property="createTime"/>
+        <result column="update_time" property="updateTime"/>
+        <result column="task_id" property="taskId"/>
+        <result column="estimate_grid_x" property="estimateGridX"/>
+        <result column="estimate_grid_y" property="estimateGridY"/>
+        <result column="neutron_count" property="neutronCount"/>
+        <result column="cur_grid_x" property="curGridX"/>
+        <result column="cur_grid_y" property="curGridY"/>
+    </resultMap>
+    <!-- 通用查询结果列 -->
+    <sql id="Base_Column_List">
+        id
+        ,
+                    create_time as createTime ,
+                    update_time as updateTime ,
+                    task_id as taskId ,
+                    estimate_grid_x as estimateGridX ,
+                    estimate_grid_y as estimateGridY ,
+                    neutron_count as neutronCount ,
+                    cur_grid_x as curGridX ,
+                    cur_grid_y as curGridY
+    </sql>
+    <sql id="Base_Column_Alias">
+        a
+        .
+        id
+        ,
+            a.create_time
+            as createTime ,
+            a.update_time
+            as updateTime ,
+            a.task_id
+            as taskId ,
+            a.estimate_grid_x
+            as estimateGridX ,
+            a.estimate_grid_y
+            as estimateGridY ,
+            a.neutron_count
+            as neutronCount ,
+            a.cur_grid_x
+            as curGridX ,
+            a.cur_grid_y
+            as curGridY
+    </sql>
+    <select id="selectTaskGridInfoPage" resultType="com.casic.missiles.modular.robot.model.TaskGridInfo">
+        select
+        <include refid="Base_Column_Alias"/>
+        from task_grid_info a
+        <where>
+            <if test="ew != null">
+                ${ew.sqlSegment}
+            </if>
+        </where>
+    </select>
+    <sql id="Base_Column_Query">
+        <if test="request!=null">
+            <if test="request.taskId!= null and request.taskId!=''">
+                and task_id = #{request.taskId}
+            </if>
+            <if test="request.estimateGridX!= null and request.estimateGridX!=''">
+                and estimate_grid_x = #{request.estimateGridX}
+            </if>
+            <if test="request.estimateGridY!= null and request.estimateGridY!=''">
+                and estimate_grid_y = #{request.estimateGridY}
+            </if>
+            <if test="request.curGridX!= null and request.curGridX!=''">
+                and cur_grid_x = #{request.curGridX}
+            </if>
+            <if test="request.curGridY!= null and request.curGridY!=''">
+                and cur_grid_y = #{request.curGridY}
+            </if>
+        </if>
+    </sql>
+</mapper>
\ No newline at end of file
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java
index 749f559..4559074 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java
@@ -13,4 +13,9 @@
      * 规划点位信息
      */
     private PointNextDTO pointNextDTO;
+    /**
+     * 寻源目标地址
+     */
+    private PointNextDTO coordinate;
+    private boolean isEnd;
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java
index 39ff2a2..cef3238 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java
@@ -12,6 +12,9 @@
     private BigDecimal y;
 
     public static GridPointDTO convertGridPoint(PointNextDTO nextDTO) {
+        if(nextDTO==null){
+            return null;
+        }
         GridPointDTO gridPointDTO = new GridPointDTO();
         gridPointDTO.setX(Convert.toBigDecimal(nextDTO.getX()));
         gridPointDTO.setY(Convert.toBigDecimal(nextDTO.getY()));
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java
index 8c88242..33be25d 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java
@@ -12,25 +12,19 @@
 public class PathInitDTO {
     //---------------------算法参数
     /**
-     * x坐标最大值（x方向栅格总数）
+     * 初始位置x坐标(栅格地图下的坐标)
      */
-    private String xMax;
+    private String xNow;
     /**
-     * y坐标最大值（y方向栅格总数）
+     * 初始位置y坐标(栅格地图下的坐标)
      */
-    private String yMax;
+    private String yNow;
     /**
      * 初始位置计数率
      */
-    private String count;
-    /**
-     * 初始位置x坐标(0<=x<x_max)
-     */
-    private String x;
-    /**
-     * 初始位置y坐标(0<=y<y_max)
-     */
-    private String y;
+    private String zNow;
+
+    private Integer stepLen;
 
     //---------------------其他执行参数
     private String robotId;
@@ -38,11 +32,11 @@
 
     public List<String> toArray() {
         List<String> list = new ArrayList<>();
-        list.add(xMax);
-        list.add(yMax);
-        list.add(count);
-        list.add(x);
-        list.add(y);
+        list.add(xNow);
+        list.add(yNow);
+        list.add(stepLen+"");
+        list.add(zNow);
+        list.add(taskId+"");
         return list;
     }
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java
index 109e95c..ada6774 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java
@@ -10,21 +10,17 @@
  */
 @Data
 public class PathPlanDTO extends PathInitDTO {
-
     /**
-     * 长度为8的数组，记录(x-1, y-1), (x-1, y), (x-1, y+1), (x, y-1), (x, y+1), (x+1, y-1), (x+1, y), (x+1, y+1)8个坐标的可通行信息。
-     * 若某坐标有障碍物（不可通行行）则记为0，否则记为1
+     * 浮点阈值(阈值，超过该值后结束巡检开始寻源)
      */
-    private String open;
-
+    private String th;
     public List<String> toArray() {
         List<String> list = new ArrayList<>();
-        list.add(getXMax());
-        list.add(getYMax());
-        list.add(open);
-        list.add(getCount());
-        list.add(getX());
-        list.add(getY());
+
+        list.add(getStepLen()+"");
+        list.add(getTh());
+        list.add(getZNow());
+        list.add(getTaskId()+"");
         return list;
     }
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskGridInfo.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskGridInfo.java
new file mode 100644
index 0000000..101c10a
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskGridInfo.java
@@ -0,0 +1,83 @@
+package com.casic.missiles.modular.robot.model;
+
+import com.baomidou.mybatisplus.annotation.TableField;
+import com.baomidou.mybatisplus.annotation.TableName;
+import com.baomidou.mybatisplus.extension.activerecord.Model;
+import io.swagger.annotations.ApiModelProperty;
+import lombok.Data;
+
+import java.math.BigDecimal;
+import java.util.Date;
+
+/**
+ * 任务网格信息对象 task_grid_info
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+@Data
+@TableName("task_grid_info")
+public class TaskGridInfo extends Model<TaskGridInfo> {
+    private static final long serialVersionUID = 1L;
+    @TableField(exist = false)
+    private Integer index;
+    @ApiModelProperty(value = "主键", dataType = "Long")
+    @TableField("id")
+    private Long id;
+
+    @ApiModelProperty(value = "创建时间", dataType = "Date")
+    @TableField("create_time")
+    private Date createTime;
+
+    @ApiModelProperty(value = "修改时间", dataType = "Date")
+    @TableField("update_time")
+    private Date updateTime;
+
+    @ApiModelProperty(value = "任务ID", dataType = "Long")
+    @TableField("task_id")
+    private Long taskId;
+
+    @ApiModelProperty(value = "估算栅格x", dataType = "Long")
+    @TableField("estimate_grid_x")
+    private String estimateGridX;
+
+    @ApiModelProperty(value = "估算栅格y", dataType = "Long")
+    @TableField("estimate_grid_y")
+    private String estimateGridY;
+
+    @ApiModelProperty(value = "中子计数率", dataType = "BigDecimal")
+    @TableField("neutron_count")
+    private BigDecimal neutronCount;
+
+    @ApiModelProperty(value = "当前栅格x", dataType = "Long")
+    @TableField("cur_grid_x")
+    private Integer curGridX;
+
+    @ApiModelProperty(value = "当前栅格y", dataType = "Long")
+    @TableField("cur_grid_y")
+    private Integer curGridY;
+
+    public String getCurPoint(){
+        return "("+curGridX+","+curGridY+")";
+    }
+    public String getEstimatePoint(){
+        if(estimateGridX==null&estimateGridY==null){
+            return "结束";
+        }
+        return "("+estimateGridX+","+estimateGridY+")";
+    }
+    @Override
+    public String toString() {
+        return "TaskGridInfo{"
+                + "id=" + id
+                + "createTime=" + createTime
+                + "updateTime=" + updateTime
+                + "taskId=" + taskId
+                + "estimateGridX=" + estimateGridX
+                + "estimateGridY=" + estimateGridY
+                + "neutronCount=" + neutronCount
+                + "curGridX=" + curGridX
+                + "curGridY=" + curGridY
+                + "}";
+    }
+}
\ No newline at end of file
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java
index edc104e..a8acb2a 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java
@@ -53,6 +53,13 @@
     @TableField("route_id")
     private Long routeId;
 
+    @ApiModelProperty(value = "寻源初始步长", dataType = "Integer")
+    @TableField("step_len")
+    private Integer stepLen;
+
+    @ApiModelProperty(value = "寻源阈值设定", dataType = "BigDecimal")
+    @TableField("max_threshold")
+    private BigDecimal maxThreshold;
     /**
      * 任务类型(1巡检任务)
      */
@@ -62,7 +69,6 @@
     @ApiModelProperty(value = "任务类型字典名称", dataType = "String")
     @TableField(exist = false)
     private String taskTypeName;
-
     @ApiModelProperty(value = "识别类型(discernType 1中子源识别)", dataType = "Integer")
     @TableField("discern_type")
     private Integer discernType;
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java
index c4bc25d..c6620f3 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java
@@ -53,7 +53,7 @@
     }),
     SET_SPEED("/set_speed" , "/set_speed_response" , "最大运动速度设置" , true, new TypeReference<RobotMsgResponseDTO<Integer>>() {
     }),
-    SET_TARGETPOINT("/set_targetpoint" , "/set_targetpoint_response" , "设置导航目标点" , false, new TypeReference<RobotMsgResponseDTO<Integer>>() {
+    SET_TARGETPOINT("/set_targetpoint" , "/set_targetpoint_response" , "设置导航目标点" , true, new TypeReference<RobotMsgResponseDTO<Integer>>() {
     }),
 
     GET_DATA_LIST("/get_data_list" , "/get_data_list_response" , "获取数据包列表" , true, new TypeReference<RobotMsgResponseDTO<List<String>>>() {
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/AbstractSocketHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/AbstractSocketHandler.java
index 83f4cb2..e47e343 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/AbstractSocketHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/AbstractSocketHandler.java
@@ -49,7 +49,7 @@
                 wsMsgBaseDTO.setMsgKey(enums.getMsgKey());
                 wsMsgBaseDTO.setData(jsonObject.get("msg"));
                 String msg = JSON.toJSONString(wsMsgBaseDTO);
-                log.info("推送{}消息：{}", enums.getRemarks(), msg);
+//                log.info("推送{}消息：{}", enums.getRemarks(), msg);
                 webSocket.sendOneMessage(robotId, msg);
             }
         } catch (Exception e) {
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/cmd/TargetPointCmdHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/cmd/TargetPointCmdHandler.java
index c1c09fa..43b4460 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/cmd/TargetPointCmdHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/cmd/TargetPointCmdHandler.java
@@ -34,7 +34,11 @@
     @Override
     public ResponseDataDTO senderCmd(TargetPointRequest request, InstructCodeEnums enums) {
         MessageRequestDTO<TargetPointRequest> messageDTO = new MessageRequestDTO();
-        //读取默认关键点信息
+        //读取默认关键点信息 {"x":"1.715708819927424","y":"-0.9100042544250901","z":"0.017993879527185876","theta":"-0.039278239414312105"}
+//        request.setX("1.715708819927424");
+//        request.setY("-0.9100042544250901");
+//        request.setZ("0.017993879527185876");
+//        request.setTheta("-0.039278239414312105");
         messageDTO.setMsg(request);
         ResponseDataDTO<Integer> list = optService.robotCmd(getRobotInfo(), enums, messageDTO);
         return list;
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/AppCostMapHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/AppCostMapHandler.java
index 0335693..88c9f79 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/AppCostMapHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/AppCostMapHandler.java
@@ -1,6 +1,11 @@
 package com.casic.missiles.modular.robot.opt.handler.msg;
 
+import com.casic.missiles.core.cache.CacheUtil;
+import com.casic.missiles.modular.constants.RobotDictConstants;
+import com.casic.missiles.modular.robot.IBaseRobotService;
+import com.casic.missiles.modular.robot.opt.enums.InstructCodeEnums;
 import com.casic.missiles.modular.robot.opt.handler.AbstractSocketHandler;
+import com.casic.missiles.modular.robot.opt.instruct.dto.CostMapResolutionRequest;
 import com.casic.missiles.modular.robot.service.IRobotStatusInfoService;
 import lombok.extern.slf4j.Slf4j;
 import org.eclipse.paho.client.mqttv3.MqttMessage;
@@ -16,14 +21,27 @@
 @Component("/app_costmap_sender")
 public class AppCostMapHandler extends AbstractSocketHandler {
     private final IRobotStatusInfoService statusInfoService;
+    private final IBaseRobotService baseRobotService;
     private static Map<String, Integer> index = new ConcurrentHashMap<>();
 
-    public AppCostMapHandler(IRobotStatusInfoService statusInfoService) {
+    public AppCostMapHandler(IRobotStatusInfoService statusInfoService, IBaseRobotService baseRobotService) {
         this.statusInfoService = statusInfoService;
+        this.baseRobotService = baseRobotService;
     }
 
     @Override
     public void process(String robotId, String topic, MqttMessage message) {
         statusInfoService.saveCostMap(robotId, message);
+
+        CostMapResolutionRequest costMapSet = CacheUtil.get(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CONFIG_COSTMAP_RESOLUTION_SET +robotId);
+        if(costMapSet != null){
+            CacheUtil.remove(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CONFIG_COSTMAP_RESOLUTION_SET +robotId);
+            new Thread(() -> {
+                costMapSet.setRobotId(Long.valueOf(robotId));
+                baseRobotService.execCmdHandler(costMapSet, InstructCodeEnums.COSTMAP_RESOLUTION);
+            }).start();
+
+        }
+
     }
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/PoseMessageMsgHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/PoseMessageMsgHandler.java
index 29f8919..6c3c3bd 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/PoseMessageMsgHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/PoseMessageMsgHandler.java
@@ -17,11 +17,9 @@
 @Component("/pose_message")
 public class PoseMessageMsgHandler extends AbstractSocketHandler {
 
-
     private final EventPublisher publisher;
 
     public PoseMessageMsgHandler(EventPublisher publisher) {
-
         this.publisher = publisher;
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/StopStateMsgHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/StopStateMsgHandler.java
index 8950815..8a71e0b 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/StopStateMsgHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/StopStateMsgHandler.java
@@ -30,7 +30,7 @@
     public void process(String robotId, String topic, MqttMessage message) {
 
         index.put(index_key, index.get("index") + 1);
-        if (index.get(index_key)>50) {
+        if (index.get(index_key)>30) {
             index.put(index_key, 0);
             log.info("主键：{},停障状态topic:{},消息内容:{}", robotId, topic, message.toString());
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/impl/RobotOptServiceImpl.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/impl/RobotOptServiceImpl.java
index 8471e7a..7940a24 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/impl/RobotOptServiceImpl.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/impl/RobotOptServiceImpl.java
@@ -146,7 +146,7 @@
     public void startTrack(StartTrackRequest request) {
         //增加循迹任务记录
         taskInfoService.reloadTask(request, robotInfoService.getById(request.getRobotId()));
-        this.execCmdHandler(request, InstructCodeEnums.START_TRACK);
+//        this.execCmdHandler(request, InstructCodeEnums.START_TRACK);
     }
 
     public IRobotRouteOptService getRobotRouteOptService() {
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/BaseRobotCmdDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/BaseRobotCmdDTO.java
index 5608f2b..06b53de 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/BaseRobotCmdDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/BaseRobotCmdDTO.java
@@ -3,11 +3,13 @@
 import io.swagger.annotations.ApiModelProperty;
 import lombok.Data;
 
+import java.io.Serializable;
+
 /**
  * 机器人指令基类
  */
 @Data
-public class BaseRobotCmdDTO {
+public class BaseRobotCmdDTO implements Serializable {
     @ApiModelProperty(value = "机器人主键", dataType = "String")
     protected Long robotId;
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/StartTrackRequest.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/StartTrackRequest.java
index a18321b..85546a7 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/StartTrackRequest.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/StartTrackRequest.java
@@ -3,6 +3,8 @@
 import io.swagger.annotations.ApiModelProperty;
 import lombok.Data;
 
+import java.math.BigDecimal;
+
 /**
  * 导航下发指令
  */
@@ -10,18 +12,19 @@
 public class StartTrackRequest extends BaseRobotCmdDTO {
     @ApiModelProperty(value = "sn编码(可不填 默认机器ID)", dataType = "String")
     private String sn;
-
     @ApiModelProperty(value = "下发状态 1:开启 0:关闭", dataType = "String")
     private Integer action;
     @ApiModelProperty(value = "等待状态 1:等待 0:不等待", dataType = "String")
     private Integer wait;
     @ApiModelProperty(value = "等待状态 0:循迹 1:任务", dataType = "isTask")
     private Integer isTask;
-    @ApiModelProperty(value = "轨迹名称", dataType = "String")
+    @ApiModelProperty(value = "轨迹名称(废弃)", dataType = "String")
     private String track_name;
     @ApiModelProperty(value = "关键点名称", dataType = "String", hidden = true)
     private String taskpoint_name;
-
+    @ApiModelProperty(value = "初始寻源步长", dataType = "Integer")
+    private Integer stepLen;
+    @ApiModelProperty(value = "寻源阈值", dataType = "BigDecimal")
+    private BigDecimal maxThreshold;
     private String obs_mode="1";
-
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/response/StopStateResponseDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/response/StopStateResponseDTO.java
index eb514bf..e0679d7 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/response/StopStateResponseDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/response/StopStateResponseDTO.java
@@ -3,11 +3,13 @@
 import io.swagger.annotations.ApiModelProperty;
 import lombok.Data;
 
+import java.io.Serializable;
+
 /**
  * 电压信息返回
  */
 @Data
-public class StopStateResponseDTO {
+public class StopStateResponseDTO implements Serializable {
     @ApiModelProperty(value = "软件急停(0：无触发 1：触发)" , dataType = "Integer")
     private Integer soft;
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IAlgorithmService.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IAlgorithmService.java
index 56b3e1d..f62523b 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IAlgorithmService.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IAlgorithmService.java
@@ -6,6 +6,7 @@
 import com.casic.missiles.modular.robot.dto.PathPlanDTO;
 import com.casic.missiles.modular.robot.model.RobotInfo;
 
+import java.io.File;
 import java.math.BigDecimal;
 import java.util.List;
 
@@ -38,5 +39,5 @@
      * @return
      */
     GridPointDTO convertGridCenterPoint(GridPointDTO pointDTO, BigDecimal resolution, RobotInfo robotInfo);
-    void readAlgorithmResponse(String fileName, AlgorithmResponse response, Long taskId, List<String> res);
+    void readAlgorithmResponse(String fileName, File repetitionFile, AlgorithmResponse response, Long taskId, List<String> res);
 }
\ No newline at end of file
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IRobotStatusInfoService.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IRobotStatusInfoService.java
index 2af210a..757f8d0 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IRobotStatusInfoService.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IRobotStatusInfoService.java
@@ -67,10 +67,9 @@
     /**
      * 更新车辆路线及任务信息
      *
-     * @param currRouteId 当前路线信息
      * @param currTaskId  当前任务信息
      */
-    void updateCurrTask(String robotId, Long currRouteId, Long currTaskId);
+    void updateCurrTask(String robotId, Long currTaskId);
 
     RobotStatusInfo getStatusByRobotId(String robotId);
 

diff --git a/.gitignore b/.gitignore
index 9e20bc1..f42a9cb 100644
--- a/.gitignore
+++ b/.gitignore
@@ -2,3 +2,4 @@
 **/target/**
 logs/
 *.iml
+*.exe
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java b/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
index c7b5d28..a849528 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
@@ -28,6 +28,7 @@
     String KEY_CUR_TASK_ID = "CUR_TASK_ID_";
     String KEY_CUR_TASK_INIT = "CUR_TASK_INIT_";
 
+    String KEY_COST_MAP = "KEY_COST_MAP";
     /**
      * PSD数据ID
      */
@@ -41,6 +42,7 @@
      */
     String KEY_CUR_XY_POINT = "KEY_CUR_XY_POINT_";
     String KEY_CUR_ESTIMATE_GRID = "KEY_CUR_ESTIMATE_GRID_";
+    String KEY_PLANING_FLAG = "KEY_PLANING_FLAG_";
     /**
      * 机器人巡点栅格
      */
@@ -55,6 +57,7 @@
      * 探测器配置参数缓存
      */
     String KEY_CONFIG_PARAMS_ID = "KEY_CONFIG_PARAMS_ID_";
+    String KEY_CONFIG_COSTMAP_RESOLUTION_SET = "KEY_CONFIG_COSTMAP_RESOLUTION_SET_";
 
     /**
      * 暂停状态缓存信息
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
index c4c905d..3bb475f 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
@@ -1,9 +1,14 @@
 package com.casic.missiles.modular.robot.controller;
 
+import cn.hutool.core.lang.Assert;
 import cn.hutool.core.util.StrUtil;
 import com.casic.missiles.core.base.controller.BaseController;
+import com.casic.missiles.core.cache.CacheUtil;
 import com.casic.missiles.core.util.SpringContextHolder;
+import com.casic.missiles.model.exception.RequestValidException;
+import com.casic.missiles.model.response.ResponseData;
 import com.casic.missiles.model.response.dto.ResponseDataDTO;
+import com.casic.missiles.modular.constants.RobotDictConstants;
 import com.casic.missiles.modular.robot.IBaseRobotService;
 import com.casic.missiles.modular.robot.dto.RobotInfoSetDTO;
 import com.casic.missiles.modular.robot.model.RobotInfo;
@@ -19,10 +24,16 @@
 import io.swagger.annotations.ApiOperation;
 import io.swagger.annotations.ApiParam;
 import lombok.SneakyThrows;
+import lombok.extern.slf4j.Slf4j;
+import org.apache.ibatis.annotations.Param;
+import org.springframework.beans.factory.annotation.Value;
 import org.springframework.web.bind.annotation.*;
+import org.springframework.web.multipart.MultipartFile;
 
 import javax.servlet.http.HttpServletResponse;
+import java.io.File;
 import java.util.List;
+import java.util.Optional;
 
 /**
  * 机器人操作类
@@ -30,6 +41,7 @@
  * @author lwh
  * @date 2023-10-30
  */
+@Slf4j
 @Api(tags = "机器人控制接口")
 @RestController
 @RequestMapping("/robot/opt")
@@ -61,6 +73,28 @@
         return baseRobotService.execCmdHandler(request, InstructCodeEnums.CHANGE_PCD);
     }
 
+    @Value("${casic.file.uploadPath}")
+    private String filePath;
+
+    @SneakyThrows
+    @ApiOperation(value = "PCD上传接口")
+    @PostMapping(value = "/uploadPcd")
+    public ResponseData uploadPcd(@RequestParam("file") MultipartFile file, @ApiParam("机器人ID,url拼接传参") String robotId) {
+        Assert.isFalse(StrUtil.isEmpty(robotId), () -> new RequestValidException("机器ID不能为空", Optional.of(new Throwable().getStackTrace()[2])));
+        RobotInfo info = robotInfoService.getById(robotId);
+        if (file != null && !file.isEmpty()) {
+            File pcdFile = new File(filePath + "finalCloud_" + info.getId() + ".pcd");
+            try {
+                file.transferTo(pcdFile);
+            } catch (Exception e) {
+                log.error(e.getMessage());
+            }
+            return ResponseData.success();
+        } else {
+            return ResponseData.error("附件上传失败!");
+        }
+    }
+
     @SneakyThrows
     @ApiOperation(value = "PCD下载接口")
     @PostMapping(value = "/pcdDownload")
@@ -136,6 +170,10 @@
     @ApiOperation(value = "启动或关闭循迹功能", notes = "返回 1成功 0失败  wait 启动时是否等待，0代表循迹启动即运动，不等待；1代表循迹启动原地等待。若无此字段，默认循迹启动不等待直接运动")
     @PostMapping(value = "/startTrack")
     public ResponseDataDTO<Integer> startTrack(@RequestBody StartTrackRequest request) {
+        request.setIsTask(1);
+        if (request.getStepLen() == null) {
+            request.setStepLen(2);
+        }
         baseRobotService.startTrack(request);
         return ResponseDataDTO.success();
     }
@@ -156,6 +194,13 @@
         MessageRequestDTO<CrlNavRequest> messageDTO = new MessageRequestDTO();
         messageDTO.setMsg(request);
         ResponseDataDTO<Integer> list = baseRobotService.getRobotRouteOptService().robotCmd(info, InstructCodeEnums.CTRL_NAV, messageDTO);
+        if (request.getAction() == 1) {
+            CostMapResolutionRequest costMap = new CostMapResolutionRequest();
+            costMap.setResolution(info.getResolution().doubleValue());
+            request.setRobotId(request.getRobotId());
+            //是否下发分辨率
+            CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CONFIG_COSTMAP_RESOLUTION_SET + request.getRobotId(), costMap);
+        }
         return list;
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java
new file mode 100644
index 0000000..5317651
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java
@@ -0,0 +1,86 @@
+package com.casic.missiles.modular.robot.controller;
+
+import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
+import com.baomidou.mybatisplus.extension.plugins.pagination.Page;
+import com.casic.missiles.core.base.controller.BaseController;
+import com.casic.missiles.core.page.PageFactory;
+import com.casic.missiles.core.page.PageInfoBT;
+import com.casic.missiles.model.form.IdForms;
+import com.casic.missiles.model.response.dto.ResponseDataDTO;
+import com.casic.missiles.modular.robot.model.TaskGridInfo;
+import com.casic.missiles.modular.robot.service.ITaskGridInfoService;
+import com.casic.missiles.modular.robot.vo.TaskGridInfoVO;
+import io.swagger.annotations.Api;
+import io.swagger.annotations.ApiOperation;
+import org.springframework.web.bind.annotation.*;
+
+import java.util.List;
+
+/**
+ * 任务网格信息Controller
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+@Api(tags = "任务网格信息")
+@RestController
+@RequestMapping("/robot/taskGridInfo")
+public class TaskGridInfoController extends BaseController {
+
+    private final ITaskGridInfoService taskGridInfoService;
+
+    public TaskGridInfoController(ITaskGridInfoService taskGridInfoService) {
+        this.taskGridInfoService = taskGridInfoService;
+    }
+
+    @ApiOperation(value = "列表查询")
+    @GetMapping(value = "/list")
+    public ResponseDataDTO<List<TaskGridInfo>> list(TaskGridInfoVO taskGridInfoVO) {
+        QueryWrapper<TaskGridInfo> query = taskGridInfoVO.genQuery();
+        return ResponseDataDTO.success(taskGridInfoService.list(query));
+    }
+
+    @ApiOperation(value = "分页查询")
+    @GetMapping(value = "/listPage")
+    public ResponseDataDTO<PageInfoBT<TaskGridInfo>> listPage(TaskGridInfoVO taskGridInfoVO) {
+        Page<TaskGridInfo> page = PageFactory.defaultPage();
+        QueryWrapper<TaskGridInfo> query = taskGridInfoVO.genQuery();
+        page = taskGridInfoService.page(page, query);
+        return ResponseDataDTO.success(super.packForBT(page));
+    }
+
+
+    @ApiOperation(value = "新增接口")
+    @PostMapping(value = "/add")
+    public ResponseDataDTO add(@RequestBody TaskGridInfo taskGridInfo) {
+        taskGridInfoService.save(taskGridInfo);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "修改接口")
+    @PostMapping(value = "/update")
+    public ResponseDataDTO update(@RequestBody TaskGridInfo taskGridInfo) {
+        taskGridInfoService.updateById(taskGridInfo);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "删除接口")
+    @GetMapping(value = "/delete")
+    public ResponseDataDTO delete(String id) {
+        taskGridInfoService.removeById(id);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation("批量删除")
+    @PostMapping("/batchDelete")
+    public ResponseDataDTO batchDelete(@RequestBody IdForms ids) {
+        taskGridInfoService.removeByIds(ids.getIds());
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "详情查询")
+    @GetMapping(value = "/detail")
+    public ResponseDataDTO<TaskGridInfo> detail(String id) {
+        return ResponseDataDTO.success(taskGridInfoService.getById(id));
+    }
+}
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
index e82a792..6992274 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
@@ -98,7 +98,7 @@
     public ResponseDataDTO<AlgorithmResponse> test() {
         AlgorithmResponse response = new AlgorithmResponse();
         response.setTaskHeatMap(new TaskHeatMap());
-        algorithmService.readAlgorithmResponse("step3.txt",response,123L,null);
+        algorithmService.readAlgorithmResponse("step3.txt",null,response,123L,null);
         sendWebSocket(InstructStatusMsgEnums.HEAT_MAP, "1", response.getTaskHeatMap());
         return ResponseDataDTO.success(response);
     }
@@ -121,7 +121,7 @@
     @GetMapping(value = "/testGrid")
     public ResponseDataDTO<List<RouteInfo>> testGrid(@RequestBody PathInitDTO pathInitDTO) {
         RobotInfo robotInfo = infoService.getById(pathInitDTO.getRobotId());
-        GridPointDTO gridPointDTO = algorithmService.convertSlamToGrid(new BigDecimal(pathInitDTO.getX()), new BigDecimal(10), new BigDecimal(4), robotInfo);
+        GridPointDTO gridPointDTO = algorithmService.convertSlamToGrid(new BigDecimal(pathInitDTO.getXNow()), new BigDecimal(10), new BigDecimal(4), robotInfo);
         return ResponseDataDTO.success(algorithmService.convertGridCenterPoint(gridPointDTO, new BigDecimal(10), robotInfo));
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/TaskGridInfoMapper.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/TaskGridInfoMapper.java
new file mode 100644
index 0000000..5c3a70b
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/TaskGridInfoMapper.java
@@ -0,0 +1,23 @@
+package com.casic.missiles.modular.robot.dao;
+
+
+import com.baomidou.mybatisplus.core.mapper.BaseMapper;
+import com.casic.missiles.modular.robot.model.TaskGridInfo;
+import org.apache.ibatis.annotations.Param;
+import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
+import com.baomidou.mybatisplus.extension.plugins.pagination.Page;
+import java.util.List;
+/**
+ * <p>
+ *  任务网格信息 Mapper 接口
+ * </p>
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+public interface TaskGridInfoMapper extends BaseMapper<TaskGridInfo> {
+/**
+* 任务网格信息 分页检索
+*/
+List<TaskGridInfo> selectTaskGridInfoPage( @Param("page") Page<TaskGridInfo> page,@Param("ew" ) QueryWrapper<TaskGridInfo> query);
+}
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/mapping/TaskGridInfoMapper.xml b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/mapping/TaskGridInfoMapper.xml
new file mode 100644
index 0000000..be573e0
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/mapping/TaskGridInfoMapper.xml
@@ -0,0 +1,80 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!DOCTYPE mapper PUBLIC "-//mybatis.org//DTD Mapper 3.0//EN" "http://mybatis.org/dtd/mybatis-3-mapper.dtd">
+<mapper namespace="com.casic.missiles.modular.robot.dao.TaskGridInfoMapper">
+    <!-- 通用查询映射结果 -->
+    <resultMap id="BaseResultMap" type="com.casic.missiles.modular.robot.model.TaskGridInfo">
+        <result column="id" property="id"/>
+        <result column="create_time" property="createTime"/>
+        <result column="update_time" property="updateTime"/>
+        <result column="task_id" property="taskId"/>
+        <result column="estimate_grid_x" property="estimateGridX"/>
+        <result column="estimate_grid_y" property="estimateGridY"/>
+        <result column="neutron_count" property="neutronCount"/>
+        <result column="cur_grid_x" property="curGridX"/>
+        <result column="cur_grid_y" property="curGridY"/>
+    </resultMap>
+    <!-- 通用查询结果列 -->
+    <sql id="Base_Column_List">
+        id
+        ,
+                    create_time as createTime ,
+                    update_time as updateTime ,
+                    task_id as taskId ,
+                    estimate_grid_x as estimateGridX ,
+                    estimate_grid_y as estimateGridY ,
+                    neutron_count as neutronCount ,
+                    cur_grid_x as curGridX ,
+                    cur_grid_y as curGridY
+    </sql>
+    <sql id="Base_Column_Alias">
+        a
+        .
+        id
+        ,
+            a.create_time
+            as createTime ,
+            a.update_time
+            as updateTime ,
+            a.task_id
+            as taskId ,
+            a.estimate_grid_x
+            as estimateGridX ,
+            a.estimate_grid_y
+            as estimateGridY ,
+            a.neutron_count
+            as neutronCount ,
+            a.cur_grid_x
+            as curGridX ,
+            a.cur_grid_y
+            as curGridY
+    </sql>
+    <select id="selectTaskGridInfoPage" resultType="com.casic.missiles.modular.robot.model.TaskGridInfo">
+        select
+        <include refid="Base_Column_Alias"/>
+        from task_grid_info a
+        <where>
+            <if test="ew != null">
+                ${ew.sqlSegment}
+            </if>
+        </where>
+    </select>
+    <sql id="Base_Column_Query">
+        <if test="request!=null">
+            <if test="request.taskId!= null and request.taskId!=''">
+                and task_id = #{request.taskId}
+            </if>
+            <if test="request.estimateGridX!= null and request.estimateGridX!=''">
+                and estimate_grid_x = #{request.estimateGridX}
+            </if>
+            <if test="request.estimateGridY!= null and request.estimateGridY!=''">
+                and estimate_grid_y = #{request.estimateGridY}
+            </if>
+            <if test="request.curGridX!= null and request.curGridX!=''">
+                and cur_grid_x = #{request.curGridX}
+            </if>
+            <if test="request.curGridY!= null and request.curGridY!=''">
+                and cur_grid_y = #{request.curGridY}
+            </if>
+        </if>
+    </sql>
+</mapper>
\ No newline at end of file
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java
index 749f559..4559074 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java
@@ -13,4 +13,9 @@
      * 规划点位信息
      */
     private PointNextDTO pointNextDTO;
+    /**
+     * 寻源目标地址
+     */
+    private PointNextDTO coordinate;
+    private boolean isEnd;
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java
index 39ff2a2..cef3238 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java
@@ -12,6 +12,9 @@
     private BigDecimal y;
 
     public static GridPointDTO convertGridPoint(PointNextDTO nextDTO) {
+        if(nextDTO==null){
+            return null;
+        }
         GridPointDTO gridPointDTO = new GridPointDTO();
         gridPointDTO.setX(Convert.toBigDecimal(nextDTO.getX()));
         gridPointDTO.setY(Convert.toBigDecimal(nextDTO.getY()));
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java
index 8c88242..33be25d 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java
@@ -12,25 +12,19 @@
 public class PathInitDTO {
     //---------------------算法参数
     /**
-     * x坐标最大值（x方向栅格总数）
+     * 初始位置x坐标(栅格地图下的坐标)
      */
-    private String xMax;
+    private String xNow;
     /**
-     * y坐标最大值（y方向栅格总数）
+     * 初始位置y坐标(栅格地图下的坐标)
      */
-    private String yMax;
+    private String yNow;
     /**
      * 初始位置计数率
      */
-    private String count;
-    /**
-     * 初始位置x坐标(0<=x<x_max)
-     */
-    private String x;
-    /**
-     * 初始位置y坐标(0<=y<y_max)
-     */
-    private String y;
+    private String zNow;
+
+    private Integer stepLen;
 
     //---------------------其他执行参数
     private String robotId;
@@ -38,11 +32,11 @@
 
     public List<String> toArray() {
         List<String> list = new ArrayList<>();
-        list.add(xMax);
-        list.add(yMax);
-        list.add(count);
-        list.add(x);
-        list.add(y);
+        list.add(xNow);
+        list.add(yNow);
+        list.add(stepLen+"");
+        list.add(zNow);
+        list.add(taskId+"");
         return list;
     }
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java
index 109e95c..ada6774 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java
@@ -10,21 +10,17 @@
  */
 @Data
 public class PathPlanDTO extends PathInitDTO {
-
     /**
-     * 长度为8的数组，记录(x-1, y-1), (x-1, y), (x-1, y+1), (x, y-1), (x, y+1), (x+1, y-1), (x+1, y), (x+1, y+1)8个坐标的可通行信息。
-     * 若某坐标有障碍物（不可通行行）则记为0，否则记为1
+     * 浮点阈值(阈值，超过该值后结束巡检开始寻源)
      */
-    private String open;
-
+    private String th;
     public List<String> toArray() {
         List<String> list = new ArrayList<>();
-        list.add(getXMax());
-        list.add(getYMax());
-        list.add(open);
-        list.add(getCount());
-        list.add(getX());
-        list.add(getY());
+
+        list.add(getStepLen()+"");
+        list.add(getTh());
+        list.add(getZNow());
+        list.add(getTaskId()+"");
         return list;
     }
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskGridInfo.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskGridInfo.java
new file mode 100644
index 0000000..101c10a
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskGridInfo.java
@@ -0,0 +1,83 @@
+package com.casic.missiles.modular.robot.model;
+
+import com.baomidou.mybatisplus.annotation.TableField;
+import com.baomidou.mybatisplus.annotation.TableName;
+import com.baomidou.mybatisplus.extension.activerecord.Model;
+import io.swagger.annotations.ApiModelProperty;
+import lombok.Data;
+
+import java.math.BigDecimal;
+import java.util.Date;
+
+/**
+ * 任务网格信息对象 task_grid_info
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+@Data
+@TableName("task_grid_info")
+public class TaskGridInfo extends Model<TaskGridInfo> {
+    private static final long serialVersionUID = 1L;
+    @TableField(exist = false)
+    private Integer index;
+    @ApiModelProperty(value = "主键", dataType = "Long")
+    @TableField("id")
+    private Long id;
+
+    @ApiModelProperty(value = "创建时间", dataType = "Date")
+    @TableField("create_time")
+    private Date createTime;
+
+    @ApiModelProperty(value = "修改时间", dataType = "Date")
+    @TableField("update_time")
+    private Date updateTime;
+
+    @ApiModelProperty(value = "任务ID", dataType = "Long")
+    @TableField("task_id")
+    private Long taskId;
+
+    @ApiModelProperty(value = "估算栅格x", dataType = "Long")
+    @TableField("estimate_grid_x")
+    private String estimateGridX;
+
+    @ApiModelProperty(value = "估算栅格y", dataType = "Long")
+    @TableField("estimate_grid_y")
+    private String estimateGridY;
+
+    @ApiModelProperty(value = "中子计数率", dataType = "BigDecimal")
+    @TableField("neutron_count")
+    private BigDecimal neutronCount;
+
+    @ApiModelProperty(value = "当前栅格x", dataType = "Long")
+    @TableField("cur_grid_x")
+    private Integer curGridX;
+
+    @ApiModelProperty(value = "当前栅格y", dataType = "Long")
+    @TableField("cur_grid_y")
+    private Integer curGridY;
+
+    public String getCurPoint(){
+        return "("+curGridX+","+curGridY+")";
+    }
+    public String getEstimatePoint(){
+        if(estimateGridX==null&estimateGridY==null){
+            return "结束";
+        }
+        return "("+estimateGridX+","+estimateGridY+")";
+    }
+    @Override
+    public String toString() {
+        return "TaskGridInfo{"
+                + "id=" + id
+                + "createTime=" + createTime
+                + "updateTime=" + updateTime
+                + "taskId=" + taskId
+                + "estimateGridX=" + estimateGridX
+                + "estimateGridY=" + estimateGridY
+                + "neutronCount=" + neutronCount
+                + "curGridX=" + curGridX
+                + "curGridY=" + curGridY
+                + "}";
+    }
+}
\ No newline at end of file
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java
index edc104e..a8acb2a 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java
@@ -53,6 +53,13 @@
     @TableField("route_id")
     private Long routeId;
 
+    @ApiModelProperty(value = "寻源初始步长", dataType = "Integer")
+    @TableField("step_len")
+    private Integer stepLen;
+
+    @ApiModelProperty(value = "寻源阈值设定", dataType = "BigDecimal")
+    @TableField("max_threshold")
+    private BigDecimal maxThreshold;
     /**
      * 任务类型(1巡检任务)
      */
@@ -62,7 +69,6 @@
     @ApiModelProperty(value = "任务类型字典名称", dataType = "String")
     @TableField(exist = false)
     private String taskTypeName;
-
     @ApiModelProperty(value = "识别类型(discernType 1中子源识别)", dataType = "Integer")
     @TableField("discern_type")
     private Integer discernType;
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java
index c4bc25d..c6620f3 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java
@@ -53,7 +53,7 @@
     }),
     SET_SPEED("/set_speed" , "/set_speed_response" , "最大运动速度设置" , true, new TypeReference<RobotMsgResponseDTO<Integer>>() {
     }),
-    SET_TARGETPOINT("/set_targetpoint" , "/set_targetpoint_response" , "设置导航目标点" , false, new TypeReference<RobotMsgResponseDTO<Integer>>() {
+    SET_TARGETPOINT("/set_targetpoint" , "/set_targetpoint_response" , "设置导航目标点" , true, new TypeReference<RobotMsgResponseDTO<Integer>>() {
     }),
 
     GET_DATA_LIST("/get_data_list" , "/get_data_list_response" , "获取数据包列表" , true, new TypeReference<RobotMsgResponseDTO<List<String>>>() {
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/AbstractSocketHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/AbstractSocketHandler.java
index 83f4cb2..e47e343 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/AbstractSocketHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/AbstractSocketHandler.java
@@ -49,7 +49,7 @@
                 wsMsgBaseDTO.setMsgKey(enums.getMsgKey());
                 wsMsgBaseDTO.setData(jsonObject.get("msg"));
                 String msg = JSON.toJSONString(wsMsgBaseDTO);
-                log.info("推送{}消息：{}", enums.getRemarks(), msg);
+//                log.info("推送{}消息：{}", enums.getRemarks(), msg);
                 webSocket.sendOneMessage(robotId, msg);
             }
         } catch (Exception e) {
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/cmd/TargetPointCmdHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/cmd/TargetPointCmdHandler.java
index c1c09fa..43b4460 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/cmd/TargetPointCmdHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/cmd/TargetPointCmdHandler.java
@@ -34,7 +34,11 @@
     @Override
     public ResponseDataDTO senderCmd(TargetPointRequest request, InstructCodeEnums enums) {
         MessageRequestDTO<TargetPointRequest> messageDTO = new MessageRequestDTO();
-        //读取默认关键点信息
+        //读取默认关键点信息 {"x":"1.715708819927424","y":"-0.9100042544250901","z":"0.017993879527185876","theta":"-0.039278239414312105"}
+//        request.setX("1.715708819927424");
+//        request.setY("-0.9100042544250901");
+//        request.setZ("0.017993879527185876");
+//        request.setTheta("-0.039278239414312105");
         messageDTO.setMsg(request);
         ResponseDataDTO<Integer> list = optService.robotCmd(getRobotInfo(), enums, messageDTO);
         return list;
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/AppCostMapHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/AppCostMapHandler.java
index 0335693..88c9f79 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/AppCostMapHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/AppCostMapHandler.java
@@ -1,6 +1,11 @@
 package com.casic.missiles.modular.robot.opt.handler.msg;
 
+import com.casic.missiles.core.cache.CacheUtil;
+import com.casic.missiles.modular.constants.RobotDictConstants;
+import com.casic.missiles.modular.robot.IBaseRobotService;
+import com.casic.missiles.modular.robot.opt.enums.InstructCodeEnums;
 import com.casic.missiles.modular.robot.opt.handler.AbstractSocketHandler;
+import com.casic.missiles.modular.robot.opt.instruct.dto.CostMapResolutionRequest;
 import com.casic.missiles.modular.robot.service.IRobotStatusInfoService;
 import lombok.extern.slf4j.Slf4j;
 import org.eclipse.paho.client.mqttv3.MqttMessage;
@@ -16,14 +21,27 @@
 @Component("/app_costmap_sender")
 public class AppCostMapHandler extends AbstractSocketHandler {
     private final IRobotStatusInfoService statusInfoService;
+    private final IBaseRobotService baseRobotService;
     private static Map<String, Integer> index = new ConcurrentHashMap<>();
 
-    public AppCostMapHandler(IRobotStatusInfoService statusInfoService) {
+    public AppCostMapHandler(IRobotStatusInfoService statusInfoService, IBaseRobotService baseRobotService) {
         this.statusInfoService = statusInfoService;
+        this.baseRobotService = baseRobotService;
     }
 
     @Override
     public void process(String robotId, String topic, MqttMessage message) {
         statusInfoService.saveCostMap(robotId, message);
+
+        CostMapResolutionRequest costMapSet = CacheUtil.get(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CONFIG_COSTMAP_RESOLUTION_SET +robotId);
+        if(costMapSet != null){
+            CacheUtil.remove(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CONFIG_COSTMAP_RESOLUTION_SET +robotId);
+            new Thread(() -> {
+                costMapSet.setRobotId(Long.valueOf(robotId));
+                baseRobotService.execCmdHandler(costMapSet, InstructCodeEnums.COSTMAP_RESOLUTION);
+            }).start();
+
+        }
+
     }
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/PoseMessageMsgHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/PoseMessageMsgHandler.java
index 29f8919..6c3c3bd 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/PoseMessageMsgHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/PoseMessageMsgHandler.java
@@ -17,11 +17,9 @@
 @Component("/pose_message")
 public class PoseMessageMsgHandler extends AbstractSocketHandler {
 
-
     private final EventPublisher publisher;
 
     public PoseMessageMsgHandler(EventPublisher publisher) {
-
         this.publisher = publisher;
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/StopStateMsgHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/StopStateMsgHandler.java
index 8950815..8a71e0b 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/StopStateMsgHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/StopStateMsgHandler.java
@@ -30,7 +30,7 @@
     public void process(String robotId, String topic, MqttMessage message) {
 
         index.put(index_key, index.get("index") + 1);
-        if (index.get(index_key)>50) {
+        if (index.get(index_key)>30) {
             index.put(index_key, 0);
             log.info("主键：{},停障状态topic:{},消息内容:{}", robotId, topic, message.toString());
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/impl/RobotOptServiceImpl.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/impl/RobotOptServiceImpl.java
index 8471e7a..7940a24 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/impl/RobotOptServiceImpl.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/impl/RobotOptServiceImpl.java
@@ -146,7 +146,7 @@
     public void startTrack(StartTrackRequest request) {
         //增加循迹任务记录
         taskInfoService.reloadTask(request, robotInfoService.getById(request.getRobotId()));
-        this.execCmdHandler(request, InstructCodeEnums.START_TRACK);
+//        this.execCmdHandler(request, InstructCodeEnums.START_TRACK);
     }
 
     public IRobotRouteOptService getRobotRouteOptService() {
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/BaseRobotCmdDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/BaseRobotCmdDTO.java
index 5608f2b..06b53de 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/BaseRobotCmdDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/BaseRobotCmdDTO.java
@@ -3,11 +3,13 @@
 import io.swagger.annotations.ApiModelProperty;
 import lombok.Data;
 
+import java.io.Serializable;
+
 /**
  * 机器人指令基类
  */
 @Data
-public class BaseRobotCmdDTO {
+public class BaseRobotCmdDTO implements Serializable {
     @ApiModelProperty(value = "机器人主键", dataType = "String")
     protected Long robotId;
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/StartTrackRequest.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/StartTrackRequest.java
index a18321b..85546a7 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/StartTrackRequest.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/StartTrackRequest.java
@@ -3,6 +3,8 @@
 import io.swagger.annotations.ApiModelProperty;
 import lombok.Data;
 
+import java.math.BigDecimal;
+
 /**
  * 导航下发指令
  */
@@ -10,18 +12,19 @@
 public class StartTrackRequest extends BaseRobotCmdDTO {
     @ApiModelProperty(value = "sn编码(可不填 默认机器ID)", dataType = "String")
     private String sn;
-
     @ApiModelProperty(value = "下发状态 1:开启 0:关闭", dataType = "String")
     private Integer action;
     @ApiModelProperty(value = "等待状态 1:等待 0:不等待", dataType = "String")
     private Integer wait;
     @ApiModelProperty(value = "等待状态 0:循迹 1:任务", dataType = "isTask")
     private Integer isTask;
-    @ApiModelProperty(value = "轨迹名称", dataType = "String")
+    @ApiModelProperty(value = "轨迹名称(废弃)", dataType = "String")
     private String track_name;
     @ApiModelProperty(value = "关键点名称", dataType = "String", hidden = true)
     private String taskpoint_name;
-
+    @ApiModelProperty(value = "初始寻源步长", dataType = "Integer")
+    private Integer stepLen;
+    @ApiModelProperty(value = "寻源阈值", dataType = "BigDecimal")
+    private BigDecimal maxThreshold;
     private String obs_mode="1";
-
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/response/StopStateResponseDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/response/StopStateResponseDTO.java
index eb514bf..e0679d7 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/response/StopStateResponseDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/response/StopStateResponseDTO.java
@@ -3,11 +3,13 @@
 import io.swagger.annotations.ApiModelProperty;
 import lombok.Data;
 
+import java.io.Serializable;
+
 /**
  * 电压信息返回
  */
 @Data
-public class StopStateResponseDTO {
+public class StopStateResponseDTO implements Serializable {
     @ApiModelProperty(value = "软件急停(0：无触发 1：触发)" , dataType = "Integer")
     private Integer soft;
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IAlgorithmService.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IAlgorithmService.java
index 56b3e1d..f62523b 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IAlgorithmService.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IAlgorithmService.java
@@ -6,6 +6,7 @@
 import com.casic.missiles.modular.robot.dto.PathPlanDTO;
 import com.casic.missiles.modular.robot.model.RobotInfo;
 
+import java.io.File;
 import java.math.BigDecimal;
 import java.util.List;
 
@@ -38,5 +39,5 @@
      * @return
      */
     GridPointDTO convertGridCenterPoint(GridPointDTO pointDTO, BigDecimal resolution, RobotInfo robotInfo);
-    void readAlgorithmResponse(String fileName, AlgorithmResponse response, Long taskId, List<String> res);
+    void readAlgorithmResponse(String fileName, File repetitionFile, AlgorithmResponse response, Long taskId, List<String> res);
 }
\ No newline at end of file
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IRobotStatusInfoService.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IRobotStatusInfoService.java
index 2af210a..757f8d0 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IRobotStatusInfoService.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IRobotStatusInfoService.java
@@ -67,10 +67,9 @@
     /**
      * 更新车辆路线及任务信息
      *
-     * @param currRouteId 当前路线信息
      * @param currTaskId  当前任务信息
      */
-    void updateCurrTask(String robotId, Long currRouteId, Long currTaskId);
+    void updateCurrTask(String robotId, Long currTaskId);
 
     RobotStatusInfo getStatusByRobotId(String robotId);
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/ITaskGridInfoService.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/ITaskGridInfoService.java
new file mode 100644
index 0000000..fc0da93
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/ITaskGridInfoService.java
@@ -0,0 +1,22 @@
+package com.casic.missiles.modular.robot.service;
+
+import com.baomidou.mybatisplus.extension.service.IService;
+import  com.casic.missiles.modular.robot.model.TaskGridInfo;
+import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
+import com.baomidou.mybatisplus.extension.plugins.pagination.Page;
+import java.util.List;
+
+
+/**
+ * <p>
+ * 任务网格信息 服务类
+ * </p>
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+public interface ITaskGridInfoService extends IService<TaskGridInfo> {
+/**
+* 任务网格信息 分页检索
+*/
+List<TaskGridInfo> selectTaskGridInfoPage(Page<TaskGridInfo> page,QueryWrapper<TaskGridInfo> query);}

diff --git a/.gitignore b/.gitignore
index 9e20bc1..f42a9cb 100644
--- a/.gitignore
+++ b/.gitignore
@@ -2,3 +2,4 @@
 **/target/**
 logs/
 *.iml
+*.exe
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java b/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
index c7b5d28..a849528 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
@@ -28,6 +28,7 @@
     String KEY_CUR_TASK_ID = "CUR_TASK_ID_";
     String KEY_CUR_TASK_INIT = "CUR_TASK_INIT_";
 
+    String KEY_COST_MAP = "KEY_COST_MAP";
     /**
      * PSD数据ID
      */
@@ -41,6 +42,7 @@
      */
     String KEY_CUR_XY_POINT = "KEY_CUR_XY_POINT_";
     String KEY_CUR_ESTIMATE_GRID = "KEY_CUR_ESTIMATE_GRID_";
+    String KEY_PLANING_FLAG = "KEY_PLANING_FLAG_";
     /**
      * 机器人巡点栅格
      */
@@ -55,6 +57,7 @@
      * 探测器配置参数缓存
      */
     String KEY_CONFIG_PARAMS_ID = "KEY_CONFIG_PARAMS_ID_";
+    String KEY_CONFIG_COSTMAP_RESOLUTION_SET = "KEY_CONFIG_COSTMAP_RESOLUTION_SET_";
 
     /**
      * 暂停状态缓存信息
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
index c4c905d..3bb475f 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
@@ -1,9 +1,14 @@
 package com.casic.missiles.modular.robot.controller;
 
+import cn.hutool.core.lang.Assert;
 import cn.hutool.core.util.StrUtil;
 import com.casic.missiles.core.base.controller.BaseController;
+import com.casic.missiles.core.cache.CacheUtil;
 import com.casic.missiles.core.util.SpringContextHolder;
+import com.casic.missiles.model.exception.RequestValidException;
+import com.casic.missiles.model.response.ResponseData;
 import com.casic.missiles.model.response.dto.ResponseDataDTO;
+import com.casic.missiles.modular.constants.RobotDictConstants;
 import com.casic.missiles.modular.robot.IBaseRobotService;
 import com.casic.missiles.modular.robot.dto.RobotInfoSetDTO;
 import com.casic.missiles.modular.robot.model.RobotInfo;
@@ -19,10 +24,16 @@
 import io.swagger.annotations.ApiOperation;
 import io.swagger.annotations.ApiParam;
 import lombok.SneakyThrows;
+import lombok.extern.slf4j.Slf4j;
+import org.apache.ibatis.annotations.Param;
+import org.springframework.beans.factory.annotation.Value;
 import org.springframework.web.bind.annotation.*;
+import org.springframework.web.multipart.MultipartFile;
 
 import javax.servlet.http.HttpServletResponse;
+import java.io.File;
 import java.util.List;
+import java.util.Optional;
 
 /**
  * 机器人操作类
@@ -30,6 +41,7 @@
  * @author lwh
  * @date 2023-10-30
  */
+@Slf4j
 @Api(tags = "机器人控制接口")
 @RestController
 @RequestMapping("/robot/opt")
@@ -61,6 +73,28 @@
         return baseRobotService.execCmdHandler(request, InstructCodeEnums.CHANGE_PCD);
     }
 
+    @Value("${casic.file.uploadPath}")
+    private String filePath;
+
+    @SneakyThrows
+    @ApiOperation(value = "PCD上传接口")
+    @PostMapping(value = "/uploadPcd")
+    public ResponseData uploadPcd(@RequestParam("file") MultipartFile file, @ApiParam("机器人ID,url拼接传参") String robotId) {
+        Assert.isFalse(StrUtil.isEmpty(robotId), () -> new RequestValidException("机器ID不能为空", Optional.of(new Throwable().getStackTrace()[2])));
+        RobotInfo info = robotInfoService.getById(robotId);
+        if (file != null && !file.isEmpty()) {
+            File pcdFile = new File(filePath + "finalCloud_" + info.getId() + ".pcd");
+            try {
+                file.transferTo(pcdFile);
+            } catch (Exception e) {
+                log.error(e.getMessage());
+            }
+            return ResponseData.success();
+        } else {
+            return ResponseData.error("附件上传失败!");
+        }
+    }
+
     @SneakyThrows
     @ApiOperation(value = "PCD下载接口")
     @PostMapping(value = "/pcdDownload")
@@ -136,6 +170,10 @@
     @ApiOperation(value = "启动或关闭循迹功能", notes = "返回 1成功 0失败  wait 启动时是否等待，0代表循迹启动即运动，不等待；1代表循迹启动原地等待。若无此字段，默认循迹启动不等待直接运动")
     @PostMapping(value = "/startTrack")
     public ResponseDataDTO<Integer> startTrack(@RequestBody StartTrackRequest request) {
+        request.setIsTask(1);
+        if (request.getStepLen() == null) {
+            request.setStepLen(2);
+        }
         baseRobotService.startTrack(request);
         return ResponseDataDTO.success();
     }
@@ -156,6 +194,13 @@
         MessageRequestDTO<CrlNavRequest> messageDTO = new MessageRequestDTO();
         messageDTO.setMsg(request);
         ResponseDataDTO<Integer> list = baseRobotService.getRobotRouteOptService().robotCmd(info, InstructCodeEnums.CTRL_NAV, messageDTO);
+        if (request.getAction() == 1) {
+            CostMapResolutionRequest costMap = new CostMapResolutionRequest();
+            costMap.setResolution(info.getResolution().doubleValue());
+            request.setRobotId(request.getRobotId());
+            //是否下发分辨率
+            CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CONFIG_COSTMAP_RESOLUTION_SET + request.getRobotId(), costMap);
+        }
         return list;
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java
new file mode 100644
index 0000000..5317651
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java
@@ -0,0 +1,86 @@
+package com.casic.missiles.modular.robot.controller;
+
+import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
+import com.baomidou.mybatisplus.extension.plugins.pagination.Page;
+import com.casic.missiles.core.base.controller.BaseController;
+import com.casic.missiles.core.page.PageFactory;
+import com.casic.missiles.core.page.PageInfoBT;
+import com.casic.missiles.model.form.IdForms;
+import com.casic.missiles.model.response.dto.ResponseDataDTO;
+import com.casic.missiles.modular.robot.model.TaskGridInfo;
+import com.casic.missiles.modular.robot.service.ITaskGridInfoService;
+import com.casic.missiles.modular.robot.vo.TaskGridInfoVO;
+import io.swagger.annotations.Api;
+import io.swagger.annotations.ApiOperation;
+import org.springframework.web.bind.annotation.*;
+
+import java.util.List;
+
+/**
+ * 任务网格信息Controller
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+@Api(tags = "任务网格信息")
+@RestController
+@RequestMapping("/robot/taskGridInfo")
+public class TaskGridInfoController extends BaseController {
+
+    private final ITaskGridInfoService taskGridInfoService;
+
+    public TaskGridInfoController(ITaskGridInfoService taskGridInfoService) {
+        this.taskGridInfoService = taskGridInfoService;
+    }
+
+    @ApiOperation(value = "列表查询")
+    @GetMapping(value = "/list")
+    public ResponseDataDTO<List<TaskGridInfo>> list(TaskGridInfoVO taskGridInfoVO) {
+        QueryWrapper<TaskGridInfo> query = taskGridInfoVO.genQuery();
+        return ResponseDataDTO.success(taskGridInfoService.list(query));
+    }
+
+    @ApiOperation(value = "分页查询")
+    @GetMapping(value = "/listPage")
+    public ResponseDataDTO<PageInfoBT<TaskGridInfo>> listPage(TaskGridInfoVO taskGridInfoVO) {
+        Page<TaskGridInfo> page = PageFactory.defaultPage();
+        QueryWrapper<TaskGridInfo> query = taskGridInfoVO.genQuery();
+        page = taskGridInfoService.page(page, query);
+        return ResponseDataDTO.success(super.packForBT(page));
+    }
+
+
+    @ApiOperation(value = "新增接口")
+    @PostMapping(value = "/add")
+    public ResponseDataDTO add(@RequestBody TaskGridInfo taskGridInfo) {
+        taskGridInfoService.save(taskGridInfo);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "修改接口")
+    @PostMapping(value = "/update")
+    public ResponseDataDTO update(@RequestBody TaskGridInfo taskGridInfo) {
+        taskGridInfoService.updateById(taskGridInfo);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "删除接口")
+    @GetMapping(value = "/delete")
+    public ResponseDataDTO delete(String id) {
+        taskGridInfoService.removeById(id);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation("批量删除")
+    @PostMapping("/batchDelete")
+    public ResponseDataDTO batchDelete(@RequestBody IdForms ids) {
+        taskGridInfoService.removeByIds(ids.getIds());
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "详情查询")
+    @GetMapping(value = "/detail")
+    public ResponseDataDTO<TaskGridInfo> detail(String id) {
+        return ResponseDataDTO.success(taskGridInfoService.getById(id));
+    }
+}
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
index e82a792..6992274 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
@@ -98,7 +98,7 @@
     public ResponseDataDTO<AlgorithmResponse> test() {
         AlgorithmResponse response = new AlgorithmResponse();
         response.setTaskHeatMap(new TaskHeatMap());
-        algorithmService.readAlgorithmResponse("step3.txt",response,123L,null);
+        algorithmService.readAlgorithmResponse("step3.txt",null,response,123L,null);
         sendWebSocket(InstructStatusMsgEnums.HEAT_MAP, "1", response.getTaskHeatMap());
         return ResponseDataDTO.success(response);
     }
@@ -121,7 +121,7 @@
     @GetMapping(value = "/testGrid")
     public ResponseDataDTO<List<RouteInfo>> testGrid(@RequestBody PathInitDTO pathInitDTO) {
         RobotInfo robotInfo = infoService.getById(pathInitDTO.getRobotId());
-        GridPointDTO gridPointDTO = algorithmService.convertSlamToGrid(new BigDecimal(pathInitDTO.getX()), new BigDecimal(10), new BigDecimal(4), robotInfo);
+        GridPointDTO gridPointDTO = algorithmService.convertSlamToGrid(new BigDecimal(pathInitDTO.getXNow()), new BigDecimal(10), new BigDecimal(4), robotInfo);
         return ResponseDataDTO.success(algorithmService.convertGridCenterPoint(gridPointDTO, new BigDecimal(10), robotInfo));
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/TaskGridInfoMapper.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/TaskGridInfoMapper.java
new file mode 100644
index 0000000..5c3a70b
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/TaskGridInfoMapper.java
@@ -0,0 +1,23 @@
+package com.casic.missiles.modular.robot.dao;
+
+
+import com.baomidou.mybatisplus.core.mapper.BaseMapper;
+import com.casic.missiles.modular.robot.model.TaskGridInfo;
+import org.apache.ibatis.annotations.Param;
+import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
+import com.baomidou.mybatisplus.extension.plugins.pagination.Page;
+import java.util.List;
+/**
+ * <p>
+ *  任务网格信息 Mapper 接口
+ * </p>
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+public interface TaskGridInfoMapper extends BaseMapper<TaskGridInfo> {
+/**
+* 任务网格信息 分页检索
+*/
+List<TaskGridInfo> selectTaskGridInfoPage( @Param("page") Page<TaskGridInfo> page,@Param("ew" ) QueryWrapper<TaskGridInfo> query);
+}
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/mapping/TaskGridInfoMapper.xml b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/mapping/TaskGridInfoMapper.xml
new file mode 100644
index 0000000..be573e0
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/mapping/TaskGridInfoMapper.xml
@@ -0,0 +1,80 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!DOCTYPE mapper PUBLIC "-//mybatis.org//DTD Mapper 3.0//EN" "http://mybatis.org/dtd/mybatis-3-mapper.dtd">
+<mapper namespace="com.casic.missiles.modular.robot.dao.TaskGridInfoMapper">
+    <!-- 通用查询映射结果 -->
+    <resultMap id="BaseResultMap" type="com.casic.missiles.modular.robot.model.TaskGridInfo">
+        <result column="id" property="id"/>
+        <result column="create_time" property="createTime"/>
+        <result column="update_time" property="updateTime"/>
+        <result column="task_id" property="taskId"/>
+        <result column="estimate_grid_x" property="estimateGridX"/>
+        <result column="estimate_grid_y" property="estimateGridY"/>
+        <result column="neutron_count" property="neutronCount"/>
+        <result column="cur_grid_x" property="curGridX"/>
+        <result column="cur_grid_y" property="curGridY"/>
+    </resultMap>
+    <!-- 通用查询结果列 -->
+    <sql id="Base_Column_List">
+        id
+        ,
+                    create_time as createTime ,
+                    update_time as updateTime ,
+                    task_id as taskId ,
+                    estimate_grid_x as estimateGridX ,
+                    estimate_grid_y as estimateGridY ,
+                    neutron_count as neutronCount ,
+                    cur_grid_x as curGridX ,
+                    cur_grid_y as curGridY
+    </sql>
+    <sql id="Base_Column_Alias">
+        a
+        .
+        id
+        ,
+            a.create_time
+            as createTime ,
+            a.update_time
+            as updateTime ,
+            a.task_id
+            as taskId ,
+            a.estimate_grid_x
+            as estimateGridX ,
+            a.estimate_grid_y
+            as estimateGridY ,
+            a.neutron_count
+            as neutronCount ,
+            a.cur_grid_x
+            as curGridX ,
+            a.cur_grid_y
+            as curGridY
+    </sql>
+    <select id="selectTaskGridInfoPage" resultType="com.casic.missiles.modular.robot.model.TaskGridInfo">
+        select
+        <include refid="Base_Column_Alias"/>
+        from task_grid_info a
+        <where>
+            <if test="ew != null">
+                ${ew.sqlSegment}
+            </if>
+        </where>
+    </select>
+    <sql id="Base_Column_Query">
+        <if test="request!=null">
+            <if test="request.taskId!= null and request.taskId!=''">
+                and task_id = #{request.taskId}
+            </if>
+            <if test="request.estimateGridX!= null and request.estimateGridX!=''">
+                and estimate_grid_x = #{request.estimateGridX}
+            </if>
+            <if test="request.estimateGridY!= null and request.estimateGridY!=''">
+                and estimate_grid_y = #{request.estimateGridY}
+            </if>
+            <if test="request.curGridX!= null and request.curGridX!=''">
+                and cur_grid_x = #{request.curGridX}
+            </if>
+            <if test="request.curGridY!= null and request.curGridY!=''">
+                and cur_grid_y = #{request.curGridY}
+            </if>
+        </if>
+    </sql>
+</mapper>
\ No newline at end of file
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java
index 749f559..4559074 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java
@@ -13,4 +13,9 @@
      * 规划点位信息
      */
     private PointNextDTO pointNextDTO;
+    /**
+     * 寻源目标地址
+     */
+    private PointNextDTO coordinate;
+    private boolean isEnd;
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java
index 39ff2a2..cef3238 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java
@@ -12,6 +12,9 @@
     private BigDecimal y;
 
     public static GridPointDTO convertGridPoint(PointNextDTO nextDTO) {
+        if(nextDTO==null){
+            return null;
+        }
         GridPointDTO gridPointDTO = new GridPointDTO();
         gridPointDTO.setX(Convert.toBigDecimal(nextDTO.getX()));
         gridPointDTO.setY(Convert.toBigDecimal(nextDTO.getY()));
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java
index 8c88242..33be25d 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java
@@ -12,25 +12,19 @@
 public class PathInitDTO {
     //---------------------算法参数
     /**
-     * x坐标最大值（x方向栅格总数）
+     * 初始位置x坐标(栅格地图下的坐标)
      */
-    private String xMax;
+    private String xNow;
     /**
-     * y坐标最大值（y方向栅格总数）
+     * 初始位置y坐标(栅格地图下的坐标)
      */
-    private String yMax;
+    private String yNow;
     /**
      * 初始位置计数率
      */
-    private String count;
-    /**
-     * 初始位置x坐标(0<=x<x_max)
-     */
-    private String x;
-    /**
-     * 初始位置y坐标(0<=y<y_max)
-     */
-    private String y;
+    private String zNow;
+
+    private Integer stepLen;
 
     //---------------------其他执行参数
     private String robotId;
@@ -38,11 +32,11 @@
 
     public List<String> toArray() {
         List<String> list = new ArrayList<>();
-        list.add(xMax);
-        list.add(yMax);
-        list.add(count);
-        list.add(x);
-        list.add(y);
+        list.add(xNow);
+        list.add(yNow);
+        list.add(stepLen+"");
+        list.add(zNow);
+        list.add(taskId+"");
         return list;
     }
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java
index 109e95c..ada6774 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java
@@ -10,21 +10,17 @@
  */
 @Data
 public class PathPlanDTO extends PathInitDTO {
-
     /**
-     * 长度为8的数组，记录(x-1, y-1), (x-1, y), (x-1, y+1), (x, y-1), (x, y+1), (x+1, y-1), (x+1, y), (x+1, y+1)8个坐标的可通行信息。
-     * 若某坐标有障碍物（不可通行行）则记为0，否则记为1
+     * 浮点阈值(阈值，超过该值后结束巡检开始寻源)
      */
-    private String open;
-
+    private String th;
     public List<String> toArray() {
         List<String> list = new ArrayList<>();
-        list.add(getXMax());
-        list.add(getYMax());
-        list.add(open);
-        list.add(getCount());
-        list.add(getX());
-        list.add(getY());
+
+        list.add(getStepLen()+"");
+        list.add(getTh());
+        list.add(getZNow());
+        list.add(getTaskId()+"");
         return list;
     }
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskGridInfo.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskGridInfo.java
new file mode 100644
index 0000000..101c10a
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskGridInfo.java
@@ -0,0 +1,83 @@
+package com.casic.missiles.modular.robot.model;
+
+import com.baomidou.mybatisplus.annotation.TableField;
+import com.baomidou.mybatisplus.annotation.TableName;
+import com.baomidou.mybatisplus.extension.activerecord.Model;
+import io.swagger.annotations.ApiModelProperty;
+import lombok.Data;
+
+import java.math.BigDecimal;
+import java.util.Date;
+
+/**
+ * 任务网格信息对象 task_grid_info
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+@Data
+@TableName("task_grid_info")
+public class TaskGridInfo extends Model<TaskGridInfo> {
+    private static final long serialVersionUID = 1L;
+    @TableField(exist = false)
+    private Integer index;
+    @ApiModelProperty(value = "主键", dataType = "Long")
+    @TableField("id")
+    private Long id;
+
+    @ApiModelProperty(value = "创建时间", dataType = "Date")
+    @TableField("create_time")
+    private Date createTime;
+
+    @ApiModelProperty(value = "修改时间", dataType = "Date")
+    @TableField("update_time")
+    private Date updateTime;
+
+    @ApiModelProperty(value = "任务ID", dataType = "Long")
+    @TableField("task_id")
+    private Long taskId;
+
+    @ApiModelProperty(value = "估算栅格x", dataType = "Long")
+    @TableField("estimate_grid_x")
+    private String estimateGridX;
+
+    @ApiModelProperty(value = "估算栅格y", dataType = "Long")
+    @TableField("estimate_grid_y")
+    private String estimateGridY;
+
+    @ApiModelProperty(value = "中子计数率", dataType = "BigDecimal")
+    @TableField("neutron_count")
+    private BigDecimal neutronCount;
+
+    @ApiModelProperty(value = "当前栅格x", dataType = "Long")
+    @TableField("cur_grid_x")
+    private Integer curGridX;
+
+    @ApiModelProperty(value = "当前栅格y", dataType = "Long")
+    @TableField("cur_grid_y")
+    private Integer curGridY;
+
+    public String getCurPoint(){
+        return "("+curGridX+","+curGridY+")";
+    }
+    public String getEstimatePoint(){
+        if(estimateGridX==null&estimateGridY==null){
+            return "结束";
+        }
+        return "("+estimateGridX+","+estimateGridY+")";
+    }
+    @Override
+    public String toString() {
+        return "TaskGridInfo{"
+                + "id=" + id
+                + "createTime=" + createTime
+                + "updateTime=" + updateTime
+                + "taskId=" + taskId
+                + "estimateGridX=" + estimateGridX
+                + "estimateGridY=" + estimateGridY
+                + "neutronCount=" + neutronCount
+                + "curGridX=" + curGridX
+                + "curGridY=" + curGridY
+                + "}";
+    }
+}
\ No newline at end of file
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java
index edc104e..a8acb2a 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java
@@ -53,6 +53,13 @@
     @TableField("route_id")
     private Long routeId;
 
+    @ApiModelProperty(value = "寻源初始步长", dataType = "Integer")
+    @TableField("step_len")
+    private Integer stepLen;
+
+    @ApiModelProperty(value = "寻源阈值设定", dataType = "BigDecimal")
+    @TableField("max_threshold")
+    private BigDecimal maxThreshold;
     /**
      * 任务类型(1巡检任务)
      */
@@ -62,7 +69,6 @@
     @ApiModelProperty(value = "任务类型字典名称", dataType = "String")
     @TableField(exist = false)
     private String taskTypeName;
-
     @ApiModelProperty(value = "识别类型(discernType 1中子源识别)", dataType = "Integer")
     @TableField("discern_type")
     private Integer discernType;
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java
index c4bc25d..c6620f3 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java
@@ -53,7 +53,7 @@
     }),
     SET_SPEED("/set_speed" , "/set_speed_response" , "最大运动速度设置" , true, new TypeReference<RobotMsgResponseDTO<Integer>>() {
     }),
-    SET_TARGETPOINT("/set_targetpoint" , "/set_targetpoint_response" , "设置导航目标点" , false, new TypeReference<RobotMsgResponseDTO<Integer>>() {
+    SET_TARGETPOINT("/set_targetpoint" , "/set_targetpoint_response" , "设置导航目标点" , true, new TypeReference<RobotMsgResponseDTO<Integer>>() {
     }),
 
     GET_DATA_LIST("/get_data_list" , "/get_data_list_response" , "获取数据包列表" , true, new TypeReference<RobotMsgResponseDTO<List<String>>>() {
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/AbstractSocketHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/AbstractSocketHandler.java
index 83f4cb2..e47e343 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/AbstractSocketHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/AbstractSocketHandler.java
@@ -49,7 +49,7 @@
                 wsMsgBaseDTO.setMsgKey(enums.getMsgKey());
                 wsMsgBaseDTO.setData(jsonObject.get("msg"));
                 String msg = JSON.toJSONString(wsMsgBaseDTO);
-                log.info("推送{}消息：{}", enums.getRemarks(), msg);
+//                log.info("推送{}消息：{}", enums.getRemarks(), msg);
                 webSocket.sendOneMessage(robotId, msg);
             }
         } catch (Exception e) {
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/cmd/TargetPointCmdHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/cmd/TargetPointCmdHandler.java
index c1c09fa..43b4460 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/cmd/TargetPointCmdHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/cmd/TargetPointCmdHandler.java
@@ -34,7 +34,11 @@
     @Override
     public ResponseDataDTO senderCmd(TargetPointRequest request, InstructCodeEnums enums) {
         MessageRequestDTO<TargetPointRequest> messageDTO = new MessageRequestDTO();
-        //读取默认关键点信息
+        //读取默认关键点信息 {"x":"1.715708819927424","y":"-0.9100042544250901","z":"0.017993879527185876","theta":"-0.039278239414312105"}
+//        request.setX("1.715708819927424");
+//        request.setY("-0.9100042544250901");
+//        request.setZ("0.017993879527185876");
+//        request.setTheta("-0.039278239414312105");
         messageDTO.setMsg(request);
         ResponseDataDTO<Integer> list = optService.robotCmd(getRobotInfo(), enums, messageDTO);
         return list;
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/AppCostMapHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/AppCostMapHandler.java
index 0335693..88c9f79 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/AppCostMapHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/AppCostMapHandler.java
@@ -1,6 +1,11 @@
 package com.casic.missiles.modular.robot.opt.handler.msg;
 
+import com.casic.missiles.core.cache.CacheUtil;
+import com.casic.missiles.modular.constants.RobotDictConstants;
+import com.casic.missiles.modular.robot.IBaseRobotService;
+import com.casic.missiles.modular.robot.opt.enums.InstructCodeEnums;
 import com.casic.missiles.modular.robot.opt.handler.AbstractSocketHandler;
+import com.casic.missiles.modular.robot.opt.instruct.dto.CostMapResolutionRequest;
 import com.casic.missiles.modular.robot.service.IRobotStatusInfoService;
 import lombok.extern.slf4j.Slf4j;
 import org.eclipse.paho.client.mqttv3.MqttMessage;
@@ -16,14 +21,27 @@
 @Component("/app_costmap_sender")
 public class AppCostMapHandler extends AbstractSocketHandler {
     private final IRobotStatusInfoService statusInfoService;
+    private final IBaseRobotService baseRobotService;
     private static Map<String, Integer> index = new ConcurrentHashMap<>();
 
-    public AppCostMapHandler(IRobotStatusInfoService statusInfoService) {
+    public AppCostMapHandler(IRobotStatusInfoService statusInfoService, IBaseRobotService baseRobotService) {
         this.statusInfoService = statusInfoService;
+        this.baseRobotService = baseRobotService;
     }
 
     @Override
     public void process(String robotId, String topic, MqttMessage message) {
         statusInfoService.saveCostMap(robotId, message);
+
+        CostMapResolutionRequest costMapSet = CacheUtil.get(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CONFIG_COSTMAP_RESOLUTION_SET +robotId);
+        if(costMapSet != null){
+            CacheUtil.remove(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CONFIG_COSTMAP_RESOLUTION_SET +robotId);
+            new Thread(() -> {
+                costMapSet.setRobotId(Long.valueOf(robotId));
+                baseRobotService.execCmdHandler(costMapSet, InstructCodeEnums.COSTMAP_RESOLUTION);
+            }).start();
+
+        }
+
     }
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/PoseMessageMsgHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/PoseMessageMsgHandler.java
index 29f8919..6c3c3bd 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/PoseMessageMsgHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/PoseMessageMsgHandler.java
@@ -17,11 +17,9 @@
 @Component("/pose_message")
 public class PoseMessageMsgHandler extends AbstractSocketHandler {
 
-
     private final EventPublisher publisher;
 
     public PoseMessageMsgHandler(EventPublisher publisher) {
-
         this.publisher = publisher;
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/StopStateMsgHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/StopStateMsgHandler.java
index 8950815..8a71e0b 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/StopStateMsgHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/StopStateMsgHandler.java
@@ -30,7 +30,7 @@
     public void process(String robotId, String topic, MqttMessage message) {
 
         index.put(index_key, index.get("index") + 1);
-        if (index.get(index_key)>50) {
+        if (index.get(index_key)>30) {
             index.put(index_key, 0);
             log.info("主键：{},停障状态topic:{},消息内容:{}", robotId, topic, message.toString());
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/impl/RobotOptServiceImpl.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/impl/RobotOptServiceImpl.java
index 8471e7a..7940a24 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/impl/RobotOptServiceImpl.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/impl/RobotOptServiceImpl.java
@@ -146,7 +146,7 @@
     public void startTrack(StartTrackRequest request) {
         //增加循迹任务记录
         taskInfoService.reloadTask(request, robotInfoService.getById(request.getRobotId()));
-        this.execCmdHandler(request, InstructCodeEnums.START_TRACK);
+//        this.execCmdHandler(request, InstructCodeEnums.START_TRACK);
     }
 
     public IRobotRouteOptService getRobotRouteOptService() {
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/BaseRobotCmdDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/BaseRobotCmdDTO.java
index 5608f2b..06b53de 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/BaseRobotCmdDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/BaseRobotCmdDTO.java
@@ -3,11 +3,13 @@
 import io.swagger.annotations.ApiModelProperty;
 import lombok.Data;
 
+import java.io.Serializable;
+
 /**
  * 机器人指令基类
  */
 @Data
-public class BaseRobotCmdDTO {
+public class BaseRobotCmdDTO implements Serializable {
     @ApiModelProperty(value = "机器人主键", dataType = "String")
     protected Long robotId;
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/StartTrackRequest.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/StartTrackRequest.java
index a18321b..85546a7 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/StartTrackRequest.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/StartTrackRequest.java
@@ -3,6 +3,8 @@
 import io.swagger.annotations.ApiModelProperty;
 import lombok.Data;
 
+import java.math.BigDecimal;
+
 /**
  * 导航下发指令
  */
@@ -10,18 +12,19 @@
 public class StartTrackRequest extends BaseRobotCmdDTO {
     @ApiModelProperty(value = "sn编码(可不填 默认机器ID)", dataType = "String")
     private String sn;
-
     @ApiModelProperty(value = "下发状态 1:开启 0:关闭", dataType = "String")
     private Integer action;
     @ApiModelProperty(value = "等待状态 1:等待 0:不等待", dataType = "String")
     private Integer wait;
     @ApiModelProperty(value = "等待状态 0:循迹 1:任务", dataType = "isTask")
     private Integer isTask;
-    @ApiModelProperty(value = "轨迹名称", dataType = "String")
+    @ApiModelProperty(value = "轨迹名称(废弃)", dataType = "String")
     private String track_name;
     @ApiModelProperty(value = "关键点名称", dataType = "String", hidden = true)
     private String taskpoint_name;
-
+    @ApiModelProperty(value = "初始寻源步长", dataType = "Integer")
+    private Integer stepLen;
+    @ApiModelProperty(value = "寻源阈值", dataType = "BigDecimal")
+    private BigDecimal maxThreshold;
     private String obs_mode="1";
-
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/response/StopStateResponseDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/response/StopStateResponseDTO.java
index eb514bf..e0679d7 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/response/StopStateResponseDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/response/StopStateResponseDTO.java
@@ -3,11 +3,13 @@
 import io.swagger.annotations.ApiModelProperty;
 import lombok.Data;
 
+import java.io.Serializable;
+
 /**
  * 电压信息返回
  */
 @Data
-public class StopStateResponseDTO {
+public class StopStateResponseDTO implements Serializable {
     @ApiModelProperty(value = "软件急停(0：无触发 1：触发)" , dataType = "Integer")
     private Integer soft;
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IAlgorithmService.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IAlgorithmService.java
index 56b3e1d..f62523b 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IAlgorithmService.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IAlgorithmService.java
@@ -6,6 +6,7 @@
 import com.casic.missiles.modular.robot.dto.PathPlanDTO;
 import com.casic.missiles.modular.robot.model.RobotInfo;
 
+import java.io.File;
 import java.math.BigDecimal;
 import java.util.List;
 
@@ -38,5 +39,5 @@
      * @return
      */
     GridPointDTO convertGridCenterPoint(GridPointDTO pointDTO, BigDecimal resolution, RobotInfo robotInfo);
-    void readAlgorithmResponse(String fileName, AlgorithmResponse response, Long taskId, List<String> res);
+    void readAlgorithmResponse(String fileName, File repetitionFile, AlgorithmResponse response, Long taskId, List<String> res);
 }
\ No newline at end of file
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IRobotStatusInfoService.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IRobotStatusInfoService.java
index 2af210a..757f8d0 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IRobotStatusInfoService.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IRobotStatusInfoService.java
@@ -67,10 +67,9 @@
     /**
      * 更新车辆路线及任务信息
      *
-     * @param currRouteId 当前路线信息
      * @param currTaskId  当前任务信息
      */
-    void updateCurrTask(String robotId, Long currRouteId, Long currTaskId);
+    void updateCurrTask(String robotId, Long currTaskId);
 
     RobotStatusInfo getStatusByRobotId(String robotId);
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/ITaskGridInfoService.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/ITaskGridInfoService.java
new file mode 100644
index 0000000..fc0da93
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/ITaskGridInfoService.java
@@ -0,0 +1,22 @@
+package com.casic.missiles.modular.robot.service;
+
+import com.baomidou.mybatisplus.extension.service.IService;
+import  com.casic.missiles.modular.robot.model.TaskGridInfo;
+import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
+import com.baomidou.mybatisplus.extension.plugins.pagination.Page;
+import java.util.List;
+
+
+/**
+ * <p>
+ * 任务网格信息 服务类
+ * </p>
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+public interface ITaskGridInfoService extends IService<TaskGridInfo> {
+/**
+* 任务网格信息 分页检索
+*/
+List<TaskGridInfo> selectTaskGridInfoPage(Page<TaskGridInfo> page,QueryWrapper<TaskGridInfo> query);}
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/ITaskInfoService.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/ITaskInfoService.java
index 31d7ba6..b395993 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/ITaskInfoService.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/ITaskInfoService.java
@@ -51,6 +51,13 @@
      * @param dto 实体
      */
     void pointCallBack(PoseMessageResponseDTO dto, String robotId);
+
+    /**
+     * 车辆路线规划
+     * @param dto
+     * @param robotId
+     */
+    void carPathPlanning(PoseMessageResponseDTO dto, String robotId);
     void pointTestCallBack(PoseMessageResponseDTO dto, String robotId);
     /**
      * 图片路径保存

diff --git a/.gitignore b/.gitignore
index 9e20bc1..f42a9cb 100644
--- a/.gitignore
+++ b/.gitignore
@@ -2,3 +2,4 @@
 **/target/**
 logs/
 *.iml
+*.exe
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java b/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
index c7b5d28..a849528 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
@@ -28,6 +28,7 @@
     String KEY_CUR_TASK_ID = "CUR_TASK_ID_";
     String KEY_CUR_TASK_INIT = "CUR_TASK_INIT_";
 
+    String KEY_COST_MAP = "KEY_COST_MAP";
     /**
      * PSD数据ID
      */
@@ -41,6 +42,7 @@
      */
     String KEY_CUR_XY_POINT = "KEY_CUR_XY_POINT_";
     String KEY_CUR_ESTIMATE_GRID = "KEY_CUR_ESTIMATE_GRID_";
+    String KEY_PLANING_FLAG = "KEY_PLANING_FLAG_";
     /**
      * 机器人巡点栅格
      */
@@ -55,6 +57,7 @@
      * 探测器配置参数缓存
      */
     String KEY_CONFIG_PARAMS_ID = "KEY_CONFIG_PARAMS_ID_";
+    String KEY_CONFIG_COSTMAP_RESOLUTION_SET = "KEY_CONFIG_COSTMAP_RESOLUTION_SET_";
 
     /**
      * 暂停状态缓存信息
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
index c4c905d..3bb475f 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
@@ -1,9 +1,14 @@
 package com.casic.missiles.modular.robot.controller;
 
+import cn.hutool.core.lang.Assert;
 import cn.hutool.core.util.StrUtil;
 import com.casic.missiles.core.base.controller.BaseController;
+import com.casic.missiles.core.cache.CacheUtil;
 import com.casic.missiles.core.util.SpringContextHolder;
+import com.casic.missiles.model.exception.RequestValidException;
+import com.casic.missiles.model.response.ResponseData;
 import com.casic.missiles.model.response.dto.ResponseDataDTO;
+import com.casic.missiles.modular.constants.RobotDictConstants;
 import com.casic.missiles.modular.robot.IBaseRobotService;
 import com.casic.missiles.modular.robot.dto.RobotInfoSetDTO;
 import com.casic.missiles.modular.robot.model.RobotInfo;
@@ -19,10 +24,16 @@
 import io.swagger.annotations.ApiOperation;
 import io.swagger.annotations.ApiParam;
 import lombok.SneakyThrows;
+import lombok.extern.slf4j.Slf4j;
+import org.apache.ibatis.annotations.Param;
+import org.springframework.beans.factory.annotation.Value;
 import org.springframework.web.bind.annotation.*;
+import org.springframework.web.multipart.MultipartFile;
 
 import javax.servlet.http.HttpServletResponse;
+import java.io.File;
 import java.util.List;
+import java.util.Optional;
 
 /**
  * 机器人操作类
@@ -30,6 +41,7 @@
  * @author lwh
  * @date 2023-10-30
  */
+@Slf4j
 @Api(tags = "机器人控制接口")
 @RestController
 @RequestMapping("/robot/opt")
@@ -61,6 +73,28 @@
         return baseRobotService.execCmdHandler(request, InstructCodeEnums.CHANGE_PCD);
     }
 
+    @Value("${casic.file.uploadPath}")
+    private String filePath;
+
+    @SneakyThrows
+    @ApiOperation(value = "PCD上传接口")
+    @PostMapping(value = "/uploadPcd")
+    public ResponseData uploadPcd(@RequestParam("file") MultipartFile file, @ApiParam("机器人ID,url拼接传参") String robotId) {
+        Assert.isFalse(StrUtil.isEmpty(robotId), () -> new RequestValidException("机器ID不能为空", Optional.of(new Throwable().getStackTrace()[2])));
+        RobotInfo info = robotInfoService.getById(robotId);
+        if (file != null && !file.isEmpty()) {
+            File pcdFile = new File(filePath + "finalCloud_" + info.getId() + ".pcd");
+            try {
+                file.transferTo(pcdFile);
+            } catch (Exception e) {
+                log.error(e.getMessage());
+            }
+            return ResponseData.success();
+        } else {
+            return ResponseData.error("附件上传失败!");
+        }
+    }
+
     @SneakyThrows
     @ApiOperation(value = "PCD下载接口")
     @PostMapping(value = "/pcdDownload")
@@ -136,6 +170,10 @@
     @ApiOperation(value = "启动或关闭循迹功能", notes = "返回 1成功 0失败  wait 启动时是否等待，0代表循迹启动即运动，不等待；1代表循迹启动原地等待。若无此字段，默认循迹启动不等待直接运动")
     @PostMapping(value = "/startTrack")
     public ResponseDataDTO<Integer> startTrack(@RequestBody StartTrackRequest request) {
+        request.setIsTask(1);
+        if (request.getStepLen() == null) {
+            request.setStepLen(2);
+        }
         baseRobotService.startTrack(request);
         return ResponseDataDTO.success();
     }
@@ -156,6 +194,13 @@
         MessageRequestDTO<CrlNavRequest> messageDTO = new MessageRequestDTO();
         messageDTO.setMsg(request);
         ResponseDataDTO<Integer> list = baseRobotService.getRobotRouteOptService().robotCmd(info, InstructCodeEnums.CTRL_NAV, messageDTO);
+        if (request.getAction() == 1) {
+            CostMapResolutionRequest costMap = new CostMapResolutionRequest();
+            costMap.setResolution(info.getResolution().doubleValue());
+            request.setRobotId(request.getRobotId());
+            //是否下发分辨率
+            CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CONFIG_COSTMAP_RESOLUTION_SET + request.getRobotId(), costMap);
+        }
         return list;
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java
new file mode 100644
index 0000000..5317651
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java
@@ -0,0 +1,86 @@
+package com.casic.missiles.modular.robot.controller;
+
+import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
+import com.baomidou.mybatisplus.extension.plugins.pagination.Page;
+import com.casic.missiles.core.base.controller.BaseController;
+import com.casic.missiles.core.page.PageFactory;
+import com.casic.missiles.core.page.PageInfoBT;
+import com.casic.missiles.model.form.IdForms;
+import com.casic.missiles.model.response.dto.ResponseDataDTO;
+import com.casic.missiles.modular.robot.model.TaskGridInfo;
+import com.casic.missiles.modular.robot.service.ITaskGridInfoService;
+import com.casic.missiles.modular.robot.vo.TaskGridInfoVO;
+import io.swagger.annotations.Api;
+import io.swagger.annotations.ApiOperation;
+import org.springframework.web.bind.annotation.*;
+
+import java.util.List;
+
+/**
+ * 任务网格信息Controller
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+@Api(tags = "任务网格信息")
+@RestController
+@RequestMapping("/robot/taskGridInfo")
+public class TaskGridInfoController extends BaseController {
+
+    private final ITaskGridInfoService taskGridInfoService;
+
+    public TaskGridInfoController(ITaskGridInfoService taskGridInfoService) {
+        this.taskGridInfoService = taskGridInfoService;
+    }
+
+    @ApiOperation(value = "列表查询")
+    @GetMapping(value = "/list")
+    public ResponseDataDTO<List<TaskGridInfo>> list(TaskGridInfoVO taskGridInfoVO) {
+        QueryWrapper<TaskGridInfo> query = taskGridInfoVO.genQuery();
+        return ResponseDataDTO.success(taskGridInfoService.list(query));
+    }
+
+    @ApiOperation(value = "分页查询")
+    @GetMapping(value = "/listPage")
+    public ResponseDataDTO<PageInfoBT<TaskGridInfo>> listPage(TaskGridInfoVO taskGridInfoVO) {
+        Page<TaskGridInfo> page = PageFactory.defaultPage();
+        QueryWrapper<TaskGridInfo> query = taskGridInfoVO.genQuery();
+        page = taskGridInfoService.page(page, query);
+        return ResponseDataDTO.success(super.packForBT(page));
+    }
+
+
+    @ApiOperation(value = "新增接口")
+    @PostMapping(value = "/add")
+    public ResponseDataDTO add(@RequestBody TaskGridInfo taskGridInfo) {
+        taskGridInfoService.save(taskGridInfo);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "修改接口")
+    @PostMapping(value = "/update")
+    public ResponseDataDTO update(@RequestBody TaskGridInfo taskGridInfo) {
+        taskGridInfoService.updateById(taskGridInfo);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "删除接口")
+    @GetMapping(value = "/delete")
+    public ResponseDataDTO delete(String id) {
+        taskGridInfoService.removeById(id);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation("批量删除")
+    @PostMapping("/batchDelete")
+    public ResponseDataDTO batchDelete(@RequestBody IdForms ids) {
+        taskGridInfoService.removeByIds(ids.getIds());
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "详情查询")
+    @GetMapping(value = "/detail")
+    public ResponseDataDTO<TaskGridInfo> detail(String id) {
+        return ResponseDataDTO.success(taskGridInfoService.getById(id));
+    }
+}
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
index e82a792..6992274 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
@@ -98,7 +98,7 @@
     public ResponseDataDTO<AlgorithmResponse> test() {
         AlgorithmResponse response = new AlgorithmResponse();
         response.setTaskHeatMap(new TaskHeatMap());
-        algorithmService.readAlgorithmResponse("step3.txt",response,123L,null);
+        algorithmService.readAlgorithmResponse("step3.txt",null,response,123L,null);
         sendWebSocket(InstructStatusMsgEnums.HEAT_MAP, "1", response.getTaskHeatMap());
         return ResponseDataDTO.success(response);
     }
@@ -121,7 +121,7 @@
     @GetMapping(value = "/testGrid")
     public ResponseDataDTO<List<RouteInfo>> testGrid(@RequestBody PathInitDTO pathInitDTO) {
         RobotInfo robotInfo = infoService.getById(pathInitDTO.getRobotId());
-        GridPointDTO gridPointDTO = algorithmService.convertSlamToGrid(new BigDecimal(pathInitDTO.getX()), new BigDecimal(10), new BigDecimal(4), robotInfo);
+        GridPointDTO gridPointDTO = algorithmService.convertSlamToGrid(new BigDecimal(pathInitDTO.getXNow()), new BigDecimal(10), new BigDecimal(4), robotInfo);
         return ResponseDataDTO.success(algorithmService.convertGridCenterPoint(gridPointDTO, new BigDecimal(10), robotInfo));
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/TaskGridInfoMapper.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/TaskGridInfoMapper.java
new file mode 100644
index 0000000..5c3a70b
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/TaskGridInfoMapper.java
@@ -0,0 +1,23 @@
+package com.casic.missiles.modular.robot.dao;
+
+
+import com.baomidou.mybatisplus.core.mapper.BaseMapper;
+import com.casic.missiles.modular.robot.model.TaskGridInfo;
+import org.apache.ibatis.annotations.Param;
+import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
+import com.baomidou.mybatisplus.extension.plugins.pagination.Page;
+import java.util.List;
+/**
+ * <p>
+ *  任务网格信息 Mapper 接口
+ * </p>
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+public interface TaskGridInfoMapper extends BaseMapper<TaskGridInfo> {
+/**
+* 任务网格信息 分页检索
+*/
+List<TaskGridInfo> selectTaskGridInfoPage( @Param("page") Page<TaskGridInfo> page,@Param("ew" ) QueryWrapper<TaskGridInfo> query);
+}
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/mapping/TaskGridInfoMapper.xml b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/mapping/TaskGridInfoMapper.xml
new file mode 100644
index 0000000..be573e0
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/mapping/TaskGridInfoMapper.xml
@@ -0,0 +1,80 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!DOCTYPE mapper PUBLIC "-//mybatis.org//DTD Mapper 3.0//EN" "http://mybatis.org/dtd/mybatis-3-mapper.dtd">
+<mapper namespace="com.casic.missiles.modular.robot.dao.TaskGridInfoMapper">
+    <!-- 通用查询映射结果 -->
+    <resultMap id="BaseResultMap" type="com.casic.missiles.modular.robot.model.TaskGridInfo">
+        <result column="id" property="id"/>
+        <result column="create_time" property="createTime"/>
+        <result column="update_time" property="updateTime"/>
+        <result column="task_id" property="taskId"/>
+        <result column="estimate_grid_x" property="estimateGridX"/>
+        <result column="estimate_grid_y" property="estimateGridY"/>
+        <result column="neutron_count" property="neutronCount"/>
+        <result column="cur_grid_x" property="curGridX"/>
+        <result column="cur_grid_y" property="curGridY"/>
+    </resultMap>
+    <!-- 通用查询结果列 -->
+    <sql id="Base_Column_List">
+        id
+        ,
+                    create_time as createTime ,
+                    update_time as updateTime ,
+                    task_id as taskId ,
+                    estimate_grid_x as estimateGridX ,
+                    estimate_grid_y as estimateGridY ,
+                    neutron_count as neutronCount ,
+                    cur_grid_x as curGridX ,
+                    cur_grid_y as curGridY
+    </sql>
+    <sql id="Base_Column_Alias">
+        a
+        .
+        id
+        ,
+            a.create_time
+            as createTime ,
+            a.update_time
+            as updateTime ,
+            a.task_id
+            as taskId ,
+            a.estimate_grid_x
+            as estimateGridX ,
+            a.estimate_grid_y
+            as estimateGridY ,
+            a.neutron_count
+            as neutronCount ,
+            a.cur_grid_x
+            as curGridX ,
+            a.cur_grid_y
+            as curGridY
+    </sql>
+    <select id="selectTaskGridInfoPage" resultType="com.casic.missiles.modular.robot.model.TaskGridInfo">
+        select
+        <include refid="Base_Column_Alias"/>
+        from task_grid_info a
+        <where>
+            <if test="ew != null">
+                ${ew.sqlSegment}
+            </if>
+        </where>
+    </select>
+    <sql id="Base_Column_Query">
+        <if test="request!=null">
+            <if test="request.taskId!= null and request.taskId!=''">
+                and task_id = #{request.taskId}
+            </if>
+            <if test="request.estimateGridX!= null and request.estimateGridX!=''">
+                and estimate_grid_x = #{request.estimateGridX}
+            </if>
+            <if test="request.estimateGridY!= null and request.estimateGridY!=''">
+                and estimate_grid_y = #{request.estimateGridY}
+            </if>
+            <if test="request.curGridX!= null and request.curGridX!=''">
+                and cur_grid_x = #{request.curGridX}
+            </if>
+            <if test="request.curGridY!= null and request.curGridY!=''">
+                and cur_grid_y = #{request.curGridY}
+            </if>
+        </if>
+    </sql>
+</mapper>
\ No newline at end of file
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java
index 749f559..4559074 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java
@@ -13,4 +13,9 @@
      * 规划点位信息
      */
     private PointNextDTO pointNextDTO;
+    /**
+     * 寻源目标地址
+     */
+    private PointNextDTO coordinate;
+    private boolean isEnd;
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java
index 39ff2a2..cef3238 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java
@@ -12,6 +12,9 @@
     private BigDecimal y;
 
     public static GridPointDTO convertGridPoint(PointNextDTO nextDTO) {
+        if(nextDTO==null){
+            return null;
+        }
         GridPointDTO gridPointDTO = new GridPointDTO();
         gridPointDTO.setX(Convert.toBigDecimal(nextDTO.getX()));
         gridPointDTO.setY(Convert.toBigDecimal(nextDTO.getY()));
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java
index 8c88242..33be25d 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java
@@ -12,25 +12,19 @@
 public class PathInitDTO {
     //---------------------算法参数
     /**
-     * x坐标最大值（x方向栅格总数）
+     * 初始位置x坐标(栅格地图下的坐标)
      */
-    private String xMax;
+    private String xNow;
     /**
-     * y坐标最大值（y方向栅格总数）
+     * 初始位置y坐标(栅格地图下的坐标)
      */
-    private String yMax;
+    private String yNow;
     /**
      * 初始位置计数率
      */
-    private String count;
-    /**
-     * 初始位置x坐标(0<=x<x_max)
-     */
-    private String x;
-    /**
-     * 初始位置y坐标(0<=y<y_max)
-     */
-    private String y;
+    private String zNow;
+
+    private Integer stepLen;
 
     //---------------------其他执行参数
     private String robotId;
@@ -38,11 +32,11 @@
 
     public List<String> toArray() {
         List<String> list = new ArrayList<>();
-        list.add(xMax);
-        list.add(yMax);
-        list.add(count);
-        list.add(x);
-        list.add(y);
+        list.add(xNow);
+        list.add(yNow);
+        list.add(stepLen+"");
+        list.add(zNow);
+        list.add(taskId+"");
         return list;
     }
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java
index 109e95c..ada6774 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java
@@ -10,21 +10,17 @@
  */
 @Data
 public class PathPlanDTO extends PathInitDTO {
-
     /**
-     * 长度为8的数组，记录(x-1, y-1), (x-1, y), (x-1, y+1), (x, y-1), (x, y+1), (x+1, y-1), (x+1, y), (x+1, y+1)8个坐标的可通行信息。
-     * 若某坐标有障碍物（不可通行行）则记为0，否则记为1
+     * 浮点阈值(阈值，超过该值后结束巡检开始寻源)
      */
-    private String open;
-
+    private String th;
     public List<String> toArray() {
         List<String> list = new ArrayList<>();
-        list.add(getXMax());
-        list.add(getYMax());
-        list.add(open);
-        list.add(getCount());
-        list.add(getX());
-        list.add(getY());
+
+        list.add(getStepLen()+"");
+        list.add(getTh());
+        list.add(getZNow());
+        list.add(getTaskId()+"");
         return list;
     }
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskGridInfo.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskGridInfo.java
new file mode 100644
index 0000000..101c10a
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskGridInfo.java
@@ -0,0 +1,83 @@
+package com.casic.missiles.modular.robot.model;
+
+import com.baomidou.mybatisplus.annotation.TableField;
+import com.baomidou.mybatisplus.annotation.TableName;
+import com.baomidou.mybatisplus.extension.activerecord.Model;
+import io.swagger.annotations.ApiModelProperty;
+import lombok.Data;
+
+import java.math.BigDecimal;
+import java.util.Date;
+
+/**
+ * 任务网格信息对象 task_grid_info
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+@Data
+@TableName("task_grid_info")
+public class TaskGridInfo extends Model<TaskGridInfo> {
+    private static final long serialVersionUID = 1L;
+    @TableField(exist = false)
+    private Integer index;
+    @ApiModelProperty(value = "主键", dataType = "Long")
+    @TableField("id")
+    private Long id;
+
+    @ApiModelProperty(value = "创建时间", dataType = "Date")
+    @TableField("create_time")
+    private Date createTime;
+
+    @ApiModelProperty(value = "修改时间", dataType = "Date")
+    @TableField("update_time")
+    private Date updateTime;
+
+    @ApiModelProperty(value = "任务ID", dataType = "Long")
+    @TableField("task_id")
+    private Long taskId;
+
+    @ApiModelProperty(value = "估算栅格x", dataType = "Long")
+    @TableField("estimate_grid_x")
+    private String estimateGridX;
+
+    @ApiModelProperty(value = "估算栅格y", dataType = "Long")
+    @TableField("estimate_grid_y")
+    private String estimateGridY;
+
+    @ApiModelProperty(value = "中子计数率", dataType = "BigDecimal")
+    @TableField("neutron_count")
+    private BigDecimal neutronCount;
+
+    @ApiModelProperty(value = "当前栅格x", dataType = "Long")
+    @TableField("cur_grid_x")
+    private Integer curGridX;
+
+    @ApiModelProperty(value = "当前栅格y", dataType = "Long")
+    @TableField("cur_grid_y")
+    private Integer curGridY;
+
+    public String getCurPoint(){
+        return "("+curGridX+","+curGridY+")";
+    }
+    public String getEstimatePoint(){
+        if(estimateGridX==null&estimateGridY==null){
+            return "结束";
+        }
+        return "("+estimateGridX+","+estimateGridY+")";
+    }
+    @Override
+    public String toString() {
+        return "TaskGridInfo{"
+                + "id=" + id
+                + "createTime=" + createTime
+                + "updateTime=" + updateTime
+                + "taskId=" + taskId
+                + "estimateGridX=" + estimateGridX
+                + "estimateGridY=" + estimateGridY
+                + "neutronCount=" + neutronCount
+                + "curGridX=" + curGridX
+                + "curGridY=" + curGridY
+                + "}";
+    }
+}
\ No newline at end of file
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java
index edc104e..a8acb2a 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java
@@ -53,6 +53,13 @@
     @TableField("route_id")
     private Long routeId;
 
+    @ApiModelProperty(value = "寻源初始步长", dataType = "Integer")
+    @TableField("step_len")
+    private Integer stepLen;
+
+    @ApiModelProperty(value = "寻源阈值设定", dataType = "BigDecimal")
+    @TableField("max_threshold")
+    private BigDecimal maxThreshold;
     /**
      * 任务类型(1巡检任务)
      */
@@ -62,7 +69,6 @@
     @ApiModelProperty(value = "任务类型字典名称", dataType = "String")
     @TableField(exist = false)
     private String taskTypeName;
-
     @ApiModelProperty(value = "识别类型(discernType 1中子源识别)", dataType = "Integer")
     @TableField("discern_type")
     private Integer discernType;
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java
index c4bc25d..c6620f3 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java
@@ -53,7 +53,7 @@
     }),
     SET_SPEED("/set_speed" , "/set_speed_response" , "最大运动速度设置" , true, new TypeReference<RobotMsgResponseDTO<Integer>>() {
     }),
-    SET_TARGETPOINT("/set_targetpoint" , "/set_targetpoint_response" , "设置导航目标点" , false, new TypeReference<RobotMsgResponseDTO<Integer>>() {
+    SET_TARGETPOINT("/set_targetpoint" , "/set_targetpoint_response" , "设置导航目标点" , true, new TypeReference<RobotMsgResponseDTO<Integer>>() {
     }),
 
     GET_DATA_LIST("/get_data_list" , "/get_data_list_response" , "获取数据包列表" , true, new TypeReference<RobotMsgResponseDTO<List<String>>>() {
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/AbstractSocketHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/AbstractSocketHandler.java
index 83f4cb2..e47e343 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/AbstractSocketHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/AbstractSocketHandler.java
@@ -49,7 +49,7 @@
                 wsMsgBaseDTO.setMsgKey(enums.getMsgKey());
                 wsMsgBaseDTO.setData(jsonObject.get("msg"));
                 String msg = JSON.toJSONString(wsMsgBaseDTO);
-                log.info("推送{}消息：{}", enums.getRemarks(), msg);
+//                log.info("推送{}消息：{}", enums.getRemarks(), msg);
                 webSocket.sendOneMessage(robotId, msg);
             }
         } catch (Exception e) {
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/cmd/TargetPointCmdHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/cmd/TargetPointCmdHandler.java
index c1c09fa..43b4460 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/cmd/TargetPointCmdHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/cmd/TargetPointCmdHandler.java
@@ -34,7 +34,11 @@
     @Override
     public ResponseDataDTO senderCmd(TargetPointRequest request, InstructCodeEnums enums) {
         MessageRequestDTO<TargetPointRequest> messageDTO = new MessageRequestDTO();
-        //读取默认关键点信息
+        //读取默认关键点信息 {"x":"1.715708819927424","y":"-0.9100042544250901","z":"0.017993879527185876","theta":"-0.039278239414312105"}
+//        request.setX("1.715708819927424");
+//        request.setY("-0.9100042544250901");
+//        request.setZ("0.017993879527185876");
+//        request.setTheta("-0.039278239414312105");
         messageDTO.setMsg(request);
         ResponseDataDTO<Integer> list = optService.robotCmd(getRobotInfo(), enums, messageDTO);
         return list;
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/AppCostMapHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/AppCostMapHandler.java
index 0335693..88c9f79 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/AppCostMapHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/AppCostMapHandler.java
@@ -1,6 +1,11 @@
 package com.casic.missiles.modular.robot.opt.handler.msg;
 
+import com.casic.missiles.core.cache.CacheUtil;
+import com.casic.missiles.modular.constants.RobotDictConstants;
+import com.casic.missiles.modular.robot.IBaseRobotService;
+import com.casic.missiles.modular.robot.opt.enums.InstructCodeEnums;
 import com.casic.missiles.modular.robot.opt.handler.AbstractSocketHandler;
+import com.casic.missiles.modular.robot.opt.instruct.dto.CostMapResolutionRequest;
 import com.casic.missiles.modular.robot.service.IRobotStatusInfoService;
 import lombok.extern.slf4j.Slf4j;
 import org.eclipse.paho.client.mqttv3.MqttMessage;
@@ -16,14 +21,27 @@
 @Component("/app_costmap_sender")
 public class AppCostMapHandler extends AbstractSocketHandler {
     private final IRobotStatusInfoService statusInfoService;
+    private final IBaseRobotService baseRobotService;
     private static Map<String, Integer> index = new ConcurrentHashMap<>();
 
-    public AppCostMapHandler(IRobotStatusInfoService statusInfoService) {
+    public AppCostMapHandler(IRobotStatusInfoService statusInfoService, IBaseRobotService baseRobotService) {
         this.statusInfoService = statusInfoService;
+        this.baseRobotService = baseRobotService;
     }
 
     @Override
     public void process(String robotId, String topic, MqttMessage message) {
         statusInfoService.saveCostMap(robotId, message);
+
+        CostMapResolutionRequest costMapSet = CacheUtil.get(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CONFIG_COSTMAP_RESOLUTION_SET +robotId);
+        if(costMapSet != null){
+            CacheUtil.remove(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CONFIG_COSTMAP_RESOLUTION_SET +robotId);
+            new Thread(() -> {
+                costMapSet.setRobotId(Long.valueOf(robotId));
+                baseRobotService.execCmdHandler(costMapSet, InstructCodeEnums.COSTMAP_RESOLUTION);
+            }).start();
+
+        }
+
     }
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/PoseMessageMsgHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/PoseMessageMsgHandler.java
index 29f8919..6c3c3bd 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/PoseMessageMsgHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/PoseMessageMsgHandler.java
@@ -17,11 +17,9 @@
 @Component("/pose_message")
 public class PoseMessageMsgHandler extends AbstractSocketHandler {
 
-
     private final EventPublisher publisher;
 
     public PoseMessageMsgHandler(EventPublisher publisher) {
-
         this.publisher = publisher;
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/StopStateMsgHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/StopStateMsgHandler.java
index 8950815..8a71e0b 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/StopStateMsgHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/StopStateMsgHandler.java
@@ -30,7 +30,7 @@
     public void process(String robotId, String topic, MqttMessage message) {
 
         index.put(index_key, index.get("index") + 1);
-        if (index.get(index_key)>50) {
+        if (index.get(index_key)>30) {
             index.put(index_key, 0);
             log.info("主键：{},停障状态topic:{},消息内容:{}", robotId, topic, message.toString());
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/impl/RobotOptServiceImpl.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/impl/RobotOptServiceImpl.java
index 8471e7a..7940a24 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/impl/RobotOptServiceImpl.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/impl/RobotOptServiceImpl.java
@@ -146,7 +146,7 @@
     public void startTrack(StartTrackRequest request) {
         //增加循迹任务记录
         taskInfoService.reloadTask(request, robotInfoService.getById(request.getRobotId()));
-        this.execCmdHandler(request, InstructCodeEnums.START_TRACK);
+//        this.execCmdHandler(request, InstructCodeEnums.START_TRACK);
     }
 
     public IRobotRouteOptService getRobotRouteOptService() {
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/BaseRobotCmdDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/BaseRobotCmdDTO.java
index 5608f2b..06b53de 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/BaseRobotCmdDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/BaseRobotCmdDTO.java
@@ -3,11 +3,13 @@
 import io.swagger.annotations.ApiModelProperty;
 import lombok.Data;
 
+import java.io.Serializable;
+
 /**
  * 机器人指令基类
  */
 @Data
-public class BaseRobotCmdDTO {
+public class BaseRobotCmdDTO implements Serializable {
     @ApiModelProperty(value = "机器人主键", dataType = "String")
     protected Long robotId;
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/StartTrackRequest.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/StartTrackRequest.java
index a18321b..85546a7 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/StartTrackRequest.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/StartTrackRequest.java
@@ -3,6 +3,8 @@
 import io.swagger.annotations.ApiModelProperty;
 import lombok.Data;
 
+import java.math.BigDecimal;
+
 /**
  * 导航下发指令
  */
@@ -10,18 +12,19 @@
 public class StartTrackRequest extends BaseRobotCmdDTO {
     @ApiModelProperty(value = "sn编码(可不填 默认机器ID)", dataType = "String")
     private String sn;
-
     @ApiModelProperty(value = "下发状态 1:开启 0:关闭", dataType = "String")
     private Integer action;
     @ApiModelProperty(value = "等待状态 1:等待 0:不等待", dataType = "String")
     private Integer wait;
     @ApiModelProperty(value = "等待状态 0:循迹 1:任务", dataType = "isTask")
     private Integer isTask;
-    @ApiModelProperty(value = "轨迹名称", dataType = "String")
+    @ApiModelProperty(value = "轨迹名称(废弃)", dataType = "String")
     private String track_name;
     @ApiModelProperty(value = "关键点名称", dataType = "String", hidden = true)
     private String taskpoint_name;
-
+    @ApiModelProperty(value = "初始寻源步长", dataType = "Integer")
+    private Integer stepLen;
+    @ApiModelProperty(value = "寻源阈值", dataType = "BigDecimal")
+    private BigDecimal maxThreshold;
     private String obs_mode="1";
-
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/response/StopStateResponseDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/response/StopStateResponseDTO.java
index eb514bf..e0679d7 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/response/StopStateResponseDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/response/StopStateResponseDTO.java
@@ -3,11 +3,13 @@
 import io.swagger.annotations.ApiModelProperty;
 import lombok.Data;
 
+import java.io.Serializable;
+
 /**
  * 电压信息返回
  */
 @Data
-public class StopStateResponseDTO {
+public class StopStateResponseDTO implements Serializable {
     @ApiModelProperty(value = "软件急停(0：无触发 1：触发)" , dataType = "Integer")
     private Integer soft;
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IAlgorithmService.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IAlgorithmService.java
index 56b3e1d..f62523b 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IAlgorithmService.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IAlgorithmService.java
@@ -6,6 +6,7 @@
 import com.casic.missiles.modular.robot.dto.PathPlanDTO;
 import com.casic.missiles.modular.robot.model.RobotInfo;
 
+import java.io.File;
 import java.math.BigDecimal;
 import java.util.List;
 
@@ -38,5 +39,5 @@
      * @return
      */
     GridPointDTO convertGridCenterPoint(GridPointDTO pointDTO, BigDecimal resolution, RobotInfo robotInfo);
-    void readAlgorithmResponse(String fileName, AlgorithmResponse response, Long taskId, List<String> res);
+    void readAlgorithmResponse(String fileName, File repetitionFile, AlgorithmResponse response, Long taskId, List<String> res);
 }
\ No newline at end of file
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IRobotStatusInfoService.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IRobotStatusInfoService.java
index 2af210a..757f8d0 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IRobotStatusInfoService.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IRobotStatusInfoService.java
@@ -67,10 +67,9 @@
     /**
      * 更新车辆路线及任务信息
      *
-     * @param currRouteId 当前路线信息
      * @param currTaskId  当前任务信息
      */
-    void updateCurrTask(String robotId, Long currRouteId, Long currTaskId);
+    void updateCurrTask(String robotId, Long currTaskId);
 
     RobotStatusInfo getStatusByRobotId(String robotId);
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/ITaskGridInfoService.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/ITaskGridInfoService.java
new file mode 100644
index 0000000..fc0da93
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/ITaskGridInfoService.java
@@ -0,0 +1,22 @@
+package com.casic.missiles.modular.robot.service;
+
+import com.baomidou.mybatisplus.extension.service.IService;
+import  com.casic.missiles.modular.robot.model.TaskGridInfo;
+import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
+import com.baomidou.mybatisplus.extension.plugins.pagination.Page;
+import java.util.List;
+
+
+/**
+ * <p>
+ * 任务网格信息 服务类
+ * </p>
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+public interface ITaskGridInfoService extends IService<TaskGridInfo> {
+/**
+* 任务网格信息 分页检索
+*/
+List<TaskGridInfo> selectTaskGridInfoPage(Page<TaskGridInfo> page,QueryWrapper<TaskGridInfo> query);}
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/ITaskInfoService.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/ITaskInfoService.java
index 31d7ba6..b395993 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/ITaskInfoService.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/ITaskInfoService.java
@@ -51,6 +51,13 @@
      * @param dto 实体
      */
     void pointCallBack(PoseMessageResponseDTO dto, String robotId);
+
+    /**
+     * 车辆路线规划
+     * @param dto
+     * @param robotId
+     */
+    void carPathPlanning(PoseMessageResponseDTO dto, String robotId);
     void pointTestCallBack(PoseMessageResponseDTO dto, String robotId);
     /**
      * 图片路径保存
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/AlgorithmServiceImpl.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/AlgorithmServiceImpl.java
index f808fa0..a4dbe0b 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/AlgorithmServiceImpl.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/AlgorithmServiceImpl.java
@@ -29,10 +29,12 @@
 @Slf4j
 @Service
 public class AlgorithmServiceImpl implements IAlgorithmService {
-    @Value("${casic.algorithm.initPath:-1}")
-    private String initPath;
-    @Value("${casic.algorithm.planPath:-1}")
-    private String planPath;
+    @Value("${casic.algorithm.path:-1}")
+    private String path;
+    @Value("${casic.algorithm.initPathName:-1}")
+    private String initPathName;
+    @Value("${casic.algorithm.planPathName:-1}")
+    private String planPathName;
     private final ITaskHeatMapService heatMapService;
 
     public AlgorithmServiceImpl(ITaskHeatMapService heatMapService) {
@@ -42,13 +44,13 @@
     @Transactional
     @Override
     public AlgorithmResponse pathPlanningInit(PathInitDTO initDTO) {
-        if (StrUtil.isEmpty(initDTO.getCount())) {
-            initDTO.setCount("0.00");
+        if (StrUtil.isEmpty(initDTO.getZNow())) {
+            initDTO.setZNow("0.00");
         }
         log.info(JSON.toJSONString(initDTO));
         //算法路径及入参配置
         List<String> commends = new ArrayList<>();
-        commends.add(initPath);
+        commends.add(path + initPathName);
         commends.addAll(initDTO.toArray());
         List<String> res = AlgorithmUtils.invokeJob(commends);
 
@@ -63,38 +65,72 @@
     @Transactional
     @Override
     public AlgorithmResponse pathPlanning(PathPlanDTO planDTO) {
-        if (StrUtil.isEmpty(planDTO.getCount())) {
-            planDTO.setCount("0.001");
+        if (StrUtil.isEmpty(planDTO.getZNow())) {
+            planDTO.setZNow("0.001");
         }
         //算法路径及入参配置
         List<String> commends = new ArrayList<>();
-        commends.add(planPath);
+        commends.add(path + planPathName);
         commends.addAll(planDTO.toArray());
         log.info("寻源算法调用:{}", JSON.toJSONString(planDTO));
         List<String> res = AlgorithmUtils.invokeJob(commends);
         AlgorithmResponse response = getAlgorithmResponse(planDTO, res);
-        //next 点位编写
-        if (CollectionUtil.isNotEmpty(res) && res.size() == 2) {
-            response.getPointNextDTO().setX(formatBigDecimal(res.get(0).replaceAll("next_X: ", "")));
-            response.getPointNextDTO().setY(formatBigDecimal(res.get(1).replaceAll("next_Y: ", "")));
-        }
+
         return response;
     }
 
     private AlgorithmResponse getAlgorithmResponse(PathInitDTO initDTO, List<String> res) {
-        File repetitionFile = new File("repetition.txt");
+        //读取当前路径图片
+        File repetitionFileDir = new File(path + "\\" + initDTO.getTaskId() + "\\predicts");
+        File[] files = repetitionFileDir.listFiles((dir, name) -> name.endsWith(".txt") && name.startsWith("step"));
+        File repetitionFile = null;
+        //判定当前执行路径最大step2文件夹
+        if (files != null && files.length > 0) {
+             repetitionFile = files[files.length - 1];
+        }
         AlgorithmResponse response = new AlgorithmResponse();
         if (initDTO.getTaskId() != null && repetitionFile.exists()) {
-            readAlgorithmResponse("repetition.txt", response, initDTO.getTaskId(), res);
-            readAlgorithmResponse("step.txt", response, initDTO.getTaskId(), res);
+            //读取热力图
+            readAlgorithmResponse("repetition.txt",repetitionFile, response, initDTO.getTaskId(), res);
         } else {
             log.info("taskId is empty!");
         }
-        readAlgorithmResponse("xy_train.txt", response, initDTO.getTaskId(), res);
-        readAlgorithmResponse("z_train.txt", response, initDTO.getTaskId(), res);
+        //读取规划路径
+        if (CollectionUtil.isNotEmpty(res)) {
+            response.setEnd(false);
+            readAlgorithmResponse(response, res);
+        }
         return response;
     }
 
+    private void readAlgorithmResponse(AlgorithmResponse response, List<String> res) {
+        List<String> resList = new ArrayList<>();
+        if (res.size() >= 2) {
+            resList = res.subList(res.size() - 2, res.size());
+        }
+        //判定是否结束寻源
+        if (res.contains("end")) {
+            response.setEnd(true);
+            for (String s : resList) {
+                if (s.startsWith("source_coordinate:")) {
+                    s = s.trim();
+                    log.info("寻源结束,目标点位：{}",s);
+                    List<String> lists = StrUtil.split(s.replaceAll("source_coordinate:", ""), ",");
+                    PointNextDTO nextDTO = new PointNextDTO();
+                    nextDTO.setX(lists.get(0));
+                    nextDTO.setY(lists.get(1));
+                    response.setCoordinate(nextDTO);
+                }
+            }
+        } else {
+            PointNextDTO nextDTO = new PointNextDTO();
+            nextDTO.setX(resList.get(0));
+            nextDTO.setY(resList.get(1));
+            response.setPointNextDTO(nextDTO);
+        }
+
+    }
+
     /**
      * 读取算法输出数据
      *
@@ -102,49 +138,18 @@
      * @param response 结果集
      * @param taskId   任务ID
      */
-    public void readAlgorithmResponse(String fileName, AlgorithmResponse response, Long taskId, List<String> res) {
-        File file = new File(fileName);
-        if (!file.exists()&&!fileName.startsWith("step")) {
+    public void readAlgorithmResponse(String fileName,File repetitionFile, AlgorithmResponse response, Long taskId, List<String> res) {
+        if (!repetitionFile.exists() && !fileName.startsWith("step")) {
             log.info("{} file is empty!", fileName);
             return;
         }
         if (response.getPointNextDTO() == null) {
             response.setPointNextDTO(new PointNextDTO());
         }
-        switch (fileName) {
-            case "repetition.txt":
-                String msg = FileUtil.readString(file, CharsetUtil.CHARSET_UTF_8);
-                TaskHeatMap taskHeatMap = createEmptyTaskHeatMap(taskId);
-                taskHeatMap.setRepetition(msg);
-                response.setTaskHeatMap(taskHeatMap);
-                break;
-            case "xy_train.txt":
-                List<String> xyTrains = FileUtil.readLines(file, CharsetUtil.CHARSET_UTF_8);
-                if (CollectionUtil.isNotEmpty(xyTrains)) {
-                    if (CollectionUtil.isNotEmpty(xyTrains)) {
-                        List<String> list = StrUtil.split(xyTrains.get(xyTrains.size() - 1), " ");
-                        response.getPointNextDTO().setX(formatBigDecimal(list.get(0)));
-                        response.getPointNextDTO().setY(formatBigDecimal(list.get(1)));
-                    }
-                }
-                break;
-            case "z_train.txt":
-                List<String> zTrains = FileUtil.readLines(file, CharsetUtil.CHARSET_UTF_8);
-                if (CollectionUtil.isNotEmpty(zTrains)) {
-                    String zTrain = zTrains.get(zTrains.size() - 1);
-                    response.getPointNextDTO().setZ(formatBigDecimal(zTrain));
-                }
-            default:
-                if (StrUtil.isNotEmpty(fileName) && fileName.startsWith("step")) {
-                    File fileDir = new File(".");
-                    File[] files = fileDir.listFiles((dir, name) -> name.endsWith(".txt") && name.startsWith("step"));
-                    if (files != null && files.length > 0) {
-                        String steps = FileUtil.readString(files[files.length - 1], CharsetUtil.CHARSET_UTF_8);
-                        response.getTaskHeatMap().setRepetition(steps);
-                    }
-                }
-                break;
-        }
+        String msg = FileUtil.readString(repetitionFile, CharsetUtil.CHARSET_UTF_8);
+        TaskHeatMap taskHeatMap = createEmptyTaskHeatMap(taskId);
+        taskHeatMap.setRepetition(msg);
+        response.setTaskHeatMap(taskHeatMap);
     }
 
     /**
@@ -157,13 +162,11 @@
     public GridPointDTO convertSlamToGrid(BigDecimal x, BigDecimal y, BigDecimal resolution, RobotInfo robotInfo) {
         GridPointDTO pointDTO = new GridPointDTO();
         BigDecimal scale = resolution;
-
         //减去栅格原点坐标
         x = x.subtract(robotInfo.getOriginX());
         y = y.subtract(robotInfo.getOriginY());
-
-        pointDTO.setX(new BigDecimal(x.divide(scale,3,BigDecimal.ROUND_HALF_UP).intValue()+1));
-        pointDTO.setY(new BigDecimal(y.divide(scale,3,BigDecimal.ROUND_HALF_UP).intValue()+1));
+        pointDTO.setX(new BigDecimal(x.divide(scale, 3, BigDecimal.ROUND_HALF_UP).intValue()+1));
+        pointDTO.setY(new BigDecimal(y.divide(scale, 3, BigDecimal.ROUND_HALF_UP).intValue() +1));
         return pointDTO;
     }
 
@@ -174,19 +177,17 @@
      * @return
      */
     public GridPointDTO convertGridCenterPoint(GridPointDTO pointDTO, BigDecimal resolution, RobotInfo robotInfo) {
-
         GridPointDTO centerPoint = new GridPointDTO();
         BigDecimal scale = resolution;
-        //点位转换
-        BigDecimal slamX = pointDTO.getX().divide(scale, 5, BigDecimal.ROUND_HALF_UP);
-        BigDecimal slamY = pointDTO.getY().divide(scale, 5, BigDecimal.ROUND_HALF_UP);
+        //点位转换 转换后栅格－1 从1开始计算
+        BigDecimal slamX = (pointDTO.getX().subtract(new BigDecimal(1))).multiply(scale);
+        BigDecimal slamY = (pointDTO.getY().subtract(new BigDecimal(1))).multiply(scale);
         //中心栅格点位中心添加
-        centerPoint.setX(slamX.add(scale.divide(new BigDecimal(2), 5, BigDecimal.ROUND_HALF_UP)));
-        centerPoint.setY(slamY.add(scale.divide(new BigDecimal(2), 5, BigDecimal.ROUND_HALF_UP)));
+        centerPoint.setX(slamX.add(scale.divide(new BigDecimal(2), 10, BigDecimal.ROUND_HALF_UP)));
+        centerPoint.setY(slamY.add(scale.divide(new BigDecimal(2), 10, BigDecimal.ROUND_HALF_UP)));
         //加上栅格原点坐标
         centerPoint.setX(centerPoint.getX().add(robotInfo.getOriginX()));
         centerPoint.setY(centerPoint.getY().add(robotInfo.getOriginY()));
-
         return centerPoint;
     }
 

diff --git a/.gitignore b/.gitignore
index 9e20bc1..f42a9cb 100644
--- a/.gitignore
+++ b/.gitignore
@@ -2,3 +2,4 @@
 **/target/**
 logs/
 *.iml
+*.exe
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java b/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
index c7b5d28..a849528 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
@@ -28,6 +28,7 @@
     String KEY_CUR_TASK_ID = "CUR_TASK_ID_";
     String KEY_CUR_TASK_INIT = "CUR_TASK_INIT_";
 
+    String KEY_COST_MAP = "KEY_COST_MAP";
     /**
      * PSD数据ID
      */
@@ -41,6 +42,7 @@
      */
     String KEY_CUR_XY_POINT = "KEY_CUR_XY_POINT_";
     String KEY_CUR_ESTIMATE_GRID = "KEY_CUR_ESTIMATE_GRID_";
+    String KEY_PLANING_FLAG = "KEY_PLANING_FLAG_";
     /**
      * 机器人巡点栅格
      */
@@ -55,6 +57,7 @@
      * 探测器配置参数缓存
      */
     String KEY_CONFIG_PARAMS_ID = "KEY_CONFIG_PARAMS_ID_";
+    String KEY_CONFIG_COSTMAP_RESOLUTION_SET = "KEY_CONFIG_COSTMAP_RESOLUTION_SET_";
 
     /**
      * 暂停状态缓存信息
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
index c4c905d..3bb475f 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
@@ -1,9 +1,14 @@
 package com.casic.missiles.modular.robot.controller;
 
+import cn.hutool.core.lang.Assert;
 import cn.hutool.core.util.StrUtil;
 import com.casic.missiles.core.base.controller.BaseController;
+import com.casic.missiles.core.cache.CacheUtil;
 import com.casic.missiles.core.util.SpringContextHolder;
+import com.casic.missiles.model.exception.RequestValidException;
+import com.casic.missiles.model.response.ResponseData;
 import com.casic.missiles.model.response.dto.ResponseDataDTO;
+import com.casic.missiles.modular.constants.RobotDictConstants;
 import com.casic.missiles.modular.robot.IBaseRobotService;
 import com.casic.missiles.modular.robot.dto.RobotInfoSetDTO;
 import com.casic.missiles.modular.robot.model.RobotInfo;
@@ -19,10 +24,16 @@
 import io.swagger.annotations.ApiOperation;
 import io.swagger.annotations.ApiParam;
 import lombok.SneakyThrows;
+import lombok.extern.slf4j.Slf4j;
+import org.apache.ibatis.annotations.Param;
+import org.springframework.beans.factory.annotation.Value;
 import org.springframework.web.bind.annotation.*;
+import org.springframework.web.multipart.MultipartFile;
 
 import javax.servlet.http.HttpServletResponse;
+import java.io.File;
 import java.util.List;
+import java.util.Optional;
 
 /**
  * 机器人操作类
@@ -30,6 +41,7 @@
  * @author lwh
  * @date 2023-10-30
  */
+@Slf4j
 @Api(tags = "机器人控制接口")
 @RestController
 @RequestMapping("/robot/opt")
@@ -61,6 +73,28 @@
         return baseRobotService.execCmdHandler(request, InstructCodeEnums.CHANGE_PCD);
     }
 
+    @Value("${casic.file.uploadPath}")
+    private String filePath;
+
+    @SneakyThrows
+    @ApiOperation(value = "PCD上传接口")
+    @PostMapping(value = "/uploadPcd")
+    public ResponseData uploadPcd(@RequestParam("file") MultipartFile file, @ApiParam("机器人ID,url拼接传参") String robotId) {
+        Assert.isFalse(StrUtil.isEmpty(robotId), () -> new RequestValidException("机器ID不能为空", Optional.of(new Throwable().getStackTrace()[2])));
+        RobotInfo info = robotInfoService.getById(robotId);
+        if (file != null && !file.isEmpty()) {
+            File pcdFile = new File(filePath + "finalCloud_" + info.getId() + ".pcd");
+            try {
+                file.transferTo(pcdFile);
+            } catch (Exception e) {
+                log.error(e.getMessage());
+            }
+            return ResponseData.success();
+        } else {
+            return ResponseData.error("附件上传失败!");
+        }
+    }
+
     @SneakyThrows
     @ApiOperation(value = "PCD下载接口")
     @PostMapping(value = "/pcdDownload")
@@ -136,6 +170,10 @@
     @ApiOperation(value = "启动或关闭循迹功能", notes = "返回 1成功 0失败  wait 启动时是否等待，0代表循迹启动即运动，不等待；1代表循迹启动原地等待。若无此字段，默认循迹启动不等待直接运动")
     @PostMapping(value = "/startTrack")
     public ResponseDataDTO<Integer> startTrack(@RequestBody StartTrackRequest request) {
+        request.setIsTask(1);
+        if (request.getStepLen() == null) {
+            request.setStepLen(2);
+        }
         baseRobotService.startTrack(request);
         return ResponseDataDTO.success();
     }
@@ -156,6 +194,13 @@
         MessageRequestDTO<CrlNavRequest> messageDTO = new MessageRequestDTO();
         messageDTO.setMsg(request);
         ResponseDataDTO<Integer> list = baseRobotService.getRobotRouteOptService().robotCmd(info, InstructCodeEnums.CTRL_NAV, messageDTO);
+        if (request.getAction() == 1) {
+            CostMapResolutionRequest costMap = new CostMapResolutionRequest();
+            costMap.setResolution(info.getResolution().doubleValue());
+            request.setRobotId(request.getRobotId());
+            //是否下发分辨率
+            CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CONFIG_COSTMAP_RESOLUTION_SET + request.getRobotId(), costMap);
+        }
         return list;
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java
new file mode 100644
index 0000000..5317651
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java
@@ -0,0 +1,86 @@
+package com.casic.missiles.modular.robot.controller;
+
+import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
+import com.baomidou.mybatisplus.extension.plugins.pagination.Page;
+import com.casic.missiles.core.base.controller.BaseController;
+import com.casic.missiles.core.page.PageFactory;
+import com.casic.missiles.core.page.PageInfoBT;
+import com.casic.missiles.model.form.IdForms;
+import com.casic.missiles.model.response.dto.ResponseDataDTO;
+import com.casic.missiles.modular.robot.model.TaskGridInfo;
+import com.casic.missiles.modular.robot.service.ITaskGridInfoService;
+import com.casic.missiles.modular.robot.vo.TaskGridInfoVO;
+import io.swagger.annotations.Api;
+import io.swagger.annotations.ApiOperation;
+import org.springframework.web.bind.annotation.*;
+
+import java.util.List;
+
+/**
+ * 任务网格信息Controller
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+@Api(tags = "任务网格信息")
+@RestController
+@RequestMapping("/robot/taskGridInfo")
+public class TaskGridInfoController extends BaseController {
+
+    private final ITaskGridInfoService taskGridInfoService;
+
+    public TaskGridInfoController(ITaskGridInfoService taskGridInfoService) {
+        this.taskGridInfoService = taskGridInfoService;
+    }
+
+    @ApiOperation(value = "列表查询")
+    @GetMapping(value = "/list")
+    public ResponseDataDTO<List<TaskGridInfo>> list(TaskGridInfoVO taskGridInfoVO) {
+        QueryWrapper<TaskGridInfo> query = taskGridInfoVO.genQuery();
+        return ResponseDataDTO.success(taskGridInfoService.list(query));
+    }
+
+    @ApiOperation(value = "分页查询")
+    @GetMapping(value = "/listPage")
+    public ResponseDataDTO<PageInfoBT<TaskGridInfo>> listPage(TaskGridInfoVO taskGridInfoVO) {
+        Page<TaskGridInfo> page = PageFactory.defaultPage();
+        QueryWrapper<TaskGridInfo> query = taskGridInfoVO.genQuery();
+        page = taskGridInfoService.page(page, query);
+        return ResponseDataDTO.success(super.packForBT(page));
+    }
+
+
+    @ApiOperation(value = "新增接口")
+    @PostMapping(value = "/add")
+    public ResponseDataDTO add(@RequestBody TaskGridInfo taskGridInfo) {
+        taskGridInfoService.save(taskGridInfo);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "修改接口")
+    @PostMapping(value = "/update")
+    public ResponseDataDTO update(@RequestBody TaskGridInfo taskGridInfo) {
+        taskGridInfoService.updateById(taskGridInfo);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "删除接口")
+    @GetMapping(value = "/delete")
+    public ResponseDataDTO delete(String id) {
+        taskGridInfoService.removeById(id);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation("批量删除")
+    @PostMapping("/batchDelete")
+    public ResponseDataDTO batchDelete(@RequestBody IdForms ids) {
+        taskGridInfoService.removeByIds(ids.getIds());
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "详情查询")
+    @GetMapping(value = "/detail")
+    public ResponseDataDTO<TaskGridInfo> detail(String id) {
+        return ResponseDataDTO.success(taskGridInfoService.getById(id));
+    }
+}
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
index e82a792..6992274 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
@@ -98,7 +98,7 @@
     public ResponseDataDTO<AlgorithmResponse> test() {
         AlgorithmResponse response = new AlgorithmResponse();
         response.setTaskHeatMap(new TaskHeatMap());
-        algorithmService.readAlgorithmResponse("step3.txt",response,123L,null);
+        algorithmService.readAlgorithmResponse("step3.txt",null,response,123L,null);
         sendWebSocket(InstructStatusMsgEnums.HEAT_MAP, "1", response.getTaskHeatMap());
         return ResponseDataDTO.success(response);
     }
@@ -121,7 +121,7 @@
     @GetMapping(value = "/testGrid")
     public ResponseDataDTO<List<RouteInfo>> testGrid(@RequestBody PathInitDTO pathInitDTO) {
         RobotInfo robotInfo = infoService.getById(pathInitDTO.getRobotId());
-        GridPointDTO gridPointDTO = algorithmService.convertSlamToGrid(new BigDecimal(pathInitDTO.getX()), new BigDecimal(10), new BigDecimal(4), robotInfo);
+        GridPointDTO gridPointDTO = algorithmService.convertSlamToGrid(new BigDecimal(pathInitDTO.getXNow()), new BigDecimal(10), new BigDecimal(4), robotInfo);
         return ResponseDataDTO.success(algorithmService.convertGridCenterPoint(gridPointDTO, new BigDecimal(10), robotInfo));
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/TaskGridInfoMapper.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/TaskGridInfoMapper.java
new file mode 100644
index 0000000..5c3a70b
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/TaskGridInfoMapper.java
@@ -0,0 +1,23 @@
+package com.casic.missiles.modular.robot.dao;
+
+
+import com.baomidou.mybatisplus.core.mapper.BaseMapper;
+import com.casic.missiles.modular.robot.model.TaskGridInfo;
+import org.apache.ibatis.annotations.Param;
+import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
+import com.baomidou.mybatisplus.extension.plugins.pagination.Page;
+import java.util.List;
+/**
+ * <p>
+ *  任务网格信息 Mapper 接口
+ * </p>
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+public interface TaskGridInfoMapper extends BaseMapper<TaskGridInfo> {
+/**
+* 任务网格信息 分页检索
+*/
+List<TaskGridInfo> selectTaskGridInfoPage( @Param("page") Page<TaskGridInfo> page,@Param("ew" ) QueryWrapper<TaskGridInfo> query);
+}
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/mapping/TaskGridInfoMapper.xml b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/mapping/TaskGridInfoMapper.xml
new file mode 100644
index 0000000..be573e0
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/mapping/TaskGridInfoMapper.xml
@@ -0,0 +1,80 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!DOCTYPE mapper PUBLIC "-//mybatis.org//DTD Mapper 3.0//EN" "http://mybatis.org/dtd/mybatis-3-mapper.dtd">
+<mapper namespace="com.casic.missiles.modular.robot.dao.TaskGridInfoMapper">
+    <!-- 通用查询映射结果 -->
+    <resultMap id="BaseResultMap" type="com.casic.missiles.modular.robot.model.TaskGridInfo">
+        <result column="id" property="id"/>
+        <result column="create_time" property="createTime"/>
+        <result column="update_time" property="updateTime"/>
+        <result column="task_id" property="taskId"/>
+        <result column="estimate_grid_x" property="estimateGridX"/>
+        <result column="estimate_grid_y" property="estimateGridY"/>
+        <result column="neutron_count" property="neutronCount"/>
+        <result column="cur_grid_x" property="curGridX"/>
+        <result column="cur_grid_y" property="curGridY"/>
+    </resultMap>
+    <!-- 通用查询结果列 -->
+    <sql id="Base_Column_List">
+        id
+        ,
+                    create_time as createTime ,
+                    update_time as updateTime ,
+                    task_id as taskId ,
+                    estimate_grid_x as estimateGridX ,
+                    estimate_grid_y as estimateGridY ,
+                    neutron_count as neutronCount ,
+                    cur_grid_x as curGridX ,
+                    cur_grid_y as curGridY
+    </sql>
+    <sql id="Base_Column_Alias">
+        a
+        .
+        id
+        ,
+            a.create_time
+            as createTime ,
+            a.update_time
+            as updateTime ,
+            a.task_id
+            as taskId ,
+            a.estimate_grid_x
+            as estimateGridX ,
+            a.estimate_grid_y
+            as estimateGridY ,
+            a.neutron_count
+            as neutronCount ,
+            a.cur_grid_x
+            as curGridX ,
+            a.cur_grid_y
+            as curGridY
+    </sql>
+    <select id="selectTaskGridInfoPage" resultType="com.casic.missiles.modular.robot.model.TaskGridInfo">
+        select
+        <include refid="Base_Column_Alias"/>
+        from task_grid_info a
+        <where>
+            <if test="ew != null">
+                ${ew.sqlSegment}
+            </if>
+        </where>
+    </select>
+    <sql id="Base_Column_Query">
+        <if test="request!=null">
+            <if test="request.taskId!= null and request.taskId!=''">
+                and task_id = #{request.taskId}
+            </if>
+            <if test="request.estimateGridX!= null and request.estimateGridX!=''">
+                and estimate_grid_x = #{request.estimateGridX}
+            </if>
+            <if test="request.estimateGridY!= null and request.estimateGridY!=''">
+                and estimate_grid_y = #{request.estimateGridY}
+            </if>
+            <if test="request.curGridX!= null and request.curGridX!=''">
+                and cur_grid_x = #{request.curGridX}
+            </if>
+            <if test="request.curGridY!= null and request.curGridY!=''">
+                and cur_grid_y = #{request.curGridY}
+            </if>
+        </if>
+    </sql>
+</mapper>
\ No newline at end of file
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java
index 749f559..4559074 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java
@@ -13,4 +13,9 @@
      * 规划点位信息
      */
     private PointNextDTO pointNextDTO;
+    /**
+     * 寻源目标地址
+     */
+    private PointNextDTO coordinate;
+    private boolean isEnd;
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java
index 39ff2a2..cef3238 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java
@@ -12,6 +12,9 @@
     private BigDecimal y;
 
     public static GridPointDTO convertGridPoint(PointNextDTO nextDTO) {
+        if(nextDTO==null){
+            return null;
+        }
         GridPointDTO gridPointDTO = new GridPointDTO();
         gridPointDTO.setX(Convert.toBigDecimal(nextDTO.getX()));
         gridPointDTO.setY(Convert.toBigDecimal(nextDTO.getY()));
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java
index 8c88242..33be25d 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java
@@ -12,25 +12,19 @@
 public class PathInitDTO {
     //---------------------算法参数
     /**
-     * x坐标最大值（x方向栅格总数）
+     * 初始位置x坐标(栅格地图下的坐标)
      */
-    private String xMax;
+    private String xNow;
     /**
-     * y坐标最大值（y方向栅格总数）
+     * 初始位置y坐标(栅格地图下的坐标)
      */
-    private String yMax;
+    private String yNow;
     /**
      * 初始位置计数率
      */
-    private String count;
-    /**
-     * 初始位置x坐标(0<=x<x_max)
-     */
-    private String x;
-    /**
-     * 初始位置y坐标(0<=y<y_max)
-     */
-    private String y;
+    private String zNow;
+
+    private Integer stepLen;
 
     //---------------------其他执行参数
     private String robotId;
@@ -38,11 +32,11 @@
 
     public List<String> toArray() {
         List<String> list = new ArrayList<>();
-        list.add(xMax);
-        list.add(yMax);
-        list.add(count);
-        list.add(x);
-        list.add(y);
+        list.add(xNow);
+        list.add(yNow);
+        list.add(stepLen+"");
+        list.add(zNow);
+        list.add(taskId+"");
         return list;
     }
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java
index 109e95c..ada6774 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java
@@ -10,21 +10,17 @@
  */
 @Data
 public class PathPlanDTO extends PathInitDTO {
-
     /**
-     * 长度为8的数组，记录(x-1, y-1), (x-1, y), (x-1, y+1), (x, y-1), (x, y+1), (x+1, y-1), (x+1, y), (x+1, y+1)8个坐标的可通行信息。
-     * 若某坐标有障碍物（不可通行行）则记为0，否则记为1
+     * 浮点阈值(阈值，超过该值后结束巡检开始寻源)
      */
-    private String open;
-
+    private String th;
     public List<String> toArray() {
         List<String> list = new ArrayList<>();
-        list.add(getXMax());
-        list.add(getYMax());
-        list.add(open);
-        list.add(getCount());
-        list.add(getX());
-        list.add(getY());
+
+        list.add(getStepLen()+"");
+        list.add(getTh());
+        list.add(getZNow());
+        list.add(getTaskId()+"");
         return list;
     }
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskGridInfo.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskGridInfo.java
new file mode 100644
index 0000000..101c10a
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskGridInfo.java
@@ -0,0 +1,83 @@
+package com.casic.missiles.modular.robot.model;
+
+import com.baomidou.mybatisplus.annotation.TableField;
+import com.baomidou.mybatisplus.annotation.TableName;
+import com.baomidou.mybatisplus.extension.activerecord.Model;
+import io.swagger.annotations.ApiModelProperty;
+import lombok.Data;
+
+import java.math.BigDecimal;
+import java.util.Date;
+
+/**
+ * 任务网格信息对象 task_grid_info
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+@Data
+@TableName("task_grid_info")
+public class TaskGridInfo extends Model<TaskGridInfo> {
+    private static final long serialVersionUID = 1L;
+    @TableField(exist = false)
+    private Integer index;
+    @ApiModelProperty(value = "主键", dataType = "Long")
+    @TableField("id")
+    private Long id;
+
+    @ApiModelProperty(value = "创建时间", dataType = "Date")
+    @TableField("create_time")
+    private Date createTime;
+
+    @ApiModelProperty(value = "修改时间", dataType = "Date")
+    @TableField("update_time")
+    private Date updateTime;
+
+    @ApiModelProperty(value = "任务ID", dataType = "Long")
+    @TableField("task_id")
+    private Long taskId;
+
+    @ApiModelProperty(value = "估算栅格x", dataType = "Long")
+    @TableField("estimate_grid_x")
+    private String estimateGridX;
+
+    @ApiModelProperty(value = "估算栅格y", dataType = "Long")
+    @TableField("estimate_grid_y")
+    private String estimateGridY;
+
+    @ApiModelProperty(value = "中子计数率", dataType = "BigDecimal")
+    @TableField("neutron_count")
+    private BigDecimal neutronCount;
+
+    @ApiModelProperty(value = "当前栅格x", dataType = "Long")
+    @TableField("cur_grid_x")
+    private Integer curGridX;
+
+    @ApiModelProperty(value = "当前栅格y", dataType = "Long")
+    @TableField("cur_grid_y")
+    private Integer curGridY;
+
+    public String getCurPoint(){
+        return "("+curGridX+","+curGridY+")";
+    }
+    public String getEstimatePoint(){
+        if(estimateGridX==null&estimateGridY==null){
+            return "结束";
+        }
+        return "("+estimateGridX+","+estimateGridY+")";
+    }
+    @Override
+    public String toString() {
+        return "TaskGridInfo{"
+                + "id=" + id
+                + "createTime=" + createTime
+                + "updateTime=" + updateTime
+                + "taskId=" + taskId
+                + "estimateGridX=" + estimateGridX
+                + "estimateGridY=" + estimateGridY
+                + "neutronCount=" + neutronCount
+                + "curGridX=" + curGridX
+                + "curGridY=" + curGridY
+                + "}";
+    }
+}
\ No newline at end of file
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java
index edc104e..a8acb2a 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java
@@ -53,6 +53,13 @@
     @TableField("route_id")
     private Long routeId;
 
+    @ApiModelProperty(value = "寻源初始步长", dataType = "Integer")
+    @TableField("step_len")
+    private Integer stepLen;
+
+    @ApiModelProperty(value = "寻源阈值设定", dataType = "BigDecimal")
+    @TableField("max_threshold")
+    private BigDecimal maxThreshold;
     /**
      * 任务类型(1巡检任务)
      */
@@ -62,7 +69,6 @@
     @ApiModelProperty(value = "任务类型字典名称", dataType = "String")
     @TableField(exist = false)
     private String taskTypeName;
-
     @ApiModelProperty(value = "识别类型(discernType 1中子源识别)", dataType = "Integer")
     @TableField("discern_type")
     private Integer discernType;
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java
index c4bc25d..c6620f3 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java
@@ -53,7 +53,7 @@
     }),
     SET_SPEED("/set_speed" , "/set_speed_response" , "最大运动速度设置" , true, new TypeReference<RobotMsgResponseDTO<Integer>>() {
     }),
-    SET_TARGETPOINT("/set_targetpoint" , "/set_targetpoint_response" , "设置导航目标点" , false, new TypeReference<RobotMsgResponseDTO<Integer>>() {
+    SET_TARGETPOINT("/set_targetpoint" , "/set_targetpoint_response" , "设置导航目标点" , true, new TypeReference<RobotMsgResponseDTO<Integer>>() {
     }),
 
     GET_DATA_LIST("/get_data_list" , "/get_data_list_response" , "获取数据包列表" , true, new TypeReference<RobotMsgResponseDTO<List<String>>>() {
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/AbstractSocketHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/AbstractSocketHandler.java
index 83f4cb2..e47e343 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/AbstractSocketHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/AbstractSocketHandler.java
@@ -49,7 +49,7 @@
                 wsMsgBaseDTO.setMsgKey(enums.getMsgKey());
                 wsMsgBaseDTO.setData(jsonObject.get("msg"));
                 String msg = JSON.toJSONString(wsMsgBaseDTO);
-                log.info("推送{}消息：{}", enums.getRemarks(), msg);
+//                log.info("推送{}消息：{}", enums.getRemarks(), msg);
                 webSocket.sendOneMessage(robotId, msg);
             }
         } catch (Exception e) {
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/cmd/TargetPointCmdHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/cmd/TargetPointCmdHandler.java
index c1c09fa..43b4460 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/cmd/TargetPointCmdHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/cmd/TargetPointCmdHandler.java
@@ -34,7 +34,11 @@
     @Override
     public ResponseDataDTO senderCmd(TargetPointRequest request, InstructCodeEnums enums) {
         MessageRequestDTO<TargetPointRequest> messageDTO = new MessageRequestDTO();
-        //读取默认关键点信息
+        //读取默认关键点信息 {"x":"1.715708819927424","y":"-0.9100042544250901","z":"0.017993879527185876","theta":"-0.039278239414312105"}
+//        request.setX("1.715708819927424");
+//        request.setY("-0.9100042544250901");
+//        request.setZ("0.017993879527185876");
+//        request.setTheta("-0.039278239414312105");
         messageDTO.setMsg(request);
         ResponseDataDTO<Integer> list = optService.robotCmd(getRobotInfo(), enums, messageDTO);
         return list;
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/AppCostMapHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/AppCostMapHandler.java
index 0335693..88c9f79 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/AppCostMapHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/AppCostMapHandler.java
@@ -1,6 +1,11 @@
 package com.casic.missiles.modular.robot.opt.handler.msg;
 
+import com.casic.missiles.core.cache.CacheUtil;
+import com.casic.missiles.modular.constants.RobotDictConstants;
+import com.casic.missiles.modular.robot.IBaseRobotService;
+import com.casic.missiles.modular.robot.opt.enums.InstructCodeEnums;
 import com.casic.missiles.modular.robot.opt.handler.AbstractSocketHandler;
+import com.casic.missiles.modular.robot.opt.instruct.dto.CostMapResolutionRequest;
 import com.casic.missiles.modular.robot.service.IRobotStatusInfoService;
 import lombok.extern.slf4j.Slf4j;
 import org.eclipse.paho.client.mqttv3.MqttMessage;
@@ -16,14 +21,27 @@
 @Component("/app_costmap_sender")
 public class AppCostMapHandler extends AbstractSocketHandler {
     private final IRobotStatusInfoService statusInfoService;
+    private final IBaseRobotService baseRobotService;
     private static Map<String, Integer> index = new ConcurrentHashMap<>();
 
-    public AppCostMapHandler(IRobotStatusInfoService statusInfoService) {
+    public AppCostMapHandler(IRobotStatusInfoService statusInfoService, IBaseRobotService baseRobotService) {
         this.statusInfoService = statusInfoService;
+        this.baseRobotService = baseRobotService;
     }
 
     @Override
     public void process(String robotId, String topic, MqttMessage message) {
         statusInfoService.saveCostMap(robotId, message);
+
+        CostMapResolutionRequest costMapSet = CacheUtil.get(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CONFIG_COSTMAP_RESOLUTION_SET +robotId);
+        if(costMapSet != null){
+            CacheUtil.remove(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CONFIG_COSTMAP_RESOLUTION_SET +robotId);
+            new Thread(() -> {
+                costMapSet.setRobotId(Long.valueOf(robotId));
+                baseRobotService.execCmdHandler(costMapSet, InstructCodeEnums.COSTMAP_RESOLUTION);
+            }).start();
+
+        }
+
     }
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/PoseMessageMsgHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/PoseMessageMsgHandler.java
index 29f8919..6c3c3bd 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/PoseMessageMsgHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/PoseMessageMsgHandler.java
@@ -17,11 +17,9 @@
 @Component("/pose_message")
 public class PoseMessageMsgHandler extends AbstractSocketHandler {
 
-
     private final EventPublisher publisher;
 
     public PoseMessageMsgHandler(EventPublisher publisher) {
-
         this.publisher = publisher;
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/StopStateMsgHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/StopStateMsgHandler.java
index 8950815..8a71e0b 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/StopStateMsgHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/StopStateMsgHandler.java
@@ -30,7 +30,7 @@
     public void process(String robotId, String topic, MqttMessage message) {
 
         index.put(index_key, index.get("index") + 1);
-        if (index.get(index_key)>50) {
+        if (index.get(index_key)>30) {
             index.put(index_key, 0);
             log.info("主键：{},停障状态topic:{},消息内容:{}", robotId, topic, message.toString());
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/impl/RobotOptServiceImpl.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/impl/RobotOptServiceImpl.java
index 8471e7a..7940a24 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/impl/RobotOptServiceImpl.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/impl/RobotOptServiceImpl.java
@@ -146,7 +146,7 @@
     public void startTrack(StartTrackRequest request) {
         //增加循迹任务记录
         taskInfoService.reloadTask(request, robotInfoService.getById(request.getRobotId()));
-        this.execCmdHandler(request, InstructCodeEnums.START_TRACK);
+//        this.execCmdHandler(request, InstructCodeEnums.START_TRACK);
     }
 
     public IRobotRouteOptService getRobotRouteOptService() {
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/BaseRobotCmdDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/BaseRobotCmdDTO.java
index 5608f2b..06b53de 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/BaseRobotCmdDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/BaseRobotCmdDTO.java
@@ -3,11 +3,13 @@
 import io.swagger.annotations.ApiModelProperty;
 import lombok.Data;
 
+import java.io.Serializable;
+
 /**
  * 机器人指令基类
  */
 @Data
-public class BaseRobotCmdDTO {
+public class BaseRobotCmdDTO implements Serializable {
     @ApiModelProperty(value = "机器人主键", dataType = "String")
     protected Long robotId;
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/StartTrackRequest.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/StartTrackRequest.java
index a18321b..85546a7 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/StartTrackRequest.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/StartTrackRequest.java
@@ -3,6 +3,8 @@
 import io.swagger.annotations.ApiModelProperty;
 import lombok.Data;
 
+import java.math.BigDecimal;
+
 /**
  * 导航下发指令
  */
@@ -10,18 +12,19 @@
 public class StartTrackRequest extends BaseRobotCmdDTO {
     @ApiModelProperty(value = "sn编码(可不填 默认机器ID)", dataType = "String")
     private String sn;
-
     @ApiModelProperty(value = "下发状态 1:开启 0:关闭", dataType = "String")
     private Integer action;
     @ApiModelProperty(value = "等待状态 1:等待 0:不等待", dataType = "String")
     private Integer wait;
     @ApiModelProperty(value = "等待状态 0:循迹 1:任务", dataType = "isTask")
     private Integer isTask;
-    @ApiModelProperty(value = "轨迹名称", dataType = "String")
+    @ApiModelProperty(value = "轨迹名称(废弃)", dataType = "String")
     private String track_name;
     @ApiModelProperty(value = "关键点名称", dataType = "String", hidden = true)
     private String taskpoint_name;
-
+    @ApiModelProperty(value = "初始寻源步长", dataType = "Integer")
+    private Integer stepLen;
+    @ApiModelProperty(value = "寻源阈值", dataType = "BigDecimal")
+    private BigDecimal maxThreshold;
     private String obs_mode="1";
-
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/response/StopStateResponseDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/response/StopStateResponseDTO.java
index eb514bf..e0679d7 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/response/StopStateResponseDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/response/StopStateResponseDTO.java
@@ -3,11 +3,13 @@
 import io.swagger.annotations.ApiModelProperty;
 import lombok.Data;
 
+import java.io.Serializable;
+
 /**
  * 电压信息返回
  */
 @Data
-public class StopStateResponseDTO {
+public class StopStateResponseDTO implements Serializable {
     @ApiModelProperty(value = "软件急停(0：无触发 1：触发)" , dataType = "Integer")
     private Integer soft;
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IAlgorithmService.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IAlgorithmService.java
index 56b3e1d..f62523b 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IAlgorithmService.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IAlgorithmService.java
@@ -6,6 +6,7 @@
 import com.casic.missiles.modular.robot.dto.PathPlanDTO;
 import com.casic.missiles.modular.robot.model.RobotInfo;
 
+import java.io.File;
 import java.math.BigDecimal;
 import java.util.List;
 
@@ -38,5 +39,5 @@
      * @return
      */
     GridPointDTO convertGridCenterPoint(GridPointDTO pointDTO, BigDecimal resolution, RobotInfo robotInfo);
-    void readAlgorithmResponse(String fileName, AlgorithmResponse response, Long taskId, List<String> res);
+    void readAlgorithmResponse(String fileName, File repetitionFile, AlgorithmResponse response, Long taskId, List<String> res);
 }
\ No newline at end of file
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IRobotStatusInfoService.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IRobotStatusInfoService.java
index 2af210a..757f8d0 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IRobotStatusInfoService.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IRobotStatusInfoService.java
@@ -67,10 +67,9 @@
     /**
      * 更新车辆路线及任务信息
      *
-     * @param currRouteId 当前路线信息
      * @param currTaskId  当前任务信息
      */
-    void updateCurrTask(String robotId, Long currRouteId, Long currTaskId);
+    void updateCurrTask(String robotId, Long currTaskId);
 
     RobotStatusInfo getStatusByRobotId(String robotId);
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/ITaskGridInfoService.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/ITaskGridInfoService.java
new file mode 100644
index 0000000..fc0da93
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/ITaskGridInfoService.java
@@ -0,0 +1,22 @@
+package com.casic.missiles.modular.robot.service;
+
+import com.baomidou.mybatisplus.extension.service.IService;
+import  com.casic.missiles.modular.robot.model.TaskGridInfo;
+import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
+import com.baomidou.mybatisplus.extension.plugins.pagination.Page;
+import java.util.List;
+
+
+/**
+ * <p>
+ * 任务网格信息 服务类
+ * </p>
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+public interface ITaskGridInfoService extends IService<TaskGridInfo> {
+/**
+* 任务网格信息 分页检索
+*/
+List<TaskGridInfo> selectTaskGridInfoPage(Page<TaskGridInfo> page,QueryWrapper<TaskGridInfo> query);}
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/ITaskInfoService.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/ITaskInfoService.java
index 31d7ba6..b395993 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/ITaskInfoService.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/ITaskInfoService.java
@@ -51,6 +51,13 @@
      * @param dto 实体
      */
     void pointCallBack(PoseMessageResponseDTO dto, String robotId);
+
+    /**
+     * 车辆路线规划
+     * @param dto
+     * @param robotId
+     */
+    void carPathPlanning(PoseMessageResponseDTO dto, String robotId);
     void pointTestCallBack(PoseMessageResponseDTO dto, String robotId);
     /**
      * 图片路径保存
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/AlgorithmServiceImpl.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/AlgorithmServiceImpl.java
index f808fa0..a4dbe0b 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/AlgorithmServiceImpl.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/AlgorithmServiceImpl.java
@@ -29,10 +29,12 @@
 @Slf4j
 @Service
 public class AlgorithmServiceImpl implements IAlgorithmService {
-    @Value("${casic.algorithm.initPath:-1}")
-    private String initPath;
-    @Value("${casic.algorithm.planPath:-1}")
-    private String planPath;
+    @Value("${casic.algorithm.path:-1}")
+    private String path;
+    @Value("${casic.algorithm.initPathName:-1}")
+    private String initPathName;
+    @Value("${casic.algorithm.planPathName:-1}")
+    private String planPathName;
     private final ITaskHeatMapService heatMapService;
 
     public AlgorithmServiceImpl(ITaskHeatMapService heatMapService) {
@@ -42,13 +44,13 @@
     @Transactional
     @Override
     public AlgorithmResponse pathPlanningInit(PathInitDTO initDTO) {
-        if (StrUtil.isEmpty(initDTO.getCount())) {
-            initDTO.setCount("0.00");
+        if (StrUtil.isEmpty(initDTO.getZNow())) {
+            initDTO.setZNow("0.00");
         }
         log.info(JSON.toJSONString(initDTO));
         //算法路径及入参配置
         List<String> commends = new ArrayList<>();
-        commends.add(initPath);
+        commends.add(path + initPathName);
         commends.addAll(initDTO.toArray());
         List<String> res = AlgorithmUtils.invokeJob(commends);
 
@@ -63,38 +65,72 @@
     @Transactional
     @Override
     public AlgorithmResponse pathPlanning(PathPlanDTO planDTO) {
-        if (StrUtil.isEmpty(planDTO.getCount())) {
-            planDTO.setCount("0.001");
+        if (StrUtil.isEmpty(planDTO.getZNow())) {
+            planDTO.setZNow("0.001");
         }
         //算法路径及入参配置
         List<String> commends = new ArrayList<>();
-        commends.add(planPath);
+        commends.add(path + planPathName);
         commends.addAll(planDTO.toArray());
         log.info("寻源算法调用:{}", JSON.toJSONString(planDTO));
         List<String> res = AlgorithmUtils.invokeJob(commends);
         AlgorithmResponse response = getAlgorithmResponse(planDTO, res);
-        //next 点位编写
-        if (CollectionUtil.isNotEmpty(res) && res.size() == 2) {
-            response.getPointNextDTO().setX(formatBigDecimal(res.get(0).replaceAll("next_X: ", "")));
-            response.getPointNextDTO().setY(formatBigDecimal(res.get(1).replaceAll("next_Y: ", "")));
-        }
+
         return response;
     }
 
     private AlgorithmResponse getAlgorithmResponse(PathInitDTO initDTO, List<String> res) {
-        File repetitionFile = new File("repetition.txt");
+        //读取当前路径图片
+        File repetitionFileDir = new File(path + "\\" + initDTO.getTaskId() + "\\predicts");
+        File[] files = repetitionFileDir.listFiles((dir, name) -> name.endsWith(".txt") && name.startsWith("step"));
+        File repetitionFile = null;
+        //判定当前执行路径最大step2文件夹
+        if (files != null && files.length > 0) {
+             repetitionFile = files[files.length - 1];
+        }
         AlgorithmResponse response = new AlgorithmResponse();
         if (initDTO.getTaskId() != null && repetitionFile.exists()) {
-            readAlgorithmResponse("repetition.txt", response, initDTO.getTaskId(), res);
-            readAlgorithmResponse("step.txt", response, initDTO.getTaskId(), res);
+            //读取热力图
+            readAlgorithmResponse("repetition.txt",repetitionFile, response, initDTO.getTaskId(), res);
         } else {
             log.info("taskId is empty!");
         }
-        readAlgorithmResponse("xy_train.txt", response, initDTO.getTaskId(), res);
-        readAlgorithmResponse("z_train.txt", response, initDTO.getTaskId(), res);
+        //读取规划路径
+        if (CollectionUtil.isNotEmpty(res)) {
+            response.setEnd(false);
+            readAlgorithmResponse(response, res);
+        }
         return response;
     }
 
+    private void readAlgorithmResponse(AlgorithmResponse response, List<String> res) {
+        List<String> resList = new ArrayList<>();
+        if (res.size() >= 2) {
+            resList = res.subList(res.size() - 2, res.size());
+        }
+        //判定是否结束寻源
+        if (res.contains("end")) {
+            response.setEnd(true);
+            for (String s : resList) {
+                if (s.startsWith("source_coordinate:")) {
+                    s = s.trim();
+                    log.info("寻源结束,目标点位：{}",s);
+                    List<String> lists = StrUtil.split(s.replaceAll("source_coordinate:", ""), ",");
+                    PointNextDTO nextDTO = new PointNextDTO();
+                    nextDTO.setX(lists.get(0));
+                    nextDTO.setY(lists.get(1));
+                    response.setCoordinate(nextDTO);
+                }
+            }
+        } else {
+            PointNextDTO nextDTO = new PointNextDTO();
+            nextDTO.setX(resList.get(0));
+            nextDTO.setY(resList.get(1));
+            response.setPointNextDTO(nextDTO);
+        }
+
+    }
+
     /**
      * 读取算法输出数据
      *
@@ -102,49 +138,18 @@
      * @param response 结果集
      * @param taskId   任务ID
      */
-    public void readAlgorithmResponse(String fileName, AlgorithmResponse response, Long taskId, List<String> res) {
-        File file = new File(fileName);
-        if (!file.exists()&&!fileName.startsWith("step")) {
+    public void readAlgorithmResponse(String fileName,File repetitionFile, AlgorithmResponse response, Long taskId, List<String> res) {
+        if (!repetitionFile.exists() && !fileName.startsWith("step")) {
             log.info("{} file is empty!", fileName);
             return;
         }
         if (response.getPointNextDTO() == null) {
             response.setPointNextDTO(new PointNextDTO());
         }
-        switch (fileName) {
-            case "repetition.txt":
-                String msg = FileUtil.readString(file, CharsetUtil.CHARSET_UTF_8);
-                TaskHeatMap taskHeatMap = createEmptyTaskHeatMap(taskId);
-                taskHeatMap.setRepetition(msg);
-                response.setTaskHeatMap(taskHeatMap);
-                break;
-            case "xy_train.txt":
-                List<String> xyTrains = FileUtil.readLines(file, CharsetUtil.CHARSET_UTF_8);
-                if (CollectionUtil.isNotEmpty(xyTrains)) {
-                    if (CollectionUtil.isNotEmpty(xyTrains)) {
-                        List<String> list = StrUtil.split(xyTrains.get(xyTrains.size() - 1), " ");
-                        response.getPointNextDTO().setX(formatBigDecimal(list.get(0)));
-                        response.getPointNextDTO().setY(formatBigDecimal(list.get(1)));
-                    }
-                }
-                break;
-            case "z_train.txt":
-                List<String> zTrains = FileUtil.readLines(file, CharsetUtil.CHARSET_UTF_8);
-                if (CollectionUtil.isNotEmpty(zTrains)) {
-                    String zTrain = zTrains.get(zTrains.size() - 1);
-                    response.getPointNextDTO().setZ(formatBigDecimal(zTrain));
-                }
-            default:
-                if (StrUtil.isNotEmpty(fileName) && fileName.startsWith("step")) {
-                    File fileDir = new File(".");
-                    File[] files = fileDir.listFiles((dir, name) -> name.endsWith(".txt") && name.startsWith("step"));
-                    if (files != null && files.length > 0) {
-                        String steps = FileUtil.readString(files[files.length - 1], CharsetUtil.CHARSET_UTF_8);
-                        response.getTaskHeatMap().setRepetition(steps);
-                    }
-                }
-                break;
-        }
+        String msg = FileUtil.readString(repetitionFile, CharsetUtil.CHARSET_UTF_8);
+        TaskHeatMap taskHeatMap = createEmptyTaskHeatMap(taskId);
+        taskHeatMap.setRepetition(msg);
+        response.setTaskHeatMap(taskHeatMap);
     }
 
     /**
@@ -157,13 +162,11 @@
     public GridPointDTO convertSlamToGrid(BigDecimal x, BigDecimal y, BigDecimal resolution, RobotInfo robotInfo) {
         GridPointDTO pointDTO = new GridPointDTO();
         BigDecimal scale = resolution;
-
         //减去栅格原点坐标
         x = x.subtract(robotInfo.getOriginX());
         y = y.subtract(robotInfo.getOriginY());
-
-        pointDTO.setX(new BigDecimal(x.divide(scale,3,BigDecimal.ROUND_HALF_UP).intValue()+1));
-        pointDTO.setY(new BigDecimal(y.divide(scale,3,BigDecimal.ROUND_HALF_UP).intValue()+1));
+        pointDTO.setX(new BigDecimal(x.divide(scale, 3, BigDecimal.ROUND_HALF_UP).intValue()+1));
+        pointDTO.setY(new BigDecimal(y.divide(scale, 3, BigDecimal.ROUND_HALF_UP).intValue() +1));
         return pointDTO;
     }
 
@@ -174,19 +177,17 @@
      * @return
      */
     public GridPointDTO convertGridCenterPoint(GridPointDTO pointDTO, BigDecimal resolution, RobotInfo robotInfo) {
-
         GridPointDTO centerPoint = new GridPointDTO();
         BigDecimal scale = resolution;
-        //点位转换
-        BigDecimal slamX = pointDTO.getX().divide(scale, 5, BigDecimal.ROUND_HALF_UP);
-        BigDecimal slamY = pointDTO.getY().divide(scale, 5, BigDecimal.ROUND_HALF_UP);
+        //点位转换 转换后栅格－1 从1开始计算
+        BigDecimal slamX = (pointDTO.getX().subtract(new BigDecimal(1))).multiply(scale);
+        BigDecimal slamY = (pointDTO.getY().subtract(new BigDecimal(1))).multiply(scale);
         //中心栅格点位中心添加
-        centerPoint.setX(slamX.add(scale.divide(new BigDecimal(2), 5, BigDecimal.ROUND_HALF_UP)));
-        centerPoint.setY(slamY.add(scale.divide(new BigDecimal(2), 5, BigDecimal.ROUND_HALF_UP)));
+        centerPoint.setX(slamX.add(scale.divide(new BigDecimal(2), 10, BigDecimal.ROUND_HALF_UP)));
+        centerPoint.setY(slamY.add(scale.divide(new BigDecimal(2), 10, BigDecimal.ROUND_HALF_UP)));
         //加上栅格原点坐标
         centerPoint.setX(centerPoint.getX().add(robotInfo.getOriginX()));
         centerPoint.setY(centerPoint.getY().add(robotInfo.getOriginY()));
-
         return centerPoint;
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotInfoServiceImpl.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotInfoServiceImpl.java
index 043e05d..fdded64 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotInfoServiceImpl.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotInfoServiceImpl.java
@@ -104,7 +104,7 @@
             //下发障碍物栅格
             sendCostMapResolution(robotInfo.getResolution(), robotInfo.getId());
             //重新生成栅格地图
-            sendChangePcd(robotInfo.getId());
+            //sendChangePcd(robotInfo.getId());
         }
         update.eq(RobotInfo::getId, robotInfo.getId());
         update(update);

diff --git a/.gitignore b/.gitignore
index 9e20bc1..f42a9cb 100644
--- a/.gitignore
+++ b/.gitignore
@@ -2,3 +2,4 @@
 **/target/**
 logs/
 *.iml
+*.exe
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java b/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
index c7b5d28..a849528 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
@@ -28,6 +28,7 @@
     String KEY_CUR_TASK_ID = "CUR_TASK_ID_";
     String KEY_CUR_TASK_INIT = "CUR_TASK_INIT_";
 
+    String KEY_COST_MAP = "KEY_COST_MAP";
     /**
      * PSD数据ID
      */
@@ -41,6 +42,7 @@
      */
     String KEY_CUR_XY_POINT = "KEY_CUR_XY_POINT_";
     String KEY_CUR_ESTIMATE_GRID = "KEY_CUR_ESTIMATE_GRID_";
+    String KEY_PLANING_FLAG = "KEY_PLANING_FLAG_";
     /**
      * 机器人巡点栅格
      */
@@ -55,6 +57,7 @@
      * 探测器配置参数缓存
      */
     String KEY_CONFIG_PARAMS_ID = "KEY_CONFIG_PARAMS_ID_";
+    String KEY_CONFIG_COSTMAP_RESOLUTION_SET = "KEY_CONFIG_COSTMAP_RESOLUTION_SET_";
 
     /**
      * 暂停状态缓存信息
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
index c4c905d..3bb475f 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
@@ -1,9 +1,14 @@
 package com.casic.missiles.modular.robot.controller;
 
+import cn.hutool.core.lang.Assert;
 import cn.hutool.core.util.StrUtil;
 import com.casic.missiles.core.base.controller.BaseController;
+import com.casic.missiles.core.cache.CacheUtil;
 import com.casic.missiles.core.util.SpringContextHolder;
+import com.casic.missiles.model.exception.RequestValidException;
+import com.casic.missiles.model.response.ResponseData;
 import com.casic.missiles.model.response.dto.ResponseDataDTO;
+import com.casic.missiles.modular.constants.RobotDictConstants;
 import com.casic.missiles.modular.robot.IBaseRobotService;
 import com.casic.missiles.modular.robot.dto.RobotInfoSetDTO;
 import com.casic.missiles.modular.robot.model.RobotInfo;
@@ -19,10 +24,16 @@
 import io.swagger.annotations.ApiOperation;
 import io.swagger.annotations.ApiParam;
 import lombok.SneakyThrows;
+import lombok.extern.slf4j.Slf4j;
+import org.apache.ibatis.annotations.Param;
+import org.springframework.beans.factory.annotation.Value;
 import org.springframework.web.bind.annotation.*;
+import org.springframework.web.multipart.MultipartFile;
 
 import javax.servlet.http.HttpServletResponse;
+import java.io.File;
 import java.util.List;
+import java.util.Optional;
 
 /**
  * 机器人操作类
@@ -30,6 +41,7 @@
  * @author lwh
  * @date 2023-10-30
  */
+@Slf4j
 @Api(tags = "机器人控制接口")
 @RestController
 @RequestMapping("/robot/opt")
@@ -61,6 +73,28 @@
         return baseRobotService.execCmdHandler(request, InstructCodeEnums.CHANGE_PCD);
     }
 
+    @Value("${casic.file.uploadPath}")
+    private String filePath;
+
+    @SneakyThrows
+    @ApiOperation(value = "PCD上传接口")
+    @PostMapping(value = "/uploadPcd")
+    public ResponseData uploadPcd(@RequestParam("file") MultipartFile file, @ApiParam("机器人ID,url拼接传参") String robotId) {
+        Assert.isFalse(StrUtil.isEmpty(robotId), () -> new RequestValidException("机器ID不能为空", Optional.of(new Throwable().getStackTrace()[2])));
+        RobotInfo info = robotInfoService.getById(robotId);
+        if (file != null && !file.isEmpty()) {
+            File pcdFile = new File(filePath + "finalCloud_" + info.getId() + ".pcd");
+            try {
+                file.transferTo(pcdFile);
+            } catch (Exception e) {
+                log.error(e.getMessage());
+            }
+            return ResponseData.success();
+        } else {
+            return ResponseData.error("附件上传失败!");
+        }
+    }
+
     @SneakyThrows
     @ApiOperation(value = "PCD下载接口")
     @PostMapping(value = "/pcdDownload")
@@ -136,6 +170,10 @@
     @ApiOperation(value = "启动或关闭循迹功能", notes = "返回 1成功 0失败  wait 启动时是否等待，0代表循迹启动即运动，不等待；1代表循迹启动原地等待。若无此字段，默认循迹启动不等待直接运动")
     @PostMapping(value = "/startTrack")
     public ResponseDataDTO<Integer> startTrack(@RequestBody StartTrackRequest request) {
+        request.setIsTask(1);
+        if (request.getStepLen() == null) {
+            request.setStepLen(2);
+        }
         baseRobotService.startTrack(request);
         return ResponseDataDTO.success();
     }
@@ -156,6 +194,13 @@
         MessageRequestDTO<CrlNavRequest> messageDTO = new MessageRequestDTO();
         messageDTO.setMsg(request);
         ResponseDataDTO<Integer> list = baseRobotService.getRobotRouteOptService().robotCmd(info, InstructCodeEnums.CTRL_NAV, messageDTO);
+        if (request.getAction() == 1) {
+            CostMapResolutionRequest costMap = new CostMapResolutionRequest();
+            costMap.setResolution(info.getResolution().doubleValue());
+            request.setRobotId(request.getRobotId());
+            //是否下发分辨率
+            CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CONFIG_COSTMAP_RESOLUTION_SET + request.getRobotId(), costMap);
+        }
         return list;
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java
new file mode 100644
index 0000000..5317651
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java
@@ -0,0 +1,86 @@
+package com.casic.missiles.modular.robot.controller;
+
+import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
+import com.baomidou.mybatisplus.extension.plugins.pagination.Page;
+import com.casic.missiles.core.base.controller.BaseController;
+import com.casic.missiles.core.page.PageFactory;
+import com.casic.missiles.core.page.PageInfoBT;
+import com.casic.missiles.model.form.IdForms;
+import com.casic.missiles.model.response.dto.ResponseDataDTO;
+import com.casic.missiles.modular.robot.model.TaskGridInfo;
+import com.casic.missiles.modular.robot.service.ITaskGridInfoService;
+import com.casic.missiles.modular.robot.vo.TaskGridInfoVO;
+import io.swagger.annotations.Api;
+import io.swagger.annotations.ApiOperation;
+import org.springframework.web.bind.annotation.*;
+
+import java.util.List;
+
+/**
+ * 任务网格信息Controller
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+@Api(tags = "任务网格信息")
+@RestController
+@RequestMapping("/robot/taskGridInfo")
+public class TaskGridInfoController extends BaseController {
+
+    private final ITaskGridInfoService taskGridInfoService;
+
+    public TaskGridInfoController(ITaskGridInfoService taskGridInfoService) {
+        this.taskGridInfoService = taskGridInfoService;
+    }
+
+    @ApiOperation(value = "列表查询")
+    @GetMapping(value = "/list")
+    public ResponseDataDTO<List<TaskGridInfo>> list(TaskGridInfoVO taskGridInfoVO) {
+        QueryWrapper<TaskGridInfo> query = taskGridInfoVO.genQuery();
+        return ResponseDataDTO.success(taskGridInfoService.list(query));
+    }
+
+    @ApiOperation(value = "分页查询")
+    @GetMapping(value = "/listPage")
+    public ResponseDataDTO<PageInfoBT<TaskGridInfo>> listPage(TaskGridInfoVO taskGridInfoVO) {
+        Page<TaskGridInfo> page = PageFactory.defaultPage();
+        QueryWrapper<TaskGridInfo> query = taskGridInfoVO.genQuery();
+        page = taskGridInfoService.page(page, query);
+        return ResponseDataDTO.success(super.packForBT(page));
+    }
+
+
+    @ApiOperation(value = "新增接口")
+    @PostMapping(value = "/add")
+    public ResponseDataDTO add(@RequestBody TaskGridInfo taskGridInfo) {
+        taskGridInfoService.save(taskGridInfo);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "修改接口")
+    @PostMapping(value = "/update")
+    public ResponseDataDTO update(@RequestBody TaskGridInfo taskGridInfo) {
+        taskGridInfoService.updateById(taskGridInfo);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "删除接口")
+    @GetMapping(value = "/delete")
+    public ResponseDataDTO delete(String id) {
+        taskGridInfoService.removeById(id);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation("批量删除")
+    @PostMapping("/batchDelete")
+    public ResponseDataDTO batchDelete(@RequestBody IdForms ids) {
+        taskGridInfoService.removeByIds(ids.getIds());
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "详情查询")
+    @GetMapping(value = "/detail")
+    public ResponseDataDTO<TaskGridInfo> detail(String id) {
+        return ResponseDataDTO.success(taskGridInfoService.getById(id));
+    }
+}
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
index e82a792..6992274 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
@@ -98,7 +98,7 @@
     public ResponseDataDTO<AlgorithmResponse> test() {
         AlgorithmResponse response = new AlgorithmResponse();
         response.setTaskHeatMap(new TaskHeatMap());
-        algorithmService.readAlgorithmResponse("step3.txt",response,123L,null);
+        algorithmService.readAlgorithmResponse("step3.txt",null,response,123L,null);
         sendWebSocket(InstructStatusMsgEnums.HEAT_MAP, "1", response.getTaskHeatMap());
         return ResponseDataDTO.success(response);
     }
@@ -121,7 +121,7 @@
     @GetMapping(value = "/testGrid")
     public ResponseDataDTO<List<RouteInfo>> testGrid(@RequestBody PathInitDTO pathInitDTO) {
         RobotInfo robotInfo = infoService.getById(pathInitDTO.getRobotId());
-        GridPointDTO gridPointDTO = algorithmService.convertSlamToGrid(new BigDecimal(pathInitDTO.getX()), new BigDecimal(10), new BigDecimal(4), robotInfo);
+        GridPointDTO gridPointDTO = algorithmService.convertSlamToGrid(new BigDecimal(pathInitDTO.getXNow()), new BigDecimal(10), new BigDecimal(4), robotInfo);
         return ResponseDataDTO.success(algorithmService.convertGridCenterPoint(gridPointDTO, new BigDecimal(10), robotInfo));
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/TaskGridInfoMapper.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/TaskGridInfoMapper.java
new file mode 100644
index 0000000..5c3a70b
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/TaskGridInfoMapper.java
@@ -0,0 +1,23 @@
+package com.casic.missiles.modular.robot.dao;
+
+
+import com.baomidou.mybatisplus.core.mapper.BaseMapper;
+import com.casic.missiles.modular.robot.model.TaskGridInfo;
+import org.apache.ibatis.annotations.Param;
+import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
+import com.baomidou.mybatisplus.extension.plugins.pagination.Page;
+import java.util.List;
+/**
+ * <p>
+ *  任务网格信息 Mapper 接口
+ * </p>
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+public interface TaskGridInfoMapper extends BaseMapper<TaskGridInfo> {
+/**
+* 任务网格信息 分页检索
+*/
+List<TaskGridInfo> selectTaskGridInfoPage( @Param("page") Page<TaskGridInfo> page,@Param("ew" ) QueryWrapper<TaskGridInfo> query);
+}
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/mapping/TaskGridInfoMapper.xml b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/mapping/TaskGridInfoMapper.xml
new file mode 100644
index 0000000..be573e0
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/mapping/TaskGridInfoMapper.xml
@@ -0,0 +1,80 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!DOCTYPE mapper PUBLIC "-//mybatis.org//DTD Mapper 3.0//EN" "http://mybatis.org/dtd/mybatis-3-mapper.dtd">
+<mapper namespace="com.casic.missiles.modular.robot.dao.TaskGridInfoMapper">
+    <!-- 通用查询映射结果 -->
+    <resultMap id="BaseResultMap" type="com.casic.missiles.modular.robot.model.TaskGridInfo">
+        <result column="id" property="id"/>
+        <result column="create_time" property="createTime"/>
+        <result column="update_time" property="updateTime"/>
+        <result column="task_id" property="taskId"/>
+        <result column="estimate_grid_x" property="estimateGridX"/>
+        <result column="estimate_grid_y" property="estimateGridY"/>
+        <result column="neutron_count" property="neutronCount"/>
+        <result column="cur_grid_x" property="curGridX"/>
+        <result column="cur_grid_y" property="curGridY"/>
+    </resultMap>
+    <!-- 通用查询结果列 -->
+    <sql id="Base_Column_List">
+        id
+        ,
+                    create_time as createTime ,
+                    update_time as updateTime ,
+                    task_id as taskId ,
+                    estimate_grid_x as estimateGridX ,
+                    estimate_grid_y as estimateGridY ,
+                    neutron_count as neutronCount ,
+                    cur_grid_x as curGridX ,
+                    cur_grid_y as curGridY
+    </sql>
+    <sql id="Base_Column_Alias">
+        a
+        .
+        id
+        ,
+            a.create_time
+            as createTime ,
+            a.update_time
+            as updateTime ,
+            a.task_id
+            as taskId ,
+            a.estimate_grid_x
+            as estimateGridX ,
+            a.estimate_grid_y
+            as estimateGridY ,
+            a.neutron_count
+            as neutronCount ,
+            a.cur_grid_x
+            as curGridX ,
+            a.cur_grid_y
+            as curGridY
+    </sql>
+    <select id="selectTaskGridInfoPage" resultType="com.casic.missiles.modular.robot.model.TaskGridInfo">
+        select
+        <include refid="Base_Column_Alias"/>
+        from task_grid_info a
+        <where>
+            <if test="ew != null">
+                ${ew.sqlSegment}
+            </if>
+        </where>
+    </select>
+    <sql id="Base_Column_Query">
+        <if test="request!=null">
+            <if test="request.taskId!= null and request.taskId!=''">
+                and task_id = #{request.taskId}
+            </if>
+            <if test="request.estimateGridX!= null and request.estimateGridX!=''">
+                and estimate_grid_x = #{request.estimateGridX}
+            </if>
+            <if test="request.estimateGridY!= null and request.estimateGridY!=''">
+                and estimate_grid_y = #{request.estimateGridY}
+            </if>
+            <if test="request.curGridX!= null and request.curGridX!=''">
+                and cur_grid_x = #{request.curGridX}
+            </if>
+            <if test="request.curGridY!= null and request.curGridY!=''">
+                and cur_grid_y = #{request.curGridY}
+            </if>
+        </if>
+    </sql>
+</mapper>
\ No newline at end of file
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java
index 749f559..4559074 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java
@@ -13,4 +13,9 @@
      * 规划点位信息
      */
     private PointNextDTO pointNextDTO;
+    /**
+     * 寻源目标地址
+     */
+    private PointNextDTO coordinate;
+    private boolean isEnd;
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java
index 39ff2a2..cef3238 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java
@@ -12,6 +12,9 @@
     private BigDecimal y;
 
     public static GridPointDTO convertGridPoint(PointNextDTO nextDTO) {
+        if(nextDTO==null){
+            return null;
+        }
         GridPointDTO gridPointDTO = new GridPointDTO();
         gridPointDTO.setX(Convert.toBigDecimal(nextDTO.getX()));
         gridPointDTO.setY(Convert.toBigDecimal(nextDTO.getY()));
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java
index 8c88242..33be25d 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java
@@ -12,25 +12,19 @@
 public class PathInitDTO {
     //---------------------算法参数
     /**
-     * x坐标最大值（x方向栅格总数）
+     * 初始位置x坐标(栅格地图下的坐标)
      */
-    private String xMax;
+    private String xNow;
     /**
-     * y坐标最大值（y方向栅格总数）
+     * 初始位置y坐标(栅格地图下的坐标)
      */
-    private String yMax;
+    private String yNow;
     /**
      * 初始位置计数率
      */
-    private String count;
-    /**
-     * 初始位置x坐标(0<=x<x_max)
-     */
-    private String x;
-    /**
-     * 初始位置y坐标(0<=y<y_max)
-     */
-    private String y;
+    private String zNow;
+
+    private Integer stepLen;
 
     //---------------------其他执行参数
     private String robotId;
@@ -38,11 +32,11 @@
 
     public List<String> toArray() {
         List<String> list = new ArrayList<>();
-        list.add(xMax);
-        list.add(yMax);
-        list.add(count);
-        list.add(x);
-        list.add(y);
+        list.add(xNow);
+        list.add(yNow);
+        list.add(stepLen+"");
+        list.add(zNow);
+        list.add(taskId+"");
         return list;
     }
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java
index 109e95c..ada6774 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java
@@ -10,21 +10,17 @@
  */
 @Data
 public class PathPlanDTO extends PathInitDTO {
-
     /**
-     * 长度为8的数组，记录(x-1, y-1), (x-1, y), (x-1, y+1), (x, y-1), (x, y+1), (x+1, y-1), (x+1, y), (x+1, y+1)8个坐标的可通行信息。
-     * 若某坐标有障碍物（不可通行行）则记为0，否则记为1
+     * 浮点阈值(阈值，超过该值后结束巡检开始寻源)
      */
-    private String open;
-
+    private String th;
     public List<String> toArray() {
         List<String> list = new ArrayList<>();
-        list.add(getXMax());
-        list.add(getYMax());
-        list.add(open);
-        list.add(getCount());
-        list.add(getX());
-        list.add(getY());
+
+        list.add(getStepLen()+"");
+        list.add(getTh());
+        list.add(getZNow());
+        list.add(getTaskId()+"");
         return list;
     }
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskGridInfo.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskGridInfo.java
new file mode 100644
index 0000000..101c10a
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskGridInfo.java
@@ -0,0 +1,83 @@
+package com.casic.missiles.modular.robot.model;
+
+import com.baomidou.mybatisplus.annotation.TableField;
+import com.baomidou.mybatisplus.annotation.TableName;
+import com.baomidou.mybatisplus.extension.activerecord.Model;
+import io.swagger.annotations.ApiModelProperty;
+import lombok.Data;
+
+import java.math.BigDecimal;
+import java.util.Date;
+
+/**
+ * 任务网格信息对象 task_grid_info
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+@Data
+@TableName("task_grid_info")
+public class TaskGridInfo extends Model<TaskGridInfo> {
+    private static final long serialVersionUID = 1L;
+    @TableField(exist = false)
+    private Integer index;
+    @ApiModelProperty(value = "主键", dataType = "Long")
+    @TableField("id")
+    private Long id;
+
+    @ApiModelProperty(value = "创建时间", dataType = "Date")
+    @TableField("create_time")
+    private Date createTime;
+
+    @ApiModelProperty(value = "修改时间", dataType = "Date")
+    @TableField("update_time")
+    private Date updateTime;
+
+    @ApiModelProperty(value = "任务ID", dataType = "Long")
+    @TableField("task_id")
+    private Long taskId;
+
+    @ApiModelProperty(value = "估算栅格x", dataType = "Long")
+    @TableField("estimate_grid_x")
+    private String estimateGridX;
+
+    @ApiModelProperty(value = "估算栅格y", dataType = "Long")
+    @TableField("estimate_grid_y")
+    private String estimateGridY;
+
+    @ApiModelProperty(value = "中子计数率", dataType = "BigDecimal")
+    @TableField("neutron_count")
+    private BigDecimal neutronCount;
+
+    @ApiModelProperty(value = "当前栅格x", dataType = "Long")
+    @TableField("cur_grid_x")
+    private Integer curGridX;
+
+    @ApiModelProperty(value = "当前栅格y", dataType = "Long")
+    @TableField("cur_grid_y")
+    private Integer curGridY;
+
+    public String getCurPoint(){
+        return "("+curGridX+","+curGridY+")";
+    }
+    public String getEstimatePoint(){
+        if(estimateGridX==null&estimateGridY==null){
+            return "结束";
+        }
+        return "("+estimateGridX+","+estimateGridY+")";
+    }
+    @Override
+    public String toString() {
+        return "TaskGridInfo{"
+                + "id=" + id
+                + "createTime=" + createTime
+                + "updateTime=" + updateTime
+                + "taskId=" + taskId
+                + "estimateGridX=" + estimateGridX
+                + "estimateGridY=" + estimateGridY
+                + "neutronCount=" + neutronCount
+                + "curGridX=" + curGridX
+                + "curGridY=" + curGridY
+                + "}";
+    }
+}
\ No newline at end of file
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java
index edc104e..a8acb2a 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java
@@ -53,6 +53,13 @@
     @TableField("route_id")
     private Long routeId;
 
+    @ApiModelProperty(value = "寻源初始步长", dataType = "Integer")
+    @TableField("step_len")
+    private Integer stepLen;
+
+    @ApiModelProperty(value = "寻源阈值设定", dataType = "BigDecimal")
+    @TableField("max_threshold")
+    private BigDecimal maxThreshold;
     /**
      * 任务类型(1巡检任务)
      */
@@ -62,7 +69,6 @@
     @ApiModelProperty(value = "任务类型字典名称", dataType = "String")
     @TableField(exist = false)
     private String taskTypeName;
-
     @ApiModelProperty(value = "识别类型(discernType 1中子源识别)", dataType = "Integer")
     @TableField("discern_type")
     private Integer discernType;
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java
index c4bc25d..c6620f3 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java
@@ -53,7 +53,7 @@
     }),
     SET_SPEED("/set_speed" , "/set_speed_response" , "最大运动速度设置" , true, new TypeReference<RobotMsgResponseDTO<Integer>>() {
     }),
-    SET_TARGETPOINT("/set_targetpoint" , "/set_targetpoint_response" , "设置导航目标点" , false, new TypeReference<RobotMsgResponseDTO<Integer>>() {
+    SET_TARGETPOINT("/set_targetpoint" , "/set_targetpoint_response" , "设置导航目标点" , true, new TypeReference<RobotMsgResponseDTO<Integer>>() {
     }),
 
     GET_DATA_LIST("/get_data_list" , "/get_data_list_response" , "获取数据包列表" , true, new TypeReference<RobotMsgResponseDTO<List<String>>>() {
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/AbstractSocketHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/AbstractSocketHandler.java
index 83f4cb2..e47e343 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/AbstractSocketHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/AbstractSocketHandler.java
@@ -49,7 +49,7 @@
                 wsMsgBaseDTO.setMsgKey(enums.getMsgKey());
                 wsMsgBaseDTO.setData(jsonObject.get("msg"));
                 String msg = JSON.toJSONString(wsMsgBaseDTO);
-                log.info("推送{}消息：{}", enums.getRemarks(), msg);
+//                log.info("推送{}消息：{}", enums.getRemarks(), msg);
                 webSocket.sendOneMessage(robotId, msg);
             }
         } catch (Exception e) {
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/cmd/TargetPointCmdHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/cmd/TargetPointCmdHandler.java
index c1c09fa..43b4460 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/cmd/TargetPointCmdHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/cmd/TargetPointCmdHandler.java
@@ -34,7 +34,11 @@
     @Override
     public ResponseDataDTO senderCmd(TargetPointRequest request, InstructCodeEnums enums) {
         MessageRequestDTO<TargetPointRequest> messageDTO = new MessageRequestDTO();
-        //读取默认关键点信息
+        //读取默认关键点信息 {"x":"1.715708819927424","y":"-0.9100042544250901","z":"0.017993879527185876","theta":"-0.039278239414312105"}
+//        request.setX("1.715708819927424");
+//        request.setY("-0.9100042544250901");
+//        request.setZ("0.017993879527185876");
+//        request.setTheta("-0.039278239414312105");
         messageDTO.setMsg(request);
         ResponseDataDTO<Integer> list = optService.robotCmd(getRobotInfo(), enums, messageDTO);
         return list;
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/AppCostMapHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/AppCostMapHandler.java
index 0335693..88c9f79 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/AppCostMapHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/AppCostMapHandler.java
@@ -1,6 +1,11 @@
 package com.casic.missiles.modular.robot.opt.handler.msg;
 
+import com.casic.missiles.core.cache.CacheUtil;
+import com.casic.missiles.modular.constants.RobotDictConstants;
+import com.casic.missiles.modular.robot.IBaseRobotService;
+import com.casic.missiles.modular.robot.opt.enums.InstructCodeEnums;
 import com.casic.missiles.modular.robot.opt.handler.AbstractSocketHandler;
+import com.casic.missiles.modular.robot.opt.instruct.dto.CostMapResolutionRequest;
 import com.casic.missiles.modular.robot.service.IRobotStatusInfoService;
 import lombok.extern.slf4j.Slf4j;
 import org.eclipse.paho.client.mqttv3.MqttMessage;
@@ -16,14 +21,27 @@
 @Component("/app_costmap_sender")
 public class AppCostMapHandler extends AbstractSocketHandler {
     private final IRobotStatusInfoService statusInfoService;
+    private final IBaseRobotService baseRobotService;
     private static Map<String, Integer> index = new ConcurrentHashMap<>();
 
-    public AppCostMapHandler(IRobotStatusInfoService statusInfoService) {
+    public AppCostMapHandler(IRobotStatusInfoService statusInfoService, IBaseRobotService baseRobotService) {
         this.statusInfoService = statusInfoService;
+        this.baseRobotService = baseRobotService;
     }
 
     @Override
     public void process(String robotId, String topic, MqttMessage message) {
         statusInfoService.saveCostMap(robotId, message);
+
+        CostMapResolutionRequest costMapSet = CacheUtil.get(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CONFIG_COSTMAP_RESOLUTION_SET +robotId);
+        if(costMapSet != null){
+            CacheUtil.remove(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CONFIG_COSTMAP_RESOLUTION_SET +robotId);
+            new Thread(() -> {
+                costMapSet.setRobotId(Long.valueOf(robotId));
+                baseRobotService.execCmdHandler(costMapSet, InstructCodeEnums.COSTMAP_RESOLUTION);
+            }).start();
+
+        }
+
     }
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/PoseMessageMsgHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/PoseMessageMsgHandler.java
index 29f8919..6c3c3bd 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/PoseMessageMsgHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/PoseMessageMsgHandler.java
@@ -17,11 +17,9 @@
 @Component("/pose_message")
 public class PoseMessageMsgHandler extends AbstractSocketHandler {
 
-
     private final EventPublisher publisher;
 
     public PoseMessageMsgHandler(EventPublisher publisher) {
-
         this.publisher = publisher;
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/StopStateMsgHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/StopStateMsgHandler.java
index 8950815..8a71e0b 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/StopStateMsgHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/StopStateMsgHandler.java
@@ -30,7 +30,7 @@
     public void process(String robotId, String topic, MqttMessage message) {
 
         index.put(index_key, index.get("index") + 1);
-        if (index.get(index_key)>50) {
+        if (index.get(index_key)>30) {
             index.put(index_key, 0);
             log.info("主键：{},停障状态topic:{},消息内容:{}", robotId, topic, message.toString());
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/impl/RobotOptServiceImpl.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/impl/RobotOptServiceImpl.java
index 8471e7a..7940a24 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/impl/RobotOptServiceImpl.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/impl/RobotOptServiceImpl.java
@@ -146,7 +146,7 @@
     public void startTrack(StartTrackRequest request) {
         //增加循迹任务记录
         taskInfoService.reloadTask(request, robotInfoService.getById(request.getRobotId()));
-        this.execCmdHandler(request, InstructCodeEnums.START_TRACK);
+//        this.execCmdHandler(request, InstructCodeEnums.START_TRACK);
     }
 
     public IRobotRouteOptService getRobotRouteOptService() {
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/BaseRobotCmdDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/BaseRobotCmdDTO.java
index 5608f2b..06b53de 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/BaseRobotCmdDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/BaseRobotCmdDTO.java
@@ -3,11 +3,13 @@
 import io.swagger.annotations.ApiModelProperty;
 import lombok.Data;
 
+import java.io.Serializable;
+
 /**
  * 机器人指令基类
  */
 @Data
-public class BaseRobotCmdDTO {
+public class BaseRobotCmdDTO implements Serializable {
     @ApiModelProperty(value = "机器人主键", dataType = "String")
     protected Long robotId;
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/StartTrackRequest.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/StartTrackRequest.java
index a18321b..85546a7 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/StartTrackRequest.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/StartTrackRequest.java
@@ -3,6 +3,8 @@
 import io.swagger.annotations.ApiModelProperty;
 import lombok.Data;
 
+import java.math.BigDecimal;
+
 /**
  * 导航下发指令
  */
@@ -10,18 +12,19 @@
 public class StartTrackRequest extends BaseRobotCmdDTO {
     @ApiModelProperty(value = "sn编码(可不填 默认机器ID)", dataType = "String")
     private String sn;
-
     @ApiModelProperty(value = "下发状态 1:开启 0:关闭", dataType = "String")
     private Integer action;
     @ApiModelProperty(value = "等待状态 1:等待 0:不等待", dataType = "String")
     private Integer wait;
     @ApiModelProperty(value = "等待状态 0:循迹 1:任务", dataType = "isTask")
     private Integer isTask;
-    @ApiModelProperty(value = "轨迹名称", dataType = "String")
+    @ApiModelProperty(value = "轨迹名称(废弃)", dataType = "String")
     private String track_name;
     @ApiModelProperty(value = "关键点名称", dataType = "String", hidden = true)
     private String taskpoint_name;
-
+    @ApiModelProperty(value = "初始寻源步长", dataType = "Integer")
+    private Integer stepLen;
+    @ApiModelProperty(value = "寻源阈值", dataType = "BigDecimal")
+    private BigDecimal maxThreshold;
     private String obs_mode="1";
-
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/response/StopStateResponseDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/response/StopStateResponseDTO.java
index eb514bf..e0679d7 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/response/StopStateResponseDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/response/StopStateResponseDTO.java
@@ -3,11 +3,13 @@
 import io.swagger.annotations.ApiModelProperty;
 import lombok.Data;
 
+import java.io.Serializable;
+
 /**
  * 电压信息返回
  */
 @Data
-public class StopStateResponseDTO {
+public class StopStateResponseDTO implements Serializable {
     @ApiModelProperty(value = "软件急停(0：无触发 1：触发)" , dataType = "Integer")
     private Integer soft;
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IAlgorithmService.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IAlgorithmService.java
index 56b3e1d..f62523b 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IAlgorithmService.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IAlgorithmService.java
@@ -6,6 +6,7 @@
 import com.casic.missiles.modular.robot.dto.PathPlanDTO;
 import com.casic.missiles.modular.robot.model.RobotInfo;
 
+import java.io.File;
 import java.math.BigDecimal;
 import java.util.List;
 
@@ -38,5 +39,5 @@
      * @return
      */
     GridPointDTO convertGridCenterPoint(GridPointDTO pointDTO, BigDecimal resolution, RobotInfo robotInfo);
-    void readAlgorithmResponse(String fileName, AlgorithmResponse response, Long taskId, List<String> res);
+    void readAlgorithmResponse(String fileName, File repetitionFile, AlgorithmResponse response, Long taskId, List<String> res);
 }
\ No newline at end of file
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IRobotStatusInfoService.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IRobotStatusInfoService.java
index 2af210a..757f8d0 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IRobotStatusInfoService.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IRobotStatusInfoService.java
@@ -67,10 +67,9 @@
     /**
      * 更新车辆路线及任务信息
      *
-     * @param currRouteId 当前路线信息
      * @param currTaskId  当前任务信息
      */
-    void updateCurrTask(String robotId, Long currRouteId, Long currTaskId);
+    void updateCurrTask(String robotId, Long currTaskId);
 
     RobotStatusInfo getStatusByRobotId(String robotId);
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/ITaskGridInfoService.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/ITaskGridInfoService.java
new file mode 100644
index 0000000..fc0da93
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/ITaskGridInfoService.java
@@ -0,0 +1,22 @@
+package com.casic.missiles.modular.robot.service;
+
+import com.baomidou.mybatisplus.extension.service.IService;
+import  com.casic.missiles.modular.robot.model.TaskGridInfo;
+import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
+import com.baomidou.mybatisplus.extension.plugins.pagination.Page;
+import java.util.List;
+
+
+/**
+ * <p>
+ * 任务网格信息 服务类
+ * </p>
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+public interface ITaskGridInfoService extends IService<TaskGridInfo> {
+/**
+* 任务网格信息 分页检索
+*/
+List<TaskGridInfo> selectTaskGridInfoPage(Page<TaskGridInfo> page,QueryWrapper<TaskGridInfo> query);}
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/ITaskInfoService.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/ITaskInfoService.java
index 31d7ba6..b395993 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/ITaskInfoService.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/ITaskInfoService.java
@@ -51,6 +51,13 @@
      * @param dto 实体
      */
     void pointCallBack(PoseMessageResponseDTO dto, String robotId);
+
+    /**
+     * 车辆路线规划
+     * @param dto
+     * @param robotId
+     */
+    void carPathPlanning(PoseMessageResponseDTO dto, String robotId);
     void pointTestCallBack(PoseMessageResponseDTO dto, String robotId);
     /**
      * 图片路径保存
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/AlgorithmServiceImpl.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/AlgorithmServiceImpl.java
index f808fa0..a4dbe0b 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/AlgorithmServiceImpl.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/AlgorithmServiceImpl.java
@@ -29,10 +29,12 @@
 @Slf4j
 @Service
 public class AlgorithmServiceImpl implements IAlgorithmService {
-    @Value("${casic.algorithm.initPath:-1}")
-    private String initPath;
-    @Value("${casic.algorithm.planPath:-1}")
-    private String planPath;
+    @Value("${casic.algorithm.path:-1}")
+    private String path;
+    @Value("${casic.algorithm.initPathName:-1}")
+    private String initPathName;
+    @Value("${casic.algorithm.planPathName:-1}")
+    private String planPathName;
     private final ITaskHeatMapService heatMapService;
 
     public AlgorithmServiceImpl(ITaskHeatMapService heatMapService) {
@@ -42,13 +44,13 @@
     @Transactional
     @Override
     public AlgorithmResponse pathPlanningInit(PathInitDTO initDTO) {
-        if (StrUtil.isEmpty(initDTO.getCount())) {
-            initDTO.setCount("0.00");
+        if (StrUtil.isEmpty(initDTO.getZNow())) {
+            initDTO.setZNow("0.00");
         }
         log.info(JSON.toJSONString(initDTO));
         //算法路径及入参配置
         List<String> commends = new ArrayList<>();
-        commends.add(initPath);
+        commends.add(path + initPathName);
         commends.addAll(initDTO.toArray());
         List<String> res = AlgorithmUtils.invokeJob(commends);
 
@@ -63,38 +65,72 @@
     @Transactional
     @Override
     public AlgorithmResponse pathPlanning(PathPlanDTO planDTO) {
-        if (StrUtil.isEmpty(planDTO.getCount())) {
-            planDTO.setCount("0.001");
+        if (StrUtil.isEmpty(planDTO.getZNow())) {
+            planDTO.setZNow("0.001");
         }
         //算法路径及入参配置
         List<String> commends = new ArrayList<>();
-        commends.add(planPath);
+        commends.add(path + planPathName);
         commends.addAll(planDTO.toArray());
         log.info("寻源算法调用:{}", JSON.toJSONString(planDTO));
         List<String> res = AlgorithmUtils.invokeJob(commends);
         AlgorithmResponse response = getAlgorithmResponse(planDTO, res);
-        //next 点位编写
-        if (CollectionUtil.isNotEmpty(res) && res.size() == 2) {
-            response.getPointNextDTO().setX(formatBigDecimal(res.get(0).replaceAll("next_X: ", "")));
-            response.getPointNextDTO().setY(formatBigDecimal(res.get(1).replaceAll("next_Y: ", "")));
-        }
+
         return response;
     }
 
     private AlgorithmResponse getAlgorithmResponse(PathInitDTO initDTO, List<String> res) {
-        File repetitionFile = new File("repetition.txt");
+        //读取当前路径图片
+        File repetitionFileDir = new File(path + "\\" + initDTO.getTaskId() + "\\predicts");
+        File[] files = repetitionFileDir.listFiles((dir, name) -> name.endsWith(".txt") && name.startsWith("step"));
+        File repetitionFile = null;
+        //判定当前执行路径最大step2文件夹
+        if (files != null && files.length > 0) {
+             repetitionFile = files[files.length - 1];
+        }
         AlgorithmResponse response = new AlgorithmResponse();
         if (initDTO.getTaskId() != null && repetitionFile.exists()) {
-            readAlgorithmResponse("repetition.txt", response, initDTO.getTaskId(), res);
-            readAlgorithmResponse("step.txt", response, initDTO.getTaskId(), res);
+            //读取热力图
+            readAlgorithmResponse("repetition.txt",repetitionFile, response, initDTO.getTaskId(), res);
         } else {
             log.info("taskId is empty!");
         }
-        readAlgorithmResponse("xy_train.txt", response, initDTO.getTaskId(), res);
-        readAlgorithmResponse("z_train.txt", response, initDTO.getTaskId(), res);
+        //读取规划路径
+        if (CollectionUtil.isNotEmpty(res)) {
+            response.setEnd(false);
+            readAlgorithmResponse(response, res);
+        }
         return response;
     }
 
+    private void readAlgorithmResponse(AlgorithmResponse response, List<String> res) {
+        List<String> resList = new ArrayList<>();
+        if (res.size() >= 2) {
+            resList = res.subList(res.size() - 2, res.size());
+        }
+        //判定是否结束寻源
+        if (res.contains("end")) {
+            response.setEnd(true);
+            for (String s : resList) {
+                if (s.startsWith("source_coordinate:")) {
+                    s = s.trim();
+                    log.info("寻源结束,目标点位：{}",s);
+                    List<String> lists = StrUtil.split(s.replaceAll("source_coordinate:", ""), ",");
+                    PointNextDTO nextDTO = new PointNextDTO();
+                    nextDTO.setX(lists.get(0));
+                    nextDTO.setY(lists.get(1));
+                    response.setCoordinate(nextDTO);
+                }
+            }
+        } else {
+            PointNextDTO nextDTO = new PointNextDTO();
+            nextDTO.setX(resList.get(0));
+            nextDTO.setY(resList.get(1));
+            response.setPointNextDTO(nextDTO);
+        }
+
+    }
+
     /**
      * 读取算法输出数据
      *
@@ -102,49 +138,18 @@
      * @param response 结果集
      * @param taskId   任务ID
      */
-    public void readAlgorithmResponse(String fileName, AlgorithmResponse response, Long taskId, List<String> res) {
-        File file = new File(fileName);
-        if (!file.exists()&&!fileName.startsWith("step")) {
+    public void readAlgorithmResponse(String fileName,File repetitionFile, AlgorithmResponse response, Long taskId, List<String> res) {
+        if (!repetitionFile.exists() && !fileName.startsWith("step")) {
             log.info("{} file is empty!", fileName);
             return;
         }
         if (response.getPointNextDTO() == null) {
             response.setPointNextDTO(new PointNextDTO());
         }
-        switch (fileName) {
-            case "repetition.txt":
-                String msg = FileUtil.readString(file, CharsetUtil.CHARSET_UTF_8);
-                TaskHeatMap taskHeatMap = createEmptyTaskHeatMap(taskId);
-                taskHeatMap.setRepetition(msg);
-                response.setTaskHeatMap(taskHeatMap);
-                break;
-            case "xy_train.txt":
-                List<String> xyTrains = FileUtil.readLines(file, CharsetUtil.CHARSET_UTF_8);
-                if (CollectionUtil.isNotEmpty(xyTrains)) {
-                    if (CollectionUtil.isNotEmpty(xyTrains)) {
-                        List<String> list = StrUtil.split(xyTrains.get(xyTrains.size() - 1), " ");
-                        response.getPointNextDTO().setX(formatBigDecimal(list.get(0)));
-                        response.getPointNextDTO().setY(formatBigDecimal(list.get(1)));
-                    }
-                }
-                break;
-            case "z_train.txt":
-                List<String> zTrains = FileUtil.readLines(file, CharsetUtil.CHARSET_UTF_8);
-                if (CollectionUtil.isNotEmpty(zTrains)) {
-                    String zTrain = zTrains.get(zTrains.size() - 1);
-                    response.getPointNextDTO().setZ(formatBigDecimal(zTrain));
-                }
-            default:
-                if (StrUtil.isNotEmpty(fileName) && fileName.startsWith("step")) {
-                    File fileDir = new File(".");
-                    File[] files = fileDir.listFiles((dir, name) -> name.endsWith(".txt") && name.startsWith("step"));
-                    if (files != null && files.length > 0) {
-                        String steps = FileUtil.readString(files[files.length - 1], CharsetUtil.CHARSET_UTF_8);
-                        response.getTaskHeatMap().setRepetition(steps);
-                    }
-                }
-                break;
-        }
+        String msg = FileUtil.readString(repetitionFile, CharsetUtil.CHARSET_UTF_8);
+        TaskHeatMap taskHeatMap = createEmptyTaskHeatMap(taskId);
+        taskHeatMap.setRepetition(msg);
+        response.setTaskHeatMap(taskHeatMap);
     }
 
     /**
@@ -157,13 +162,11 @@
     public GridPointDTO convertSlamToGrid(BigDecimal x, BigDecimal y, BigDecimal resolution, RobotInfo robotInfo) {
         GridPointDTO pointDTO = new GridPointDTO();
         BigDecimal scale = resolution;
-
         //减去栅格原点坐标
         x = x.subtract(robotInfo.getOriginX());
         y = y.subtract(robotInfo.getOriginY());
-
-        pointDTO.setX(new BigDecimal(x.divide(scale,3,BigDecimal.ROUND_HALF_UP).intValue()+1));
-        pointDTO.setY(new BigDecimal(y.divide(scale,3,BigDecimal.ROUND_HALF_UP).intValue()+1));
+        pointDTO.setX(new BigDecimal(x.divide(scale, 3, BigDecimal.ROUND_HALF_UP).intValue()+1));
+        pointDTO.setY(new BigDecimal(y.divide(scale, 3, BigDecimal.ROUND_HALF_UP).intValue() +1));
         return pointDTO;
     }
 
@@ -174,19 +177,17 @@
      * @return
      */
     public GridPointDTO convertGridCenterPoint(GridPointDTO pointDTO, BigDecimal resolution, RobotInfo robotInfo) {
-
         GridPointDTO centerPoint = new GridPointDTO();
         BigDecimal scale = resolution;
-        //点位转换
-        BigDecimal slamX = pointDTO.getX().divide(scale, 5, BigDecimal.ROUND_HALF_UP);
-        BigDecimal slamY = pointDTO.getY().divide(scale, 5, BigDecimal.ROUND_HALF_UP);
+        //点位转换 转换后栅格－1 从1开始计算
+        BigDecimal slamX = (pointDTO.getX().subtract(new BigDecimal(1))).multiply(scale);
+        BigDecimal slamY = (pointDTO.getY().subtract(new BigDecimal(1))).multiply(scale);
         //中心栅格点位中心添加
-        centerPoint.setX(slamX.add(scale.divide(new BigDecimal(2), 5, BigDecimal.ROUND_HALF_UP)));
-        centerPoint.setY(slamY.add(scale.divide(new BigDecimal(2), 5, BigDecimal.ROUND_HALF_UP)));
+        centerPoint.setX(slamX.add(scale.divide(new BigDecimal(2), 10, BigDecimal.ROUND_HALF_UP)));
+        centerPoint.setY(slamY.add(scale.divide(new BigDecimal(2), 10, BigDecimal.ROUND_HALF_UP)));
         //加上栅格原点坐标
         centerPoint.setX(centerPoint.getX().add(robotInfo.getOriginX()));
         centerPoint.setY(centerPoint.getY().add(robotInfo.getOriginY()));
-
         return centerPoint;
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotInfoServiceImpl.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotInfoServiceImpl.java
index 043e05d..fdded64 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotInfoServiceImpl.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotInfoServiceImpl.java
@@ -104,7 +104,7 @@
             //下发障碍物栅格
             sendCostMapResolution(robotInfo.getResolution(), robotInfo.getId());
             //重新生成栅格地图
-            sendChangePcd(robotInfo.getId());
+            //sendChangePcd(robotInfo.getId());
         }
         update.eq(RobotInfo::getId, robotInfo.getId());
         update(update);
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotStatusInfoServiceImpl.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotStatusInfoServiceImpl.java
index 6d3ae91..46b5c3b 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotStatusInfoServiceImpl.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotStatusInfoServiceImpl.java
@@ -15,6 +15,7 @@
 import com.casic.missiles.modular.robot.dao.RobotStatusInfoMapper;
 import com.casic.missiles.modular.robot.model.RobotInfo;
 import com.casic.missiles.modular.robot.model.RobotStatusInfo;
+import com.casic.missiles.modular.robot.opt.enums.InstructCodeEnums;
 import com.casic.missiles.modular.robot.opt.instruct.base.dto.RobotMsgDTO;
 import com.casic.missiles.modular.robot.opt.instruct.base.dto.RobotMsgResponseDTO;
 import com.casic.missiles.modular.robot.opt.instruct.base.dto.RobotResponseDTO;
@@ -26,13 +27,18 @@
 import com.casic.missiles.modular.robot.service.IRobotInfoService;
 import com.casic.missiles.modular.robot.service.IRobotStatusInfoService;
 import lombok.extern.slf4j.Slf4j;
+import org.aspectj.util.FileUtil;
 import org.eclipse.paho.client.mqttv3.MqttMessage;
+import org.springframework.beans.factory.annotation.Value;
 import org.springframework.context.annotation.Lazy;
 import org.springframework.stereotype.Service;
 import org.springframework.transaction.annotation.Transactional;
 
+import java.io.File;
 import java.io.Serializable;
 import java.math.BigDecimal;
+import java.util.ArrayList;
+import java.util.Arrays;
 import java.util.List;
 
 
@@ -147,7 +153,6 @@
     @Override
     public void saveProcessMessage(String robotId, MqttMessage message) {
 //        String json = new String(message.getPayload());
-
         RobotResponseDTO<ProcessMessageResponseDTO> responseDTO = parseJson(message, new TypeReference<RobotResponseDTO<ProcessMessageResponseDTO>>() {
         });
         if (responseDTO.getMsg() != null) {
@@ -189,9 +194,8 @@
     }
 
     @Override
-    public void updateCurrTask(String robotId, Long currRouteId, Long currTaskId) {
+    public void updateCurrTask(String robotId, Long currTaskId) {
         UpdateWrapper<RobotStatusInfo> query = new UpdateWrapper<>();
-        query.set("curr_route_id", currRouteId);
         query.set("curr_task_id", currTaskId);
         query.eq("robot_id", robotId);
         update(query);
@@ -250,6 +254,15 @@
         }
     }
 
+    @Value("${casic.algorithm.path:-1}")
+    private String path;
+
+    /**
+     * 栅格阈值
+     */
+    @Value("${casic.algorithm.num:30}")
+    private Integer num;
+
     /**
      * 机器人栅格数据
      *
@@ -262,15 +275,41 @@
         RobotResponseDTO<AppCostmapResponseDTO> responseDTO = parseJson(message, new TypeReference<RobotResponseDTO<AppCostmapResponseDTO>>() {
         });
         if (responseDTO != null && responseDTO.getMsg() != null) {
+            //发送栅格分辨率设定
+
             cacheService.updateCostMap(robotId, responseDTO.getMsg());
             robotInfoService.updateRobotGrid(robotId, responseDTO.getMsg());
-//            log.info("--------------- json data:{}", JSON.toJSONString(responseDTO.getMsg()));
+            //log.info("--------------- json data:{}", JSON.toJSONString(responseDTO.getMsg()));
+            if (responseDTO != null && responseDTO.getMsg().getData() != null) {
+                ArrayList<Integer> dataNums = new ArrayList<>();
+                //根据阈值转换栅格
+                for (Integer dataNum : responseDTO.getMsg().getData()) {
+                    if (dataNum != -1) {
+                        if (dataNum >= num) {
+                            dataNums.add(0);
+                        } else {
+                            dataNums.add(1);
+                        }
+                    }
+                }
+                writeCache(dataNums,responseDTO.getMsg().getWidth(),responseDTO.getMsg().getHeight(),robotId);
+            }
         }
     }
 
+    private void writeCache(ArrayList<Integer> integers,Integer width,Integer height,String robotId) {
+        List<List<Integer>> arrays = CollectionUtil.split(integers,width);
+        ArrayList<ArrayList<Integer>> arrayList = new ArrayList<>();
+        for (List<Integer> array : arrays) {
+            arrayList.add((ArrayList)array);
+        }
+        CollectionUtil.reverse(arrayList);
+        CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_COST_MAP + robotId, arrayList);
+//        FileUtil.writeAsString(new File(path + "map.txt"), CollectionUtil.join(integers, " ").replaceAll("\\[", "").replaceAll("]", ""));
+    }
+
     @Override
     public void updateRobotPoint(PoseMessageResponseDTO dto, String robotId) {
-        //todo cache
         UpdateWrapper<RobotStatusInfo> query = new UpdateWrapper<>();
         query.set("postion_x", dto.getX());
         query.set("postion_y", dto.getY());

diff --git a/.gitignore b/.gitignore
index 9e20bc1..f42a9cb 100644
--- a/.gitignore
+++ b/.gitignore
@@ -2,3 +2,4 @@
 **/target/**
 logs/
 *.iml
+*.exe
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java b/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
index c7b5d28..a849528 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
@@ -28,6 +28,7 @@
     String KEY_CUR_TASK_ID = "CUR_TASK_ID_";
     String KEY_CUR_TASK_INIT = "CUR_TASK_INIT_";
 
+    String KEY_COST_MAP = "KEY_COST_MAP";
     /**
      * PSD数据ID
      */
@@ -41,6 +42,7 @@
      */
     String KEY_CUR_XY_POINT = "KEY_CUR_XY_POINT_";
     String KEY_CUR_ESTIMATE_GRID = "KEY_CUR_ESTIMATE_GRID_";
+    String KEY_PLANING_FLAG = "KEY_PLANING_FLAG_";
     /**
      * 机器人巡点栅格
      */
@@ -55,6 +57,7 @@
      * 探测器配置参数缓存
      */
     String KEY_CONFIG_PARAMS_ID = "KEY_CONFIG_PARAMS_ID_";
+    String KEY_CONFIG_COSTMAP_RESOLUTION_SET = "KEY_CONFIG_COSTMAP_RESOLUTION_SET_";
 
     /**
      * 暂停状态缓存信息
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
index c4c905d..3bb475f 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
@@ -1,9 +1,14 @@
 package com.casic.missiles.modular.robot.controller;
 
+import cn.hutool.core.lang.Assert;
 import cn.hutool.core.util.StrUtil;
 import com.casic.missiles.core.base.controller.BaseController;
+import com.casic.missiles.core.cache.CacheUtil;
 import com.casic.missiles.core.util.SpringContextHolder;
+import com.casic.missiles.model.exception.RequestValidException;
+import com.casic.missiles.model.response.ResponseData;
 import com.casic.missiles.model.response.dto.ResponseDataDTO;
+import com.casic.missiles.modular.constants.RobotDictConstants;
 import com.casic.missiles.modular.robot.IBaseRobotService;
 import com.casic.missiles.modular.robot.dto.RobotInfoSetDTO;
 import com.casic.missiles.modular.robot.model.RobotInfo;
@@ -19,10 +24,16 @@
 import io.swagger.annotations.ApiOperation;
 import io.swagger.annotations.ApiParam;
 import lombok.SneakyThrows;
+import lombok.extern.slf4j.Slf4j;
+import org.apache.ibatis.annotations.Param;
+import org.springframework.beans.factory.annotation.Value;
 import org.springframework.web.bind.annotation.*;
+import org.springframework.web.multipart.MultipartFile;
 
 import javax.servlet.http.HttpServletResponse;
+import java.io.File;
 import java.util.List;
+import java.util.Optional;
 
 /**
  * 机器人操作类
@@ -30,6 +41,7 @@
  * @author lwh
  * @date 2023-10-30
  */
+@Slf4j
 @Api(tags = "机器人控制接口")
 @RestController
 @RequestMapping("/robot/opt")
@@ -61,6 +73,28 @@
         return baseRobotService.execCmdHandler(request, InstructCodeEnums.CHANGE_PCD);
     }
 
+    @Value("${casic.file.uploadPath}")
+    private String filePath;
+
+    @SneakyThrows
+    @ApiOperation(value = "PCD上传接口")
+    @PostMapping(value = "/uploadPcd")
+    public ResponseData uploadPcd(@RequestParam("file") MultipartFile file, @ApiParam("机器人ID,url拼接传参") String robotId) {
+        Assert.isFalse(StrUtil.isEmpty(robotId), () -> new RequestValidException("机器ID不能为空", Optional.of(new Throwable().getStackTrace()[2])));
+        RobotInfo info = robotInfoService.getById(robotId);
+        if (file != null && !file.isEmpty()) {
+            File pcdFile = new File(filePath + "finalCloud_" + info.getId() + ".pcd");
+            try {
+                file.transferTo(pcdFile);
+            } catch (Exception e) {
+                log.error(e.getMessage());
+            }
+            return ResponseData.success();
+        } else {
+            return ResponseData.error("附件上传失败!");
+        }
+    }
+
     @SneakyThrows
     @ApiOperation(value = "PCD下载接口")
     @PostMapping(value = "/pcdDownload")
@@ -136,6 +170,10 @@
     @ApiOperation(value = "启动或关闭循迹功能", notes = "返回 1成功 0失败  wait 启动时是否等待，0代表循迹启动即运动，不等待；1代表循迹启动原地等待。若无此字段，默认循迹启动不等待直接运动")
     @PostMapping(value = "/startTrack")
     public ResponseDataDTO<Integer> startTrack(@RequestBody StartTrackRequest request) {
+        request.setIsTask(1);
+        if (request.getStepLen() == null) {
+            request.setStepLen(2);
+        }
         baseRobotService.startTrack(request);
         return ResponseDataDTO.success();
     }
@@ -156,6 +194,13 @@
         MessageRequestDTO<CrlNavRequest> messageDTO = new MessageRequestDTO();
         messageDTO.setMsg(request);
         ResponseDataDTO<Integer> list = baseRobotService.getRobotRouteOptService().robotCmd(info, InstructCodeEnums.CTRL_NAV, messageDTO);
+        if (request.getAction() == 1) {
+            CostMapResolutionRequest costMap = new CostMapResolutionRequest();
+            costMap.setResolution(info.getResolution().doubleValue());
+            request.setRobotId(request.getRobotId());
+            //是否下发分辨率
+            CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CONFIG_COSTMAP_RESOLUTION_SET + request.getRobotId(), costMap);
+        }
         return list;
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java
new file mode 100644
index 0000000..5317651
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java
@@ -0,0 +1,86 @@
+package com.casic.missiles.modular.robot.controller;
+
+import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
+import com.baomidou.mybatisplus.extension.plugins.pagination.Page;
+import com.casic.missiles.core.base.controller.BaseController;
+import com.casic.missiles.core.page.PageFactory;
+import com.casic.missiles.core.page.PageInfoBT;
+import com.casic.missiles.model.form.IdForms;
+import com.casic.missiles.model.response.dto.ResponseDataDTO;
+import com.casic.missiles.modular.robot.model.TaskGridInfo;
+import com.casic.missiles.modular.robot.service.ITaskGridInfoService;
+import com.casic.missiles.modular.robot.vo.TaskGridInfoVO;
+import io.swagger.annotations.Api;
+import io.swagger.annotations.ApiOperation;
+import org.springframework.web.bind.annotation.*;
+
+import java.util.List;
+
+/**
+ * 任务网格信息Controller
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+@Api(tags = "任务网格信息")
+@RestController
+@RequestMapping("/robot/taskGridInfo")
+public class TaskGridInfoController extends BaseController {
+
+    private final ITaskGridInfoService taskGridInfoService;
+
+    public TaskGridInfoController(ITaskGridInfoService taskGridInfoService) {
+        this.taskGridInfoService = taskGridInfoService;
+    }
+
+    @ApiOperation(value = "列表查询")
+    @GetMapping(value = "/list")
+    public ResponseDataDTO<List<TaskGridInfo>> list(TaskGridInfoVO taskGridInfoVO) {
+        QueryWrapper<TaskGridInfo> query = taskGridInfoVO.genQuery();
+        return ResponseDataDTO.success(taskGridInfoService.list(query));
+    }
+
+    @ApiOperation(value = "分页查询")
+    @GetMapping(value = "/listPage")
+    public ResponseDataDTO<PageInfoBT<TaskGridInfo>> listPage(TaskGridInfoVO taskGridInfoVO) {
+        Page<TaskGridInfo> page = PageFactory.defaultPage();
+        QueryWrapper<TaskGridInfo> query = taskGridInfoVO.genQuery();
+        page = taskGridInfoService.page(page, query);
+        return ResponseDataDTO.success(super.packForBT(page));
+    }
+
+
+    @ApiOperation(value = "新增接口")
+    @PostMapping(value = "/add")
+    public ResponseDataDTO add(@RequestBody TaskGridInfo taskGridInfo) {
+        taskGridInfoService.save(taskGridInfo);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "修改接口")
+    @PostMapping(value = "/update")
+    public ResponseDataDTO update(@RequestBody TaskGridInfo taskGridInfo) {
+        taskGridInfoService.updateById(taskGridInfo);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "删除接口")
+    @GetMapping(value = "/delete")
+    public ResponseDataDTO delete(String id) {
+        taskGridInfoService.removeById(id);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation("批量删除")
+    @PostMapping("/batchDelete")
+    public ResponseDataDTO batchDelete(@RequestBody IdForms ids) {
+        taskGridInfoService.removeByIds(ids.getIds());
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "详情查询")
+    @GetMapping(value = "/detail")
+    public ResponseDataDTO<TaskGridInfo> detail(String id) {
+        return ResponseDataDTO.success(taskGridInfoService.getById(id));
+    }
+}
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
index e82a792..6992274 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
@@ -98,7 +98,7 @@
     public ResponseDataDTO<AlgorithmResponse> test() {
         AlgorithmResponse response = new AlgorithmResponse();
         response.setTaskHeatMap(new TaskHeatMap());
-        algorithmService.readAlgorithmResponse("step3.txt",response,123L,null);
+        algorithmService.readAlgorithmResponse("step3.txt",null,response,123L,null);
         sendWebSocket(InstructStatusMsgEnums.HEAT_MAP, "1", response.getTaskHeatMap());
         return ResponseDataDTO.success(response);
     }
@@ -121,7 +121,7 @@
     @GetMapping(value = "/testGrid")
     public ResponseDataDTO<List<RouteInfo>> testGrid(@RequestBody PathInitDTO pathInitDTO) {
         RobotInfo robotInfo = infoService.getById(pathInitDTO.getRobotId());
-        GridPointDTO gridPointDTO = algorithmService.convertSlamToGrid(new BigDecimal(pathInitDTO.getX()), new BigDecimal(10), new BigDecimal(4), robotInfo);
+        GridPointDTO gridPointDTO = algorithmService.convertSlamToGrid(new BigDecimal(pathInitDTO.getXNow()), new BigDecimal(10), new BigDecimal(4), robotInfo);
         return ResponseDataDTO.success(algorithmService.convertGridCenterPoint(gridPointDTO, new BigDecimal(10), robotInfo));
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/TaskGridInfoMapper.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/TaskGridInfoMapper.java
new file mode 100644
index 0000000..5c3a70b
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/TaskGridInfoMapper.java
@@ -0,0 +1,23 @@
+package com.casic.missiles.modular.robot.dao;
+
+
+import com.baomidou.mybatisplus.core.mapper.BaseMapper;
+import com.casic.missiles.modular.robot.model.TaskGridInfo;
+import org.apache.ibatis.annotations.Param;
+import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
+import com.baomidou.mybatisplus.extension.plugins.pagination.Page;
+import java.util.List;
+/**
+ * <p>
+ *  任务网格信息 Mapper 接口
+ * </p>
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+public interface TaskGridInfoMapper extends BaseMapper<TaskGridInfo> {
+/**
+* 任务网格信息 分页检索
+*/
+List<TaskGridInfo> selectTaskGridInfoPage( @Param("page") Page<TaskGridInfo> page,@Param("ew" ) QueryWrapper<TaskGridInfo> query);
+}
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/mapping/TaskGridInfoMapper.xml b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/mapping/TaskGridInfoMapper.xml
new file mode 100644
index 0000000..be573e0
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/mapping/TaskGridInfoMapper.xml
@@ -0,0 +1,80 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!DOCTYPE mapper PUBLIC "-//mybatis.org//DTD Mapper 3.0//EN" "http://mybatis.org/dtd/mybatis-3-mapper.dtd">
+<mapper namespace="com.casic.missiles.modular.robot.dao.TaskGridInfoMapper">
+    <!-- 通用查询映射结果 -->
+    <resultMap id="BaseResultMap" type="com.casic.missiles.modular.robot.model.TaskGridInfo">
+        <result column="id" property="id"/>
+        <result column="create_time" property="createTime"/>
+        <result column="update_time" property="updateTime"/>
+        <result column="task_id" property="taskId"/>
+        <result column="estimate_grid_x" property="estimateGridX"/>
+        <result column="estimate_grid_y" property="estimateGridY"/>
+        <result column="neutron_count" property="neutronCount"/>
+        <result column="cur_grid_x" property="curGridX"/>
+        <result column="cur_grid_y" property="curGridY"/>
+    </resultMap>
+    <!-- 通用查询结果列 -->
+    <sql id="Base_Column_List">
+        id
+        ,
+                    create_time as createTime ,
+                    update_time as updateTime ,
+                    task_id as taskId ,
+                    estimate_grid_x as estimateGridX ,
+                    estimate_grid_y as estimateGridY ,
+                    neutron_count as neutronCount ,
+                    cur_grid_x as curGridX ,
+                    cur_grid_y as curGridY
+    </sql>
+    <sql id="Base_Column_Alias">
+        a
+        .
+        id
+        ,
+            a.create_time
+            as createTime ,
+            a.update_time
+            as updateTime ,
+            a.task_id
+            as taskId ,
+            a.estimate_grid_x
+            as estimateGridX ,
+            a.estimate_grid_y
+            as estimateGridY ,
+            a.neutron_count
+            as neutronCount ,
+            a.cur_grid_x
+            as curGridX ,
+            a.cur_grid_y
+            as curGridY
+    </sql>
+    <select id="selectTaskGridInfoPage" resultType="com.casic.missiles.modular.robot.model.TaskGridInfo">
+        select
+        <include refid="Base_Column_Alias"/>
+        from task_grid_info a
+        <where>
+            <if test="ew != null">
+                ${ew.sqlSegment}
+            </if>
+        </where>
+    </select>
+    <sql id="Base_Column_Query">
+        <if test="request!=null">
+            <if test="request.taskId!= null and request.taskId!=''">
+                and task_id = #{request.taskId}
+            </if>
+            <if test="request.estimateGridX!= null and request.estimateGridX!=''">
+                and estimate_grid_x = #{request.estimateGridX}
+            </if>
+            <if test="request.estimateGridY!= null and request.estimateGridY!=''">
+                and estimate_grid_y = #{request.estimateGridY}
+            </if>
+            <if test="request.curGridX!= null and request.curGridX!=''">
+                and cur_grid_x = #{request.curGridX}
+            </if>
+            <if test="request.curGridY!= null and request.curGridY!=''">
+                and cur_grid_y = #{request.curGridY}
+            </if>
+        </if>
+    </sql>
+</mapper>
\ No newline at end of file
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java
index 749f559..4559074 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java
@@ -13,4 +13,9 @@
      * 规划点位信息
      */
     private PointNextDTO pointNextDTO;
+    /**
+     * 寻源目标地址
+     */
+    private PointNextDTO coordinate;
+    private boolean isEnd;
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java
index 39ff2a2..cef3238 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java
@@ -12,6 +12,9 @@
     private BigDecimal y;
 
     public static GridPointDTO convertGridPoint(PointNextDTO nextDTO) {
+        if(nextDTO==null){
+            return null;
+        }
         GridPointDTO gridPointDTO = new GridPointDTO();
         gridPointDTO.setX(Convert.toBigDecimal(nextDTO.getX()));
         gridPointDTO.setY(Convert.toBigDecimal(nextDTO.getY()));
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java
index 8c88242..33be25d 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java
@@ -12,25 +12,19 @@
 public class PathInitDTO {
     //---------------------算法参数
     /**
-     * x坐标最大值（x方向栅格总数）
+     * 初始位置x坐标(栅格地图下的坐标)
      */
-    private String xMax;
+    private String xNow;
     /**
-     * y坐标最大值（y方向栅格总数）
+     * 初始位置y坐标(栅格地图下的坐标)
      */
-    private String yMax;
+    private String yNow;
     /**
      * 初始位置计数率
      */
-    private String count;
-    /**
-     * 初始位置x坐标(0<=x<x_max)
-     */
-    private String x;
-    /**
-     * 初始位置y坐标(0<=y<y_max)
-     */
-    private String y;
+    private String zNow;
+
+    private Integer stepLen;
 
     //---------------------其他执行参数
     private String robotId;
@@ -38,11 +32,11 @@
 
     public List<String> toArray() {
         List<String> list = new ArrayList<>();
-        list.add(xMax);
-        list.add(yMax);
-        list.add(count);
-        list.add(x);
-        list.add(y);
+        list.add(xNow);
+        list.add(yNow);
+        list.add(stepLen+"");
+        list.add(zNow);
+        list.add(taskId+"");
         return list;
     }
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java
index 109e95c..ada6774 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java
@@ -10,21 +10,17 @@
  */
 @Data
 public class PathPlanDTO extends PathInitDTO {
-
     /**
-     * 长度为8的数组，记录(x-1, y-1), (x-1, y), (x-1, y+1), (x, y-1), (x, y+1), (x+1, y-1), (x+1, y), (x+1, y+1)8个坐标的可通行信息。
-     * 若某坐标有障碍物（不可通行行）则记为0，否则记为1
+     * 浮点阈值(阈值，超过该值后结束巡检开始寻源)
      */
-    private String open;
-
+    private String th;
     public List<String> toArray() {
         List<String> list = new ArrayList<>();
-        list.add(getXMax());
-        list.add(getYMax());
-        list.add(open);
-        list.add(getCount());
-        list.add(getX());
-        list.add(getY());
+
+        list.add(getStepLen()+"");
+        list.add(getTh());
+        list.add(getZNow());
+        list.add(getTaskId()+"");
         return list;
     }
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskGridInfo.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskGridInfo.java
new file mode 100644
index 0000000..101c10a
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskGridInfo.java
@@ -0,0 +1,83 @@
+package com.casic.missiles.modular.robot.model;
+
+import com.baomidou.mybatisplus.annotation.TableField;
+import com.baomidou.mybatisplus.annotation.TableName;
+import com.baomidou.mybatisplus.extension.activerecord.Model;
+import io.swagger.annotations.ApiModelProperty;
+import lombok.Data;
+
+import java.math.BigDecimal;
+import java.util.Date;
+
+/**
+ * 任务网格信息对象 task_grid_info
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+@Data
+@TableName("task_grid_info")
+public class TaskGridInfo extends Model<TaskGridInfo> {
+    private static final long serialVersionUID = 1L;
+    @TableField(exist = false)
+    private Integer index;
+    @ApiModelProperty(value = "主键", dataType = "Long")
+    @TableField("id")
+    private Long id;
+
+    @ApiModelProperty(value = "创建时间", dataType = "Date")
+    @TableField("create_time")
+    private Date createTime;
+
+    @ApiModelProperty(value = "修改时间", dataType = "Date")
+    @TableField("update_time")
+    private Date updateTime;
+
+    @ApiModelProperty(value = "任务ID", dataType = "Long")
+    @TableField("task_id")
+    private Long taskId;
+
+    @ApiModelProperty(value = "估算栅格x", dataType = "Long")
+    @TableField("estimate_grid_x")
+    private String estimateGridX;
+
+    @ApiModelProperty(value = "估算栅格y", dataType = "Long")
+    @TableField("estimate_grid_y")
+    private String estimateGridY;
+
+    @ApiModelProperty(value = "中子计数率", dataType = "BigDecimal")
+    @TableField("neutron_count")
+    private BigDecimal neutronCount;
+
+    @ApiModelProperty(value = "当前栅格x", dataType = "Long")
+    @TableField("cur_grid_x")
+    private Integer curGridX;
+
+    @ApiModelProperty(value = "当前栅格y", dataType = "Long")
+    @TableField("cur_grid_y")
+    private Integer curGridY;
+
+    public String getCurPoint(){
+        return "("+curGridX+","+curGridY+")";
+    }
+    public String getEstimatePoint(){
+        if(estimateGridX==null&estimateGridY==null){
+            return "结束";
+        }
+        return "("+estimateGridX+","+estimateGridY+")";
+    }
+    @Override
+    public String toString() {
+        return "TaskGridInfo{"
+                + "id=" + id
+                + "createTime=" + createTime
+                + "updateTime=" + updateTime
+                + "taskId=" + taskId
+                + "estimateGridX=" + estimateGridX
+                + "estimateGridY=" + estimateGridY
+                + "neutronCount=" + neutronCount
+                + "curGridX=" + curGridX
+                + "curGridY=" + curGridY
+                + "}";
+    }
+}
\ No newline at end of file
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java
index edc104e..a8acb2a 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java
@@ -53,6 +53,13 @@
     @TableField("route_id")
     private Long routeId;
 
+    @ApiModelProperty(value = "寻源初始步长", dataType = "Integer")
+    @TableField("step_len")
+    private Integer stepLen;
+
+    @ApiModelProperty(value = "寻源阈值设定", dataType = "BigDecimal")
+    @TableField("max_threshold")
+    private BigDecimal maxThreshold;
     /**
      * 任务类型(1巡检任务)
      */
@@ -62,7 +69,6 @@
     @ApiModelProperty(value = "任务类型字典名称", dataType = "String")
     @TableField(exist = false)
     private String taskTypeName;
-
     @ApiModelProperty(value = "识别类型(discernType 1中子源识别)", dataType = "Integer")
     @TableField("discern_type")
     private Integer discernType;
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java
index c4bc25d..c6620f3 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java
@@ -53,7 +53,7 @@
     }),
     SET_SPEED("/set_speed" , "/set_speed_response" , "最大运动速度设置" , true, new TypeReference<RobotMsgResponseDTO<Integer>>() {
     }),
-    SET_TARGETPOINT("/set_targetpoint" , "/set_targetpoint_response" , "设置导航目标点" , false, new TypeReference<RobotMsgResponseDTO<Integer>>() {
+    SET_TARGETPOINT("/set_targetpoint" , "/set_targetpoint_response" , "设置导航目标点" , true, new TypeReference<RobotMsgResponseDTO<Integer>>() {
     }),
 
     GET_DATA_LIST("/get_data_list" , "/get_data_list_response" , "获取数据包列表" , true, new TypeReference<RobotMsgResponseDTO<List<String>>>() {
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/AbstractSocketHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/AbstractSocketHandler.java
index 83f4cb2..e47e343 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/AbstractSocketHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/AbstractSocketHandler.java
@@ -49,7 +49,7 @@
                 wsMsgBaseDTO.setMsgKey(enums.getMsgKey());
                 wsMsgBaseDTO.setData(jsonObject.get("msg"));
                 String msg = JSON.toJSONString(wsMsgBaseDTO);
-                log.info("推送{}消息：{}", enums.getRemarks(), msg);
+//                log.info("推送{}消息：{}", enums.getRemarks(), msg);
                 webSocket.sendOneMessage(robotId, msg);
             }
         } catch (Exception e) {
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/cmd/TargetPointCmdHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/cmd/TargetPointCmdHandler.java
index c1c09fa..43b4460 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/cmd/TargetPointCmdHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/cmd/TargetPointCmdHandler.java
@@ -34,7 +34,11 @@
     @Override
     public ResponseDataDTO senderCmd(TargetPointRequest request, InstructCodeEnums enums) {
         MessageRequestDTO<TargetPointRequest> messageDTO = new MessageRequestDTO();
-        //读取默认关键点信息
+        //读取默认关键点信息 {"x":"1.715708819927424","y":"-0.9100042544250901","z":"0.017993879527185876","theta":"-0.039278239414312105"}
+//        request.setX("1.715708819927424");
+//        request.setY("-0.9100042544250901");
+//        request.setZ("0.017993879527185876");
+//        request.setTheta("-0.039278239414312105");
         messageDTO.setMsg(request);
         ResponseDataDTO<Integer> list = optService.robotCmd(getRobotInfo(), enums, messageDTO);
         return list;
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/AppCostMapHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/AppCostMapHandler.java
index 0335693..88c9f79 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/AppCostMapHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/AppCostMapHandler.java
@@ -1,6 +1,11 @@
 package com.casic.missiles.modular.robot.opt.handler.msg;
 
+import com.casic.missiles.core.cache.CacheUtil;
+import com.casic.missiles.modular.constants.RobotDictConstants;
+import com.casic.missiles.modular.robot.IBaseRobotService;
+import com.casic.missiles.modular.robot.opt.enums.InstructCodeEnums;
 import com.casic.missiles.modular.robot.opt.handler.AbstractSocketHandler;
+import com.casic.missiles.modular.robot.opt.instruct.dto.CostMapResolutionRequest;
 import com.casic.missiles.modular.robot.service.IRobotStatusInfoService;
 import lombok.extern.slf4j.Slf4j;
 import org.eclipse.paho.client.mqttv3.MqttMessage;
@@ -16,14 +21,27 @@
 @Component("/app_costmap_sender")
 public class AppCostMapHandler extends AbstractSocketHandler {
     private final IRobotStatusInfoService statusInfoService;
+    private final IBaseRobotService baseRobotService;
     private static Map<String, Integer> index = new ConcurrentHashMap<>();
 
-    public AppCostMapHandler(IRobotStatusInfoService statusInfoService) {
+    public AppCostMapHandler(IRobotStatusInfoService statusInfoService, IBaseRobotService baseRobotService) {
         this.statusInfoService = statusInfoService;
+        this.baseRobotService = baseRobotService;
     }
 
     @Override
     public void process(String robotId, String topic, MqttMessage message) {
         statusInfoService.saveCostMap(robotId, message);
+
+        CostMapResolutionRequest costMapSet = CacheUtil.get(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CONFIG_COSTMAP_RESOLUTION_SET +robotId);
+        if(costMapSet != null){
+            CacheUtil.remove(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CONFIG_COSTMAP_RESOLUTION_SET +robotId);
+            new Thread(() -> {
+                costMapSet.setRobotId(Long.valueOf(robotId));
+                baseRobotService.execCmdHandler(costMapSet, InstructCodeEnums.COSTMAP_RESOLUTION);
+            }).start();
+
+        }
+
     }
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/PoseMessageMsgHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/PoseMessageMsgHandler.java
index 29f8919..6c3c3bd 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/PoseMessageMsgHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/PoseMessageMsgHandler.java
@@ -17,11 +17,9 @@
 @Component("/pose_message")
 public class PoseMessageMsgHandler extends AbstractSocketHandler {
 
-
     private final EventPublisher publisher;
 
     public PoseMessageMsgHandler(EventPublisher publisher) {
-
         this.publisher = publisher;
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/StopStateMsgHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/StopStateMsgHandler.java
index 8950815..8a71e0b 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/StopStateMsgHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/StopStateMsgHandler.java
@@ -30,7 +30,7 @@
     public void process(String robotId, String topic, MqttMessage message) {
 
         index.put(index_key, index.get("index") + 1);
-        if (index.get(index_key)>50) {
+        if (index.get(index_key)>30) {
             index.put(index_key, 0);
             log.info("主键：{},停障状态topic:{},消息内容:{}", robotId, topic, message.toString());
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/impl/RobotOptServiceImpl.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/impl/RobotOptServiceImpl.java
index 8471e7a..7940a24 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/impl/RobotOptServiceImpl.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/impl/RobotOptServiceImpl.java
@@ -146,7 +146,7 @@
     public void startTrack(StartTrackRequest request) {
         //增加循迹任务记录
         taskInfoService.reloadTask(request, robotInfoService.getById(request.getRobotId()));
-        this.execCmdHandler(request, InstructCodeEnums.START_TRACK);
+//        this.execCmdHandler(request, InstructCodeEnums.START_TRACK);
     }
 
     public IRobotRouteOptService getRobotRouteOptService() {
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/BaseRobotCmdDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/BaseRobotCmdDTO.java
index 5608f2b..06b53de 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/BaseRobotCmdDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/BaseRobotCmdDTO.java
@@ -3,11 +3,13 @@
 import io.swagger.annotations.ApiModelProperty;
 import lombok.Data;
 
+import java.io.Serializable;
+
 /**
  * 机器人指令基类
  */
 @Data
-public class BaseRobotCmdDTO {
+public class BaseRobotCmdDTO implements Serializable {
     @ApiModelProperty(value = "机器人主键", dataType = "String")
     protected Long robotId;
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/StartTrackRequest.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/StartTrackRequest.java
index a18321b..85546a7 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/StartTrackRequest.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/StartTrackRequest.java
@@ -3,6 +3,8 @@
 import io.swagger.annotations.ApiModelProperty;
 import lombok.Data;
 
+import java.math.BigDecimal;
+
 /**
  * 导航下发指令
  */
@@ -10,18 +12,19 @@
 public class StartTrackRequest extends BaseRobotCmdDTO {
     @ApiModelProperty(value = "sn编码(可不填 默认机器ID)", dataType = "String")
     private String sn;
-
     @ApiModelProperty(value = "下发状态 1:开启 0:关闭", dataType = "String")
     private Integer action;
     @ApiModelProperty(value = "等待状态 1:等待 0:不等待", dataType = "String")
     private Integer wait;
     @ApiModelProperty(value = "等待状态 0:循迹 1:任务", dataType = "isTask")
     private Integer isTask;
-    @ApiModelProperty(value = "轨迹名称", dataType = "String")
+    @ApiModelProperty(value = "轨迹名称(废弃)", dataType = "String")
     private String track_name;
     @ApiModelProperty(value = "关键点名称", dataType = "String", hidden = true)
     private String taskpoint_name;
-
+    @ApiModelProperty(value = "初始寻源步长", dataType = "Integer")
+    private Integer stepLen;
+    @ApiModelProperty(value = "寻源阈值", dataType = "BigDecimal")
+    private BigDecimal maxThreshold;
     private String obs_mode="1";
-
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/response/StopStateResponseDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/response/StopStateResponseDTO.java
index eb514bf..e0679d7 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/response/StopStateResponseDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/response/StopStateResponseDTO.java
@@ -3,11 +3,13 @@
 import io.swagger.annotations.ApiModelProperty;
 import lombok.Data;
 
+import java.io.Serializable;
+
 /**
  * 电压信息返回
  */
 @Data
-public class StopStateResponseDTO {
+public class StopStateResponseDTO implements Serializable {
     @ApiModelProperty(value = "软件急停(0：无触发 1：触发)" , dataType = "Integer")
     private Integer soft;
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IAlgorithmService.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IAlgorithmService.java
index 56b3e1d..f62523b 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IAlgorithmService.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IAlgorithmService.java
@@ -6,6 +6,7 @@
 import com.casic.missiles.modular.robot.dto.PathPlanDTO;
 import com.casic.missiles.modular.robot.model.RobotInfo;
 
+import java.io.File;
 import java.math.BigDecimal;
 import java.util.List;
 
@@ -38,5 +39,5 @@
      * @return
      */
     GridPointDTO convertGridCenterPoint(GridPointDTO pointDTO, BigDecimal resolution, RobotInfo robotInfo);
-    void readAlgorithmResponse(String fileName, AlgorithmResponse response, Long taskId, List<String> res);
+    void readAlgorithmResponse(String fileName, File repetitionFile, AlgorithmResponse response, Long taskId, List<String> res);
 }
\ No newline at end of file
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IRobotStatusInfoService.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IRobotStatusInfoService.java
index 2af210a..757f8d0 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IRobotStatusInfoService.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IRobotStatusInfoService.java
@@ -67,10 +67,9 @@
     /**
      * 更新车辆路线及任务信息
      *
-     * @param currRouteId 当前路线信息
      * @param currTaskId  当前任务信息
      */
-    void updateCurrTask(String robotId, Long currRouteId, Long currTaskId);
+    void updateCurrTask(String robotId, Long currTaskId);
 
     RobotStatusInfo getStatusByRobotId(String robotId);
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/ITaskGridInfoService.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/ITaskGridInfoService.java
new file mode 100644
index 0000000..fc0da93
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/ITaskGridInfoService.java
@@ -0,0 +1,22 @@
+package com.casic.missiles.modular.robot.service;
+
+import com.baomidou.mybatisplus.extension.service.IService;
+import  com.casic.missiles.modular.robot.model.TaskGridInfo;
+import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
+import com.baomidou.mybatisplus.extension.plugins.pagination.Page;
+import java.util.List;
+
+
+/**
+ * <p>
+ * 任务网格信息 服务类
+ * </p>
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+public interface ITaskGridInfoService extends IService<TaskGridInfo> {
+/**
+* 任务网格信息 分页检索
+*/
+List<TaskGridInfo> selectTaskGridInfoPage(Page<TaskGridInfo> page,QueryWrapper<TaskGridInfo> query);}
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/ITaskInfoService.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/ITaskInfoService.java
index 31d7ba6..b395993 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/ITaskInfoService.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/ITaskInfoService.java
@@ -51,6 +51,13 @@
      * @param dto 实体
      */
     void pointCallBack(PoseMessageResponseDTO dto, String robotId);
+
+    /**
+     * 车辆路线规划
+     * @param dto
+     * @param robotId
+     */
+    void carPathPlanning(PoseMessageResponseDTO dto, String robotId);
     void pointTestCallBack(PoseMessageResponseDTO dto, String robotId);
     /**
      * 图片路径保存
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/AlgorithmServiceImpl.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/AlgorithmServiceImpl.java
index f808fa0..a4dbe0b 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/AlgorithmServiceImpl.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/AlgorithmServiceImpl.java
@@ -29,10 +29,12 @@
 @Slf4j
 @Service
 public class AlgorithmServiceImpl implements IAlgorithmService {
-    @Value("${casic.algorithm.initPath:-1}")
-    private String initPath;
-    @Value("${casic.algorithm.planPath:-1}")
-    private String planPath;
+    @Value("${casic.algorithm.path:-1}")
+    private String path;
+    @Value("${casic.algorithm.initPathName:-1}")
+    private String initPathName;
+    @Value("${casic.algorithm.planPathName:-1}")
+    private String planPathName;
     private final ITaskHeatMapService heatMapService;
 
     public AlgorithmServiceImpl(ITaskHeatMapService heatMapService) {
@@ -42,13 +44,13 @@
     @Transactional
     @Override
     public AlgorithmResponse pathPlanningInit(PathInitDTO initDTO) {
-        if (StrUtil.isEmpty(initDTO.getCount())) {
-            initDTO.setCount("0.00");
+        if (StrUtil.isEmpty(initDTO.getZNow())) {
+            initDTO.setZNow("0.00");
         }
         log.info(JSON.toJSONString(initDTO));
         //算法路径及入参配置
         List<String> commends = new ArrayList<>();
-        commends.add(initPath);
+        commends.add(path + initPathName);
         commends.addAll(initDTO.toArray());
         List<String> res = AlgorithmUtils.invokeJob(commends);
 
@@ -63,38 +65,72 @@
     @Transactional
     @Override
     public AlgorithmResponse pathPlanning(PathPlanDTO planDTO) {
-        if (StrUtil.isEmpty(planDTO.getCount())) {
-            planDTO.setCount("0.001");
+        if (StrUtil.isEmpty(planDTO.getZNow())) {
+            planDTO.setZNow("0.001");
         }
         //算法路径及入参配置
         List<String> commends = new ArrayList<>();
-        commends.add(planPath);
+        commends.add(path + planPathName);
         commends.addAll(planDTO.toArray());
         log.info("寻源算法调用:{}", JSON.toJSONString(planDTO));
         List<String> res = AlgorithmUtils.invokeJob(commends);
         AlgorithmResponse response = getAlgorithmResponse(planDTO, res);
-        //next 点位编写
-        if (CollectionUtil.isNotEmpty(res) && res.size() == 2) {
-            response.getPointNextDTO().setX(formatBigDecimal(res.get(0).replaceAll("next_X: ", "")));
-            response.getPointNextDTO().setY(formatBigDecimal(res.get(1).replaceAll("next_Y: ", "")));
-        }
+
         return response;
     }
 
     private AlgorithmResponse getAlgorithmResponse(PathInitDTO initDTO, List<String> res) {
-        File repetitionFile = new File("repetition.txt");
+        //读取当前路径图片
+        File repetitionFileDir = new File(path + "\\" + initDTO.getTaskId() + "\\predicts");
+        File[] files = repetitionFileDir.listFiles((dir, name) -> name.endsWith(".txt") && name.startsWith("step"));
+        File repetitionFile = null;
+        //判定当前执行路径最大step2文件夹
+        if (files != null && files.length > 0) {
+             repetitionFile = files[files.length - 1];
+        }
         AlgorithmResponse response = new AlgorithmResponse();
         if (initDTO.getTaskId() != null && repetitionFile.exists()) {
-            readAlgorithmResponse("repetition.txt", response, initDTO.getTaskId(), res);
-            readAlgorithmResponse("step.txt", response, initDTO.getTaskId(), res);
+            //读取热力图
+            readAlgorithmResponse("repetition.txt",repetitionFile, response, initDTO.getTaskId(), res);
         } else {
             log.info("taskId is empty!");
         }
-        readAlgorithmResponse("xy_train.txt", response, initDTO.getTaskId(), res);
-        readAlgorithmResponse("z_train.txt", response, initDTO.getTaskId(), res);
+        //读取规划路径
+        if (CollectionUtil.isNotEmpty(res)) {
+            response.setEnd(false);
+            readAlgorithmResponse(response, res);
+        }
         return response;
     }
 
+    private void readAlgorithmResponse(AlgorithmResponse response, List<String> res) {
+        List<String> resList = new ArrayList<>();
+        if (res.size() >= 2) {
+            resList = res.subList(res.size() - 2, res.size());
+        }
+        //判定是否结束寻源
+        if (res.contains("end")) {
+            response.setEnd(true);
+            for (String s : resList) {
+                if (s.startsWith("source_coordinate:")) {
+                    s = s.trim();
+                    log.info("寻源结束,目标点位：{}",s);
+                    List<String> lists = StrUtil.split(s.replaceAll("source_coordinate:", ""), ",");
+                    PointNextDTO nextDTO = new PointNextDTO();
+                    nextDTO.setX(lists.get(0));
+                    nextDTO.setY(lists.get(1));
+                    response.setCoordinate(nextDTO);
+                }
+            }
+        } else {
+            PointNextDTO nextDTO = new PointNextDTO();
+            nextDTO.setX(resList.get(0));
+            nextDTO.setY(resList.get(1));
+            response.setPointNextDTO(nextDTO);
+        }
+
+    }
+
     /**
      * 读取算法输出数据
      *
@@ -102,49 +138,18 @@
      * @param response 结果集
      * @param taskId   任务ID
      */
-    public void readAlgorithmResponse(String fileName, AlgorithmResponse response, Long taskId, List<String> res) {
-        File file = new File(fileName);
-        if (!file.exists()&&!fileName.startsWith("step")) {
+    public void readAlgorithmResponse(String fileName,File repetitionFile, AlgorithmResponse response, Long taskId, List<String> res) {
+        if (!repetitionFile.exists() && !fileName.startsWith("step")) {
             log.info("{} file is empty!", fileName);
             return;
         }
         if (response.getPointNextDTO() == null) {
             response.setPointNextDTO(new PointNextDTO());
         }
-        switch (fileName) {
-            case "repetition.txt":
-                String msg = FileUtil.readString(file, CharsetUtil.CHARSET_UTF_8);
-                TaskHeatMap taskHeatMap = createEmptyTaskHeatMap(taskId);
-                taskHeatMap.setRepetition(msg);
-                response.setTaskHeatMap(taskHeatMap);
-                break;
-            case "xy_train.txt":
-                List<String> xyTrains = FileUtil.readLines(file, CharsetUtil.CHARSET_UTF_8);
-                if (CollectionUtil.isNotEmpty(xyTrains)) {
-                    if (CollectionUtil.isNotEmpty(xyTrains)) {
-                        List<String> list = StrUtil.split(xyTrains.get(xyTrains.size() - 1), " ");
-                        response.getPointNextDTO().setX(formatBigDecimal(list.get(0)));
-                        response.getPointNextDTO().setY(formatBigDecimal(list.get(1)));
-                    }
-                }
-                break;
-            case "z_train.txt":
-                List<String> zTrains = FileUtil.readLines(file, CharsetUtil.CHARSET_UTF_8);
-                if (CollectionUtil.isNotEmpty(zTrains)) {
-                    String zTrain = zTrains.get(zTrains.size() - 1);
-                    response.getPointNextDTO().setZ(formatBigDecimal(zTrain));
-                }
-            default:
-                if (StrUtil.isNotEmpty(fileName) && fileName.startsWith("step")) {
-                    File fileDir = new File(".");
-                    File[] files = fileDir.listFiles((dir, name) -> name.endsWith(".txt") && name.startsWith("step"));
-                    if (files != null && files.length > 0) {
-                        String steps = FileUtil.readString(files[files.length - 1], CharsetUtil.CHARSET_UTF_8);
-                        response.getTaskHeatMap().setRepetition(steps);
-                    }
-                }
-                break;
-        }
+        String msg = FileUtil.readString(repetitionFile, CharsetUtil.CHARSET_UTF_8);
+        TaskHeatMap taskHeatMap = createEmptyTaskHeatMap(taskId);
+        taskHeatMap.setRepetition(msg);
+        response.setTaskHeatMap(taskHeatMap);
     }
 
     /**
@@ -157,13 +162,11 @@
     public GridPointDTO convertSlamToGrid(BigDecimal x, BigDecimal y, BigDecimal resolution, RobotInfo robotInfo) {
         GridPointDTO pointDTO = new GridPointDTO();
         BigDecimal scale = resolution;
-
         //减去栅格原点坐标
         x = x.subtract(robotInfo.getOriginX());
         y = y.subtract(robotInfo.getOriginY());
-
-        pointDTO.setX(new BigDecimal(x.divide(scale,3,BigDecimal.ROUND_HALF_UP).intValue()+1));
-        pointDTO.setY(new BigDecimal(y.divide(scale,3,BigDecimal.ROUND_HALF_UP).intValue()+1));
+        pointDTO.setX(new BigDecimal(x.divide(scale, 3, BigDecimal.ROUND_HALF_UP).intValue()+1));
+        pointDTO.setY(new BigDecimal(y.divide(scale, 3, BigDecimal.ROUND_HALF_UP).intValue() +1));
         return pointDTO;
     }
 
@@ -174,19 +177,17 @@
      * @return
      */
     public GridPointDTO convertGridCenterPoint(GridPointDTO pointDTO, BigDecimal resolution, RobotInfo robotInfo) {
-
         GridPointDTO centerPoint = new GridPointDTO();
         BigDecimal scale = resolution;
-        //点位转换
-        BigDecimal slamX = pointDTO.getX().divide(scale, 5, BigDecimal.ROUND_HALF_UP);
-        BigDecimal slamY = pointDTO.getY().divide(scale, 5, BigDecimal.ROUND_HALF_UP);
+        //点位转换 转换后栅格－1 从1开始计算
+        BigDecimal slamX = (pointDTO.getX().subtract(new BigDecimal(1))).multiply(scale);
+        BigDecimal slamY = (pointDTO.getY().subtract(new BigDecimal(1))).multiply(scale);
         //中心栅格点位中心添加
-        centerPoint.setX(slamX.add(scale.divide(new BigDecimal(2), 5, BigDecimal.ROUND_HALF_UP)));
-        centerPoint.setY(slamY.add(scale.divide(new BigDecimal(2), 5, BigDecimal.ROUND_HALF_UP)));
+        centerPoint.setX(slamX.add(scale.divide(new BigDecimal(2), 10, BigDecimal.ROUND_HALF_UP)));
+        centerPoint.setY(slamY.add(scale.divide(new BigDecimal(2), 10, BigDecimal.ROUND_HALF_UP)));
         //加上栅格原点坐标
         centerPoint.setX(centerPoint.getX().add(robotInfo.getOriginX()));
         centerPoint.setY(centerPoint.getY().add(robotInfo.getOriginY()));
-
         return centerPoint;
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotInfoServiceImpl.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotInfoServiceImpl.java
index 043e05d..fdded64 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotInfoServiceImpl.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotInfoServiceImpl.java
@@ -104,7 +104,7 @@
             //下发障碍物栅格
             sendCostMapResolution(robotInfo.getResolution(), robotInfo.getId());
             //重新生成栅格地图
-            sendChangePcd(robotInfo.getId());
+            //sendChangePcd(robotInfo.getId());
         }
         update.eq(RobotInfo::getId, robotInfo.getId());
         update(update);
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotStatusInfoServiceImpl.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotStatusInfoServiceImpl.java
index 6d3ae91..46b5c3b 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotStatusInfoServiceImpl.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotStatusInfoServiceImpl.java
@@ -15,6 +15,7 @@
 import com.casic.missiles.modular.robot.dao.RobotStatusInfoMapper;
 import com.casic.missiles.modular.robot.model.RobotInfo;
 import com.casic.missiles.modular.robot.model.RobotStatusInfo;
+import com.casic.missiles.modular.robot.opt.enums.InstructCodeEnums;
 import com.casic.missiles.modular.robot.opt.instruct.base.dto.RobotMsgDTO;
 import com.casic.missiles.modular.robot.opt.instruct.base.dto.RobotMsgResponseDTO;
 import com.casic.missiles.modular.robot.opt.instruct.base.dto.RobotResponseDTO;
@@ -26,13 +27,18 @@
 import com.casic.missiles.modular.robot.service.IRobotInfoService;
 import com.casic.missiles.modular.robot.service.IRobotStatusInfoService;
 import lombok.extern.slf4j.Slf4j;
+import org.aspectj.util.FileUtil;
 import org.eclipse.paho.client.mqttv3.MqttMessage;
+import org.springframework.beans.factory.annotation.Value;
 import org.springframework.context.annotation.Lazy;
 import org.springframework.stereotype.Service;
 import org.springframework.transaction.annotation.Transactional;
 
+import java.io.File;
 import java.io.Serializable;
 import java.math.BigDecimal;
+import java.util.ArrayList;
+import java.util.Arrays;
 import java.util.List;
 
 
@@ -147,7 +153,6 @@
     @Override
     public void saveProcessMessage(String robotId, MqttMessage message) {
 //        String json = new String(message.getPayload());
-
         RobotResponseDTO<ProcessMessageResponseDTO> responseDTO = parseJson(message, new TypeReference<RobotResponseDTO<ProcessMessageResponseDTO>>() {
         });
         if (responseDTO.getMsg() != null) {
@@ -189,9 +194,8 @@
     }
 
     @Override
-    public void updateCurrTask(String robotId, Long currRouteId, Long currTaskId) {
+    public void updateCurrTask(String robotId, Long currTaskId) {
         UpdateWrapper<RobotStatusInfo> query = new UpdateWrapper<>();
-        query.set("curr_route_id", currRouteId);
         query.set("curr_task_id", currTaskId);
         query.eq("robot_id", robotId);
         update(query);
@@ -250,6 +254,15 @@
         }
     }
 
+    @Value("${casic.algorithm.path:-1}")
+    private String path;
+
+    /**
+     * 栅格阈值
+     */
+    @Value("${casic.algorithm.num:30}")
+    private Integer num;
+
     /**
      * 机器人栅格数据
      *
@@ -262,15 +275,41 @@
         RobotResponseDTO<AppCostmapResponseDTO> responseDTO = parseJson(message, new TypeReference<RobotResponseDTO<AppCostmapResponseDTO>>() {
         });
         if (responseDTO != null && responseDTO.getMsg() != null) {
+            //发送栅格分辨率设定
+
             cacheService.updateCostMap(robotId, responseDTO.getMsg());
             robotInfoService.updateRobotGrid(robotId, responseDTO.getMsg());
-//            log.info("--------------- json data:{}", JSON.toJSONString(responseDTO.getMsg()));
+            //log.info("--------------- json data:{}", JSON.toJSONString(responseDTO.getMsg()));
+            if (responseDTO != null && responseDTO.getMsg().getData() != null) {
+                ArrayList<Integer> dataNums = new ArrayList<>();
+                //根据阈值转换栅格
+                for (Integer dataNum : responseDTO.getMsg().getData()) {
+                    if (dataNum != -1) {
+                        if (dataNum >= num) {
+                            dataNums.add(0);
+                        } else {
+                            dataNums.add(1);
+                        }
+                    }
+                }
+                writeCache(dataNums,responseDTO.getMsg().getWidth(),responseDTO.getMsg().getHeight(),robotId);
+            }
         }
     }
 
+    private void writeCache(ArrayList<Integer> integers,Integer width,Integer height,String robotId) {
+        List<List<Integer>> arrays = CollectionUtil.split(integers,width);
+        ArrayList<ArrayList<Integer>> arrayList = new ArrayList<>();
+        for (List<Integer> array : arrays) {
+            arrayList.add((ArrayList)array);
+        }
+        CollectionUtil.reverse(arrayList);
+        CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_COST_MAP + robotId, arrayList);
+//        FileUtil.writeAsString(new File(path + "map.txt"), CollectionUtil.join(integers, " ").replaceAll("\\[", "").replaceAll("]", ""));
+    }
+
     @Override
     public void updateRobotPoint(PoseMessageResponseDTO dto, String robotId) {
-        //todo cache
         UpdateWrapper<RobotStatusInfo> query = new UpdateWrapper<>();
         query.set("postion_x", dto.getX());
         query.set("postion_y", dto.getY());
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotTrajectoryInfoServiceImpl.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotTrajectoryInfoServiceImpl.java
index 6c9625f..f1262a2 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotTrajectoryInfoServiceImpl.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotTrajectoryInfoServiceImpl.java
@@ -57,29 +57,29 @@
         });
 
         //读取导航状态,如果导航未开启则不存储
-//        RobotStatusInfo statusInfo = robotStatusInfoService.getStatusByRobotId(robotId);
-        Long routeId= cacheService.readRouteId(Long.valueOf(robotId));
-        if (routeId != null && responseDTO.getMsg() != null) {
+        //RobotStatusInfo statusInfo = robotStatusInfoService.getStatusByRobotId(robotId);
+
+        if ( responseDTO.getMsg() != null) {
             PoseMessageResponseDTO dto = responseDTO.getMsg();
             //执行循迹 保存当前轨迹信息
             //更新实时定位信息
             CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.ROBOT_POINT + robotId, dto);
-            infoService.pointTestCallBack(dto, robotId);
-            if (routeId != null) {
-                RobotTrajectoryInfo info = new RobotTrajectoryInfo();
-                info.setRobotId(Long.valueOf(robotId));
-                info.setCreateTime(new Date());
-                info.setPostionX(dto.getX());
-                info.setPostionY(dto.getY());
-                info.setPostionZ(dto.getZ());
-                info.setTheta(dto.getTheta());
-                save(info);
-                try {
-                    infoService.pointCallBack(dto, robotId);
-                } catch (Exception e) {
-                    log.error("task error", e);
-                }
+            try {
+                infoService.carPathPlanning(dto, robotId);
+            } catch (Exception e) {
+                log.error("task error", e);
             }
+//            if (routeId != null) {
+//                RobotTrajectoryInfo info = new RobotTrajectoryInfo();
+//                info.setRobotId(Long.valueOf(robotId));
+//                info.setCreateTime(new Date());
+//                info.setPostionX(dto.getX());
+//                info.setPostionY(dto.getY());
+//                info.setPostionZ(dto.getZ());
+//                info.setTheta(dto.getTheta());
+//                save(info);
+
+//            }
         }
     }
 

diff --git a/.gitignore b/.gitignore
index 9e20bc1..f42a9cb 100644
--- a/.gitignore
+++ b/.gitignore
@@ -2,3 +2,4 @@
 **/target/**
 logs/
 *.iml
+*.exe
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java b/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
index c7b5d28..a849528 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
@@ -28,6 +28,7 @@
     String KEY_CUR_TASK_ID = "CUR_TASK_ID_";
     String KEY_CUR_TASK_INIT = "CUR_TASK_INIT_";
 
+    String KEY_COST_MAP = "KEY_COST_MAP";
     /**
      * PSD数据ID
      */
@@ -41,6 +42,7 @@
      */
     String KEY_CUR_XY_POINT = "KEY_CUR_XY_POINT_";
     String KEY_CUR_ESTIMATE_GRID = "KEY_CUR_ESTIMATE_GRID_";
+    String KEY_PLANING_FLAG = "KEY_PLANING_FLAG_";
     /**
      * 机器人巡点栅格
      */
@@ -55,6 +57,7 @@
      * 探测器配置参数缓存
      */
     String KEY_CONFIG_PARAMS_ID = "KEY_CONFIG_PARAMS_ID_";
+    String KEY_CONFIG_COSTMAP_RESOLUTION_SET = "KEY_CONFIG_COSTMAP_RESOLUTION_SET_";
 
     /**
      * 暂停状态缓存信息
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
index c4c905d..3bb475f 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
@@ -1,9 +1,14 @@
 package com.casic.missiles.modular.robot.controller;
 
+import cn.hutool.core.lang.Assert;
 import cn.hutool.core.util.StrUtil;
 import com.casic.missiles.core.base.controller.BaseController;
+import com.casic.missiles.core.cache.CacheUtil;
 import com.casic.missiles.core.util.SpringContextHolder;
+import com.casic.missiles.model.exception.RequestValidException;
+import com.casic.missiles.model.response.ResponseData;
 import com.casic.missiles.model.response.dto.ResponseDataDTO;
+import com.casic.missiles.modular.constants.RobotDictConstants;
 import com.casic.missiles.modular.robot.IBaseRobotService;
 import com.casic.missiles.modular.robot.dto.RobotInfoSetDTO;
 import com.casic.missiles.modular.robot.model.RobotInfo;
@@ -19,10 +24,16 @@
 import io.swagger.annotations.ApiOperation;
 import io.swagger.annotations.ApiParam;
 import lombok.SneakyThrows;
+import lombok.extern.slf4j.Slf4j;
+import org.apache.ibatis.annotations.Param;
+import org.springframework.beans.factory.annotation.Value;
 import org.springframework.web.bind.annotation.*;
+import org.springframework.web.multipart.MultipartFile;
 
 import javax.servlet.http.HttpServletResponse;
+import java.io.File;
 import java.util.List;
+import java.util.Optional;
 
 /**
  * 机器人操作类
@@ -30,6 +41,7 @@
  * @author lwh
  * @date 2023-10-30
  */
+@Slf4j
 @Api(tags = "机器人控制接口")
 @RestController
 @RequestMapping("/robot/opt")
@@ -61,6 +73,28 @@
         return baseRobotService.execCmdHandler(request, InstructCodeEnums.CHANGE_PCD);
     }
 
+    @Value("${casic.file.uploadPath}")
+    private String filePath;
+
+    @SneakyThrows
+    @ApiOperation(value = "PCD上传接口")
+    @PostMapping(value = "/uploadPcd")
+    public ResponseData uploadPcd(@RequestParam("file") MultipartFile file, @ApiParam("机器人ID,url拼接传参") String robotId) {
+        Assert.isFalse(StrUtil.isEmpty(robotId), () -> new RequestValidException("机器ID不能为空", Optional.of(new Throwable().getStackTrace()[2])));
+        RobotInfo info = robotInfoService.getById(robotId);
+        if (file != null && !file.isEmpty()) {
+            File pcdFile = new File(filePath + "finalCloud_" + info.getId() + ".pcd");
+            try {
+                file.transferTo(pcdFile);
+            } catch (Exception e) {
+                log.error(e.getMessage());
+            }
+            return ResponseData.success();
+        } else {
+            return ResponseData.error("附件上传失败!");
+        }
+    }
+
     @SneakyThrows
     @ApiOperation(value = "PCD下载接口")
     @PostMapping(value = "/pcdDownload")
@@ -136,6 +170,10 @@
     @ApiOperation(value = "启动或关闭循迹功能", notes = "返回 1成功 0失败  wait 启动时是否等待，0代表循迹启动即运动，不等待；1代表循迹启动原地等待。若无此字段，默认循迹启动不等待直接运动")
     @PostMapping(value = "/startTrack")
     public ResponseDataDTO<Integer> startTrack(@RequestBody StartTrackRequest request) {
+        request.setIsTask(1);
+        if (request.getStepLen() == null) {
+            request.setStepLen(2);
+        }
         baseRobotService.startTrack(request);
         return ResponseDataDTO.success();
     }
@@ -156,6 +194,13 @@
         MessageRequestDTO<CrlNavRequest> messageDTO = new MessageRequestDTO();
         messageDTO.setMsg(request);
         ResponseDataDTO<Integer> list = baseRobotService.getRobotRouteOptService().robotCmd(info, InstructCodeEnums.CTRL_NAV, messageDTO);
+        if (request.getAction() == 1) {
+            CostMapResolutionRequest costMap = new CostMapResolutionRequest();
+            costMap.setResolution(info.getResolution().doubleValue());
+            request.setRobotId(request.getRobotId());
+            //是否下发分辨率
+            CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CONFIG_COSTMAP_RESOLUTION_SET + request.getRobotId(), costMap);
+        }
         return list;
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java
new file mode 100644
index 0000000..5317651
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java
@@ -0,0 +1,86 @@
+package com.casic.missiles.modular.robot.controller;
+
+import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
+import com.baomidou.mybatisplus.extension.plugins.pagination.Page;
+import com.casic.missiles.core.base.controller.BaseController;
+import com.casic.missiles.core.page.PageFactory;
+import com.casic.missiles.core.page.PageInfoBT;
+import com.casic.missiles.model.form.IdForms;
+import com.casic.missiles.model.response.dto.ResponseDataDTO;
+import com.casic.missiles.modular.robot.model.TaskGridInfo;
+import com.casic.missiles.modular.robot.service.ITaskGridInfoService;
+import com.casic.missiles.modular.robot.vo.TaskGridInfoVO;
+import io.swagger.annotations.Api;
+import io.swagger.annotations.ApiOperation;
+import org.springframework.web.bind.annotation.*;
+
+import java.util.List;
+
+/**
+ * 任务网格信息Controller
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+@Api(tags = "任务网格信息")
+@RestController
+@RequestMapping("/robot/taskGridInfo")
+public class TaskGridInfoController extends BaseController {
+
+    private final ITaskGridInfoService taskGridInfoService;
+
+    public TaskGridInfoController(ITaskGridInfoService taskGridInfoService) {
+        this.taskGridInfoService = taskGridInfoService;
+    }
+
+    @ApiOperation(value = "列表查询")
+    @GetMapping(value = "/list")
+    public ResponseDataDTO<List<TaskGridInfo>> list(TaskGridInfoVO taskGridInfoVO) {
+        QueryWrapper<TaskGridInfo> query = taskGridInfoVO.genQuery();
+        return ResponseDataDTO.success(taskGridInfoService.list(query));
+    }
+
+    @ApiOperation(value = "分页查询")
+    @GetMapping(value = "/listPage")
+    public ResponseDataDTO<PageInfoBT<TaskGridInfo>> listPage(TaskGridInfoVO taskGridInfoVO) {
+        Page<TaskGridInfo> page = PageFactory.defaultPage();
+        QueryWrapper<TaskGridInfo> query = taskGridInfoVO.genQuery();
+        page = taskGridInfoService.page(page, query);
+        return ResponseDataDTO.success(super.packForBT(page));
+    }
+
+
+    @ApiOperation(value = "新增接口")
+    @PostMapping(value = "/add")
+    public ResponseDataDTO add(@RequestBody TaskGridInfo taskGridInfo) {
+        taskGridInfoService.save(taskGridInfo);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "修改接口")
+    @PostMapping(value = "/update")
+    public ResponseDataDTO update(@RequestBody TaskGridInfo taskGridInfo) {
+        taskGridInfoService.updateById(taskGridInfo);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "删除接口")
+    @GetMapping(value = "/delete")
+    public ResponseDataDTO delete(String id) {
+        taskGridInfoService.removeById(id);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation("批量删除")
+    @PostMapping("/batchDelete")
+    public ResponseDataDTO batchDelete(@RequestBody IdForms ids) {
+        taskGridInfoService.removeByIds(ids.getIds());
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "详情查询")
+    @GetMapping(value = "/detail")
+    public ResponseDataDTO<TaskGridInfo> detail(String id) {
+        return ResponseDataDTO.success(taskGridInfoService.getById(id));
+    }
+}
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
index e82a792..6992274 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
@@ -98,7 +98,7 @@
     public ResponseDataDTO<AlgorithmResponse> test() {
         AlgorithmResponse response = new AlgorithmResponse();
         response.setTaskHeatMap(new TaskHeatMap());
-        algorithmService.readAlgorithmResponse("step3.txt",response,123L,null);
+        algorithmService.readAlgorithmResponse("step3.txt",null,response,123L,null);
         sendWebSocket(InstructStatusMsgEnums.HEAT_MAP, "1", response.getTaskHeatMap());
         return ResponseDataDTO.success(response);
     }
@@ -121,7 +121,7 @@
     @GetMapping(value = "/testGrid")
     public ResponseDataDTO<List<RouteInfo>> testGrid(@RequestBody PathInitDTO pathInitDTO) {
         RobotInfo robotInfo = infoService.getById(pathInitDTO.getRobotId());
-        GridPointDTO gridPointDTO = algorithmService.convertSlamToGrid(new BigDecimal(pathInitDTO.getX()), new BigDecimal(10), new BigDecimal(4), robotInfo);
+        GridPointDTO gridPointDTO = algorithmService.convertSlamToGrid(new BigDecimal(pathInitDTO.getXNow()), new BigDecimal(10), new BigDecimal(4), robotInfo);
         return ResponseDataDTO.success(algorithmService.convertGridCenterPoint(gridPointDTO, new BigDecimal(10), robotInfo));
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/TaskGridInfoMapper.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/TaskGridInfoMapper.java
new file mode 100644
index 0000000..5c3a70b
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/TaskGridInfoMapper.java
@@ -0,0 +1,23 @@
+package com.casic.missiles.modular.robot.dao;
+
+
+import com.baomidou.mybatisplus.core.mapper.BaseMapper;
+import com.casic.missiles.modular.robot.model.TaskGridInfo;
+import org.apache.ibatis.annotations.Param;
+import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
+import com.baomidou.mybatisplus.extension.plugins.pagination.Page;
+import java.util.List;
+/**
+ * <p>
+ *  任务网格信息 Mapper 接口
+ * </p>
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+public interface TaskGridInfoMapper extends BaseMapper<TaskGridInfo> {
+/**
+* 任务网格信息 分页检索
+*/
+List<TaskGridInfo> selectTaskGridInfoPage( @Param("page") Page<TaskGridInfo> page,@Param("ew" ) QueryWrapper<TaskGridInfo> query);
+}
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/mapping/TaskGridInfoMapper.xml b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/mapping/TaskGridInfoMapper.xml
new file mode 100644
index 0000000..be573e0
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/mapping/TaskGridInfoMapper.xml
@@ -0,0 +1,80 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!DOCTYPE mapper PUBLIC "-//mybatis.org//DTD Mapper 3.0//EN" "http://mybatis.org/dtd/mybatis-3-mapper.dtd">
+<mapper namespace="com.casic.missiles.modular.robot.dao.TaskGridInfoMapper">
+    <!-- 通用查询映射结果 -->
+    <resultMap id="BaseResultMap" type="com.casic.missiles.modular.robot.model.TaskGridInfo">
+        <result column="id" property="id"/>
+        <result column="create_time" property="createTime"/>
+        <result column="update_time" property="updateTime"/>
+        <result column="task_id" property="taskId"/>
+        <result column="estimate_grid_x" property="estimateGridX"/>
+        <result column="estimate_grid_y" property="estimateGridY"/>
+        <result column="neutron_count" property="neutronCount"/>
+        <result column="cur_grid_x" property="curGridX"/>
+        <result column="cur_grid_y" property="curGridY"/>
+    </resultMap>
+    <!-- 通用查询结果列 -->
+    <sql id="Base_Column_List">
+        id
+        ,
+                    create_time as createTime ,
+                    update_time as updateTime ,
+                    task_id as taskId ,
+                    estimate_grid_x as estimateGridX ,
+                    estimate_grid_y as estimateGridY ,
+                    neutron_count as neutronCount ,
+                    cur_grid_x as curGridX ,
+                    cur_grid_y as curGridY
+    </sql>
+    <sql id="Base_Column_Alias">
+        a
+        .
+        id
+        ,
+            a.create_time
+            as createTime ,
+            a.update_time
+            as updateTime ,
+            a.task_id
+            as taskId ,
+            a.estimate_grid_x
+            as estimateGridX ,
+            a.estimate_grid_y
+            as estimateGridY ,
+            a.neutron_count
+            as neutronCount ,
+            a.cur_grid_x
+            as curGridX ,
+            a.cur_grid_y
+            as curGridY
+    </sql>
+    <select id="selectTaskGridInfoPage" resultType="com.casic.missiles.modular.robot.model.TaskGridInfo">
+        select
+        <include refid="Base_Column_Alias"/>
+        from task_grid_info a
+        <where>
+            <if test="ew != null">
+                ${ew.sqlSegment}
+            </if>
+        </where>
+    </select>
+    <sql id="Base_Column_Query">
+        <if test="request!=null">
+            <if test="request.taskId!= null and request.taskId!=''">
+                and task_id = #{request.taskId}
+            </if>
+            <if test="request.estimateGridX!= null and request.estimateGridX!=''">
+                and estimate_grid_x = #{request.estimateGridX}
+            </if>
+            <if test="request.estimateGridY!= null and request.estimateGridY!=''">
+                and estimate_grid_y = #{request.estimateGridY}
+            </if>
+            <if test="request.curGridX!= null and request.curGridX!=''">
+                and cur_grid_x = #{request.curGridX}
+            </if>
+            <if test="request.curGridY!= null and request.curGridY!=''">
+                and cur_grid_y = #{request.curGridY}
+            </if>
+        </if>
+    </sql>
+</mapper>
\ No newline at end of file
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java
index 749f559..4559074 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java
@@ -13,4 +13,9 @@
      * 规划点位信息
      */
     private PointNextDTO pointNextDTO;
+    /**
+     * 寻源目标地址
+     */
+    private PointNextDTO coordinate;
+    private boolean isEnd;
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java
index 39ff2a2..cef3238 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java
@@ -12,6 +12,9 @@
     private BigDecimal y;
 
     public static GridPointDTO convertGridPoint(PointNextDTO nextDTO) {
+        if(nextDTO==null){
+            return null;
+        }
         GridPointDTO gridPointDTO = new GridPointDTO();
         gridPointDTO.setX(Convert.toBigDecimal(nextDTO.getX()));
         gridPointDTO.setY(Convert.toBigDecimal(nextDTO.getY()));
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java
index 8c88242..33be25d 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java
@@ -12,25 +12,19 @@
 public class PathInitDTO {
     //---------------------算法参数
     /**
-     * x坐标最大值（x方向栅格总数）
+     * 初始位置x坐标(栅格地图下的坐标)
      */
-    private String xMax;
+    private String xNow;
     /**
-     * y坐标最大值（y方向栅格总数）
+     * 初始位置y坐标(栅格地图下的坐标)
      */
-    private String yMax;
+    private String yNow;
     /**
      * 初始位置计数率
      */
-    private String count;
-    /**
-     * 初始位置x坐标(0<=x<x_max)
-     */
-    private String x;
-    /**
-     * 初始位置y坐标(0<=y<y_max)
-     */
-    private String y;
+    private String zNow;
+
+    private Integer stepLen;
 
     //---------------------其他执行参数
     private String robotId;
@@ -38,11 +32,11 @@
 
     public List<String> toArray() {
         List<String> list = new ArrayList<>();
-        list.add(xMax);
-        list.add(yMax);
-        list.add(count);
-        list.add(x);
-        list.add(y);
+        list.add(xNow);
+        list.add(yNow);
+        list.add(stepLen+"");
+        list.add(zNow);
+        list.add(taskId+"");
         return list;
     }
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java
index 109e95c..ada6774 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java
@@ -10,21 +10,17 @@
  */
 @Data
 public class PathPlanDTO extends PathInitDTO {
-
     /**
-     * 长度为8的数组，记录(x-1, y-1), (x-1, y), (x-1, y+1), (x, y-1), (x, y+1), (x+1, y-1), (x+1, y), (x+1, y+1)8个坐标的可通行信息。
-     * 若某坐标有障碍物（不可通行行）则记为0，否则记为1
+     * 浮点阈值(阈值，超过该值后结束巡检开始寻源)
      */
-    private String open;
-
+    private String th;
     public List<String> toArray() {
         List<String> list = new ArrayList<>();
-        list.add(getXMax());
-        list.add(getYMax());
-        list.add(open);
-        list.add(getCount());
-        list.add(getX());
-        list.add(getY());
+
+        list.add(getStepLen()+"");
+        list.add(getTh());
+        list.add(getZNow());
+        list.add(getTaskId()+"");
         return list;
     }
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskGridInfo.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskGridInfo.java
new file mode 100644
index 0000000..101c10a
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskGridInfo.java
@@ -0,0 +1,83 @@
+package com.casic.missiles.modular.robot.model;
+
+import com.baomidou.mybatisplus.annotation.TableField;
+import com.baomidou.mybatisplus.annotation.TableName;
+import com.baomidou.mybatisplus.extension.activerecord.Model;
+import io.swagger.annotations.ApiModelProperty;
+import lombok.Data;
+
+import java.math.BigDecimal;
+import java.util.Date;
+
+/**
+ * 任务网格信息对象 task_grid_info
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+@Data
+@TableName("task_grid_info")
+public class TaskGridInfo extends Model<TaskGridInfo> {
+    private static final long serialVersionUID = 1L;
+    @TableField(exist = false)
+    private Integer index;
+    @ApiModelProperty(value = "主键", dataType = "Long")
+    @TableField("id")
+    private Long id;
+
+    @ApiModelProperty(value = "创建时间", dataType = "Date")
+    @TableField("create_time")
+    private Date createTime;
+
+    @ApiModelProperty(value = "修改时间", dataType = "Date")
+    @TableField("update_time")
+    private Date updateTime;
+
+    @ApiModelProperty(value = "任务ID", dataType = "Long")
+    @TableField("task_id")
+    private Long taskId;
+
+    @ApiModelProperty(value = "估算栅格x", dataType = "Long")
+    @TableField("estimate_grid_x")
+    private String estimateGridX;
+
+    @ApiModelProperty(value = "估算栅格y", dataType = "Long")
+    @TableField("estimate_grid_y")
+    private String estimateGridY;
+
+    @ApiModelProperty(value = "中子计数率", dataType = "BigDecimal")
+    @TableField("neutron_count")
+    private BigDecimal neutronCount;
+
+    @ApiModelProperty(value = "当前栅格x", dataType = "Long")
+    @TableField("cur_grid_x")
+    private Integer curGridX;
+
+    @ApiModelProperty(value = "当前栅格y", dataType = "Long")
+    @TableField("cur_grid_y")
+    private Integer curGridY;
+
+    public String getCurPoint(){
+        return "("+curGridX+","+curGridY+")";
+    }
+    public String getEstimatePoint(){
+        if(estimateGridX==null&estimateGridY==null){
+            return "结束";
+        }
+        return "("+estimateGridX+","+estimateGridY+")";
+    }
+    @Override
+    public String toString() {
+        return "TaskGridInfo{"
+                + "id=" + id
+                + "createTime=" + createTime
+                + "updateTime=" + updateTime
+                + "taskId=" + taskId
+                + "estimateGridX=" + estimateGridX
+                + "estimateGridY=" + estimateGridY
+                + "neutronCount=" + neutronCount
+                + "curGridX=" + curGridX
+                + "curGridY=" + curGridY
+                + "}";
+    }
+}
\ No newline at end of file
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java
index edc104e..a8acb2a 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java
@@ -53,6 +53,13 @@
     @TableField("route_id")
     private Long routeId;
 
+    @ApiModelProperty(value = "寻源初始步长", dataType = "Integer")
+    @TableField("step_len")
+    private Integer stepLen;
+
+    @ApiModelProperty(value = "寻源阈值设定", dataType = "BigDecimal")
+    @TableField("max_threshold")
+    private BigDecimal maxThreshold;
     /**
      * 任务类型(1巡检任务)
      */
@@ -62,7 +69,6 @@
     @ApiModelProperty(value = "任务类型字典名称", dataType = "String")
     @TableField(exist = false)
     private String taskTypeName;
-
     @ApiModelProperty(value = "识别类型(discernType 1中子源识别)", dataType = "Integer")
     @TableField("discern_type")
     private Integer discernType;
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java
index c4bc25d..c6620f3 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java
@@ -53,7 +53,7 @@
     }),
     SET_SPEED("/set_speed" , "/set_speed_response" , "最大运动速度设置" , true, new TypeReference<RobotMsgResponseDTO<Integer>>() {
     }),
-    SET_TARGETPOINT("/set_targetpoint" , "/set_targetpoint_response" , "设置导航目标点" , false, new TypeReference<RobotMsgResponseDTO<Integer>>() {
+    SET_TARGETPOINT("/set_targetpoint" , "/set_targetpoint_response" , "设置导航目标点" , true, new TypeReference<RobotMsgResponseDTO<Integer>>() {
     }),
 
     GET_DATA_LIST("/get_data_list" , "/get_data_list_response" , "获取数据包列表" , true, new TypeReference<RobotMsgResponseDTO<List<String>>>() {
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/AbstractSocketHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/AbstractSocketHandler.java
index 83f4cb2..e47e343 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/AbstractSocketHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/AbstractSocketHandler.java
@@ -49,7 +49,7 @@
                 wsMsgBaseDTO.setMsgKey(enums.getMsgKey());
                 wsMsgBaseDTO.setData(jsonObject.get("msg"));
                 String msg = JSON.toJSONString(wsMsgBaseDTO);
-                log.info("推送{}消息：{}", enums.getRemarks(), msg);
+//                log.info("推送{}消息：{}", enums.getRemarks(), msg);
                 webSocket.sendOneMessage(robotId, msg);
             }
         } catch (Exception e) {
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/cmd/TargetPointCmdHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/cmd/TargetPointCmdHandler.java
index c1c09fa..43b4460 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/cmd/TargetPointCmdHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/cmd/TargetPointCmdHandler.java
@@ -34,7 +34,11 @@
     @Override
     public ResponseDataDTO senderCmd(TargetPointRequest request, InstructCodeEnums enums) {
         MessageRequestDTO<TargetPointRequest> messageDTO = new MessageRequestDTO();
-        //读取默认关键点信息
+        //读取默认关键点信息 {"x":"1.715708819927424","y":"-0.9100042544250901","z":"0.017993879527185876","theta":"-0.039278239414312105"}
+//        request.setX("1.715708819927424");
+//        request.setY("-0.9100042544250901");
+//        request.setZ("0.017993879527185876");
+//        request.setTheta("-0.039278239414312105");
         messageDTO.setMsg(request);
         ResponseDataDTO<Integer> list = optService.robotCmd(getRobotInfo(), enums, messageDTO);
         return list;
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/AppCostMapHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/AppCostMapHandler.java
index 0335693..88c9f79 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/AppCostMapHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/AppCostMapHandler.java
@@ -1,6 +1,11 @@
 package com.casic.missiles.modular.robot.opt.handler.msg;
 
+import com.casic.missiles.core.cache.CacheUtil;
+import com.casic.missiles.modular.constants.RobotDictConstants;
+import com.casic.missiles.modular.robot.IBaseRobotService;
+import com.casic.missiles.modular.robot.opt.enums.InstructCodeEnums;
 import com.casic.missiles.modular.robot.opt.handler.AbstractSocketHandler;
+import com.casic.missiles.modular.robot.opt.instruct.dto.CostMapResolutionRequest;
 import com.casic.missiles.modular.robot.service.IRobotStatusInfoService;
 import lombok.extern.slf4j.Slf4j;
 import org.eclipse.paho.client.mqttv3.MqttMessage;
@@ -16,14 +21,27 @@
 @Component("/app_costmap_sender")
 public class AppCostMapHandler extends AbstractSocketHandler {
     private final IRobotStatusInfoService statusInfoService;
+    private final IBaseRobotService baseRobotService;
     private static Map<String, Integer> index = new ConcurrentHashMap<>();
 
-    public AppCostMapHandler(IRobotStatusInfoService statusInfoService) {
+    public AppCostMapHandler(IRobotStatusInfoService statusInfoService, IBaseRobotService baseRobotService) {
         this.statusInfoService = statusInfoService;
+        this.baseRobotService = baseRobotService;
     }
 
     @Override
     public void process(String robotId, String topic, MqttMessage message) {
         statusInfoService.saveCostMap(robotId, message);
+
+        CostMapResolutionRequest costMapSet = CacheUtil.get(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CONFIG_COSTMAP_RESOLUTION_SET +robotId);
+        if(costMapSet != null){
+            CacheUtil.remove(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CONFIG_COSTMAP_RESOLUTION_SET +robotId);
+            new Thread(() -> {
+                costMapSet.setRobotId(Long.valueOf(robotId));
+                baseRobotService.execCmdHandler(costMapSet, InstructCodeEnums.COSTMAP_RESOLUTION);
+            }).start();
+
+        }
+
     }
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/PoseMessageMsgHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/PoseMessageMsgHandler.java
index 29f8919..6c3c3bd 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/PoseMessageMsgHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/PoseMessageMsgHandler.java
@@ -17,11 +17,9 @@
 @Component("/pose_message")
 public class PoseMessageMsgHandler extends AbstractSocketHandler {
 
-
     private final EventPublisher publisher;
 
     public PoseMessageMsgHandler(EventPublisher publisher) {
-
         this.publisher = publisher;
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/StopStateMsgHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/StopStateMsgHandler.java
index 8950815..8a71e0b 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/StopStateMsgHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/StopStateMsgHandler.java
@@ -30,7 +30,7 @@
     public void process(String robotId, String topic, MqttMessage message) {
 
         index.put(index_key, index.get("index") + 1);
-        if (index.get(index_key)>50) {
+        if (index.get(index_key)>30) {
             index.put(index_key, 0);
             log.info("主键：{},停障状态topic:{},消息内容:{}", robotId, topic, message.toString());
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/impl/RobotOptServiceImpl.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/impl/RobotOptServiceImpl.java
index 8471e7a..7940a24 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/impl/RobotOptServiceImpl.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/impl/RobotOptServiceImpl.java
@@ -146,7 +146,7 @@
     public void startTrack(StartTrackRequest request) {
         //增加循迹任务记录
         taskInfoService.reloadTask(request, robotInfoService.getById(request.getRobotId()));
-        this.execCmdHandler(request, InstructCodeEnums.START_TRACK);
+//        this.execCmdHandler(request, InstructCodeEnums.START_TRACK);
     }
 
     public IRobotRouteOptService getRobotRouteOptService() {
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/BaseRobotCmdDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/BaseRobotCmdDTO.java
index 5608f2b..06b53de 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/BaseRobotCmdDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/BaseRobotCmdDTO.java
@@ -3,11 +3,13 @@
 import io.swagger.annotations.ApiModelProperty;
 import lombok.Data;
 
+import java.io.Serializable;
+
 /**
  * 机器人指令基类
  */
 @Data
-public class BaseRobotCmdDTO {
+public class BaseRobotCmdDTO implements Serializable {
     @ApiModelProperty(value = "机器人主键", dataType = "String")
     protected Long robotId;
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/StartTrackRequest.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/StartTrackRequest.java
index a18321b..85546a7 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/StartTrackRequest.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/StartTrackRequest.java
@@ -3,6 +3,8 @@
 import io.swagger.annotations.ApiModelProperty;
 import lombok.Data;
 
+import java.math.BigDecimal;
+
 /**
  * 导航下发指令
  */
@@ -10,18 +12,19 @@
 public class StartTrackRequest extends BaseRobotCmdDTO {
     @ApiModelProperty(value = "sn编码(可不填 默认机器ID)", dataType = "String")
     private String sn;
-
     @ApiModelProperty(value = "下发状态 1:开启 0:关闭", dataType = "String")
     private Integer action;
     @ApiModelProperty(value = "等待状态 1:等待 0:不等待", dataType = "String")
     private Integer wait;
     @ApiModelProperty(value = "等待状态 0:循迹 1:任务", dataType = "isTask")
     private Integer isTask;
-    @ApiModelProperty(value = "轨迹名称", dataType = "String")
+    @ApiModelProperty(value = "轨迹名称(废弃)", dataType = "String")
     private String track_name;
     @ApiModelProperty(value = "关键点名称", dataType = "String", hidden = true)
     private String taskpoint_name;
-
+    @ApiModelProperty(value = "初始寻源步长", dataType = "Integer")
+    private Integer stepLen;
+    @ApiModelProperty(value = "寻源阈值", dataType = "BigDecimal")
+    private BigDecimal maxThreshold;
     private String obs_mode="1";
-
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/response/StopStateResponseDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/response/StopStateResponseDTO.java
index eb514bf..e0679d7 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/response/StopStateResponseDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/response/StopStateResponseDTO.java
@@ -3,11 +3,13 @@
 import io.swagger.annotations.ApiModelProperty;
 import lombok.Data;
 
+import java.io.Serializable;
+
 /**
  * 电压信息返回
  */
 @Data
-public class StopStateResponseDTO {
+public class StopStateResponseDTO implements Serializable {
     @ApiModelProperty(value = "软件急停(0：无触发 1：触发)" , dataType = "Integer")
     private Integer soft;
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IAlgorithmService.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IAlgorithmService.java
index 56b3e1d..f62523b 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IAlgorithmService.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IAlgorithmService.java
@@ -6,6 +6,7 @@
 import com.casic.missiles.modular.robot.dto.PathPlanDTO;
 import com.casic.missiles.modular.robot.model.RobotInfo;
 
+import java.io.File;
 import java.math.BigDecimal;
 import java.util.List;
 
@@ -38,5 +39,5 @@
      * @return
      */
     GridPointDTO convertGridCenterPoint(GridPointDTO pointDTO, BigDecimal resolution, RobotInfo robotInfo);
-    void readAlgorithmResponse(String fileName, AlgorithmResponse response, Long taskId, List<String> res);
+    void readAlgorithmResponse(String fileName, File repetitionFile, AlgorithmResponse response, Long taskId, List<String> res);
 }
\ No newline at end of file
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IRobotStatusInfoService.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IRobotStatusInfoService.java
index 2af210a..757f8d0 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IRobotStatusInfoService.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IRobotStatusInfoService.java
@@ -67,10 +67,9 @@
     /**
      * 更新车辆路线及任务信息
      *
-     * @param currRouteId 当前路线信息
      * @param currTaskId  当前任务信息
      */
-    void updateCurrTask(String robotId, Long currRouteId, Long currTaskId);
+    void updateCurrTask(String robotId, Long currTaskId);
 
     RobotStatusInfo getStatusByRobotId(String robotId);
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/ITaskGridInfoService.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/ITaskGridInfoService.java
new file mode 100644
index 0000000..fc0da93
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/ITaskGridInfoService.java
@@ -0,0 +1,22 @@
+package com.casic.missiles.modular.robot.service;
+
+import com.baomidou.mybatisplus.extension.service.IService;
+import  com.casic.missiles.modular.robot.model.TaskGridInfo;
+import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
+import com.baomidou.mybatisplus.extension.plugins.pagination.Page;
+import java.util.List;
+
+
+/**
+ * <p>
+ * 任务网格信息 服务类
+ * </p>
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+public interface ITaskGridInfoService extends IService<TaskGridInfo> {
+/**
+* 任务网格信息 分页检索
+*/
+List<TaskGridInfo> selectTaskGridInfoPage(Page<TaskGridInfo> page,QueryWrapper<TaskGridInfo> query);}
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/ITaskInfoService.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/ITaskInfoService.java
index 31d7ba6..b395993 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/ITaskInfoService.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/ITaskInfoService.java
@@ -51,6 +51,13 @@
      * @param dto 实体
      */
     void pointCallBack(PoseMessageResponseDTO dto, String robotId);
+
+    /**
+     * 车辆路线规划
+     * @param dto
+     * @param robotId
+     */
+    void carPathPlanning(PoseMessageResponseDTO dto, String robotId);
     void pointTestCallBack(PoseMessageResponseDTO dto, String robotId);
     /**
      * 图片路径保存
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/AlgorithmServiceImpl.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/AlgorithmServiceImpl.java
index f808fa0..a4dbe0b 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/AlgorithmServiceImpl.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/AlgorithmServiceImpl.java
@@ -29,10 +29,12 @@
 @Slf4j
 @Service
 public class AlgorithmServiceImpl implements IAlgorithmService {
-    @Value("${casic.algorithm.initPath:-1}")
-    private String initPath;
-    @Value("${casic.algorithm.planPath:-1}")
-    private String planPath;
+    @Value("${casic.algorithm.path:-1}")
+    private String path;
+    @Value("${casic.algorithm.initPathName:-1}")
+    private String initPathName;
+    @Value("${casic.algorithm.planPathName:-1}")
+    private String planPathName;
     private final ITaskHeatMapService heatMapService;
 
     public AlgorithmServiceImpl(ITaskHeatMapService heatMapService) {
@@ -42,13 +44,13 @@
     @Transactional
     @Override
     public AlgorithmResponse pathPlanningInit(PathInitDTO initDTO) {
-        if (StrUtil.isEmpty(initDTO.getCount())) {
-            initDTO.setCount("0.00");
+        if (StrUtil.isEmpty(initDTO.getZNow())) {
+            initDTO.setZNow("0.00");
         }
         log.info(JSON.toJSONString(initDTO));
         //算法路径及入参配置
         List<String> commends = new ArrayList<>();
-        commends.add(initPath);
+        commends.add(path + initPathName);
         commends.addAll(initDTO.toArray());
         List<String> res = AlgorithmUtils.invokeJob(commends);
 
@@ -63,38 +65,72 @@
     @Transactional
     @Override
     public AlgorithmResponse pathPlanning(PathPlanDTO planDTO) {
-        if (StrUtil.isEmpty(planDTO.getCount())) {
-            planDTO.setCount("0.001");
+        if (StrUtil.isEmpty(planDTO.getZNow())) {
+            planDTO.setZNow("0.001");
         }
         //算法路径及入参配置
         List<String> commends = new ArrayList<>();
-        commends.add(planPath);
+        commends.add(path + planPathName);
         commends.addAll(planDTO.toArray());
         log.info("寻源算法调用:{}", JSON.toJSONString(planDTO));
         List<String> res = AlgorithmUtils.invokeJob(commends);
         AlgorithmResponse response = getAlgorithmResponse(planDTO, res);
-        //next 点位编写
-        if (CollectionUtil.isNotEmpty(res) && res.size() == 2) {
-            response.getPointNextDTO().setX(formatBigDecimal(res.get(0).replaceAll("next_X: ", "")));
-            response.getPointNextDTO().setY(formatBigDecimal(res.get(1).replaceAll("next_Y: ", "")));
-        }
+
         return response;
     }
 
     private AlgorithmResponse getAlgorithmResponse(PathInitDTO initDTO, List<String> res) {
-        File repetitionFile = new File("repetition.txt");
+        //读取当前路径图片
+        File repetitionFileDir = new File(path + "\\" + initDTO.getTaskId() + "\\predicts");
+        File[] files = repetitionFileDir.listFiles((dir, name) -> name.endsWith(".txt") && name.startsWith("step"));
+        File repetitionFile = null;
+        //判定当前执行路径最大step2文件夹
+        if (files != null && files.length > 0) {
+             repetitionFile = files[files.length - 1];
+        }
         AlgorithmResponse response = new AlgorithmResponse();
         if (initDTO.getTaskId() != null && repetitionFile.exists()) {
-            readAlgorithmResponse("repetition.txt", response, initDTO.getTaskId(), res);
-            readAlgorithmResponse("step.txt", response, initDTO.getTaskId(), res);
+            //读取热力图
+            readAlgorithmResponse("repetition.txt",repetitionFile, response, initDTO.getTaskId(), res);
         } else {
             log.info("taskId is empty!");
         }
-        readAlgorithmResponse("xy_train.txt", response, initDTO.getTaskId(), res);
-        readAlgorithmResponse("z_train.txt", response, initDTO.getTaskId(), res);
+        //读取规划路径
+        if (CollectionUtil.isNotEmpty(res)) {
+            response.setEnd(false);
+            readAlgorithmResponse(response, res);
+        }
         return response;
     }
 
+    private void readAlgorithmResponse(AlgorithmResponse response, List<String> res) {
+        List<String> resList = new ArrayList<>();
+        if (res.size() >= 2) {
+            resList = res.subList(res.size() - 2, res.size());
+        }
+        //判定是否结束寻源
+        if (res.contains("end")) {
+            response.setEnd(true);
+            for (String s : resList) {
+                if (s.startsWith("source_coordinate:")) {
+                    s = s.trim();
+                    log.info("寻源结束,目标点位：{}",s);
+                    List<String> lists = StrUtil.split(s.replaceAll("source_coordinate:", ""), ",");
+                    PointNextDTO nextDTO = new PointNextDTO();
+                    nextDTO.setX(lists.get(0));
+                    nextDTO.setY(lists.get(1));
+                    response.setCoordinate(nextDTO);
+                }
+            }
+        } else {
+            PointNextDTO nextDTO = new PointNextDTO();
+            nextDTO.setX(resList.get(0));
+            nextDTO.setY(resList.get(1));
+            response.setPointNextDTO(nextDTO);
+        }
+
+    }
+
     /**
      * 读取算法输出数据
      *
@@ -102,49 +138,18 @@
      * @param response 结果集
      * @param taskId   任务ID
      */
-    public void readAlgorithmResponse(String fileName, AlgorithmResponse response, Long taskId, List<String> res) {
-        File file = new File(fileName);
-        if (!file.exists()&&!fileName.startsWith("step")) {
+    public void readAlgorithmResponse(String fileName,File repetitionFile, AlgorithmResponse response, Long taskId, List<String> res) {
+        if (!repetitionFile.exists() && !fileName.startsWith("step")) {
             log.info("{} file is empty!", fileName);
             return;
         }
         if (response.getPointNextDTO() == null) {
             response.setPointNextDTO(new PointNextDTO());
         }
-        switch (fileName) {
-            case "repetition.txt":
-                String msg = FileUtil.readString(file, CharsetUtil.CHARSET_UTF_8);
-                TaskHeatMap taskHeatMap = createEmptyTaskHeatMap(taskId);
-                taskHeatMap.setRepetition(msg);
-                response.setTaskHeatMap(taskHeatMap);
-                break;
-            case "xy_train.txt":
-                List<String> xyTrains = FileUtil.readLines(file, CharsetUtil.CHARSET_UTF_8);
-                if (CollectionUtil.isNotEmpty(xyTrains)) {
-                    if (CollectionUtil.isNotEmpty(xyTrains)) {
-                        List<String> list = StrUtil.split(xyTrains.get(xyTrains.size() - 1), " ");
-                        response.getPointNextDTO().setX(formatBigDecimal(list.get(0)));
-                        response.getPointNextDTO().setY(formatBigDecimal(list.get(1)));
-                    }
-                }
-                break;
-            case "z_train.txt":
-                List<String> zTrains = FileUtil.readLines(file, CharsetUtil.CHARSET_UTF_8);
-                if (CollectionUtil.isNotEmpty(zTrains)) {
-                    String zTrain = zTrains.get(zTrains.size() - 1);
-                    response.getPointNextDTO().setZ(formatBigDecimal(zTrain));
-                }
-            default:
-                if (StrUtil.isNotEmpty(fileName) && fileName.startsWith("step")) {
-                    File fileDir = new File(".");
-                    File[] files = fileDir.listFiles((dir, name) -> name.endsWith(".txt") && name.startsWith("step"));
-                    if (files != null && files.length > 0) {
-                        String steps = FileUtil.readString(files[files.length - 1], CharsetUtil.CHARSET_UTF_8);
-                        response.getTaskHeatMap().setRepetition(steps);
-                    }
-                }
-                break;
-        }
+        String msg = FileUtil.readString(repetitionFile, CharsetUtil.CHARSET_UTF_8);
+        TaskHeatMap taskHeatMap = createEmptyTaskHeatMap(taskId);
+        taskHeatMap.setRepetition(msg);
+        response.setTaskHeatMap(taskHeatMap);
     }
 
     /**
@@ -157,13 +162,11 @@
     public GridPointDTO convertSlamToGrid(BigDecimal x, BigDecimal y, BigDecimal resolution, RobotInfo robotInfo) {
         GridPointDTO pointDTO = new GridPointDTO();
         BigDecimal scale = resolution;
-
         //减去栅格原点坐标
         x = x.subtract(robotInfo.getOriginX());
         y = y.subtract(robotInfo.getOriginY());
-
-        pointDTO.setX(new BigDecimal(x.divide(scale,3,BigDecimal.ROUND_HALF_UP).intValue()+1));
-        pointDTO.setY(new BigDecimal(y.divide(scale,3,BigDecimal.ROUND_HALF_UP).intValue()+1));
+        pointDTO.setX(new BigDecimal(x.divide(scale, 3, BigDecimal.ROUND_HALF_UP).intValue()+1));
+        pointDTO.setY(new BigDecimal(y.divide(scale, 3, BigDecimal.ROUND_HALF_UP).intValue() +1));
         return pointDTO;
     }
 
@@ -174,19 +177,17 @@
      * @return
      */
     public GridPointDTO convertGridCenterPoint(GridPointDTO pointDTO, BigDecimal resolution, RobotInfo robotInfo) {
-
         GridPointDTO centerPoint = new GridPointDTO();
         BigDecimal scale = resolution;
-        //点位转换
-        BigDecimal slamX = pointDTO.getX().divide(scale, 5, BigDecimal.ROUND_HALF_UP);
-        BigDecimal slamY = pointDTO.getY().divide(scale, 5, BigDecimal.ROUND_HALF_UP);
+        //点位转换 转换后栅格－1 从1开始计算
+        BigDecimal slamX = (pointDTO.getX().subtract(new BigDecimal(1))).multiply(scale);
+        BigDecimal slamY = (pointDTO.getY().subtract(new BigDecimal(1))).multiply(scale);
         //中心栅格点位中心添加
-        centerPoint.setX(slamX.add(scale.divide(new BigDecimal(2), 5, BigDecimal.ROUND_HALF_UP)));
-        centerPoint.setY(slamY.add(scale.divide(new BigDecimal(2), 5, BigDecimal.ROUND_HALF_UP)));
+        centerPoint.setX(slamX.add(scale.divide(new BigDecimal(2), 10, BigDecimal.ROUND_HALF_UP)));
+        centerPoint.setY(slamY.add(scale.divide(new BigDecimal(2), 10, BigDecimal.ROUND_HALF_UP)));
         //加上栅格原点坐标
         centerPoint.setX(centerPoint.getX().add(robotInfo.getOriginX()));
         centerPoint.setY(centerPoint.getY().add(robotInfo.getOriginY()));
-
         return centerPoint;
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotInfoServiceImpl.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotInfoServiceImpl.java
index 043e05d..fdded64 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotInfoServiceImpl.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotInfoServiceImpl.java
@@ -104,7 +104,7 @@
             //下发障碍物栅格
             sendCostMapResolution(robotInfo.getResolution(), robotInfo.getId());
             //重新生成栅格地图
-            sendChangePcd(robotInfo.getId());
+            //sendChangePcd(robotInfo.getId());
         }
         update.eq(RobotInfo::getId, robotInfo.getId());
         update(update);
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotStatusInfoServiceImpl.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotStatusInfoServiceImpl.java
index 6d3ae91..46b5c3b 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotStatusInfoServiceImpl.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotStatusInfoServiceImpl.java
@@ -15,6 +15,7 @@
 import com.casic.missiles.modular.robot.dao.RobotStatusInfoMapper;
 import com.casic.missiles.modular.robot.model.RobotInfo;
 import com.casic.missiles.modular.robot.model.RobotStatusInfo;
+import com.casic.missiles.modular.robot.opt.enums.InstructCodeEnums;
 import com.casic.missiles.modular.robot.opt.instruct.base.dto.RobotMsgDTO;
 import com.casic.missiles.modular.robot.opt.instruct.base.dto.RobotMsgResponseDTO;
 import com.casic.missiles.modular.robot.opt.instruct.base.dto.RobotResponseDTO;
@@ -26,13 +27,18 @@
 import com.casic.missiles.modular.robot.service.IRobotInfoService;
 import com.casic.missiles.modular.robot.service.IRobotStatusInfoService;
 import lombok.extern.slf4j.Slf4j;
+import org.aspectj.util.FileUtil;
 import org.eclipse.paho.client.mqttv3.MqttMessage;
+import org.springframework.beans.factory.annotation.Value;
 import org.springframework.context.annotation.Lazy;
 import org.springframework.stereotype.Service;
 import org.springframework.transaction.annotation.Transactional;
 
+import java.io.File;
 import java.io.Serializable;
 import java.math.BigDecimal;
+import java.util.ArrayList;
+import java.util.Arrays;
 import java.util.List;
 
 
@@ -147,7 +153,6 @@
     @Override
     public void saveProcessMessage(String robotId, MqttMessage message) {
 //        String json = new String(message.getPayload());
-
         RobotResponseDTO<ProcessMessageResponseDTO> responseDTO = parseJson(message, new TypeReference<RobotResponseDTO<ProcessMessageResponseDTO>>() {
         });
         if (responseDTO.getMsg() != null) {
@@ -189,9 +194,8 @@
     }
 
     @Override
-    public void updateCurrTask(String robotId, Long currRouteId, Long currTaskId) {
+    public void updateCurrTask(String robotId, Long currTaskId) {
         UpdateWrapper<RobotStatusInfo> query = new UpdateWrapper<>();
-        query.set("curr_route_id", currRouteId);
         query.set("curr_task_id", currTaskId);
         query.eq("robot_id", robotId);
         update(query);
@@ -250,6 +254,15 @@
         }
     }
 
+    @Value("${casic.algorithm.path:-1}")
+    private String path;
+
+    /**
+     * 栅格阈值
+     */
+    @Value("${casic.algorithm.num:30}")
+    private Integer num;
+
     /**
      * 机器人栅格数据
      *
@@ -262,15 +275,41 @@
         RobotResponseDTO<AppCostmapResponseDTO> responseDTO = parseJson(message, new TypeReference<RobotResponseDTO<AppCostmapResponseDTO>>() {
         });
         if (responseDTO != null && responseDTO.getMsg() != null) {
+            //发送栅格分辨率设定
+
             cacheService.updateCostMap(robotId, responseDTO.getMsg());
             robotInfoService.updateRobotGrid(robotId, responseDTO.getMsg());
-//            log.info("--------------- json data:{}", JSON.toJSONString(responseDTO.getMsg()));
+            //log.info("--------------- json data:{}", JSON.toJSONString(responseDTO.getMsg()));
+            if (responseDTO != null && responseDTO.getMsg().getData() != null) {
+                ArrayList<Integer> dataNums = new ArrayList<>();
+                //根据阈值转换栅格
+                for (Integer dataNum : responseDTO.getMsg().getData()) {
+                    if (dataNum != -1) {
+                        if (dataNum >= num) {
+                            dataNums.add(0);
+                        } else {
+                            dataNums.add(1);
+                        }
+                    }
+                }
+                writeCache(dataNums,responseDTO.getMsg().getWidth(),responseDTO.getMsg().getHeight(),robotId);
+            }
         }
     }
 
+    private void writeCache(ArrayList<Integer> integers,Integer width,Integer height,String robotId) {
+        List<List<Integer>> arrays = CollectionUtil.split(integers,width);
+        ArrayList<ArrayList<Integer>> arrayList = new ArrayList<>();
+        for (List<Integer> array : arrays) {
+            arrayList.add((ArrayList)array);
+        }
+        CollectionUtil.reverse(arrayList);
+        CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_COST_MAP + robotId, arrayList);
+//        FileUtil.writeAsString(new File(path + "map.txt"), CollectionUtil.join(integers, " ").replaceAll("\\[", "").replaceAll("]", ""));
+    }
+
     @Override
     public void updateRobotPoint(PoseMessageResponseDTO dto, String robotId) {
-        //todo cache
         UpdateWrapper<RobotStatusInfo> query = new UpdateWrapper<>();
         query.set("postion_x", dto.getX());
         query.set("postion_y", dto.getY());
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotTrajectoryInfoServiceImpl.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotTrajectoryInfoServiceImpl.java
index 6c9625f..f1262a2 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotTrajectoryInfoServiceImpl.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotTrajectoryInfoServiceImpl.java
@@ -57,29 +57,29 @@
         });
 
         //读取导航状态,如果导航未开启则不存储
-//        RobotStatusInfo statusInfo = robotStatusInfoService.getStatusByRobotId(robotId);
-        Long routeId= cacheService.readRouteId(Long.valueOf(robotId));
-        if (routeId != null && responseDTO.getMsg() != null) {
+        //RobotStatusInfo statusInfo = robotStatusInfoService.getStatusByRobotId(robotId);
+
+        if ( responseDTO.getMsg() != null) {
             PoseMessageResponseDTO dto = responseDTO.getMsg();
             //执行循迹 保存当前轨迹信息
             //更新实时定位信息
             CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.ROBOT_POINT + robotId, dto);
-            infoService.pointTestCallBack(dto, robotId);
-            if (routeId != null) {
-                RobotTrajectoryInfo info = new RobotTrajectoryInfo();
-                info.setRobotId(Long.valueOf(robotId));
-                info.setCreateTime(new Date());
-                info.setPostionX(dto.getX());
-                info.setPostionY(dto.getY());
-                info.setPostionZ(dto.getZ());
-                info.setTheta(dto.getTheta());
-                save(info);
-                try {
-                    infoService.pointCallBack(dto, robotId);
-                } catch (Exception e) {
-                    log.error("task error", e);
-                }
+            try {
+                infoService.carPathPlanning(dto, robotId);
+            } catch (Exception e) {
+                log.error("task error", e);
             }
+//            if (routeId != null) {
+//                RobotTrajectoryInfo info = new RobotTrajectoryInfo();
+//                info.setRobotId(Long.valueOf(robotId));
+//                info.setCreateTime(new Date());
+//                info.setPostionX(dto.getX());
+//                info.setPostionY(dto.getY());
+//                info.setPostionZ(dto.getZ());
+//                info.setTheta(dto.getTheta());
+//                save(info);
+
+//            }
         }
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/TaskGridInfoServiceImpl.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/TaskGridInfoServiceImpl.java
new file mode 100644
index 0000000..cf61def
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/TaskGridInfoServiceImpl.java
@@ -0,0 +1,28 @@
+package com.casic.missiles.modular.robot.service.impl;
+
+import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
+import com.baomidou.mybatisplus.extension.plugins.pagination.Page;
+import com.baomidou.mybatisplus.extension.service.impl.ServiceImpl;
+import com.casic.missiles.modular.robot.dao.TaskGridInfoMapper;
+import com.casic.missiles.modular.robot.model.TaskGridInfo;
+import com.casic.missiles.modular.robot.service.ITaskGridInfoService;
+import org.springframework.stereotype.Service;
+
+import java.util.List;
+
+
+/**
+ * <p>
+ * 任务网格信息 服务实现类
+ * </p>
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+@Service
+public class TaskGridInfoServiceImpl extends ServiceImpl<TaskGridInfoMapper, TaskGridInfo> implements ITaskGridInfoService {
+    @Override
+    public List<TaskGridInfo> selectTaskGridInfoPage(Page<TaskGridInfo> page, QueryWrapper<TaskGridInfo> query) {
+        return this.baseMapper.selectTaskGridInfoPage(page, query);
+    }
+}

diff --git a/.gitignore b/.gitignore
index 9e20bc1..f42a9cb 100644
--- a/.gitignore
+++ b/.gitignore
@@ -2,3 +2,4 @@
 **/target/**
 logs/
 *.iml
+*.exe
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java b/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
index c7b5d28..a849528 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
@@ -28,6 +28,7 @@
     String KEY_CUR_TASK_ID = "CUR_TASK_ID_";
     String KEY_CUR_TASK_INIT = "CUR_TASK_INIT_";
 
+    String KEY_COST_MAP = "KEY_COST_MAP";
     /**
      * PSD数据ID
      */
@@ -41,6 +42,7 @@
      */
     String KEY_CUR_XY_POINT = "KEY_CUR_XY_POINT_";
     String KEY_CUR_ESTIMATE_GRID = "KEY_CUR_ESTIMATE_GRID_";
+    String KEY_PLANING_FLAG = "KEY_PLANING_FLAG_";
     /**
      * 机器人巡点栅格
      */
@@ -55,6 +57,7 @@
      * 探测器配置参数缓存
      */
     String KEY_CONFIG_PARAMS_ID = "KEY_CONFIG_PARAMS_ID_";
+    String KEY_CONFIG_COSTMAP_RESOLUTION_SET = "KEY_CONFIG_COSTMAP_RESOLUTION_SET_";
 
     /**
      * 暂停状态缓存信息
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
index c4c905d..3bb475f 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
@@ -1,9 +1,14 @@
 package com.casic.missiles.modular.robot.controller;
 
+import cn.hutool.core.lang.Assert;
 import cn.hutool.core.util.StrUtil;
 import com.casic.missiles.core.base.controller.BaseController;
+import com.casic.missiles.core.cache.CacheUtil;
 import com.casic.missiles.core.util.SpringContextHolder;
+import com.casic.missiles.model.exception.RequestValidException;
+import com.casic.missiles.model.response.ResponseData;
 import com.casic.missiles.model.response.dto.ResponseDataDTO;
+import com.casic.missiles.modular.constants.RobotDictConstants;
 import com.casic.missiles.modular.robot.IBaseRobotService;
 import com.casic.missiles.modular.robot.dto.RobotInfoSetDTO;
 import com.casic.missiles.modular.robot.model.RobotInfo;
@@ -19,10 +24,16 @@
 import io.swagger.annotations.ApiOperation;
 import io.swagger.annotations.ApiParam;
 import lombok.SneakyThrows;
+import lombok.extern.slf4j.Slf4j;
+import org.apache.ibatis.annotations.Param;
+import org.springframework.beans.factory.annotation.Value;
 import org.springframework.web.bind.annotation.*;
+import org.springframework.web.multipart.MultipartFile;
 
 import javax.servlet.http.HttpServletResponse;
+import java.io.File;
 import java.util.List;
+import java.util.Optional;
 
 /**
  * 机器人操作类
@@ -30,6 +41,7 @@
  * @author lwh
  * @date 2023-10-30
  */
+@Slf4j
 @Api(tags = "机器人控制接口")
 @RestController
 @RequestMapping("/robot/opt")
@@ -61,6 +73,28 @@
         return baseRobotService.execCmdHandler(request, InstructCodeEnums.CHANGE_PCD);
     }
 
+    @Value("${casic.file.uploadPath}")
+    private String filePath;
+
+    @SneakyThrows
+    @ApiOperation(value = "PCD上传接口")
+    @PostMapping(value = "/uploadPcd")
+    public ResponseData uploadPcd(@RequestParam("file") MultipartFile file, @ApiParam("机器人ID,url拼接传参") String robotId) {
+        Assert.isFalse(StrUtil.isEmpty(robotId), () -> new RequestValidException("机器ID不能为空", Optional.of(new Throwable().getStackTrace()[2])));
+        RobotInfo info = robotInfoService.getById(robotId);
+        if (file != null && !file.isEmpty()) {
+            File pcdFile = new File(filePath + "finalCloud_" + info.getId() + ".pcd");
+            try {
+                file.transferTo(pcdFile);
+            } catch (Exception e) {
+                log.error(e.getMessage());
+            }
+            return ResponseData.success();
+        } else {
+            return ResponseData.error("附件上传失败!");
+        }
+    }
+
     @SneakyThrows
     @ApiOperation(value = "PCD下载接口")
     @PostMapping(value = "/pcdDownload")
@@ -136,6 +170,10 @@
     @ApiOperation(value = "启动或关闭循迹功能", notes = "返回 1成功 0失败  wait 启动时是否等待，0代表循迹启动即运动，不等待；1代表循迹启动原地等待。若无此字段，默认循迹启动不等待直接运动")
     @PostMapping(value = "/startTrack")
     public ResponseDataDTO<Integer> startTrack(@RequestBody StartTrackRequest request) {
+        request.setIsTask(1);
+        if (request.getStepLen() == null) {
+            request.setStepLen(2);
+        }
         baseRobotService.startTrack(request);
         return ResponseDataDTO.success();
     }
@@ -156,6 +194,13 @@
         MessageRequestDTO<CrlNavRequest> messageDTO = new MessageRequestDTO();
         messageDTO.setMsg(request);
         ResponseDataDTO<Integer> list = baseRobotService.getRobotRouteOptService().robotCmd(info, InstructCodeEnums.CTRL_NAV, messageDTO);
+        if (request.getAction() == 1) {
+            CostMapResolutionRequest costMap = new CostMapResolutionRequest();
+            costMap.setResolution(info.getResolution().doubleValue());
+            request.setRobotId(request.getRobotId());
+            //是否下发分辨率
+            CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CONFIG_COSTMAP_RESOLUTION_SET + request.getRobotId(), costMap);
+        }
         return list;
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java
new file mode 100644
index 0000000..5317651
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java
@@ -0,0 +1,86 @@
+package com.casic.missiles.modular.robot.controller;
+
+import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
+import com.baomidou.mybatisplus.extension.plugins.pagination.Page;
+import com.casic.missiles.core.base.controller.BaseController;
+import com.casic.missiles.core.page.PageFactory;
+import com.casic.missiles.core.page.PageInfoBT;
+import com.casic.missiles.model.form.IdForms;
+import com.casic.missiles.model.response.dto.ResponseDataDTO;
+import com.casic.missiles.modular.robot.model.TaskGridInfo;
+import com.casic.missiles.modular.robot.service.ITaskGridInfoService;
+import com.casic.missiles.modular.robot.vo.TaskGridInfoVO;
+import io.swagger.annotations.Api;
+import io.swagger.annotations.ApiOperation;
+import org.springframework.web.bind.annotation.*;
+
+import java.util.List;
+
+/**
+ * 任务网格信息Controller
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+@Api(tags = "任务网格信息")
+@RestController
+@RequestMapping("/robot/taskGridInfo")
+public class TaskGridInfoController extends BaseController {
+
+    private final ITaskGridInfoService taskGridInfoService;
+
+    public TaskGridInfoController(ITaskGridInfoService taskGridInfoService) {
+        this.taskGridInfoService = taskGridInfoService;
+    }
+
+    @ApiOperation(value = "列表查询")
+    @GetMapping(value = "/list")
+    public ResponseDataDTO<List<TaskGridInfo>> list(TaskGridInfoVO taskGridInfoVO) {
+        QueryWrapper<TaskGridInfo> query = taskGridInfoVO.genQuery();
+        return ResponseDataDTO.success(taskGridInfoService.list(query));
+    }
+
+    @ApiOperation(value = "分页查询")
+    @GetMapping(value = "/listPage")
+    public ResponseDataDTO<PageInfoBT<TaskGridInfo>> listPage(TaskGridInfoVO taskGridInfoVO) {
+        Page<TaskGridInfo> page = PageFactory.defaultPage();
+        QueryWrapper<TaskGridInfo> query = taskGridInfoVO.genQuery();
+        page = taskGridInfoService.page(page, query);
+        return ResponseDataDTO.success(super.packForBT(page));
+    }
+
+
+    @ApiOperation(value = "新增接口")
+    @PostMapping(value = "/add")
+    public ResponseDataDTO add(@RequestBody TaskGridInfo taskGridInfo) {
+        taskGridInfoService.save(taskGridInfo);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "修改接口")
+    @PostMapping(value = "/update")
+    public ResponseDataDTO update(@RequestBody TaskGridInfo taskGridInfo) {
+        taskGridInfoService.updateById(taskGridInfo);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "删除接口")
+    @GetMapping(value = "/delete")
+    public ResponseDataDTO delete(String id) {
+        taskGridInfoService.removeById(id);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation("批量删除")
+    @PostMapping("/batchDelete")
+    public ResponseDataDTO batchDelete(@RequestBody IdForms ids) {
+        taskGridInfoService.removeByIds(ids.getIds());
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "详情查询")
+    @GetMapping(value = "/detail")
+    public ResponseDataDTO<TaskGridInfo> detail(String id) {
+        return ResponseDataDTO.success(taskGridInfoService.getById(id));
+    }
+}
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
index e82a792..6992274 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
@@ -98,7 +98,7 @@
     public ResponseDataDTO<AlgorithmResponse> test() {
         AlgorithmResponse response = new AlgorithmResponse();
         response.setTaskHeatMap(new TaskHeatMap());
-        algorithmService.readAlgorithmResponse("step3.txt",response,123L,null);
+        algorithmService.readAlgorithmResponse("step3.txt",null,response,123L,null);
         sendWebSocket(InstructStatusMsgEnums.HEAT_MAP, "1", response.getTaskHeatMap());
         return ResponseDataDTO.success(response);
     }
@@ -121,7 +121,7 @@
     @GetMapping(value = "/testGrid")
     public ResponseDataDTO<List<RouteInfo>> testGrid(@RequestBody PathInitDTO pathInitDTO) {
         RobotInfo robotInfo = infoService.getById(pathInitDTO.getRobotId());
-        GridPointDTO gridPointDTO = algorithmService.convertSlamToGrid(new BigDecimal(pathInitDTO.getX()), new BigDecimal(10), new BigDecimal(4), robotInfo);
+        GridPointDTO gridPointDTO = algorithmService.convertSlamToGrid(new BigDecimal(pathInitDTO.getXNow()), new BigDecimal(10), new BigDecimal(4), robotInfo);
         return ResponseDataDTO.success(algorithmService.convertGridCenterPoint(gridPointDTO, new BigDecimal(10), robotInfo));
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/TaskGridInfoMapper.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/TaskGridInfoMapper.java
new file mode 100644
index 0000000..5c3a70b
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/TaskGridInfoMapper.java
@@ -0,0 +1,23 @@
+package com.casic.missiles.modular.robot.dao;
+
+
+import com.baomidou.mybatisplus.core.mapper.BaseMapper;
+import com.casic.missiles.modular.robot.model.TaskGridInfo;
+import org.apache.ibatis.annotations.Param;
+import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
+import com.baomidou.mybatisplus.extension.plugins.pagination.Page;
+import java.util.List;
+/**
+ * <p>
+ *  任务网格信息 Mapper 接口
+ * </p>
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+public interface TaskGridInfoMapper extends BaseMapper<TaskGridInfo> {
+/**
+* 任务网格信息 分页检索
+*/
+List<TaskGridInfo> selectTaskGridInfoPage( @Param("page") Page<TaskGridInfo> page,@Param("ew" ) QueryWrapper<TaskGridInfo> query);
+}
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/mapping/TaskGridInfoMapper.xml b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/mapping/TaskGridInfoMapper.xml
new file mode 100644
index 0000000..be573e0
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/mapping/TaskGridInfoMapper.xml
@@ -0,0 +1,80 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!DOCTYPE mapper PUBLIC "-//mybatis.org//DTD Mapper 3.0//EN" "http://mybatis.org/dtd/mybatis-3-mapper.dtd">
+<mapper namespace="com.casic.missiles.modular.robot.dao.TaskGridInfoMapper">
+    <!-- 通用查询映射结果 -->
+    <resultMap id="BaseResultMap" type="com.casic.missiles.modular.robot.model.TaskGridInfo">
+        <result column="id" property="id"/>
+        <result column="create_time" property="createTime"/>
+        <result column="update_time" property="updateTime"/>
+        <result column="task_id" property="taskId"/>
+        <result column="estimate_grid_x" property="estimateGridX"/>
+        <result column="estimate_grid_y" property="estimateGridY"/>
+        <result column="neutron_count" property="neutronCount"/>
+        <result column="cur_grid_x" property="curGridX"/>
+        <result column="cur_grid_y" property="curGridY"/>
+    </resultMap>
+    <!-- 通用查询结果列 -->
+    <sql id="Base_Column_List">
+        id
+        ,
+                    create_time as createTime ,
+                    update_time as updateTime ,
+                    task_id as taskId ,
+                    estimate_grid_x as estimateGridX ,
+                    estimate_grid_y as estimateGridY ,
+                    neutron_count as neutronCount ,
+                    cur_grid_x as curGridX ,
+                    cur_grid_y as curGridY
+    </sql>
+    <sql id="Base_Column_Alias">
+        a
+        .
+        id
+        ,
+            a.create_time
+            as createTime ,
+            a.update_time
+            as updateTime ,
+            a.task_id
+            as taskId ,
+            a.estimate_grid_x
+            as estimateGridX ,
+            a.estimate_grid_y
+            as estimateGridY ,
+            a.neutron_count
+            as neutronCount ,
+            a.cur_grid_x
+            as curGridX ,
+            a.cur_grid_y
+            as curGridY
+    </sql>
+    <select id="selectTaskGridInfoPage" resultType="com.casic.missiles.modular.robot.model.TaskGridInfo">
+        select
+        <include refid="Base_Column_Alias"/>
+        from task_grid_info a
+        <where>
+            <if test="ew != null">
+                ${ew.sqlSegment}
+            </if>
+        </where>
+    </select>
+    <sql id="Base_Column_Query">
+        <if test="request!=null">
+            <if test="request.taskId!= null and request.taskId!=''">
+                and task_id = #{request.taskId}
+            </if>
+            <if test="request.estimateGridX!= null and request.estimateGridX!=''">
+                and estimate_grid_x = #{request.estimateGridX}
+            </if>
+            <if test="request.estimateGridY!= null and request.estimateGridY!=''">
+                and estimate_grid_y = #{request.estimateGridY}
+            </if>
+            <if test="request.curGridX!= null and request.curGridX!=''">
+                and cur_grid_x = #{request.curGridX}
+            </if>
+            <if test="request.curGridY!= null and request.curGridY!=''">
+                and cur_grid_y = #{request.curGridY}
+            </if>
+        </if>
+    </sql>
+</mapper>
\ No newline at end of file
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java
index 749f559..4559074 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java
@@ -13,4 +13,9 @@
      * 规划点位信息
      */
     private PointNextDTO pointNextDTO;
+    /**
+     * 寻源目标地址
+     */
+    private PointNextDTO coordinate;
+    private boolean isEnd;
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java
index 39ff2a2..cef3238 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java
@@ -12,6 +12,9 @@
     private BigDecimal y;
 
     public static GridPointDTO convertGridPoint(PointNextDTO nextDTO) {
+        if(nextDTO==null){
+            return null;
+        }
         GridPointDTO gridPointDTO = new GridPointDTO();
         gridPointDTO.setX(Convert.toBigDecimal(nextDTO.getX()));
         gridPointDTO.setY(Convert.toBigDecimal(nextDTO.getY()));
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java
index 8c88242..33be25d 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java
@@ -12,25 +12,19 @@
 public class PathInitDTO {
     //---------------------算法参数
     /**
-     * x坐标最大值（x方向栅格总数）
+     * 初始位置x坐标(栅格地图下的坐标)
      */
-    private String xMax;
+    private String xNow;
     /**
-     * y坐标最大值（y方向栅格总数）
+     * 初始位置y坐标(栅格地图下的坐标)
      */
-    private String yMax;
+    private String yNow;
     /**
      * 初始位置计数率
      */
-    private String count;
-    /**
-     * 初始位置x坐标(0<=x<x_max)
-     */
-    private String x;
-    /**
-     * 初始位置y坐标(0<=y<y_max)
-     */
-    private String y;
+    private String zNow;
+
+    private Integer stepLen;
 
     //---------------------其他执行参数
     private String robotId;
@@ -38,11 +32,11 @@
 
     public List<String> toArray() {
         List<String> list = new ArrayList<>();
-        list.add(xMax);
-        list.add(yMax);
-        list.add(count);
-        list.add(x);
-        list.add(y);
+        list.add(xNow);
+        list.add(yNow);
+        list.add(stepLen+"");
+        list.add(zNow);
+        list.add(taskId+"");
         return list;
     }
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java
index 109e95c..ada6774 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java
@@ -10,21 +10,17 @@
  */
 @Data
 public class PathPlanDTO extends PathInitDTO {
-
     /**
-     * 长度为8的数组，记录(x-1, y-1), (x-1, y), (x-1, y+1), (x, y-1), (x, y+1), (x+1, y-1), (x+1, y), (x+1, y+1)8个坐标的可通行信息。
-     * 若某坐标有障碍物（不可通行行）则记为0，否则记为1
+     * 浮点阈值(阈值，超过该值后结束巡检开始寻源)
      */
-    private String open;
-
+    private String th;
     public List<String> toArray() {
         List<String> list = new ArrayList<>();
-        list.add(getXMax());
-        list.add(getYMax());
-        list.add(open);
-        list.add(getCount());
-        list.add(getX());
-        list.add(getY());
+
+        list.add(getStepLen()+"");
+        list.add(getTh());
+        list.add(getZNow());
+        list.add(getTaskId()+"");
         return list;
     }
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskGridInfo.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskGridInfo.java
new file mode 100644
index 0000000..101c10a
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskGridInfo.java
@@ -0,0 +1,83 @@
+package com.casic.missiles.modular.robot.model;
+
+import com.baomidou.mybatisplus.annotation.TableField;
+import com.baomidou.mybatisplus.annotation.TableName;
+import com.baomidou.mybatisplus.extension.activerecord.Model;
+import io.swagger.annotations.ApiModelProperty;
+import lombok.Data;
+
+import java.math.BigDecimal;
+import java.util.Date;
+
+/**
+ * 任务网格信息对象 task_grid_info
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+@Data
+@TableName("task_grid_info")
+public class TaskGridInfo extends Model<TaskGridInfo> {
+    private static final long serialVersionUID = 1L;
+    @TableField(exist = false)
+    private Integer index;
+    @ApiModelProperty(value = "主键", dataType = "Long")
+    @TableField("id")
+    private Long id;
+
+    @ApiModelProperty(value = "创建时间", dataType = "Date")
+    @TableField("create_time")
+    private Date createTime;
+
+    @ApiModelProperty(value = "修改时间", dataType = "Date")
+    @TableField("update_time")
+    private Date updateTime;
+
+    @ApiModelProperty(value = "任务ID", dataType = "Long")
+    @TableField("task_id")
+    private Long taskId;
+
+    @ApiModelProperty(value = "估算栅格x", dataType = "Long")
+    @TableField("estimate_grid_x")
+    private String estimateGridX;
+
+    @ApiModelProperty(value = "估算栅格y", dataType = "Long")
+    @TableField("estimate_grid_y")
+    private String estimateGridY;
+
+    @ApiModelProperty(value = "中子计数率", dataType = "BigDecimal")
+    @TableField("neutron_count")
+    private BigDecimal neutronCount;
+
+    @ApiModelProperty(value = "当前栅格x", dataType = "Long")
+    @TableField("cur_grid_x")
+    private Integer curGridX;
+
+    @ApiModelProperty(value = "当前栅格y", dataType = "Long")
+    @TableField("cur_grid_y")
+    private Integer curGridY;
+
+    public String getCurPoint(){
+        return "("+curGridX+","+curGridY+")";
+    }
+    public String getEstimatePoint(){
+        if(estimateGridX==null&estimateGridY==null){
+            return "结束";
+        }
+        return "("+estimateGridX+","+estimateGridY+")";
+    }
+    @Override
+    public String toString() {
+        return "TaskGridInfo{"
+                + "id=" + id
+                + "createTime=" + createTime
+                + "updateTime=" + updateTime
+                + "taskId=" + taskId
+                + "estimateGridX=" + estimateGridX
+                + "estimateGridY=" + estimateGridY
+                + "neutronCount=" + neutronCount
+                + "curGridX=" + curGridX
+                + "curGridY=" + curGridY
+                + "}";
+    }
+}
\ No newline at end of file
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java
index edc104e..a8acb2a 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java
@@ -53,6 +53,13 @@
     @TableField("route_id")
     private Long routeId;
 
+    @ApiModelProperty(value = "寻源初始步长", dataType = "Integer")
+    @TableField("step_len")
+    private Integer stepLen;
+
+    @ApiModelProperty(value = "寻源阈值设定", dataType = "BigDecimal")
+    @TableField("max_threshold")
+    private BigDecimal maxThreshold;
     /**
      * 任务类型(1巡检任务)
      */
@@ -62,7 +69,6 @@
     @ApiModelProperty(value = "任务类型字典名称", dataType = "String")
     @TableField(exist = false)
     private String taskTypeName;
-
     @ApiModelProperty(value = "识别类型(discernType 1中子源识别)", dataType = "Integer")
     @TableField("discern_type")
     private Integer discernType;
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java
index c4bc25d..c6620f3 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java
@@ -53,7 +53,7 @@
     }),
     SET_SPEED("/set_speed" , "/set_speed_response" , "最大运动速度设置" , true, new TypeReference<RobotMsgResponseDTO<Integer>>() {
     }),
-    SET_TARGETPOINT("/set_targetpoint" , "/set_targetpoint_response" , "设置导航目标点" , false, new TypeReference<RobotMsgResponseDTO<Integer>>() {
+    SET_TARGETPOINT("/set_targetpoint" , "/set_targetpoint_response" , "设置导航目标点" , true, new TypeReference<RobotMsgResponseDTO<Integer>>() {
     }),
 
     GET_DATA_LIST("/get_data_list" , "/get_data_list_response" , "获取数据包列表" , true, new TypeReference<RobotMsgResponseDTO<List<String>>>() {
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/AbstractSocketHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/AbstractSocketHandler.java
index 83f4cb2..e47e343 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/AbstractSocketHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/AbstractSocketHandler.java
@@ -49,7 +49,7 @@
                 wsMsgBaseDTO.setMsgKey(enums.getMsgKey());
                 wsMsgBaseDTO.setData(jsonObject.get("msg"));
                 String msg = JSON.toJSONString(wsMsgBaseDTO);
-                log.info("推送{}消息：{}", enums.getRemarks(), msg);
+//                log.info("推送{}消息：{}", enums.getRemarks(), msg);
                 webSocket.sendOneMessage(robotId, msg);
             }
         } catch (Exception e) {
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/cmd/TargetPointCmdHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/cmd/TargetPointCmdHandler.java
index c1c09fa..43b4460 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/cmd/TargetPointCmdHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/cmd/TargetPointCmdHandler.java
@@ -34,7 +34,11 @@
     @Override
     public ResponseDataDTO senderCmd(TargetPointRequest request, InstructCodeEnums enums) {
         MessageRequestDTO<TargetPointRequest> messageDTO = new MessageRequestDTO();
-        //读取默认关键点信息
+        //读取默认关键点信息 {"x":"1.715708819927424","y":"-0.9100042544250901","z":"0.017993879527185876","theta":"-0.039278239414312105"}
+//        request.setX("1.715708819927424");
+//        request.setY("-0.9100042544250901");
+//        request.setZ("0.017993879527185876");
+//        request.setTheta("-0.039278239414312105");
         messageDTO.setMsg(request);
         ResponseDataDTO<Integer> list = optService.robotCmd(getRobotInfo(), enums, messageDTO);
         return list;
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/AppCostMapHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/AppCostMapHandler.java
index 0335693..88c9f79 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/AppCostMapHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/AppCostMapHandler.java
@@ -1,6 +1,11 @@
 package com.casic.missiles.modular.robot.opt.handler.msg;
 
+import com.casic.missiles.core.cache.CacheUtil;
+import com.casic.missiles.modular.constants.RobotDictConstants;
+import com.casic.missiles.modular.robot.IBaseRobotService;
+import com.casic.missiles.modular.robot.opt.enums.InstructCodeEnums;
 import com.casic.missiles.modular.robot.opt.handler.AbstractSocketHandler;
+import com.casic.missiles.modular.robot.opt.instruct.dto.CostMapResolutionRequest;
 import com.casic.missiles.modular.robot.service.IRobotStatusInfoService;
 import lombok.extern.slf4j.Slf4j;
 import org.eclipse.paho.client.mqttv3.MqttMessage;
@@ -16,14 +21,27 @@
 @Component("/app_costmap_sender")
 public class AppCostMapHandler extends AbstractSocketHandler {
     private final IRobotStatusInfoService statusInfoService;
+    private final IBaseRobotService baseRobotService;
     private static Map<String, Integer> index = new ConcurrentHashMap<>();
 
-    public AppCostMapHandler(IRobotStatusInfoService statusInfoService) {
+    public AppCostMapHandler(IRobotStatusInfoService statusInfoService, IBaseRobotService baseRobotService) {
         this.statusInfoService = statusInfoService;
+        this.baseRobotService = baseRobotService;
     }
 
     @Override
     public void process(String robotId, String topic, MqttMessage message) {
         statusInfoService.saveCostMap(robotId, message);
+
+        CostMapResolutionRequest costMapSet = CacheUtil.get(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CONFIG_COSTMAP_RESOLUTION_SET +robotId);
+        if(costMapSet != null){
+            CacheUtil.remove(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CONFIG_COSTMAP_RESOLUTION_SET +robotId);
+            new Thread(() -> {
+                costMapSet.setRobotId(Long.valueOf(robotId));
+                baseRobotService.execCmdHandler(costMapSet, InstructCodeEnums.COSTMAP_RESOLUTION);
+            }).start();
+
+        }
+
     }
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/PoseMessageMsgHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/PoseMessageMsgHandler.java
index 29f8919..6c3c3bd 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/PoseMessageMsgHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/PoseMessageMsgHandler.java
@@ -17,11 +17,9 @@
 @Component("/pose_message")
 public class PoseMessageMsgHandler extends AbstractSocketHandler {
 
-
     private final EventPublisher publisher;
 
     public PoseMessageMsgHandler(EventPublisher publisher) {
-
         this.publisher = publisher;
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/StopStateMsgHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/StopStateMsgHandler.java
index 8950815..8a71e0b 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/StopStateMsgHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/StopStateMsgHandler.java
@@ -30,7 +30,7 @@
     public void process(String robotId, String topic, MqttMessage message) {
 
         index.put(index_key, index.get("index") + 1);
-        if (index.get(index_key)>50) {
+        if (index.get(index_key)>30) {
             index.put(index_key, 0);
             log.info("主键：{},停障状态topic:{},消息内容:{}", robotId, topic, message.toString());
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/impl/RobotOptServiceImpl.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/impl/RobotOptServiceImpl.java
index 8471e7a..7940a24 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/impl/RobotOptServiceImpl.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/impl/RobotOptServiceImpl.java
@@ -146,7 +146,7 @@
     public void startTrack(StartTrackRequest request) {
         //增加循迹任务记录
         taskInfoService.reloadTask(request, robotInfoService.getById(request.getRobotId()));
-        this.execCmdHandler(request, InstructCodeEnums.START_TRACK);
+//        this.execCmdHandler(request, InstructCodeEnums.START_TRACK);
     }
 
     public IRobotRouteOptService getRobotRouteOptService() {
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/BaseRobotCmdDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/BaseRobotCmdDTO.java
index 5608f2b..06b53de 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/BaseRobotCmdDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/BaseRobotCmdDTO.java
@@ -3,11 +3,13 @@
 import io.swagger.annotations.ApiModelProperty;
 import lombok.Data;
 
+import java.io.Serializable;
+
 /**
  * 机器人指令基类
  */
 @Data
-public class BaseRobotCmdDTO {
+public class BaseRobotCmdDTO implements Serializable {
     @ApiModelProperty(value = "机器人主键", dataType = "String")
     protected Long robotId;
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/StartTrackRequest.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/StartTrackRequest.java
index a18321b..85546a7 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/StartTrackRequest.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/StartTrackRequest.java
@@ -3,6 +3,8 @@
 import io.swagger.annotations.ApiModelProperty;
 import lombok.Data;
 
+import java.math.BigDecimal;
+
 /**
  * 导航下发指令
  */
@@ -10,18 +12,19 @@
 public class StartTrackRequest extends BaseRobotCmdDTO {
     @ApiModelProperty(value = "sn编码(可不填 默认机器ID)", dataType = "String")
     private String sn;
-
     @ApiModelProperty(value = "下发状态 1:开启 0:关闭", dataType = "String")
     private Integer action;
     @ApiModelProperty(value = "等待状态 1:等待 0:不等待", dataType = "String")
     private Integer wait;
     @ApiModelProperty(value = "等待状态 0:循迹 1:任务", dataType = "isTask")
     private Integer isTask;
-    @ApiModelProperty(value = "轨迹名称", dataType = "String")
+    @ApiModelProperty(value = "轨迹名称(废弃)", dataType = "String")
     private String track_name;
     @ApiModelProperty(value = "关键点名称", dataType = "String", hidden = true)
     private String taskpoint_name;
-
+    @ApiModelProperty(value = "初始寻源步长", dataType = "Integer")
+    private Integer stepLen;
+    @ApiModelProperty(value = "寻源阈值", dataType = "BigDecimal")
+    private BigDecimal maxThreshold;
     private String obs_mode="1";
-
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/response/StopStateResponseDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/response/StopStateResponseDTO.java
index eb514bf..e0679d7 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/response/StopStateResponseDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/response/StopStateResponseDTO.java
@@ -3,11 +3,13 @@
 import io.swagger.annotations.ApiModelProperty;
 import lombok.Data;
 
+import java.io.Serializable;
+
 /**
  * 电压信息返回
  */
 @Data
-public class StopStateResponseDTO {
+public class StopStateResponseDTO implements Serializable {
     @ApiModelProperty(value = "软件急停(0：无触发 1：触发)" , dataType = "Integer")
     private Integer soft;
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IAlgorithmService.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IAlgorithmService.java
index 56b3e1d..f62523b 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IAlgorithmService.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IAlgorithmService.java
@@ -6,6 +6,7 @@
 import com.casic.missiles.modular.robot.dto.PathPlanDTO;
 import com.casic.missiles.modular.robot.model.RobotInfo;
 
+import java.io.File;
 import java.math.BigDecimal;
 import java.util.List;
 
@@ -38,5 +39,5 @@
      * @return
      */
     GridPointDTO convertGridCenterPoint(GridPointDTO pointDTO, BigDecimal resolution, RobotInfo robotInfo);
-    void readAlgorithmResponse(String fileName, AlgorithmResponse response, Long taskId, List<String> res);
+    void readAlgorithmResponse(String fileName, File repetitionFile, AlgorithmResponse response, Long taskId, List<String> res);
 }
\ No newline at end of file
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IRobotStatusInfoService.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IRobotStatusInfoService.java
index 2af210a..757f8d0 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IRobotStatusInfoService.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IRobotStatusInfoService.java
@@ -67,10 +67,9 @@
     /**
      * 更新车辆路线及任务信息
      *
-     * @param currRouteId 当前路线信息
      * @param currTaskId  当前任务信息
      */
-    void updateCurrTask(String robotId, Long currRouteId, Long currTaskId);
+    void updateCurrTask(String robotId, Long currTaskId);
 
     RobotStatusInfo getStatusByRobotId(String robotId);
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/ITaskGridInfoService.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/ITaskGridInfoService.java
new file mode 100644
index 0000000..fc0da93
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/ITaskGridInfoService.java
@@ -0,0 +1,22 @@
+package com.casic.missiles.modular.robot.service;
+
+import com.baomidou.mybatisplus.extension.service.IService;
+import  com.casic.missiles.modular.robot.model.TaskGridInfo;
+import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
+import com.baomidou.mybatisplus.extension.plugins.pagination.Page;
+import java.util.List;
+
+
+/**
+ * <p>
+ * 任务网格信息 服务类
+ * </p>
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+public interface ITaskGridInfoService extends IService<TaskGridInfo> {
+/**
+* 任务网格信息 分页检索
+*/
+List<TaskGridInfo> selectTaskGridInfoPage(Page<TaskGridInfo> page,QueryWrapper<TaskGridInfo> query);}
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/ITaskInfoService.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/ITaskInfoService.java
index 31d7ba6..b395993 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/ITaskInfoService.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/ITaskInfoService.java
@@ -51,6 +51,13 @@
      * @param dto 实体
      */
     void pointCallBack(PoseMessageResponseDTO dto, String robotId);
+
+    /**
+     * 车辆路线规划
+     * @param dto
+     * @param robotId
+     */
+    void carPathPlanning(PoseMessageResponseDTO dto, String robotId);
     void pointTestCallBack(PoseMessageResponseDTO dto, String robotId);
     /**
      * 图片路径保存
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/AlgorithmServiceImpl.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/AlgorithmServiceImpl.java
index f808fa0..a4dbe0b 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/AlgorithmServiceImpl.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/AlgorithmServiceImpl.java
@@ -29,10 +29,12 @@
 @Slf4j
 @Service
 public class AlgorithmServiceImpl implements IAlgorithmService {
-    @Value("${casic.algorithm.initPath:-1}")
-    private String initPath;
-    @Value("${casic.algorithm.planPath:-1}")
-    private String planPath;
+    @Value("${casic.algorithm.path:-1}")
+    private String path;
+    @Value("${casic.algorithm.initPathName:-1}")
+    private String initPathName;
+    @Value("${casic.algorithm.planPathName:-1}")
+    private String planPathName;
     private final ITaskHeatMapService heatMapService;
 
     public AlgorithmServiceImpl(ITaskHeatMapService heatMapService) {
@@ -42,13 +44,13 @@
     @Transactional
     @Override
     public AlgorithmResponse pathPlanningInit(PathInitDTO initDTO) {
-        if (StrUtil.isEmpty(initDTO.getCount())) {
-            initDTO.setCount("0.00");
+        if (StrUtil.isEmpty(initDTO.getZNow())) {
+            initDTO.setZNow("0.00");
         }
         log.info(JSON.toJSONString(initDTO));
         //算法路径及入参配置
         List<String> commends = new ArrayList<>();
-        commends.add(initPath);
+        commends.add(path + initPathName);
         commends.addAll(initDTO.toArray());
         List<String> res = AlgorithmUtils.invokeJob(commends);
 
@@ -63,38 +65,72 @@
     @Transactional
     @Override
     public AlgorithmResponse pathPlanning(PathPlanDTO planDTO) {
-        if (StrUtil.isEmpty(planDTO.getCount())) {
-            planDTO.setCount("0.001");
+        if (StrUtil.isEmpty(planDTO.getZNow())) {
+            planDTO.setZNow("0.001");
         }
         //算法路径及入参配置
         List<String> commends = new ArrayList<>();
-        commends.add(planPath);
+        commends.add(path + planPathName);
         commends.addAll(planDTO.toArray());
         log.info("寻源算法调用:{}", JSON.toJSONString(planDTO));
         List<String> res = AlgorithmUtils.invokeJob(commends);
         AlgorithmResponse response = getAlgorithmResponse(planDTO, res);
-        //next 点位编写
-        if (CollectionUtil.isNotEmpty(res) && res.size() == 2) {
-            response.getPointNextDTO().setX(formatBigDecimal(res.get(0).replaceAll("next_X: ", "")));
-            response.getPointNextDTO().setY(formatBigDecimal(res.get(1).replaceAll("next_Y: ", "")));
-        }
+
         return response;
     }
 
     private AlgorithmResponse getAlgorithmResponse(PathInitDTO initDTO, List<String> res) {
-        File repetitionFile = new File("repetition.txt");
+        //读取当前路径图片
+        File repetitionFileDir = new File(path + "\\" + initDTO.getTaskId() + "\\predicts");
+        File[] files = repetitionFileDir.listFiles((dir, name) -> name.endsWith(".txt") && name.startsWith("step"));
+        File repetitionFile = null;
+        //判定当前执行路径最大step2文件夹
+        if (files != null && files.length > 0) {
+             repetitionFile = files[files.length - 1];
+        }
         AlgorithmResponse response = new AlgorithmResponse();
         if (initDTO.getTaskId() != null && repetitionFile.exists()) {
-            readAlgorithmResponse("repetition.txt", response, initDTO.getTaskId(), res);
-            readAlgorithmResponse("step.txt", response, initDTO.getTaskId(), res);
+            //读取热力图
+            readAlgorithmResponse("repetition.txt",repetitionFile, response, initDTO.getTaskId(), res);
         } else {
             log.info("taskId is empty!");
         }
-        readAlgorithmResponse("xy_train.txt", response, initDTO.getTaskId(), res);
-        readAlgorithmResponse("z_train.txt", response, initDTO.getTaskId(), res);
+        //读取规划路径
+        if (CollectionUtil.isNotEmpty(res)) {
+            response.setEnd(false);
+            readAlgorithmResponse(response, res);
+        }
         return response;
     }
 
+    private void readAlgorithmResponse(AlgorithmResponse response, List<String> res) {
+        List<String> resList = new ArrayList<>();
+        if (res.size() >= 2) {
+            resList = res.subList(res.size() - 2, res.size());
+        }
+        //判定是否结束寻源
+        if (res.contains("end")) {
+            response.setEnd(true);
+            for (String s : resList) {
+                if (s.startsWith("source_coordinate:")) {
+                    s = s.trim();
+                    log.info("寻源结束,目标点位：{}",s);
+                    List<String> lists = StrUtil.split(s.replaceAll("source_coordinate:", ""), ",");
+                    PointNextDTO nextDTO = new PointNextDTO();
+                    nextDTO.setX(lists.get(0));
+                    nextDTO.setY(lists.get(1));
+                    response.setCoordinate(nextDTO);
+                }
+            }
+        } else {
+            PointNextDTO nextDTO = new PointNextDTO();
+            nextDTO.setX(resList.get(0));
+            nextDTO.setY(resList.get(1));
+            response.setPointNextDTO(nextDTO);
+        }
+
+    }
+
     /**
      * 读取算法输出数据
      *
@@ -102,49 +138,18 @@
      * @param response 结果集
      * @param taskId   任务ID
      */
-    public void readAlgorithmResponse(String fileName, AlgorithmResponse response, Long taskId, List<String> res) {
-        File file = new File(fileName);
-        if (!file.exists()&&!fileName.startsWith("step")) {
+    public void readAlgorithmResponse(String fileName,File repetitionFile, AlgorithmResponse response, Long taskId, List<String> res) {
+        if (!repetitionFile.exists() && !fileName.startsWith("step")) {
             log.info("{} file is empty!", fileName);
             return;
         }
         if (response.getPointNextDTO() == null) {
             response.setPointNextDTO(new PointNextDTO());
         }
-        switch (fileName) {
-            case "repetition.txt":
-                String msg = FileUtil.readString(file, CharsetUtil.CHARSET_UTF_8);
-                TaskHeatMap taskHeatMap = createEmptyTaskHeatMap(taskId);
-                taskHeatMap.setRepetition(msg);
-                response.setTaskHeatMap(taskHeatMap);
-                break;
-            case "xy_train.txt":
-                List<String> xyTrains = FileUtil.readLines(file, CharsetUtil.CHARSET_UTF_8);
-                if (CollectionUtil.isNotEmpty(xyTrains)) {
-                    if (CollectionUtil.isNotEmpty(xyTrains)) {
-                        List<String> list = StrUtil.split(xyTrains.get(xyTrains.size() - 1), " ");
-                        response.getPointNextDTO().setX(formatBigDecimal(list.get(0)));
-                        response.getPointNextDTO().setY(formatBigDecimal(list.get(1)));
-                    }
-                }
-                break;
-            case "z_train.txt":
-                List<String> zTrains = FileUtil.readLines(file, CharsetUtil.CHARSET_UTF_8);
-                if (CollectionUtil.isNotEmpty(zTrains)) {
-                    String zTrain = zTrains.get(zTrains.size() - 1);
-                    response.getPointNextDTO().setZ(formatBigDecimal(zTrain));
-                }
-            default:
-                if (StrUtil.isNotEmpty(fileName) && fileName.startsWith("step")) {
-                    File fileDir = new File(".");
-                    File[] files = fileDir.listFiles((dir, name) -> name.endsWith(".txt") && name.startsWith("step"));
-                    if (files != null && files.length > 0) {
-                        String steps = FileUtil.readString(files[files.length - 1], CharsetUtil.CHARSET_UTF_8);
-                        response.getTaskHeatMap().setRepetition(steps);
-                    }
-                }
-                break;
-        }
+        String msg = FileUtil.readString(repetitionFile, CharsetUtil.CHARSET_UTF_8);
+        TaskHeatMap taskHeatMap = createEmptyTaskHeatMap(taskId);
+        taskHeatMap.setRepetition(msg);
+        response.setTaskHeatMap(taskHeatMap);
     }
 
     /**
@@ -157,13 +162,11 @@
     public GridPointDTO convertSlamToGrid(BigDecimal x, BigDecimal y, BigDecimal resolution, RobotInfo robotInfo) {
         GridPointDTO pointDTO = new GridPointDTO();
         BigDecimal scale = resolution;
-
         //减去栅格原点坐标
         x = x.subtract(robotInfo.getOriginX());
         y = y.subtract(robotInfo.getOriginY());
-
-        pointDTO.setX(new BigDecimal(x.divide(scale,3,BigDecimal.ROUND_HALF_UP).intValue()+1));
-        pointDTO.setY(new BigDecimal(y.divide(scale,3,BigDecimal.ROUND_HALF_UP).intValue()+1));
+        pointDTO.setX(new BigDecimal(x.divide(scale, 3, BigDecimal.ROUND_HALF_UP).intValue()+1));
+        pointDTO.setY(new BigDecimal(y.divide(scale, 3, BigDecimal.ROUND_HALF_UP).intValue() +1));
         return pointDTO;
     }
 
@@ -174,19 +177,17 @@
      * @return
      */
     public GridPointDTO convertGridCenterPoint(GridPointDTO pointDTO, BigDecimal resolution, RobotInfo robotInfo) {
-
         GridPointDTO centerPoint = new GridPointDTO();
         BigDecimal scale = resolution;
-        //点位转换
-        BigDecimal slamX = pointDTO.getX().divide(scale, 5, BigDecimal.ROUND_HALF_UP);
-        BigDecimal slamY = pointDTO.getY().divide(scale, 5, BigDecimal.ROUND_HALF_UP);
+        //点位转换 转换后栅格－1 从1开始计算
+        BigDecimal slamX = (pointDTO.getX().subtract(new BigDecimal(1))).multiply(scale);
+        BigDecimal slamY = (pointDTO.getY().subtract(new BigDecimal(1))).multiply(scale);
         //中心栅格点位中心添加
-        centerPoint.setX(slamX.add(scale.divide(new BigDecimal(2), 5, BigDecimal.ROUND_HALF_UP)));
-        centerPoint.setY(slamY.add(scale.divide(new BigDecimal(2), 5, BigDecimal.ROUND_HALF_UP)));
+        centerPoint.setX(slamX.add(scale.divide(new BigDecimal(2), 10, BigDecimal.ROUND_HALF_UP)));
+        centerPoint.setY(slamY.add(scale.divide(new BigDecimal(2), 10, BigDecimal.ROUND_HALF_UP)));
         //加上栅格原点坐标
         centerPoint.setX(centerPoint.getX().add(robotInfo.getOriginX()));
         centerPoint.setY(centerPoint.getY().add(robotInfo.getOriginY()));
-
         return centerPoint;
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotInfoServiceImpl.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotInfoServiceImpl.java
index 043e05d..fdded64 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotInfoServiceImpl.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotInfoServiceImpl.java
@@ -104,7 +104,7 @@
             //下发障碍物栅格
             sendCostMapResolution(robotInfo.getResolution(), robotInfo.getId());
             //重新生成栅格地图
-            sendChangePcd(robotInfo.getId());
+            //sendChangePcd(robotInfo.getId());
         }
         update.eq(RobotInfo::getId, robotInfo.getId());
         update(update);
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotStatusInfoServiceImpl.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotStatusInfoServiceImpl.java
index 6d3ae91..46b5c3b 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotStatusInfoServiceImpl.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotStatusInfoServiceImpl.java
@@ -15,6 +15,7 @@
 import com.casic.missiles.modular.robot.dao.RobotStatusInfoMapper;
 import com.casic.missiles.modular.robot.model.RobotInfo;
 import com.casic.missiles.modular.robot.model.RobotStatusInfo;
+import com.casic.missiles.modular.robot.opt.enums.InstructCodeEnums;
 import com.casic.missiles.modular.robot.opt.instruct.base.dto.RobotMsgDTO;
 import com.casic.missiles.modular.robot.opt.instruct.base.dto.RobotMsgResponseDTO;
 import com.casic.missiles.modular.robot.opt.instruct.base.dto.RobotResponseDTO;
@@ -26,13 +27,18 @@
 import com.casic.missiles.modular.robot.service.IRobotInfoService;
 import com.casic.missiles.modular.robot.service.IRobotStatusInfoService;
 import lombok.extern.slf4j.Slf4j;
+import org.aspectj.util.FileUtil;
 import org.eclipse.paho.client.mqttv3.MqttMessage;
+import org.springframework.beans.factory.annotation.Value;
 import org.springframework.context.annotation.Lazy;
 import org.springframework.stereotype.Service;
 import org.springframework.transaction.annotation.Transactional;
 
+import java.io.File;
 import java.io.Serializable;
 import java.math.BigDecimal;
+import java.util.ArrayList;
+import java.util.Arrays;
 import java.util.List;
 
 
@@ -147,7 +153,6 @@
     @Override
     public void saveProcessMessage(String robotId, MqttMessage message) {
 //        String json = new String(message.getPayload());
-
         RobotResponseDTO<ProcessMessageResponseDTO> responseDTO = parseJson(message, new TypeReference<RobotResponseDTO<ProcessMessageResponseDTO>>() {
         });
         if (responseDTO.getMsg() != null) {
@@ -189,9 +194,8 @@
     }
 
     @Override
-    public void updateCurrTask(String robotId, Long currRouteId, Long currTaskId) {
+    public void updateCurrTask(String robotId, Long currTaskId) {
         UpdateWrapper<RobotStatusInfo> query = new UpdateWrapper<>();
-        query.set("curr_route_id", currRouteId);
         query.set("curr_task_id", currTaskId);
         query.eq("robot_id", robotId);
         update(query);
@@ -250,6 +254,15 @@
         }
     }
 
+    @Value("${casic.algorithm.path:-1}")
+    private String path;
+
+    /**
+     * 栅格阈值
+     */
+    @Value("${casic.algorithm.num:30}")
+    private Integer num;
+
     /**
      * 机器人栅格数据
      *
@@ -262,15 +275,41 @@
         RobotResponseDTO<AppCostmapResponseDTO> responseDTO = parseJson(message, new TypeReference<RobotResponseDTO<AppCostmapResponseDTO>>() {
         });
         if (responseDTO != null && responseDTO.getMsg() != null) {
+            //发送栅格分辨率设定
+
             cacheService.updateCostMap(robotId, responseDTO.getMsg());
             robotInfoService.updateRobotGrid(robotId, responseDTO.getMsg());
-//            log.info("--------------- json data:{}", JSON.toJSONString(responseDTO.getMsg()));
+            //log.info("--------------- json data:{}", JSON.toJSONString(responseDTO.getMsg()));
+            if (responseDTO != null && responseDTO.getMsg().getData() != null) {
+                ArrayList<Integer> dataNums = new ArrayList<>();
+                //根据阈值转换栅格
+                for (Integer dataNum : responseDTO.getMsg().getData()) {
+                    if (dataNum != -1) {
+                        if (dataNum >= num) {
+                            dataNums.add(0);
+                        } else {
+                            dataNums.add(1);
+                        }
+                    }
+                }
+                writeCache(dataNums,responseDTO.getMsg().getWidth(),responseDTO.getMsg().getHeight(),robotId);
+            }
         }
     }
 
+    private void writeCache(ArrayList<Integer> integers,Integer width,Integer height,String robotId) {
+        List<List<Integer>> arrays = CollectionUtil.split(integers,width);
+        ArrayList<ArrayList<Integer>> arrayList = new ArrayList<>();
+        for (List<Integer> array : arrays) {
+            arrayList.add((ArrayList)array);
+        }
+        CollectionUtil.reverse(arrayList);
+        CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_COST_MAP + robotId, arrayList);
+//        FileUtil.writeAsString(new File(path + "map.txt"), CollectionUtil.join(integers, " ").replaceAll("\\[", "").replaceAll("]", ""));
+    }
+
     @Override
     public void updateRobotPoint(PoseMessageResponseDTO dto, String robotId) {
-        //todo cache
         UpdateWrapper<RobotStatusInfo> query = new UpdateWrapper<>();
         query.set("postion_x", dto.getX());
         query.set("postion_y", dto.getY());
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotTrajectoryInfoServiceImpl.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotTrajectoryInfoServiceImpl.java
index 6c9625f..f1262a2 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotTrajectoryInfoServiceImpl.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotTrajectoryInfoServiceImpl.java
@@ -57,29 +57,29 @@
         });
 
         //读取导航状态,如果导航未开启则不存储
-//        RobotStatusInfo statusInfo = robotStatusInfoService.getStatusByRobotId(robotId);
-        Long routeId= cacheService.readRouteId(Long.valueOf(robotId));
-        if (routeId != null && responseDTO.getMsg() != null) {
+        //RobotStatusInfo statusInfo = robotStatusInfoService.getStatusByRobotId(robotId);
+
+        if ( responseDTO.getMsg() != null) {
             PoseMessageResponseDTO dto = responseDTO.getMsg();
             //执行循迹 保存当前轨迹信息
             //更新实时定位信息
             CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.ROBOT_POINT + robotId, dto);
-            infoService.pointTestCallBack(dto, robotId);
-            if (routeId != null) {
-                RobotTrajectoryInfo info = new RobotTrajectoryInfo();
-                info.setRobotId(Long.valueOf(robotId));
-                info.setCreateTime(new Date());
-                info.setPostionX(dto.getX());
-                info.setPostionY(dto.getY());
-                info.setPostionZ(dto.getZ());
-                info.setTheta(dto.getTheta());
-                save(info);
-                try {
-                    infoService.pointCallBack(dto, robotId);
-                } catch (Exception e) {
-                    log.error("task error", e);
-                }
+            try {
+                infoService.carPathPlanning(dto, robotId);
+            } catch (Exception e) {
+                log.error("task error", e);
             }
+//            if (routeId != null) {
+//                RobotTrajectoryInfo info = new RobotTrajectoryInfo();
+//                info.setRobotId(Long.valueOf(robotId));
+//                info.setCreateTime(new Date());
+//                info.setPostionX(dto.getX());
+//                info.setPostionY(dto.getY());
+//                info.setPostionZ(dto.getZ());
+//                info.setTheta(dto.getTheta());
+//                save(info);
+
+//            }
         }
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/TaskGridInfoServiceImpl.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/TaskGridInfoServiceImpl.java
new file mode 100644
index 0000000..cf61def
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/TaskGridInfoServiceImpl.java
@@ -0,0 +1,28 @@
+package com.casic.missiles.modular.robot.service.impl;
+
+import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
+import com.baomidou.mybatisplus.extension.plugins.pagination.Page;
+import com.baomidou.mybatisplus.extension.service.impl.ServiceImpl;
+import com.casic.missiles.modular.robot.dao.TaskGridInfoMapper;
+import com.casic.missiles.modular.robot.model.TaskGridInfo;
+import com.casic.missiles.modular.robot.service.ITaskGridInfoService;
+import org.springframework.stereotype.Service;
+
+import java.util.List;
+
+
+/**
+ * <p>
+ * 任务网格信息 服务实现类
+ * </p>
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+@Service
+public class TaskGridInfoServiceImpl extends ServiceImpl<TaskGridInfoMapper, TaskGridInfo> implements ITaskGridInfoService {
+    @Override
+    public List<TaskGridInfo> selectTaskGridInfoPage(Page<TaskGridInfo> page, QueryWrapper<TaskGridInfo> query) {
+        return this.baseMapper.selectTaskGridInfoPage(page, query);
+    }
+}
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/TaskInfoServiceImpl.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/TaskInfoServiceImpl.java
index d1417b0..fd49e2e 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/TaskInfoServiceImpl.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/TaskInfoServiceImpl.java
@@ -6,10 +6,10 @@
 import cn.hutool.core.convert.Convert;
 import cn.hutool.core.date.DateUtil;
 import cn.hutool.core.io.FileUtil;
-import cn.hutool.core.thread.ExecutorBuilder;
 import cn.hutool.core.util.StrUtil;
 import com.alibaba.fastjson.JSON;
 import com.baomidou.mybatisplus.core.conditions.Wrapper;
+import com.baomidou.mybatisplus.core.conditions.query.LambdaQueryWrapper;
 import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
 import com.baomidou.mybatisplus.core.conditions.update.UpdateWrapper;
 import com.baomidou.mybatisplus.core.metadata.IPage;
@@ -47,7 +47,6 @@
 import java.io.Serializable;
 import java.math.BigDecimal;
 import java.util.*;
-import java.util.concurrent.ThreadPoolExecutor;
 
 
 /**
@@ -70,6 +69,7 @@
     private final IRobotCacheService cacheService;
     private final IBaseRobotService baseRobotService;
     private final ITaskInfoImgService imgService;
+    private final ITaskGridInfoService taskGridInfoService;
     /**
      * 初始化线程池
      */
@@ -79,12 +79,19 @@
     @Value("${casic.doc.temp}")
     private String docTemp;
     /**
+     * 5000 ms
+     */
+    @Value("${casic.task.duration:3000}")
+    private Integer stayDuration;
+    @Value("${casic.algorithm.path:-1}")
+    private String path;
+    /**
      * 障碍物精度
      */
     @Value("${casic.obstacle:10}")
     private Integer obstacle;
 
-    public TaskInfoServiceImpl(@Lazy IRouteInfoService routeInfoService, IRobotStatusInfoService statusInfoService, AbstractDictService dictService, @Lazy IAlarmRecordService recordService, @Lazy ITaskHeatMapService taskHeatMapService, IAlgorithmService algorithmService, IRobotCacheService cacheService, @Lazy IBaseRobotService baseRobotService, ITaskInfoImgService imgService) {
+    public TaskInfoServiceImpl(@Lazy IRouteInfoService routeInfoService, IRobotStatusInfoService statusInfoService, AbstractDictService dictService, @Lazy IAlarmRecordService recordService, @Lazy ITaskHeatMapService taskHeatMapService, IAlgorithmService algorithmService, IRobotCacheService cacheService, @Lazy IBaseRobotService baseRobotService, ITaskInfoImgService imgService, ITaskGridInfoService taskGridInfoService) {
         this.routeInfoService = routeInfoService;
         this.statusInfoService = statusInfoService;
         this.dictService = dictService;
@@ -94,6 +101,7 @@
         this.cacheService = cacheService;
         this.baseRobotService = baseRobotService;
         this.imgService = imgService;
+        this.taskGridInfoService = taskGridInfoService;
     }
 
     /**
@@ -184,7 +192,7 @@
         if (CollectionUtil.isEmpty(routeInfos)) {
             throw new ServiceException(500, "路线不存在,请及时更新");
         }
-        RouteInfo routeInfo = routeInfos.get(0);
+//        RouteInfo routeInfo = routeInfos.get(0);
         Integer isTask = 0;
         //开启寻源任务
         if (request.getIsTask() == 1) {
@@ -193,22 +201,21 @@
         RobotInfo robotInfo = cacheService.getRobotInfo(Convert.toLong(robotId));
         CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_IS_TASK + robotId, isTask);
         //添加任务记录
-        TaskInfo taskInfo = addTaskByStartTrack(routeInfo, robotInfo);
+        TaskInfo taskInfo = addTaskByStartTrack( robotInfo, request);
         //No.3 更新器人执行任务信息
-        Long currRouteId = routeInfo.getId();
+//        Long currRouteId = routeInfo.getId();
         Long currTaskId = taskInfo.getId();
 
-        statusInfoService.updateCurrTask(robotId, currRouteId, currTaskId);
+        statusInfoService.updateCurrTask(robotId,  currTaskId);
         //算法初始化标志创建
         CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_TASK_ID + robotId, currTaskId);
         CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_TASK_INIT + robotId, 0);
-        CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_ROUTE_ID + robotId, routeInfo.getId());
+//        CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_ROUTE_ID + robotId, routeInfo.getId());
     }
 
     private void closeTask(StartTrackRequest request, String robotId) {
         //关闭操作
         RobotStatusInfo statusInfo = statusInfoService.getStatusByRobotId(robotId);
-
         if (statusInfo.getCurrTaskId() != null) {
             UpdateWrapper<TaskInfo> updateWrapper = new UpdateWrapper<>();
             updateWrapper.set("update_time", new Date());
@@ -221,10 +228,11 @@
 
         //清空任务缓存
         robotCacheClean(robotId);
-
+        //清楚规划任务锁
+        CacheUtil.remove(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_PLANING_FLAG + robotId);
+        CacheUtil.remove(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_ESTIMATE_GRID+ robotId);
         //清空机器当前任务
-        statusInfoService.updateCurrTask(robotId, null, null);
-
+        statusInfoService.updateCurrTask(robotId, null);
     }
 
     /**
@@ -249,7 +257,6 @@
 
         //判定任务是否已经完成初始化
         Long longId = Convert.toLong(robotId);
-        Long taskId = CacheUtil.get(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_TASK_ID + robotId);
         //缓存当前点位信息
         CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_XY_POINT + robotId, dto);
         RobotInfo robotInfo = cacheService.getRobotInfo(longId);
@@ -260,86 +267,203 @@
             log.error("no config 中子源阈值未配置");
             return;
         }
-
         //读取是否是循迹任务
         Integer isTask = cacheService.readIsTask(longId);
         if (isTask != 1) {
             log.info("循迹任务,不做处理");
             return;
         }
-        //算法初始化状态读取
-        Integer initValue = cacheService.readTaskInit(longId);
-
 
         //No.1 任务初始化
+//        pathInit(longId, robotInfo, pointDTO, taskId, robotId);
+    }
+
+    @Override
+    public void carPathPlanning(PoseMessageResponseDTO dto, String robotId) {
+        //判定任务是否已经完成初始化
+        Long longId = Convert.toLong(robotId);
+        Long taskId = CacheUtil.get(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_TASK_ID + robotId);
+        //缓存当前点位信息
+        CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_XY_POINT + robotId, dto);
+        RobotInfo robotInfo = cacheService.getRobotInfo(longId);
+        //slam经纬度转栅格
+        GridPointDTO pointDTO = algorithmService.convertSlamToGrid(dto.getX(), dto.getY(), robotInfo.getResolution(), robotInfo);
+        if (robotInfo.getAlarmThreshold() == null || robotInfo.getMaxThreshold() == null) {
+            log.error("no config 中子源阈值未配置");
+            return;
+        }
+
+        //读取是否是寻源任务
+        Integer isTask = cacheService.readIsTask(longId);
+        if (isTask != 1) {
+            log.info("循迹任务,不做处理");
+            return;
+        }
+
+        TaskInfo taskInfo = getById(taskId);
+        //No.1 任务初始化
+        pathInit(longId, robotInfo, pointDTO, taskId, robotId, taskInfo.getStepLen());
+        //No.2 路线规划
+        pathPlaning(longId, robotInfo, pointDTO, taskId, robotId, taskInfo,dto);
+    }
+
+    /**
+     * 路线规划进行加锁
+     *
+     * @param robotId 机器人ID
+     * @return
+     */
+    private synchronized boolean lockPlanIngFlag(String robotId) {
+        Boolean flag = CacheUtil.get(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_PLANING_FLAG + robotId);
+        if (flag == null) {
+            CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_PLANING_FLAG + robotId, true);
+            return true;
+        }
+        return false;
+    }
+
+    /**
+     * @param longId    机器人ID
+     * @param robotInfo 机器人信息
+     * @param pointDTO  当前点位栅格信息
+     * @param taskId    任务ID
+     * @param robotId   机器人ID
+     */
+    private void pathPlaning(Long longId, RobotInfo robotInfo, GridPointDTO pointDTO, Long taskId, String robotId, TaskInfo taskInfo,PoseMessageResponseDTO curPoint) {
+        if (taskId != null) {
+            //规划节点
+            PointNextDTO pointNextDTO = CacheUtil.get(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_ESTIMATE_GRID + robotId);
+            GridPointDTO oldEstimateGrid = GridPointDTO.convertGridPoint(pointNextDTO);
+            Boolean flag = lockPlanIngFlag(robotId);
+            //是否到达预期栅格
+            if (!flag) {
+                log.info("未获取到规划任务,直接结束：{}",pointDTO);
+                return;
+            }
+            try {
+                log.debug("oldEstimateGrid:{}", oldEstimateGrid);
+                if (pointNextDTO == null || isEquals(pointDTO, oldEstimateGrid)) {
+                    log.info("当前栅格变更......", pointDTO);
+                    CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_POINT + robotId, pointDTO);
+                    //算法调用获取热力图
+                    PathPlanDTO pathPlanDTO = createPlanDTO(robotInfo, pointDTO, taskId, robotId, taskInfo.getStepLen());
+                    if (taskInfo.getMaxThreshold() == null) {
+                        taskInfo.setMaxThreshold(robotInfo.getMaxThreshold());
+                    }
+                    pathPlanDTO.setTh(taskInfo.getMaxThreshold() + "");
+
+                    log.debug("heat map -- robotId:{},taskId:{}", robotId, taskId);
+                    pathPlanDTO.setZNow(cacheService.getNeutronCount(longId));
+
+                    //存储算法输出栅格信息
+                    AlgorithmResponse response = algorithmService.pathPlanning(pathPlanDTO);
+                    CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_ESTIMATE_GRID + robotId, response.getPointNextDTO());
+                    sendWebSocket(InstructStatusMsgEnums.HEAT_MAP, robotId, response.getTaskHeatMap());
+
+                    //存储循迹任务信息
+                    TaskGridInfo taskGridInfo = new TaskGridInfo();
+                    taskGridInfo.setCreateTime(new Date());
+                    taskGridInfo.setUpdateTime(new Date());
+                    taskGridInfo.setCurGridX(pointDTO.getX().intValue());
+                    taskGridInfo.setCurGridY(pointDTO.getY().intValue());
+                    taskGridInfo.setNeutronCount(new BigDecimal(pathPlanDTO.getZNow()));
+                    taskGridInfo.setEstimateGridX(response.getPointNextDTO().getX());
+                    taskGridInfo.setEstimateGridY(response.getPointNextDTO().getY());
+                    taskGridInfo.setTaskId(taskId);
+                    taskGridInfoService.save(taskGridInfo);
+
+                    try {
+                        //停留时间阈值
+                        Thread.sleep(stayDuration);
+                    } catch (InterruptedException e) {
+                        e.printStackTrace();
+                    }
+
+                    //是否触发停止状态
+                    if (!response.isEnd()) {
+                        log.debug("task planning-- xun dian robotId:{},taskId:{}", robotId, taskId);
+                        //机器人寻点指令下发
+                        sendTargetPoint(robotInfo, response,curPoint);
+                    } else {
+                        log.debug("task stop-- robotId:{},taskId:{}", robotId, taskId);
+                        //机器人停止指令下发
+                        SoftwareStopRequest request = new SoftwareStopRequest();
+                        request.setRobotId(robotInfo.getId());
+                        //设置为急停
+                        request.setControl(1);
+//                        baseRobotService.execCmdHandler(request, InstructCodeEnums.SOFTWARE_STOP);
+                        //循迹任务结束
+                        closeTask(null, robotId);
+                    }
+                }
+            } catch (Exception e) {
+                e.printStackTrace();
+            } finally {
+                //释放锁
+                CacheUtil.remove(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_PLANING_FLAG + robotId);
+            }
+        }
+    }
+
+    private PathPlanDTO createPlanDTO(RobotInfo robotInfo, GridPointDTO pointDTO, Long taskId, String robotId, Integer stepLen) {
+        PathPlanDTO pathPlanDTO = new PathPlanDTO();
+        pathPlanDTO.setTaskId(taskId);
+        pathPlanDTO.setRobotId(robotId);
+        pathPlanDTO.setStepLen(stepLen);
+        return pathPlanDTO;
+    }
+
+    /**
+     * 寻源初始化
+     *
+     * @param longId    机器ID
+     * @param robotInfo 机器信息
+     * @param pointDTO  当前点位信息
+     * @param taskId    任务ID
+     * @param robotId   机器ID
+     * @param stepLen   步长设置
+     */
+    private void pathInit(Long longId, RobotInfo robotInfo, GridPointDTO pointDTO, Long taskId, String robotId, Integer stepLen) {
+        //障碍物栅格上传已生成 直接读取
+        //算法初始化状态读取
+        Integer initValue = cacheService.readTaskInit(longId);
+        //No.1 任务初始化
         if (initValue != 1) {
+
             //执行初始化操作
             if (cacheService.updateTaskInit(longId, 1)) {
 
-                try {
-                    File file = new File(".");
-                    File[] files = file.listFiles((dir, name) -> (name.endsWith(".png") && name.startsWith("step"))
-                            || (name.endsWith(".txt") && name.startsWith("step")) || name.equals("repetition.txt") || name.equals("xy_train.txt") || name.equals("z_train.txt"));
-                    if (files != null) {
-                        for (int i = 0; i < files.length; i++) {
-                            FileUtil.del(files[i]);
+                List<List<Integer>> costList = CacheUtil.get(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_COST_MAP + robotId);
+                List<String> strs = new ArrayList<>();
+                List<String> strs2 = new ArrayList<>();
+                for (List<Integer> list : costList) {
+                    strs2.add(CollectionUtil.join(list, " "));
+                }
+                FileUtil.writeLines(strs2, new File(path + "map2.txt"), "utf-8");
+                //行转列 为算法输入数据
+                if(CollectionUtil.isNotEmpty(costList)){
+                    CollectionUtil.reverse(costList);
+                    for (int i = 0; i < costList.get(0).size(); i++) {
+                        ArrayList<Integer> ints = new ArrayList<>();
+                        for (List<Integer> list : costList) {
+                            ints.add(list.get(i));
                         }
+                        strs.add(CollectionUtil.join(ints, " "));
                     }
-                } catch (Exception e) {
-                    e.printStackTrace();
+//                    CollectionUtil.reverse(strs);
                 }
 
-                PathInitDTO pathInitDTO = createInitDTO(robotInfo, pointDTO, taskId, robotId);
+
+                FileUtil.writeLines(strs, new File(path + "map.txt"), "utf-8");
+
+                PathInitDTO pathInitDTO = createInitDTO(robotInfo, pointDTO, taskId, robotId, stepLen);
                 //中子源计数率读取
-                pathInitDTO.setCount(cacheService.getNeutronCount(longId));
+                pathInitDTO.setZNow(cacheService.getNeutronCount(longId));
+                log.info("init params:{}", JSON.toJSONString(pathInitDTO));
                 //执行1次算法 初始化操作
                 algorithmService.pathPlanningInit(pathInitDTO);
             }
         }
-
-        //No.2 任务执行中
-        if (taskId != null) {
-            //判断栅格变化
-            GridPointDTO oldPointDTO = CacheUtil.get(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_POINT + robotId);
-            if (oldPointDTO == null || isChange(pointDTO, oldPointDTO)) {
-                log.debug("grid change -- robotId:{},taskId:{}", robotId, taskId);
-                CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_POINT + robotId, pointDTO);
-                //算法调用获取热力图
-                PathPlanDTO pathPlanDTO = createPlanDTO(robotInfo, pointDTO, taskId, robotId);
-                //可通行标记为1 不可通行标记为0
-                String obstacle = getObstacle(robotId, pointDTO);
-                //TO-DO 障碍物判定信息
-                pathPlanDTO.setOpen(obstacle);
-                //中子源计数率读取
-                pathPlanDTO.setCount(cacheService.getNeutronCount(longId));
-                log.debug("heat map -- robotId:{},taskId:{}", robotId, taskId);
-                AlgorithmResponse response = algorithmService.pathPlanning(pathPlanDTO);
-                //存储算法输出栅格信息
-                CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_ESTIMATE_GRID + robotId, response.getPointNextDTO());
-                sendWebSocket(InstructStatusMsgEnums.HEAT_MAP, robotId, response.getTaskHeatMap());
-                //是否触发寻源操作
-                BigDecimal bigCount = new BigDecimal(pathPlanDTO.getCount());
-                if (bigCount.compareTo(robotInfo.getAlarmThreshold()) >= 0) {
-                    log.debug("task planning-- xun dian robotId:{},taskId:{}", robotId, taskId);
-                    //机器人寻点指令下发
-                    sendTargetPoint(robotInfo, response);
-                }
-
-                //是否触发停止状态
-                if (bigCount.compareTo(robotInfo.getMaxThreshold()) > 0) {
-                    log.debug("task stop-- robotId:{},taskId:{}", robotId, taskId);
-                    //机器人停止指令下发
-                    SoftwareStopRequest request = new SoftwareStopRequest();
-                    request.setRobotId(robotInfo.getId());
-                    //设置为急停
-                    request.setControl(1);
-                    baseRobotService.execCmdHandler(request, InstructCodeEnums.SOFTWARE_STOP);
-                    //循迹任务结束
-                    closeTask(null, robotId);
-                }
-
-            }
-        }
     }
 
     /**
@@ -353,7 +477,6 @@
         //判定任务是否已经完成初始化
         Long longId = Convert.toLong(robotId);
         Long taskId = CacheUtil.get(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_TASK_ID + robotId);
-
         RobotInfo robotInfo = cacheService.getRobotInfo(longId);
         //slam经纬度转栅格
         GridPointDTO pointDTO = algorithmService.convertSlamToGrid(dto.getX(), dto.getY(), robotInfo.getResolution(), robotInfo);
@@ -428,7 +551,7 @@
      * @param robotInfo 机器人信息
      * @param response  算法输出数据
      */
-    private void sendTargetPoint(RobotInfo robotInfo, AlgorithmResponse response) {
+    private void sendTargetPoint(RobotInfo robotInfo, AlgorithmResponse response,PoseMessageResponseDTO curPoint) {
         //读取已设置巡点目标信息
         PointNextDTO nextDTO = CacheUtil.get(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_TARGET_POINT_ID + robotInfo.getId());
         //巡点
@@ -439,8 +562,8 @@
             TargetPointRequest request = new TargetPointRequest();
             request.setX(Convert.toStr(gridCenterPoint.getX()));
             request.setY(Convert.toStr(gridCenterPoint.getY()));
-            request.setZ("0");
-
+            request.setZ(curPoint.getZ().toString());
+            request.setTheta(curPoint.getTheta().toString());
             request.setRobotId(robotInfo.getId());
             //执行巡点指令
             baseRobotService.execCmdHandler(request, InstructCodeEnums.SET_TARGETPOINT);
@@ -507,21 +630,20 @@
         params.put("startTime", startTime);
         params.put("endTime", endTime);
         //读取
-        List<AlarmRecord> records = recordService.selectListByTaskId(info.getId(), info.getRobotId());
         Integer index = 1;
-
-        if (CollectionUtil.isNotEmpty(records)) {
-            for (AlarmRecord record : records) {
+        LambdaQueryWrapper<TaskGridInfo> taskGridQuery = new LambdaQueryWrapper<>();
+        taskGridQuery.eq(TaskGridInfo::getTaskId,info.getId());
+        taskGridQuery.orderByAsc(TaskGridInfo::getCreateTime);
+        List<TaskGridInfo> taskGridInfos = taskGridInfoService.list(taskGridQuery);
+        if (CollectionUtil.isNotEmpty(taskGridInfos)) {
+            for (TaskGridInfo record : taskGridInfos) {
                 //栅格信息转换
-                GridPointDTO pointDTO = algorithmService.convertSlamToGrid(record.getX(), record.getY(), info.getResolution(), robotInfo);
-                record.setX(pointDTO.getX());
-                record.setY(pointDTO.getY());
                 record.setIndex(index);
                 index++;
             }
         }
 
-        params.put("records", records);
+        params.put("records", taskGridInfos);
         ImageEntity mcaImg = new ImageEntity();
         mcaImg.setHeight(600);
         mcaImg.setWidth(600);
@@ -557,7 +679,6 @@
             psdImg.setData(WordUtil.getImageBase64(uploadPath + info.getPsdImg()));
         }
         params.put("psdImg", psdImg);
-
         WordUtil.exportOutputStream(docTemp, response.getOutputStream(), params);
     }
 
@@ -589,7 +710,7 @@
     private String saveHeatImg(String taskId) {
         String newFileName = "task\\" + taskId + ".png";
         //读取当前路径图片
-        File file = new File(".");
+        File file = new File(path + "\\" + taskId + "\\figs");
         File[] files = file.listFiles((dir, name) -> name.endsWith(".png") && name.startsWith("step"));
         //判定当前执行路径最大step图片
         if (files != null && files.length > 0) {
@@ -614,33 +735,28 @@
         return true;
     }
 
-    private PathPlanDTO createPlanDTO(RobotInfo robotInfo, GridPointDTO pointDTO, Long taskId, String robotId) {
-        PathPlanDTO pathPlanDTO = new PathPlanDTO();
-        String xMax = robotInfo.getGridHeight().divide(robotInfo.getResolution(), 5, BigDecimal.ROUND_HALF_UP).intValue() + "";
-        String yMax = robotInfo.getGridWidth().divide(robotInfo.getResolution(), 5, BigDecimal.ROUND_HALF_UP).intValue() + "";
-        pathPlanDTO.setX(pointDTO.getX().intValue() + "");
-        pathPlanDTO.setY(pointDTO.getY().intValue() + "");
-        pathPlanDTO.setXMax(xMax);
-        pathPlanDTO.setYMax(yMax);
-        pathPlanDTO.setTaskId(taskId);
-        pathPlanDTO.setRobotId(robotId);
-        return pathPlanDTO;
+    private boolean isEquals(GridPointDTO pointDTO, GridPointDTO oldPointDTO) {
+        if(oldPointDTO==null||oldPointDTO.getX()==null){
+            return false;
+        }
+        if (pointDTO.getX().compareTo(oldPointDTO.getX()) == 0 && pointDTO.getY().compareTo(oldPointDTO.getY()) == 0) {
+            return true;
+        }
+        return false;
     }
 
-    private PathInitDTO createInitDTO(RobotInfo robotInfo, GridPointDTO pointDTO, Long taskId, String robotId) {
-        String xMax = robotInfo.getGridHeight().divide(robotInfo.getResolution(), 5, BigDecimal.ROUND_HALF_UP).intValue() + "";
-        String yMax = robotInfo.getGridWidth().divide(robotInfo.getResolution(), 5, BigDecimal.ROUND_HALF_UP).intValue() + "";
+
+    private PathInitDTO createInitDTO(RobotInfo robotInfo, GridPointDTO pointDTO, Long taskId, String robotId, Integer stepLen) {
         PathInitDTO pathInitDTO = new PathInitDTO();
-        pathInitDTO.setXMax(xMax);
-        pathInitDTO.setYMax(yMax);
-        pathInitDTO.setX(pointDTO.getX().intValue() + "");
-        pathInitDTO.setY(pointDTO.getY().intValue() + "");
+        pathInitDTO.setXNow(pointDTO.getX().intValue() + "");
+        pathInitDTO.setYNow(pointDTO.getY().intValue() + "");
+        pathInitDTO.setStepLen(stepLen);
         pathInitDTO.setTaskId(taskId);
         pathInitDTO.setRobotId(robotId);
         return pathInitDTO;
     }
 
-    private TaskInfo addTaskByStartTrack(RouteInfo routeInfo, RobotInfo robotInfo) {
+    private TaskInfo addTaskByStartTrack( RobotInfo robotInfo, StartTrackRequest request) {
         //No.2 新增任务信息
         TaskInfo taskInfo = new TaskInfo();
         String currDate = DateUtil.format(new Date(), "yyyyMMdd");
@@ -654,16 +770,20 @@
         taskInfo.setCreateTime(new Date());
         //任务类型 中子源
         taskInfo.setTaskType(1);
-        taskInfo.setDiscernType(1);
-        taskInfo.setTaskName(routeInfo.getRouteName() + "巡线任务");
+        taskInfo.setDiscernType(2);
+        taskInfo.setTaskName("自主巡检任务");
         //执行状态 未完成
         taskInfo.setTaskStatus(0);
         taskInfo.setStartTime(new Date());
-        taskInfo.setRouteId(routeInfo.getId());
+        taskInfo.setRouteId(null);
         taskInfo.setRobotId(robotInfo.getId());
         taskInfo.setGridHeight(robotInfo.getGridHeight());
         taskInfo.setGridWidth(robotInfo.getGridWidth());
         taskInfo.setResolution(robotInfo.getResolution());
+
+        taskInfo.setStepLen(request.getStepLen());
+        taskInfo.setMaxThreshold(request.getMaxThreshold());
+
         save(taskInfo);
         return taskInfo;
     }

diff --git a/.gitignore b/.gitignore
index 9e20bc1..f42a9cb 100644
--- a/.gitignore
+++ b/.gitignore
@@ -2,3 +2,4 @@
 **/target/**
 logs/
 *.iml
+*.exe
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java b/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
index c7b5d28..a849528 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
@@ -28,6 +28,7 @@
     String KEY_CUR_TASK_ID = "CUR_TASK_ID_";
     String KEY_CUR_TASK_INIT = "CUR_TASK_INIT_";
 
+    String KEY_COST_MAP = "KEY_COST_MAP";
     /**
      * PSD数据ID
      */
@@ -41,6 +42,7 @@
      */
     String KEY_CUR_XY_POINT = "KEY_CUR_XY_POINT_";
     String KEY_CUR_ESTIMATE_GRID = "KEY_CUR_ESTIMATE_GRID_";
+    String KEY_PLANING_FLAG = "KEY_PLANING_FLAG_";
     /**
      * 机器人巡点栅格
      */
@@ -55,6 +57,7 @@
      * 探测器配置参数缓存
      */
     String KEY_CONFIG_PARAMS_ID = "KEY_CONFIG_PARAMS_ID_";
+    String KEY_CONFIG_COSTMAP_RESOLUTION_SET = "KEY_CONFIG_COSTMAP_RESOLUTION_SET_";
 
     /**
      * 暂停状态缓存信息
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
index c4c905d..3bb475f 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
@@ -1,9 +1,14 @@
 package com.casic.missiles.modular.robot.controller;
 
+import cn.hutool.core.lang.Assert;
 import cn.hutool.core.util.StrUtil;
 import com.casic.missiles.core.base.controller.BaseController;
+import com.casic.missiles.core.cache.CacheUtil;
 import com.casic.missiles.core.util.SpringContextHolder;
+import com.casic.missiles.model.exception.RequestValidException;
+import com.casic.missiles.model.response.ResponseData;
 import com.casic.missiles.model.response.dto.ResponseDataDTO;
+import com.casic.missiles.modular.constants.RobotDictConstants;
 import com.casic.missiles.modular.robot.IBaseRobotService;
 import com.casic.missiles.modular.robot.dto.RobotInfoSetDTO;
 import com.casic.missiles.modular.robot.model.RobotInfo;
@@ -19,10 +24,16 @@
 import io.swagger.annotations.ApiOperation;
 import io.swagger.annotations.ApiParam;
 import lombok.SneakyThrows;
+import lombok.extern.slf4j.Slf4j;
+import org.apache.ibatis.annotations.Param;
+import org.springframework.beans.factory.annotation.Value;
 import org.springframework.web.bind.annotation.*;
+import org.springframework.web.multipart.MultipartFile;
 
 import javax.servlet.http.HttpServletResponse;
+import java.io.File;
 import java.util.List;
+import java.util.Optional;
 
 /**
  * 机器人操作类
@@ -30,6 +41,7 @@
  * @author lwh
  * @date 2023-10-30
  */
+@Slf4j
 @Api(tags = "机器人控制接口")
 @RestController
 @RequestMapping("/robot/opt")
@@ -61,6 +73,28 @@
         return baseRobotService.execCmdHandler(request, InstructCodeEnums.CHANGE_PCD);
     }
 
+    @Value("${casic.file.uploadPath}")
+    private String filePath;
+
+    @SneakyThrows
+    @ApiOperation(value = "PCD上传接口")
+    @PostMapping(value = "/uploadPcd")
+    public ResponseData uploadPcd(@RequestParam("file") MultipartFile file, @ApiParam("机器人ID,url拼接传参") String robotId) {
+        Assert.isFalse(StrUtil.isEmpty(robotId), () -> new RequestValidException("机器ID不能为空", Optional.of(new Throwable().getStackTrace()[2])));
+        RobotInfo info = robotInfoService.getById(robotId);
+        if (file != null && !file.isEmpty()) {
+            File pcdFile = new File(filePath + "finalCloud_" + info.getId() + ".pcd");
+            try {
+                file.transferTo(pcdFile);
+            } catch (Exception e) {
+                log.error(e.getMessage());
+            }
+            return ResponseData.success();
+        } else {
+            return ResponseData.error("附件上传失败!");
+        }
+    }
+
     @SneakyThrows
     @ApiOperation(value = "PCD下载接口")
     @PostMapping(value = "/pcdDownload")
@@ -136,6 +170,10 @@
     @ApiOperation(value = "启动或关闭循迹功能", notes = "返回 1成功 0失败  wait 启动时是否等待，0代表循迹启动即运动，不等待；1代表循迹启动原地等待。若无此字段，默认循迹启动不等待直接运动")
     @PostMapping(value = "/startTrack")
     public ResponseDataDTO<Integer> startTrack(@RequestBody StartTrackRequest request) {
+        request.setIsTask(1);
+        if (request.getStepLen() == null) {
+            request.setStepLen(2);
+        }
         baseRobotService.startTrack(request);
         return ResponseDataDTO.success();
     }
@@ -156,6 +194,13 @@
         MessageRequestDTO<CrlNavRequest> messageDTO = new MessageRequestDTO();
         messageDTO.setMsg(request);
         ResponseDataDTO<Integer> list = baseRobotService.getRobotRouteOptService().robotCmd(info, InstructCodeEnums.CTRL_NAV, messageDTO);
+        if (request.getAction() == 1) {
+            CostMapResolutionRequest costMap = new CostMapResolutionRequest();
+            costMap.setResolution(info.getResolution().doubleValue());
+            request.setRobotId(request.getRobotId());
+            //是否下发分辨率
+            CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CONFIG_COSTMAP_RESOLUTION_SET + request.getRobotId(), costMap);
+        }
         return list;
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java
new file mode 100644
index 0000000..5317651
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java
@@ -0,0 +1,86 @@
+package com.casic.missiles.modular.robot.controller;
+
+import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
+import com.baomidou.mybatisplus.extension.plugins.pagination.Page;
+import com.casic.missiles.core.base.controller.BaseController;
+import com.casic.missiles.core.page.PageFactory;
+import com.casic.missiles.core.page.PageInfoBT;
+import com.casic.missiles.model.form.IdForms;
+import com.casic.missiles.model.response.dto.ResponseDataDTO;
+import com.casic.missiles.modular.robot.model.TaskGridInfo;
+import com.casic.missiles.modular.robot.service.ITaskGridInfoService;
+import com.casic.missiles.modular.robot.vo.TaskGridInfoVO;
+import io.swagger.annotations.Api;
+import io.swagger.annotations.ApiOperation;
+import org.springframework.web.bind.annotation.*;
+
+import java.util.List;
+
+/**
+ * 任务网格信息Controller
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+@Api(tags = "任务网格信息")
+@RestController
+@RequestMapping("/robot/taskGridInfo")
+public class TaskGridInfoController extends BaseController {
+
+    private final ITaskGridInfoService taskGridInfoService;
+
+    public TaskGridInfoController(ITaskGridInfoService taskGridInfoService) {
+        this.taskGridInfoService = taskGridInfoService;
+    }
+
+    @ApiOperation(value = "列表查询")
+    @GetMapping(value = "/list")
+    public ResponseDataDTO<List<TaskGridInfo>> list(TaskGridInfoVO taskGridInfoVO) {
+        QueryWrapper<TaskGridInfo> query = taskGridInfoVO.genQuery();
+        return ResponseDataDTO.success(taskGridInfoService.list(query));
+    }
+
+    @ApiOperation(value = "分页查询")
+    @GetMapping(value = "/listPage")
+    public ResponseDataDTO<PageInfoBT<TaskGridInfo>> listPage(TaskGridInfoVO taskGridInfoVO) {
+        Page<TaskGridInfo> page = PageFactory.defaultPage();
+        QueryWrapper<TaskGridInfo> query = taskGridInfoVO.genQuery();
+        page = taskGridInfoService.page(page, query);
+        return ResponseDataDTO.success(super.packForBT(page));
+    }
+
+
+    @ApiOperation(value = "新增接口")
+    @PostMapping(value = "/add")
+    public ResponseDataDTO add(@RequestBody TaskGridInfo taskGridInfo) {
+        taskGridInfoService.save(taskGridInfo);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "修改接口")
+    @PostMapping(value = "/update")
+    public ResponseDataDTO update(@RequestBody TaskGridInfo taskGridInfo) {
+        taskGridInfoService.updateById(taskGridInfo);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "删除接口")
+    @GetMapping(value = "/delete")
+    public ResponseDataDTO delete(String id) {
+        taskGridInfoService.removeById(id);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation("批量删除")
+    @PostMapping("/batchDelete")
+    public ResponseDataDTO batchDelete(@RequestBody IdForms ids) {
+        taskGridInfoService.removeByIds(ids.getIds());
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "详情查询")
+    @GetMapping(value = "/detail")
+    public ResponseDataDTO<TaskGridInfo> detail(String id) {
+        return ResponseDataDTO.success(taskGridInfoService.getById(id));
+    }
+}
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
index e82a792..6992274 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
@@ -98,7 +98,7 @@
     public ResponseDataDTO<AlgorithmResponse> test() {
         AlgorithmResponse response = new AlgorithmResponse();
         response.setTaskHeatMap(new TaskHeatMap());
-        algorithmService.readAlgorithmResponse("step3.txt",response,123L,null);
+        algorithmService.readAlgorithmResponse("step3.txt",null,response,123L,null);
         sendWebSocket(InstructStatusMsgEnums.HEAT_MAP, "1", response.getTaskHeatMap());
         return ResponseDataDTO.success(response);
     }
@@ -121,7 +121,7 @@
     @GetMapping(value = "/testGrid")
     public ResponseDataDTO<List<RouteInfo>> testGrid(@RequestBody PathInitDTO pathInitDTO) {
         RobotInfo robotInfo = infoService.getById(pathInitDTO.getRobotId());
-        GridPointDTO gridPointDTO = algorithmService.convertSlamToGrid(new BigDecimal(pathInitDTO.getX()), new BigDecimal(10), new BigDecimal(4), robotInfo);
+        GridPointDTO gridPointDTO = algorithmService.convertSlamToGrid(new BigDecimal(pathInitDTO.getXNow()), new BigDecimal(10), new BigDecimal(4), robotInfo);
         return ResponseDataDTO.success(algorithmService.convertGridCenterPoint(gridPointDTO, new BigDecimal(10), robotInfo));
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/TaskGridInfoMapper.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/TaskGridInfoMapper.java
new file mode 100644
index 0000000..5c3a70b
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/TaskGridInfoMapper.java
@@ -0,0 +1,23 @@
+package com.casic.missiles.modular.robot.dao;
+
+
+import com.baomidou.mybatisplus.core.mapper.BaseMapper;
+import com.casic.missiles.modular.robot.model.TaskGridInfo;
+import org.apache.ibatis.annotations.Param;
+import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
+import com.baomidou.mybatisplus.extension.plugins.pagination.Page;
+import java.util.List;
+/**
+ * <p>
+ *  任务网格信息 Mapper 接口
+ * </p>
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+public interface TaskGridInfoMapper extends BaseMapper<TaskGridInfo> {
+/**
+* 任务网格信息 分页检索
+*/
+List<TaskGridInfo> selectTaskGridInfoPage( @Param("page") Page<TaskGridInfo> page,@Param("ew" ) QueryWrapper<TaskGridInfo> query);
+}
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/mapping/TaskGridInfoMapper.xml b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/mapping/TaskGridInfoMapper.xml
new file mode 100644
index 0000000..be573e0
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/mapping/TaskGridInfoMapper.xml
@@ -0,0 +1,80 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!DOCTYPE mapper PUBLIC "-//mybatis.org//DTD Mapper 3.0//EN" "http://mybatis.org/dtd/mybatis-3-mapper.dtd">
+<mapper namespace="com.casic.missiles.modular.robot.dao.TaskGridInfoMapper">
+    <!-- 通用查询映射结果 -->
+    <resultMap id="BaseResultMap" type="com.casic.missiles.modular.robot.model.TaskGridInfo">
+        <result column="id" property="id"/>
+        <result column="create_time" property="createTime"/>
+        <result column="update_time" property="updateTime"/>
+        <result column="task_id" property="taskId"/>
+        <result column="estimate_grid_x" property="estimateGridX"/>
+        <result column="estimate_grid_y" property="estimateGridY"/>
+        <result column="neutron_count" property="neutronCount"/>
+        <result column="cur_grid_x" property="curGridX"/>
+        <result column="cur_grid_y" property="curGridY"/>
+    </resultMap>
+    <!-- 通用查询结果列 -->
+    <sql id="Base_Column_List">
+        id
+        ,
+                    create_time as createTime ,
+                    update_time as updateTime ,
+                    task_id as taskId ,
+                    estimate_grid_x as estimateGridX ,
+                    estimate_grid_y as estimateGridY ,
+                    neutron_count as neutronCount ,
+                    cur_grid_x as curGridX ,
+                    cur_grid_y as curGridY
+    </sql>
+    <sql id="Base_Column_Alias">
+        a
+        .
+        id
+        ,
+            a.create_time
+            as createTime ,
+            a.update_time
+            as updateTime ,
+            a.task_id
+            as taskId ,
+            a.estimate_grid_x
+            as estimateGridX ,
+            a.estimate_grid_y
+            as estimateGridY ,
+            a.neutron_count
+            as neutronCount ,
+            a.cur_grid_x
+            as curGridX ,
+            a.cur_grid_y
+            as curGridY
+    </sql>
+    <select id="selectTaskGridInfoPage" resultType="com.casic.missiles.modular.robot.model.TaskGridInfo">
+        select
+        <include refid="Base_Column_Alias"/>
+        from task_grid_info a
+        <where>
+            <if test="ew != null">
+                ${ew.sqlSegment}
+            </if>
+        </where>
+    </select>
+    <sql id="Base_Column_Query">
+        <if test="request!=null">
+            <if test="request.taskId!= null and request.taskId!=''">
+                and task_id = #{request.taskId}
+            </if>
+            <if test="request.estimateGridX!= null and request.estimateGridX!=''">
+                and estimate_grid_x = #{request.estimateGridX}
+            </if>
+            <if test="request.estimateGridY!= null and request.estimateGridY!=''">
+                and estimate_grid_y = #{request.estimateGridY}
+            </if>
+            <if test="request.curGridX!= null and request.curGridX!=''">
+                and cur_grid_x = #{request.curGridX}
+            </if>
+            <if test="request.curGridY!= null and request.curGridY!=''">
+                and cur_grid_y = #{request.curGridY}
+            </if>
+        </if>
+    </sql>
+</mapper>
\ No newline at end of file
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java
index 749f559..4559074 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java
@@ -13,4 +13,9 @@
      * 规划点位信息
      */
     private PointNextDTO pointNextDTO;
+    /**
+     * 寻源目标地址
+     */
+    private PointNextDTO coordinate;
+    private boolean isEnd;
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java
index 39ff2a2..cef3238 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java
@@ -12,6 +12,9 @@
     private BigDecimal y;
 
     public static GridPointDTO convertGridPoint(PointNextDTO nextDTO) {
+        if(nextDTO==null){
+            return null;
+        }
         GridPointDTO gridPointDTO = new GridPointDTO();
         gridPointDTO.setX(Convert.toBigDecimal(nextDTO.getX()));
         gridPointDTO.setY(Convert.toBigDecimal(nextDTO.getY()));
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java
index 8c88242..33be25d 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java
@@ -12,25 +12,19 @@
 public class PathInitDTO {
     //---------------------算法参数
     /**
-     * x坐标最大值（x方向栅格总数）
+     * 初始位置x坐标(栅格地图下的坐标)
      */
-    private String xMax;
+    private String xNow;
     /**
-     * y坐标最大值（y方向栅格总数）
+     * 初始位置y坐标(栅格地图下的坐标)
      */
-    private String yMax;
+    private String yNow;
     /**
      * 初始位置计数率
      */
-    private String count;
-    /**
-     * 初始位置x坐标(0<=x<x_max)
-     */
-    private String x;
-    /**
-     * 初始位置y坐标(0<=y<y_max)
-     */
-    private String y;
+    private String zNow;
+
+    private Integer stepLen;
 
     //---------------------其他执行参数
     private String robotId;
@@ -38,11 +32,11 @@
 
     public List<String> toArray() {
         List<String> list = new ArrayList<>();
-        list.add(xMax);
-        list.add(yMax);
-        list.add(count);
-        list.add(x);
-        list.add(y);
+        list.add(xNow);
+        list.add(yNow);
+        list.add(stepLen+"");
+        list.add(zNow);
+        list.add(taskId+"");
         return list;
     }
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java
index 109e95c..ada6774 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java
@@ -10,21 +10,17 @@
  */
 @Data
 public class PathPlanDTO extends PathInitDTO {
-
     /**
-     * 长度为8的数组，记录(x-1, y-1), (x-1, y), (x-1, y+1), (x, y-1), (x, y+1), (x+1, y-1), (x+1, y), (x+1, y+1)8个坐标的可通行信息。
-     * 若某坐标有障碍物（不可通行行）则记为0，否则记为1
+     * 浮点阈值(阈值，超过该值后结束巡检开始寻源)
      */
-    private String open;
-
+    private String th;
     public List<String> toArray() {
         List<String> list = new ArrayList<>();
-        list.add(getXMax());
-        list.add(getYMax());
-        list.add(open);
-        list.add(getCount());
-        list.add(getX());
-        list.add(getY());
+
+        list.add(getStepLen()+"");
+        list.add(getTh());
+        list.add(getZNow());
+        list.add(getTaskId()+"");
         return list;
     }
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskGridInfo.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskGridInfo.java
new file mode 100644
index 0000000..101c10a
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskGridInfo.java
@@ -0,0 +1,83 @@
+package com.casic.missiles.modular.robot.model;
+
+import com.baomidou.mybatisplus.annotation.TableField;
+import com.baomidou.mybatisplus.annotation.TableName;
+import com.baomidou.mybatisplus.extension.activerecord.Model;
+import io.swagger.annotations.ApiModelProperty;
+import lombok.Data;
+
+import java.math.BigDecimal;
+import java.util.Date;
+
+/**
+ * 任务网格信息对象 task_grid_info
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+@Data
+@TableName("task_grid_info")
+public class TaskGridInfo extends Model<TaskGridInfo> {
+    private static final long serialVersionUID = 1L;
+    @TableField(exist = false)
+    private Integer index;
+    @ApiModelProperty(value = "主键", dataType = "Long")
+    @TableField("id")
+    private Long id;
+
+    @ApiModelProperty(value = "创建时间", dataType = "Date")
+    @TableField("create_time")
+    private Date createTime;
+
+    @ApiModelProperty(value = "修改时间", dataType = "Date")
+    @TableField("update_time")
+    private Date updateTime;
+
+    @ApiModelProperty(value = "任务ID", dataType = "Long")
+    @TableField("task_id")
+    private Long taskId;
+
+    @ApiModelProperty(value = "估算栅格x", dataType = "Long")
+    @TableField("estimate_grid_x")
+    private String estimateGridX;
+
+    @ApiModelProperty(value = "估算栅格y", dataType = "Long")
+    @TableField("estimate_grid_y")
+    private String estimateGridY;
+
+    @ApiModelProperty(value = "中子计数率", dataType = "BigDecimal")
+    @TableField("neutron_count")
+    private BigDecimal neutronCount;
+
+    @ApiModelProperty(value = "当前栅格x", dataType = "Long")
+    @TableField("cur_grid_x")
+    private Integer curGridX;
+
+    @ApiModelProperty(value = "当前栅格y", dataType = "Long")
+    @TableField("cur_grid_y")
+    private Integer curGridY;
+
+    public String getCurPoint(){
+        return "("+curGridX+","+curGridY+")";
+    }
+    public String getEstimatePoint(){
+        if(estimateGridX==null&estimateGridY==null){
+            return "结束";
+        }
+        return "("+estimateGridX+","+estimateGridY+")";
+    }
+    @Override
+    public String toString() {
+        return "TaskGridInfo{"
+                + "id=" + id
+                + "createTime=" + createTime
+                + "updateTime=" + updateTime
+                + "taskId=" + taskId
+                + "estimateGridX=" + estimateGridX
+                + "estimateGridY=" + estimateGridY
+                + "neutronCount=" + neutronCount
+                + "curGridX=" + curGridX
+                + "curGridY=" + curGridY
+                + "}";
+    }
+}
\ No newline at end of file
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java
index edc104e..a8acb2a 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java
@@ -53,6 +53,13 @@
     @TableField("route_id")
     private Long routeId;
 
+    @ApiModelProperty(value = "寻源初始步长", dataType = "Integer")
+    @TableField("step_len")
+    private Integer stepLen;
+
+    @ApiModelProperty(value = "寻源阈值设定", dataType = "BigDecimal")
+    @TableField("max_threshold")
+    private BigDecimal maxThreshold;
     /**
      * 任务类型(1巡检任务)
      */
@@ -62,7 +69,6 @@
     @ApiModelProperty(value = "任务类型字典名称", dataType = "String")
     @TableField(exist = false)
     private String taskTypeName;
-
     @ApiModelProperty(value = "识别类型(discernType 1中子源识别)", dataType = "Integer")
     @TableField("discern_type")
     private Integer discernType;
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java
index c4bc25d..c6620f3 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java
@@ -53,7 +53,7 @@
     }),
     SET_SPEED("/set_speed" , "/set_speed_response" , "最大运动速度设置" , true, new TypeReference<RobotMsgResponseDTO<Integer>>() {
     }),
-    SET_TARGETPOINT("/set_targetpoint" , "/set_targetpoint_response" , "设置导航目标点" , false, new TypeReference<RobotMsgResponseDTO<Integer>>() {
+    SET_TARGETPOINT("/set_targetpoint" , "/set_targetpoint_response" , "设置导航目标点" , true, new TypeReference<RobotMsgResponseDTO<Integer>>() {
     }),
 
     GET_DATA_LIST("/get_data_list" , "/get_data_list_response" , "获取数据包列表" , true, new TypeReference<RobotMsgResponseDTO<List<String>>>() {
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/AbstractSocketHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/AbstractSocketHandler.java
index 83f4cb2..e47e343 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/AbstractSocketHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/AbstractSocketHandler.java
@@ -49,7 +49,7 @@
                 wsMsgBaseDTO.setMsgKey(enums.getMsgKey());
                 wsMsgBaseDTO.setData(jsonObject.get("msg"));
                 String msg = JSON.toJSONString(wsMsgBaseDTO);
-                log.info("推送{}消息：{}", enums.getRemarks(), msg);
+//                log.info("推送{}消息：{}", enums.getRemarks(), msg);
                 webSocket.sendOneMessage(robotId, msg);
             }
         } catch (Exception e) {
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/cmd/TargetPointCmdHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/cmd/TargetPointCmdHandler.java
index c1c09fa..43b4460 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/cmd/TargetPointCmdHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/cmd/TargetPointCmdHandler.java
@@ -34,7 +34,11 @@
     @Override
     public ResponseDataDTO senderCmd(TargetPointRequest request, InstructCodeEnums enums) {
         MessageRequestDTO<TargetPointRequest> messageDTO = new MessageRequestDTO();
-        //读取默认关键点信息
+        //读取默认关键点信息 {"x":"1.715708819927424","y":"-0.9100042544250901","z":"0.017993879527185876","theta":"-0.039278239414312105"}
+//        request.setX("1.715708819927424");
+//        request.setY("-0.9100042544250901");
+//        request.setZ("0.017993879527185876");
+//        request.setTheta("-0.039278239414312105");
         messageDTO.setMsg(request);
         ResponseDataDTO<Integer> list = optService.robotCmd(getRobotInfo(), enums, messageDTO);
         return list;
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/AppCostMapHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/AppCostMapHandler.java
index 0335693..88c9f79 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/AppCostMapHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/AppCostMapHandler.java
@@ -1,6 +1,11 @@
 package com.casic.missiles.modular.robot.opt.handler.msg;
 
+import com.casic.missiles.core.cache.CacheUtil;
+import com.casic.missiles.modular.constants.RobotDictConstants;
+import com.casic.missiles.modular.robot.IBaseRobotService;
+import com.casic.missiles.modular.robot.opt.enums.InstructCodeEnums;
 import com.casic.missiles.modular.robot.opt.handler.AbstractSocketHandler;
+import com.casic.missiles.modular.robot.opt.instruct.dto.CostMapResolutionRequest;
 import com.casic.missiles.modular.robot.service.IRobotStatusInfoService;
 import lombok.extern.slf4j.Slf4j;
 import org.eclipse.paho.client.mqttv3.MqttMessage;
@@ -16,14 +21,27 @@
 @Component("/app_costmap_sender")
 public class AppCostMapHandler extends AbstractSocketHandler {
     private final IRobotStatusInfoService statusInfoService;
+    private final IBaseRobotService baseRobotService;
     private static Map<String, Integer> index = new ConcurrentHashMap<>();
 
-    public AppCostMapHandler(IRobotStatusInfoService statusInfoService) {
+    public AppCostMapHandler(IRobotStatusInfoService statusInfoService, IBaseRobotService baseRobotService) {
         this.statusInfoService = statusInfoService;
+        this.baseRobotService = baseRobotService;
     }
 
     @Override
     public void process(String robotId, String topic, MqttMessage message) {
         statusInfoService.saveCostMap(robotId, message);
+
+        CostMapResolutionRequest costMapSet = CacheUtil.get(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CONFIG_COSTMAP_RESOLUTION_SET +robotId);
+        if(costMapSet != null){
+            CacheUtil.remove(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CONFIG_COSTMAP_RESOLUTION_SET +robotId);
+            new Thread(() -> {
+                costMapSet.setRobotId(Long.valueOf(robotId));
+                baseRobotService.execCmdHandler(costMapSet, InstructCodeEnums.COSTMAP_RESOLUTION);
+            }).start();
+
+        }
+
     }
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/PoseMessageMsgHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/PoseMessageMsgHandler.java
index 29f8919..6c3c3bd 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/PoseMessageMsgHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/PoseMessageMsgHandler.java
@@ -17,11 +17,9 @@
 @Component("/pose_message")
 public class PoseMessageMsgHandler extends AbstractSocketHandler {
 
-
     private final EventPublisher publisher;
 
     public PoseMessageMsgHandler(EventPublisher publisher) {
-
         this.publisher = publisher;
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/StopStateMsgHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/StopStateMsgHandler.java
index 8950815..8a71e0b 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/StopStateMsgHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/StopStateMsgHandler.java
@@ -30,7 +30,7 @@
     public void process(String robotId, String topic, MqttMessage message) {
 
         index.put(index_key, index.get("index") + 1);
-        if (index.get(index_key)>50) {
+        if (index.get(index_key)>30) {
             index.put(index_key, 0);
             log.info("主键：{},停障状态topic:{},消息内容:{}", robotId, topic, message.toString());
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/impl/RobotOptServiceImpl.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/impl/RobotOptServiceImpl.java
index 8471e7a..7940a24 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/impl/RobotOptServiceImpl.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/impl/RobotOptServiceImpl.java
@@ -146,7 +146,7 @@
     public void startTrack(StartTrackRequest request) {
         //增加循迹任务记录
         taskInfoService.reloadTask(request, robotInfoService.getById(request.getRobotId()));
-        this.execCmdHandler(request, InstructCodeEnums.START_TRACK);
+//        this.execCmdHandler(request, InstructCodeEnums.START_TRACK);
     }
 
     public IRobotRouteOptService getRobotRouteOptService() {
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/BaseRobotCmdDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/BaseRobotCmdDTO.java
index 5608f2b..06b53de 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/BaseRobotCmdDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/BaseRobotCmdDTO.java
@@ -3,11 +3,13 @@
 import io.swagger.annotations.ApiModelProperty;
 import lombok.Data;
 
+import java.io.Serializable;
+
 /**
  * 机器人指令基类
  */
 @Data
-public class BaseRobotCmdDTO {
+public class BaseRobotCmdDTO implements Serializable {
     @ApiModelProperty(value = "机器人主键", dataType = "String")
     protected Long robotId;
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/StartTrackRequest.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/StartTrackRequest.java
index a18321b..85546a7 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/StartTrackRequest.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/StartTrackRequest.java
@@ -3,6 +3,8 @@
 import io.swagger.annotations.ApiModelProperty;
 import lombok.Data;
 
+import java.math.BigDecimal;
+
 /**
  * 导航下发指令
  */
@@ -10,18 +12,19 @@
 public class StartTrackRequest extends BaseRobotCmdDTO {
     @ApiModelProperty(value = "sn编码(可不填 默认机器ID)", dataType = "String")
     private String sn;
-
     @ApiModelProperty(value = "下发状态 1:开启 0:关闭", dataType = "String")
     private Integer action;
     @ApiModelProperty(value = "等待状态 1:等待 0:不等待", dataType = "String")
     private Integer wait;
     @ApiModelProperty(value = "等待状态 0:循迹 1:任务", dataType = "isTask")
     private Integer isTask;
-    @ApiModelProperty(value = "轨迹名称", dataType = "String")
+    @ApiModelProperty(value = "轨迹名称(废弃)", dataType = "String")
     private String track_name;
     @ApiModelProperty(value = "关键点名称", dataType = "String", hidden = true)
     private String taskpoint_name;
-
+    @ApiModelProperty(value = "初始寻源步长", dataType = "Integer")
+    private Integer stepLen;
+    @ApiModelProperty(value = "寻源阈值", dataType = "BigDecimal")
+    private BigDecimal maxThreshold;
     private String obs_mode="1";
-
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/response/StopStateResponseDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/response/StopStateResponseDTO.java
index eb514bf..e0679d7 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/response/StopStateResponseDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/response/StopStateResponseDTO.java
@@ -3,11 +3,13 @@
 import io.swagger.annotations.ApiModelProperty;
 import lombok.Data;
 
+import java.io.Serializable;
+
 /**
  * 电压信息返回
  */
 @Data
-public class StopStateResponseDTO {
+public class StopStateResponseDTO implements Serializable {
     @ApiModelProperty(value = "软件急停(0：无触发 1：触发)" , dataType = "Integer")
     private Integer soft;
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IAlgorithmService.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IAlgorithmService.java
index 56b3e1d..f62523b 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IAlgorithmService.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IAlgorithmService.java
@@ -6,6 +6,7 @@
 import com.casic.missiles.modular.robot.dto.PathPlanDTO;
 import com.casic.missiles.modular.robot.model.RobotInfo;
 
+import java.io.File;
 import java.math.BigDecimal;
 import java.util.List;
 
@@ -38,5 +39,5 @@
      * @return
      */
     GridPointDTO convertGridCenterPoint(GridPointDTO pointDTO, BigDecimal resolution, RobotInfo robotInfo);
-    void readAlgorithmResponse(String fileName, AlgorithmResponse response, Long taskId, List<String> res);
+    void readAlgorithmResponse(String fileName, File repetitionFile, AlgorithmResponse response, Long taskId, List<String> res);
 }
\ No newline at end of file
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IRobotStatusInfoService.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IRobotStatusInfoService.java
index 2af210a..757f8d0 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IRobotStatusInfoService.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IRobotStatusInfoService.java
@@ -67,10 +67,9 @@
     /**
      * 更新车辆路线及任务信息
      *
-     * @param currRouteId 当前路线信息
      * @param currTaskId  当前任务信息
      */
-    void updateCurrTask(String robotId, Long currRouteId, Long currTaskId);
+    void updateCurrTask(String robotId, Long currTaskId);
 
     RobotStatusInfo getStatusByRobotId(String robotId);
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/ITaskGridInfoService.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/ITaskGridInfoService.java
new file mode 100644
index 0000000..fc0da93
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/ITaskGridInfoService.java
@@ -0,0 +1,22 @@
+package com.casic.missiles.modular.robot.service;
+
+import com.baomidou.mybatisplus.extension.service.IService;
+import  com.casic.missiles.modular.robot.model.TaskGridInfo;
+import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
+import com.baomidou.mybatisplus.extension.plugins.pagination.Page;
+import java.util.List;
+
+
+/**
+ * <p>
+ * 任务网格信息 服务类
+ * </p>
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+public interface ITaskGridInfoService extends IService<TaskGridInfo> {
+/**
+* 任务网格信息 分页检索
+*/
+List<TaskGridInfo> selectTaskGridInfoPage(Page<TaskGridInfo> page,QueryWrapper<TaskGridInfo> query);}
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/ITaskInfoService.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/ITaskInfoService.java
index 31d7ba6..b395993 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/ITaskInfoService.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/ITaskInfoService.java
@@ -51,6 +51,13 @@
      * @param dto 实体
      */
     void pointCallBack(PoseMessageResponseDTO dto, String robotId);
+
+    /**
+     * 车辆路线规划
+     * @param dto
+     * @param robotId
+     */
+    void carPathPlanning(PoseMessageResponseDTO dto, String robotId);
     void pointTestCallBack(PoseMessageResponseDTO dto, String robotId);
     /**
      * 图片路径保存
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/AlgorithmServiceImpl.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/AlgorithmServiceImpl.java
index f808fa0..a4dbe0b 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/AlgorithmServiceImpl.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/AlgorithmServiceImpl.java
@@ -29,10 +29,12 @@
 @Slf4j
 @Service
 public class AlgorithmServiceImpl implements IAlgorithmService {
-    @Value("${casic.algorithm.initPath:-1}")
-    private String initPath;
-    @Value("${casic.algorithm.planPath:-1}")
-    private String planPath;
+    @Value("${casic.algorithm.path:-1}")
+    private String path;
+    @Value("${casic.algorithm.initPathName:-1}")
+    private String initPathName;
+    @Value("${casic.algorithm.planPathName:-1}")
+    private String planPathName;
     private final ITaskHeatMapService heatMapService;
 
     public AlgorithmServiceImpl(ITaskHeatMapService heatMapService) {
@@ -42,13 +44,13 @@
     @Transactional
     @Override
     public AlgorithmResponse pathPlanningInit(PathInitDTO initDTO) {
-        if (StrUtil.isEmpty(initDTO.getCount())) {
-            initDTO.setCount("0.00");
+        if (StrUtil.isEmpty(initDTO.getZNow())) {
+            initDTO.setZNow("0.00");
         }
         log.info(JSON.toJSONString(initDTO));
         //算法路径及入参配置
         List<String> commends = new ArrayList<>();
-        commends.add(initPath);
+        commends.add(path + initPathName);
         commends.addAll(initDTO.toArray());
         List<String> res = AlgorithmUtils.invokeJob(commends);
 
@@ -63,38 +65,72 @@
     @Transactional
     @Override
     public AlgorithmResponse pathPlanning(PathPlanDTO planDTO) {
-        if (StrUtil.isEmpty(planDTO.getCount())) {
-            planDTO.setCount("0.001");
+        if (StrUtil.isEmpty(planDTO.getZNow())) {
+            planDTO.setZNow("0.001");
         }
         //算法路径及入参配置
         List<String> commends = new ArrayList<>();
-        commends.add(planPath);
+        commends.add(path + planPathName);
         commends.addAll(planDTO.toArray());
         log.info("寻源算法调用:{}", JSON.toJSONString(planDTO));
         List<String> res = AlgorithmUtils.invokeJob(commends);
         AlgorithmResponse response = getAlgorithmResponse(planDTO, res);
-        //next 点位编写
-        if (CollectionUtil.isNotEmpty(res) && res.size() == 2) {
-            response.getPointNextDTO().setX(formatBigDecimal(res.get(0).replaceAll("next_X: ", "")));
-            response.getPointNextDTO().setY(formatBigDecimal(res.get(1).replaceAll("next_Y: ", "")));
-        }
+
         return response;
     }
 
     private AlgorithmResponse getAlgorithmResponse(PathInitDTO initDTO, List<String> res) {
-        File repetitionFile = new File("repetition.txt");
+        //读取当前路径图片
+        File repetitionFileDir = new File(path + "\\" + initDTO.getTaskId() + "\\predicts");
+        File[] files = repetitionFileDir.listFiles((dir, name) -> name.endsWith(".txt") && name.startsWith("step"));
+        File repetitionFile = null;
+        //判定当前执行路径最大step2文件夹
+        if (files != null && files.length > 0) {
+             repetitionFile = files[files.length - 1];
+        }
         AlgorithmResponse response = new AlgorithmResponse();
         if (initDTO.getTaskId() != null && repetitionFile.exists()) {
-            readAlgorithmResponse("repetition.txt", response, initDTO.getTaskId(), res);
-            readAlgorithmResponse("step.txt", response, initDTO.getTaskId(), res);
+            //读取热力图
+            readAlgorithmResponse("repetition.txt",repetitionFile, response, initDTO.getTaskId(), res);
         } else {
             log.info("taskId is empty!");
         }
-        readAlgorithmResponse("xy_train.txt", response, initDTO.getTaskId(), res);
-        readAlgorithmResponse("z_train.txt", response, initDTO.getTaskId(), res);
+        //读取规划路径
+        if (CollectionUtil.isNotEmpty(res)) {
+            response.setEnd(false);
+            readAlgorithmResponse(response, res);
+        }
         return response;
     }
 
+    private void readAlgorithmResponse(AlgorithmResponse response, List<String> res) {
+        List<String> resList = new ArrayList<>();
+        if (res.size() >= 2) {
+            resList = res.subList(res.size() - 2, res.size());
+        }
+        //判定是否结束寻源
+        if (res.contains("end")) {
+            response.setEnd(true);
+            for (String s : resList) {
+                if (s.startsWith("source_coordinate:")) {
+                    s = s.trim();
+                    log.info("寻源结束,目标点位：{}",s);
+                    List<String> lists = StrUtil.split(s.replaceAll("source_coordinate:", ""), ",");
+                    PointNextDTO nextDTO = new PointNextDTO();
+                    nextDTO.setX(lists.get(0));
+                    nextDTO.setY(lists.get(1));
+                    response.setCoordinate(nextDTO);
+                }
+            }
+        } else {
+            PointNextDTO nextDTO = new PointNextDTO();
+            nextDTO.setX(resList.get(0));
+            nextDTO.setY(resList.get(1));
+            response.setPointNextDTO(nextDTO);
+        }
+
+    }
+
     /**
      * 读取算法输出数据
      *
@@ -102,49 +138,18 @@
      * @param response 结果集
      * @param taskId   任务ID
      */
-    public void readAlgorithmResponse(String fileName, AlgorithmResponse response, Long taskId, List<String> res) {
-        File file = new File(fileName);
-        if (!file.exists()&&!fileName.startsWith("step")) {
+    public void readAlgorithmResponse(String fileName,File repetitionFile, AlgorithmResponse response, Long taskId, List<String> res) {
+        if (!repetitionFile.exists() && !fileName.startsWith("step")) {
             log.info("{} file is empty!", fileName);
             return;
         }
         if (response.getPointNextDTO() == null) {
             response.setPointNextDTO(new PointNextDTO());
         }
-        switch (fileName) {
-            case "repetition.txt":
-                String msg = FileUtil.readString(file, CharsetUtil.CHARSET_UTF_8);
-                TaskHeatMap taskHeatMap = createEmptyTaskHeatMap(taskId);
-                taskHeatMap.setRepetition(msg);
-                response.setTaskHeatMap(taskHeatMap);
-                break;
-            case "xy_train.txt":
-                List<String> xyTrains = FileUtil.readLines(file, CharsetUtil.CHARSET_UTF_8);
-                if (CollectionUtil.isNotEmpty(xyTrains)) {
-                    if (CollectionUtil.isNotEmpty(xyTrains)) {
-                        List<String> list = StrUtil.split(xyTrains.get(xyTrains.size() - 1), " ");
-                        response.getPointNextDTO().setX(formatBigDecimal(list.get(0)));
-                        response.getPointNextDTO().setY(formatBigDecimal(list.get(1)));
-                    }
-                }
-                break;
-            case "z_train.txt":
-                List<String> zTrains = FileUtil.readLines(file, CharsetUtil.CHARSET_UTF_8);
-                if (CollectionUtil.isNotEmpty(zTrains)) {
-                    String zTrain = zTrains.get(zTrains.size() - 1);
-                    response.getPointNextDTO().setZ(formatBigDecimal(zTrain));
-                }
-            default:
-                if (StrUtil.isNotEmpty(fileName) && fileName.startsWith("step")) {
-                    File fileDir = new File(".");
-                    File[] files = fileDir.listFiles((dir, name) -> name.endsWith(".txt") && name.startsWith("step"));
-                    if (files != null && files.length > 0) {
-                        String steps = FileUtil.readString(files[files.length - 1], CharsetUtil.CHARSET_UTF_8);
-                        response.getTaskHeatMap().setRepetition(steps);
-                    }
-                }
-                break;
-        }
+        String msg = FileUtil.readString(repetitionFile, CharsetUtil.CHARSET_UTF_8);
+        TaskHeatMap taskHeatMap = createEmptyTaskHeatMap(taskId);
+        taskHeatMap.setRepetition(msg);
+        response.setTaskHeatMap(taskHeatMap);
     }
 
     /**
@@ -157,13 +162,11 @@
     public GridPointDTO convertSlamToGrid(BigDecimal x, BigDecimal y, BigDecimal resolution, RobotInfo robotInfo) {
         GridPointDTO pointDTO = new GridPointDTO();
         BigDecimal scale = resolution;
-
         //减去栅格原点坐标
         x = x.subtract(robotInfo.getOriginX());
         y = y.subtract(robotInfo.getOriginY());
-
-        pointDTO.setX(new BigDecimal(x.divide(scale,3,BigDecimal.ROUND_HALF_UP).intValue()+1));
-        pointDTO.setY(new BigDecimal(y.divide(scale,3,BigDecimal.ROUND_HALF_UP).intValue()+1));
+        pointDTO.setX(new BigDecimal(x.divide(scale, 3, BigDecimal.ROUND_HALF_UP).intValue()+1));
+        pointDTO.setY(new BigDecimal(y.divide(scale, 3, BigDecimal.ROUND_HALF_UP).intValue() +1));
         return pointDTO;
     }
 
@@ -174,19 +177,17 @@
      * @return
      */
     public GridPointDTO convertGridCenterPoint(GridPointDTO pointDTO, BigDecimal resolution, RobotInfo robotInfo) {
-
         GridPointDTO centerPoint = new GridPointDTO();
         BigDecimal scale = resolution;
-        //点位转换
-        BigDecimal slamX = pointDTO.getX().divide(scale, 5, BigDecimal.ROUND_HALF_UP);
-        BigDecimal slamY = pointDTO.getY().divide(scale, 5, BigDecimal.ROUND_HALF_UP);
+        //点位转换 转换后栅格－1 从1开始计算
+        BigDecimal slamX = (pointDTO.getX().subtract(new BigDecimal(1))).multiply(scale);
+        BigDecimal slamY = (pointDTO.getY().subtract(new BigDecimal(1))).multiply(scale);
         //中心栅格点位中心添加
-        centerPoint.setX(slamX.add(scale.divide(new BigDecimal(2), 5, BigDecimal.ROUND_HALF_UP)));
-        centerPoint.setY(slamY.add(scale.divide(new BigDecimal(2), 5, BigDecimal.ROUND_HALF_UP)));
+        centerPoint.setX(slamX.add(scale.divide(new BigDecimal(2), 10, BigDecimal.ROUND_HALF_UP)));
+        centerPoint.setY(slamY.add(scale.divide(new BigDecimal(2), 10, BigDecimal.ROUND_HALF_UP)));
         //加上栅格原点坐标
         centerPoint.setX(centerPoint.getX().add(robotInfo.getOriginX()));
         centerPoint.setY(centerPoint.getY().add(robotInfo.getOriginY()));
-
         return centerPoint;
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotInfoServiceImpl.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotInfoServiceImpl.java
index 043e05d..fdded64 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotInfoServiceImpl.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotInfoServiceImpl.java
@@ -104,7 +104,7 @@
             //下发障碍物栅格
             sendCostMapResolution(robotInfo.getResolution(), robotInfo.getId());
             //重新生成栅格地图
-            sendChangePcd(robotInfo.getId());
+            //sendChangePcd(robotInfo.getId());
         }
         update.eq(RobotInfo::getId, robotInfo.getId());
         update(update);
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotStatusInfoServiceImpl.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotStatusInfoServiceImpl.java
index 6d3ae91..46b5c3b 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotStatusInfoServiceImpl.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotStatusInfoServiceImpl.java
@@ -15,6 +15,7 @@
 import com.casic.missiles.modular.robot.dao.RobotStatusInfoMapper;
 import com.casic.missiles.modular.robot.model.RobotInfo;
 import com.casic.missiles.modular.robot.model.RobotStatusInfo;
+import com.casic.missiles.modular.robot.opt.enums.InstructCodeEnums;
 import com.casic.missiles.modular.robot.opt.instruct.base.dto.RobotMsgDTO;
 import com.casic.missiles.modular.robot.opt.instruct.base.dto.RobotMsgResponseDTO;
 import com.casic.missiles.modular.robot.opt.instruct.base.dto.RobotResponseDTO;
@@ -26,13 +27,18 @@
 import com.casic.missiles.modular.robot.service.IRobotInfoService;
 import com.casic.missiles.modular.robot.service.IRobotStatusInfoService;
 import lombok.extern.slf4j.Slf4j;
+import org.aspectj.util.FileUtil;
 import org.eclipse.paho.client.mqttv3.MqttMessage;
+import org.springframework.beans.factory.annotation.Value;
 import org.springframework.context.annotation.Lazy;
 import org.springframework.stereotype.Service;
 import org.springframework.transaction.annotation.Transactional;
 
+import java.io.File;
 import java.io.Serializable;
 import java.math.BigDecimal;
+import java.util.ArrayList;
+import java.util.Arrays;
 import java.util.List;
 
 
@@ -147,7 +153,6 @@
     @Override
     public void saveProcessMessage(String robotId, MqttMessage message) {
 //        String json = new String(message.getPayload());
-
         RobotResponseDTO<ProcessMessageResponseDTO> responseDTO = parseJson(message, new TypeReference<RobotResponseDTO<ProcessMessageResponseDTO>>() {
         });
         if (responseDTO.getMsg() != null) {
@@ -189,9 +194,8 @@
     }
 
     @Override
-    public void updateCurrTask(String robotId, Long currRouteId, Long currTaskId) {
+    public void updateCurrTask(String robotId, Long currTaskId) {
         UpdateWrapper<RobotStatusInfo> query = new UpdateWrapper<>();
-        query.set("curr_route_id", currRouteId);
         query.set("curr_task_id", currTaskId);
         query.eq("robot_id", robotId);
         update(query);
@@ -250,6 +254,15 @@
         }
     }
 
+    @Value("${casic.algorithm.path:-1}")
+    private String path;
+
+    /**
+     * 栅格阈值
+     */
+    @Value("${casic.algorithm.num:30}")
+    private Integer num;
+
     /**
      * 机器人栅格数据
      *
@@ -262,15 +275,41 @@
         RobotResponseDTO<AppCostmapResponseDTO> responseDTO = parseJson(message, new TypeReference<RobotResponseDTO<AppCostmapResponseDTO>>() {
         });
         if (responseDTO != null && responseDTO.getMsg() != null) {
+            //发送栅格分辨率设定
+
             cacheService.updateCostMap(robotId, responseDTO.getMsg());
             robotInfoService.updateRobotGrid(robotId, responseDTO.getMsg());
-//            log.info("--------------- json data:{}", JSON.toJSONString(responseDTO.getMsg()));
+            //log.info("--------------- json data:{}", JSON.toJSONString(responseDTO.getMsg()));
+            if (responseDTO != null && responseDTO.getMsg().getData() != null) {
+                ArrayList<Integer> dataNums = new ArrayList<>();
+                //根据阈值转换栅格
+                for (Integer dataNum : responseDTO.getMsg().getData()) {
+                    if (dataNum != -1) {
+                        if (dataNum >= num) {
+                            dataNums.add(0);
+                        } else {
+                            dataNums.add(1);
+                        }
+                    }
+                }
+                writeCache(dataNums,responseDTO.getMsg().getWidth(),responseDTO.getMsg().getHeight(),robotId);
+            }
         }
     }
 
+    private void writeCache(ArrayList<Integer> integers,Integer width,Integer height,String robotId) {
+        List<List<Integer>> arrays = CollectionUtil.split(integers,width);
+        ArrayList<ArrayList<Integer>> arrayList = new ArrayList<>();
+        for (List<Integer> array : arrays) {
+            arrayList.add((ArrayList)array);
+        }
+        CollectionUtil.reverse(arrayList);
+        CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_COST_MAP + robotId, arrayList);
+//        FileUtil.writeAsString(new File(path + "map.txt"), CollectionUtil.join(integers, " ").replaceAll("\\[", "").replaceAll("]", ""));
+    }
+
     @Override
     public void updateRobotPoint(PoseMessageResponseDTO dto, String robotId) {
-        //todo cache
         UpdateWrapper<RobotStatusInfo> query = new UpdateWrapper<>();
         query.set("postion_x", dto.getX());
         query.set("postion_y", dto.getY());
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotTrajectoryInfoServiceImpl.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotTrajectoryInfoServiceImpl.java
index 6c9625f..f1262a2 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotTrajectoryInfoServiceImpl.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotTrajectoryInfoServiceImpl.java
@@ -57,29 +57,29 @@
         });
 
         //读取导航状态,如果导航未开启则不存储
-//        RobotStatusInfo statusInfo = robotStatusInfoService.getStatusByRobotId(robotId);
-        Long routeId= cacheService.readRouteId(Long.valueOf(robotId));
-        if (routeId != null && responseDTO.getMsg() != null) {
+        //RobotStatusInfo statusInfo = robotStatusInfoService.getStatusByRobotId(robotId);
+
+        if ( responseDTO.getMsg() != null) {
             PoseMessageResponseDTO dto = responseDTO.getMsg();
             //执行循迹 保存当前轨迹信息
             //更新实时定位信息
             CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.ROBOT_POINT + robotId, dto);
-            infoService.pointTestCallBack(dto, robotId);
-            if (routeId != null) {
-                RobotTrajectoryInfo info = new RobotTrajectoryInfo();
-                info.setRobotId(Long.valueOf(robotId));
-                info.setCreateTime(new Date());
-                info.setPostionX(dto.getX());
-                info.setPostionY(dto.getY());
-                info.setPostionZ(dto.getZ());
-                info.setTheta(dto.getTheta());
-                save(info);
-                try {
-                    infoService.pointCallBack(dto, robotId);
-                } catch (Exception e) {
-                    log.error("task error", e);
-                }
+            try {
+                infoService.carPathPlanning(dto, robotId);
+            } catch (Exception e) {
+                log.error("task error", e);
             }
+//            if (routeId != null) {
+//                RobotTrajectoryInfo info = new RobotTrajectoryInfo();
+//                info.setRobotId(Long.valueOf(robotId));
+//                info.setCreateTime(new Date());
+//                info.setPostionX(dto.getX());
+//                info.setPostionY(dto.getY());
+//                info.setPostionZ(dto.getZ());
+//                info.setTheta(dto.getTheta());
+//                save(info);
+
+//            }
         }
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/TaskGridInfoServiceImpl.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/TaskGridInfoServiceImpl.java
new file mode 100644
index 0000000..cf61def
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/TaskGridInfoServiceImpl.java
@@ -0,0 +1,28 @@
+package com.casic.missiles.modular.robot.service.impl;
+
+import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
+import com.baomidou.mybatisplus.extension.plugins.pagination.Page;
+import com.baomidou.mybatisplus.extension.service.impl.ServiceImpl;
+import com.casic.missiles.modular.robot.dao.TaskGridInfoMapper;
+import com.casic.missiles.modular.robot.model.TaskGridInfo;
+import com.casic.missiles.modular.robot.service.ITaskGridInfoService;
+import org.springframework.stereotype.Service;
+
+import java.util.List;
+
+
+/**
+ * <p>
+ * 任务网格信息 服务实现类
+ * </p>
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+@Service
+public class TaskGridInfoServiceImpl extends ServiceImpl<TaskGridInfoMapper, TaskGridInfo> implements ITaskGridInfoService {
+    @Override
+    public List<TaskGridInfo> selectTaskGridInfoPage(Page<TaskGridInfo> page, QueryWrapper<TaskGridInfo> query) {
+        return this.baseMapper.selectTaskGridInfoPage(page, query);
+    }
+}
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/TaskInfoServiceImpl.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/TaskInfoServiceImpl.java
index d1417b0..fd49e2e 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/TaskInfoServiceImpl.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/TaskInfoServiceImpl.java
@@ -6,10 +6,10 @@
 import cn.hutool.core.convert.Convert;
 import cn.hutool.core.date.DateUtil;
 import cn.hutool.core.io.FileUtil;
-import cn.hutool.core.thread.ExecutorBuilder;
 import cn.hutool.core.util.StrUtil;
 import com.alibaba.fastjson.JSON;
 import com.baomidou.mybatisplus.core.conditions.Wrapper;
+import com.baomidou.mybatisplus.core.conditions.query.LambdaQueryWrapper;
 import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
 import com.baomidou.mybatisplus.core.conditions.update.UpdateWrapper;
 import com.baomidou.mybatisplus.core.metadata.IPage;
@@ -47,7 +47,6 @@
 import java.io.Serializable;
 import java.math.BigDecimal;
 import java.util.*;
-import java.util.concurrent.ThreadPoolExecutor;
 
 
 /**
@@ -70,6 +69,7 @@
     private final IRobotCacheService cacheService;
     private final IBaseRobotService baseRobotService;
     private final ITaskInfoImgService imgService;
+    private final ITaskGridInfoService taskGridInfoService;
     /**
      * 初始化线程池
      */
@@ -79,12 +79,19 @@
     @Value("${casic.doc.temp}")
     private String docTemp;
     /**
+     * 5000 ms
+     */
+    @Value("${casic.task.duration:3000}")
+    private Integer stayDuration;
+    @Value("${casic.algorithm.path:-1}")
+    private String path;
+    /**
      * 障碍物精度
      */
     @Value("${casic.obstacle:10}")
     private Integer obstacle;
 
-    public TaskInfoServiceImpl(@Lazy IRouteInfoService routeInfoService, IRobotStatusInfoService statusInfoService, AbstractDictService dictService, @Lazy IAlarmRecordService recordService, @Lazy ITaskHeatMapService taskHeatMapService, IAlgorithmService algorithmService, IRobotCacheService cacheService, @Lazy IBaseRobotService baseRobotService, ITaskInfoImgService imgService) {
+    public TaskInfoServiceImpl(@Lazy IRouteInfoService routeInfoService, IRobotStatusInfoService statusInfoService, AbstractDictService dictService, @Lazy IAlarmRecordService recordService, @Lazy ITaskHeatMapService taskHeatMapService, IAlgorithmService algorithmService, IRobotCacheService cacheService, @Lazy IBaseRobotService baseRobotService, ITaskInfoImgService imgService, ITaskGridInfoService taskGridInfoService) {
         this.routeInfoService = routeInfoService;
         this.statusInfoService = statusInfoService;
         this.dictService = dictService;
@@ -94,6 +101,7 @@
         this.cacheService = cacheService;
         this.baseRobotService = baseRobotService;
         this.imgService = imgService;
+        this.taskGridInfoService = taskGridInfoService;
     }
 
     /**
@@ -184,7 +192,7 @@
         if (CollectionUtil.isEmpty(routeInfos)) {
             throw new ServiceException(500, "路线不存在,请及时更新");
         }
-        RouteInfo routeInfo = routeInfos.get(0);
+//        RouteInfo routeInfo = routeInfos.get(0);
         Integer isTask = 0;
         //开启寻源任务
         if (request.getIsTask() == 1) {
@@ -193,22 +201,21 @@
         RobotInfo robotInfo = cacheService.getRobotInfo(Convert.toLong(robotId));
         CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_IS_TASK + robotId, isTask);
         //添加任务记录
-        TaskInfo taskInfo = addTaskByStartTrack(routeInfo, robotInfo);
+        TaskInfo taskInfo = addTaskByStartTrack( robotInfo, request);
         //No.3 更新器人执行任务信息
-        Long currRouteId = routeInfo.getId();
+//        Long currRouteId = routeInfo.getId();
         Long currTaskId = taskInfo.getId();
 
-        statusInfoService.updateCurrTask(robotId, currRouteId, currTaskId);
+        statusInfoService.updateCurrTask(robotId,  currTaskId);
         //算法初始化标志创建
         CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_TASK_ID + robotId, currTaskId);
         CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_TASK_INIT + robotId, 0);
-        CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_ROUTE_ID + robotId, routeInfo.getId());
+//        CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_ROUTE_ID + robotId, routeInfo.getId());
     }
 
     private void closeTask(StartTrackRequest request, String robotId) {
         //关闭操作
         RobotStatusInfo statusInfo = statusInfoService.getStatusByRobotId(robotId);
-
         if (statusInfo.getCurrTaskId() != null) {
             UpdateWrapper<TaskInfo> updateWrapper = new UpdateWrapper<>();
             updateWrapper.set("update_time", new Date());
@@ -221,10 +228,11 @@
 
         //清空任务缓存
         robotCacheClean(robotId);
-
+        //清楚规划任务锁
+        CacheUtil.remove(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_PLANING_FLAG + robotId);
+        CacheUtil.remove(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_ESTIMATE_GRID+ robotId);
         //清空机器当前任务
-        statusInfoService.updateCurrTask(robotId, null, null);
-
+        statusInfoService.updateCurrTask(robotId, null);
     }
 
     /**
@@ -249,7 +257,6 @@
 
         //判定任务是否已经完成初始化
         Long longId = Convert.toLong(robotId);
-        Long taskId = CacheUtil.get(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_TASK_ID + robotId);
         //缓存当前点位信息
         CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_XY_POINT + robotId, dto);
         RobotInfo robotInfo = cacheService.getRobotInfo(longId);
@@ -260,86 +267,203 @@
             log.error("no config 中子源阈值未配置");
             return;
         }
-
         //读取是否是循迹任务
         Integer isTask = cacheService.readIsTask(longId);
         if (isTask != 1) {
             log.info("循迹任务,不做处理");
             return;
         }
-        //算法初始化状态读取
-        Integer initValue = cacheService.readTaskInit(longId);
-
 
         //No.1 任务初始化
+//        pathInit(longId, robotInfo, pointDTO, taskId, robotId);
+    }
+
+    @Override
+    public void carPathPlanning(PoseMessageResponseDTO dto, String robotId) {
+        //判定任务是否已经完成初始化
+        Long longId = Convert.toLong(robotId);
+        Long taskId = CacheUtil.get(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_TASK_ID + robotId);
+        //缓存当前点位信息
+        CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_XY_POINT + robotId, dto);
+        RobotInfo robotInfo = cacheService.getRobotInfo(longId);
+        //slam经纬度转栅格
+        GridPointDTO pointDTO = algorithmService.convertSlamToGrid(dto.getX(), dto.getY(), robotInfo.getResolution(), robotInfo);
+        if (robotInfo.getAlarmThreshold() == null || robotInfo.getMaxThreshold() == null) {
+            log.error("no config 中子源阈值未配置");
+            return;
+        }
+
+        //读取是否是寻源任务
+        Integer isTask = cacheService.readIsTask(longId);
+        if (isTask != 1) {
+            log.info("循迹任务,不做处理");
+            return;
+        }
+
+        TaskInfo taskInfo = getById(taskId);
+        //No.1 任务初始化
+        pathInit(longId, robotInfo, pointDTO, taskId, robotId, taskInfo.getStepLen());
+        //No.2 路线规划
+        pathPlaning(longId, robotInfo, pointDTO, taskId, robotId, taskInfo,dto);
+    }
+
+    /**
+     * 路线规划进行加锁
+     *
+     * @param robotId 机器人ID
+     * @return
+     */
+    private synchronized boolean lockPlanIngFlag(String robotId) {
+        Boolean flag = CacheUtil.get(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_PLANING_FLAG + robotId);
+        if (flag == null) {
+            CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_PLANING_FLAG + robotId, true);
+            return true;
+        }
+        return false;
+    }
+
+    /**
+     * @param longId    机器人ID
+     * @param robotInfo 机器人信息
+     * @param pointDTO  当前点位栅格信息
+     * @param taskId    任务ID
+     * @param robotId   机器人ID
+     */
+    private void pathPlaning(Long longId, RobotInfo robotInfo, GridPointDTO pointDTO, Long taskId, String robotId, TaskInfo taskInfo,PoseMessageResponseDTO curPoint) {
+        if (taskId != null) {
+            //规划节点
+            PointNextDTO pointNextDTO = CacheUtil.get(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_ESTIMATE_GRID + robotId);
+            GridPointDTO oldEstimateGrid = GridPointDTO.convertGridPoint(pointNextDTO);
+            Boolean flag = lockPlanIngFlag(robotId);
+            //是否到达预期栅格
+            if (!flag) {
+                log.info("未获取到规划任务,直接结束：{}",pointDTO);
+                return;
+            }
+            try {
+                log.debug("oldEstimateGrid:{}", oldEstimateGrid);
+                if (pointNextDTO == null || isEquals(pointDTO, oldEstimateGrid)) {
+                    log.info("当前栅格变更......", pointDTO);
+                    CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_POINT + robotId, pointDTO);
+                    //算法调用获取热力图
+                    PathPlanDTO pathPlanDTO = createPlanDTO(robotInfo, pointDTO, taskId, robotId, taskInfo.getStepLen());
+                    if (taskInfo.getMaxThreshold() == null) {
+                        taskInfo.setMaxThreshold(robotInfo.getMaxThreshold());
+                    }
+                    pathPlanDTO.setTh(taskInfo.getMaxThreshold() + "");
+
+                    log.debug("heat map -- robotId:{},taskId:{}", robotId, taskId);
+                    pathPlanDTO.setZNow(cacheService.getNeutronCount(longId));
+
+                    //存储算法输出栅格信息
+                    AlgorithmResponse response = algorithmService.pathPlanning(pathPlanDTO);
+                    CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_ESTIMATE_GRID + robotId, response.getPointNextDTO());
+                    sendWebSocket(InstructStatusMsgEnums.HEAT_MAP, robotId, response.getTaskHeatMap());
+
+                    //存储循迹任务信息
+                    TaskGridInfo taskGridInfo = new TaskGridInfo();
+                    taskGridInfo.setCreateTime(new Date());
+                    taskGridInfo.setUpdateTime(new Date());
+                    taskGridInfo.setCurGridX(pointDTO.getX().intValue());
+                    taskGridInfo.setCurGridY(pointDTO.getY().intValue());
+                    taskGridInfo.setNeutronCount(new BigDecimal(pathPlanDTO.getZNow()));
+                    taskGridInfo.setEstimateGridX(response.getPointNextDTO().getX());
+                    taskGridInfo.setEstimateGridY(response.getPointNextDTO().getY());
+                    taskGridInfo.setTaskId(taskId);
+                    taskGridInfoService.save(taskGridInfo);
+
+                    try {
+                        //停留时间阈值
+                        Thread.sleep(stayDuration);
+                    } catch (InterruptedException e) {
+                        e.printStackTrace();
+                    }
+
+                    //是否触发停止状态
+                    if (!response.isEnd()) {
+                        log.debug("task planning-- xun dian robotId:{},taskId:{}", robotId, taskId);
+                        //机器人寻点指令下发
+                        sendTargetPoint(robotInfo, response,curPoint);
+                    } else {
+                        log.debug("task stop-- robotId:{},taskId:{}", robotId, taskId);
+                        //机器人停止指令下发
+                        SoftwareStopRequest request = new SoftwareStopRequest();
+                        request.setRobotId(robotInfo.getId());
+                        //设置为急停
+                        request.setControl(1);
+//                        baseRobotService.execCmdHandler(request, InstructCodeEnums.SOFTWARE_STOP);
+                        //循迹任务结束
+                        closeTask(null, robotId);
+                    }
+                }
+            } catch (Exception e) {
+                e.printStackTrace();
+            } finally {
+                //释放锁
+                CacheUtil.remove(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_PLANING_FLAG + robotId);
+            }
+        }
+    }
+
+    private PathPlanDTO createPlanDTO(RobotInfo robotInfo, GridPointDTO pointDTO, Long taskId, String robotId, Integer stepLen) {
+        PathPlanDTO pathPlanDTO = new PathPlanDTO();
+        pathPlanDTO.setTaskId(taskId);
+        pathPlanDTO.setRobotId(robotId);
+        pathPlanDTO.setStepLen(stepLen);
+        return pathPlanDTO;
+    }
+
+    /**
+     * 寻源初始化
+     *
+     * @param longId    机器ID
+     * @param robotInfo 机器信息
+     * @param pointDTO  当前点位信息
+     * @param taskId    任务ID
+     * @param robotId   机器ID
+     * @param stepLen   步长设置
+     */
+    private void pathInit(Long longId, RobotInfo robotInfo, GridPointDTO pointDTO, Long taskId, String robotId, Integer stepLen) {
+        //障碍物栅格上传已生成 直接读取
+        //算法初始化状态读取
+        Integer initValue = cacheService.readTaskInit(longId);
+        //No.1 任务初始化
         if (initValue != 1) {
+
             //执行初始化操作
             if (cacheService.updateTaskInit(longId, 1)) {
 
-                try {
-                    File file = new File(".");
-                    File[] files = file.listFiles((dir, name) -> (name.endsWith(".png") && name.startsWith("step"))
-                            || (name.endsWith(".txt") && name.startsWith("step")) || name.equals("repetition.txt") || name.equals("xy_train.txt") || name.equals("z_train.txt"));
-                    if (files != null) {
-                        for (int i = 0; i < files.length; i++) {
-                            FileUtil.del(files[i]);
+                List<List<Integer>> costList = CacheUtil.get(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_COST_MAP + robotId);
+                List<String> strs = new ArrayList<>();
+                List<String> strs2 = new ArrayList<>();
+                for (List<Integer> list : costList) {
+                    strs2.add(CollectionUtil.join(list, " "));
+                }
+                FileUtil.writeLines(strs2, new File(path + "map2.txt"), "utf-8");
+                //行转列 为算法输入数据
+                if(CollectionUtil.isNotEmpty(costList)){
+                    CollectionUtil.reverse(costList);
+                    for (int i = 0; i < costList.get(0).size(); i++) {
+                        ArrayList<Integer> ints = new ArrayList<>();
+                        for (List<Integer> list : costList) {
+                            ints.add(list.get(i));
                         }
+                        strs.add(CollectionUtil.join(ints, " "));
                     }
-                } catch (Exception e) {
-                    e.printStackTrace();
+//                    CollectionUtil.reverse(strs);
                 }
 
-                PathInitDTO pathInitDTO = createInitDTO(robotInfo, pointDTO, taskId, robotId);
+
+                FileUtil.writeLines(strs, new File(path + "map.txt"), "utf-8");
+
+                PathInitDTO pathInitDTO = createInitDTO(robotInfo, pointDTO, taskId, robotId, stepLen);
                 //中子源计数率读取
-                pathInitDTO.setCount(cacheService.getNeutronCount(longId));
+                pathInitDTO.setZNow(cacheService.getNeutronCount(longId));
+                log.info("init params:{}", JSON.toJSONString(pathInitDTO));
                 //执行1次算法 初始化操作
                 algorithmService.pathPlanningInit(pathInitDTO);
             }
         }
-
-        //No.2 任务执行中
-        if (taskId != null) {
-            //判断栅格变化
-            GridPointDTO oldPointDTO = CacheUtil.get(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_POINT + robotId);
-            if (oldPointDTO == null || isChange(pointDTO, oldPointDTO)) {
-                log.debug("grid change -- robotId:{},taskId:{}", robotId, taskId);
-                CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_POINT + robotId, pointDTO);
-                //算法调用获取热力图
-                PathPlanDTO pathPlanDTO = createPlanDTO(robotInfo, pointDTO, taskId, robotId);
-                //可通行标记为1 不可通行标记为0
-                String obstacle = getObstacle(robotId, pointDTO);
-                //TO-DO 障碍物判定信息
-                pathPlanDTO.setOpen(obstacle);
-                //中子源计数率读取
-                pathPlanDTO.setCount(cacheService.getNeutronCount(longId));
-                log.debug("heat map -- robotId:{},taskId:{}", robotId, taskId);
-                AlgorithmResponse response = algorithmService.pathPlanning(pathPlanDTO);
-                //存储算法输出栅格信息
-                CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_ESTIMATE_GRID + robotId, response.getPointNextDTO());
-                sendWebSocket(InstructStatusMsgEnums.HEAT_MAP, robotId, response.getTaskHeatMap());
-                //是否触发寻源操作
-                BigDecimal bigCount = new BigDecimal(pathPlanDTO.getCount());
-                if (bigCount.compareTo(robotInfo.getAlarmThreshold()) >= 0) {
-                    log.debug("task planning-- xun dian robotId:{},taskId:{}", robotId, taskId);
-                    //机器人寻点指令下发
-                    sendTargetPoint(robotInfo, response);
-                }
-
-                //是否触发停止状态
-                if (bigCount.compareTo(robotInfo.getMaxThreshold()) > 0) {
-                    log.debug("task stop-- robotId:{},taskId:{}", robotId, taskId);
-                    //机器人停止指令下发
-                    SoftwareStopRequest request = new SoftwareStopRequest();
-                    request.setRobotId(robotInfo.getId());
-                    //设置为急停
-                    request.setControl(1);
-                    baseRobotService.execCmdHandler(request, InstructCodeEnums.SOFTWARE_STOP);
-                    //循迹任务结束
-                    closeTask(null, robotId);
-                }
-
-            }
-        }
     }
 
     /**
@@ -353,7 +477,6 @@
         //判定任务是否已经完成初始化
         Long longId = Convert.toLong(robotId);
         Long taskId = CacheUtil.get(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_TASK_ID + robotId);
-
         RobotInfo robotInfo = cacheService.getRobotInfo(longId);
         //slam经纬度转栅格
         GridPointDTO pointDTO = algorithmService.convertSlamToGrid(dto.getX(), dto.getY(), robotInfo.getResolution(), robotInfo);
@@ -428,7 +551,7 @@
      * @param robotInfo 机器人信息
      * @param response  算法输出数据
      */
-    private void sendTargetPoint(RobotInfo robotInfo, AlgorithmResponse response) {
+    private void sendTargetPoint(RobotInfo robotInfo, AlgorithmResponse response,PoseMessageResponseDTO curPoint) {
         //读取已设置巡点目标信息
         PointNextDTO nextDTO = CacheUtil.get(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_TARGET_POINT_ID + robotInfo.getId());
         //巡点
@@ -439,8 +562,8 @@
             TargetPointRequest request = new TargetPointRequest();
             request.setX(Convert.toStr(gridCenterPoint.getX()));
             request.setY(Convert.toStr(gridCenterPoint.getY()));
-            request.setZ("0");
-
+            request.setZ(curPoint.getZ().toString());
+            request.setTheta(curPoint.getTheta().toString());
             request.setRobotId(robotInfo.getId());
             //执行巡点指令
             baseRobotService.execCmdHandler(request, InstructCodeEnums.SET_TARGETPOINT);
@@ -507,21 +630,20 @@
         params.put("startTime", startTime);
         params.put("endTime", endTime);
         //读取
-        List<AlarmRecord> records = recordService.selectListByTaskId(info.getId(), info.getRobotId());
         Integer index = 1;
-
-        if (CollectionUtil.isNotEmpty(records)) {
-            for (AlarmRecord record : records) {
+        LambdaQueryWrapper<TaskGridInfo> taskGridQuery = new LambdaQueryWrapper<>();
+        taskGridQuery.eq(TaskGridInfo::getTaskId,info.getId());
+        taskGridQuery.orderByAsc(TaskGridInfo::getCreateTime);
+        List<TaskGridInfo> taskGridInfos = taskGridInfoService.list(taskGridQuery);
+        if (CollectionUtil.isNotEmpty(taskGridInfos)) {
+            for (TaskGridInfo record : taskGridInfos) {
                 //栅格信息转换
-                GridPointDTO pointDTO = algorithmService.convertSlamToGrid(record.getX(), record.getY(), info.getResolution(), robotInfo);
-                record.setX(pointDTO.getX());
-                record.setY(pointDTO.getY());
                 record.setIndex(index);
                 index++;
             }
         }
 
-        params.put("records", records);
+        params.put("records", taskGridInfos);
         ImageEntity mcaImg = new ImageEntity();
         mcaImg.setHeight(600);
         mcaImg.setWidth(600);
@@ -557,7 +679,6 @@
             psdImg.setData(WordUtil.getImageBase64(uploadPath + info.getPsdImg()));
         }
         params.put("psdImg", psdImg);
-
         WordUtil.exportOutputStream(docTemp, response.getOutputStream(), params);
     }
 
@@ -589,7 +710,7 @@
     private String saveHeatImg(String taskId) {
         String newFileName = "task\\" + taskId + ".png";
         //读取当前路径图片
-        File file = new File(".");
+        File file = new File(path + "\\" + taskId + "\\figs");
         File[] files = file.listFiles((dir, name) -> name.endsWith(".png") && name.startsWith("step"));
         //判定当前执行路径最大step图片
         if (files != null && files.length > 0) {
@@ -614,33 +735,28 @@
         return true;
     }
 
-    private PathPlanDTO createPlanDTO(RobotInfo robotInfo, GridPointDTO pointDTO, Long taskId, String robotId) {
-        PathPlanDTO pathPlanDTO = new PathPlanDTO();
-        String xMax = robotInfo.getGridHeight().divide(robotInfo.getResolution(), 5, BigDecimal.ROUND_HALF_UP).intValue() + "";
-        String yMax = robotInfo.getGridWidth().divide(robotInfo.getResolution(), 5, BigDecimal.ROUND_HALF_UP).intValue() + "";
-        pathPlanDTO.setX(pointDTO.getX().intValue() + "");
-        pathPlanDTO.setY(pointDTO.getY().intValue() + "");
-        pathPlanDTO.setXMax(xMax);
-        pathPlanDTO.setYMax(yMax);
-        pathPlanDTO.setTaskId(taskId);
-        pathPlanDTO.setRobotId(robotId);
-        return pathPlanDTO;
+    private boolean isEquals(GridPointDTO pointDTO, GridPointDTO oldPointDTO) {
+        if(oldPointDTO==null||oldPointDTO.getX()==null){
+            return false;
+        }
+        if (pointDTO.getX().compareTo(oldPointDTO.getX()) == 0 && pointDTO.getY().compareTo(oldPointDTO.getY()) == 0) {
+            return true;
+        }
+        return false;
     }
 
-    private PathInitDTO createInitDTO(RobotInfo robotInfo, GridPointDTO pointDTO, Long taskId, String robotId) {
-        String xMax = robotInfo.getGridHeight().divide(robotInfo.getResolution(), 5, BigDecimal.ROUND_HALF_UP).intValue() + "";
-        String yMax = robotInfo.getGridWidth().divide(robotInfo.getResolution(), 5, BigDecimal.ROUND_HALF_UP).intValue() + "";
+
+    private PathInitDTO createInitDTO(RobotInfo robotInfo, GridPointDTO pointDTO, Long taskId, String robotId, Integer stepLen) {
         PathInitDTO pathInitDTO = new PathInitDTO();
-        pathInitDTO.setXMax(xMax);
-        pathInitDTO.setYMax(yMax);
-        pathInitDTO.setX(pointDTO.getX().intValue() + "");
-        pathInitDTO.setY(pointDTO.getY().intValue() + "");
+        pathInitDTO.setXNow(pointDTO.getX().intValue() + "");
+        pathInitDTO.setYNow(pointDTO.getY().intValue() + "");
+        pathInitDTO.setStepLen(stepLen);
         pathInitDTO.setTaskId(taskId);
         pathInitDTO.setRobotId(robotId);
         return pathInitDTO;
     }
 
-    private TaskInfo addTaskByStartTrack(RouteInfo routeInfo, RobotInfo robotInfo) {
+    private TaskInfo addTaskByStartTrack( RobotInfo robotInfo, StartTrackRequest request) {
         //No.2 新增任务信息
         TaskInfo taskInfo = new TaskInfo();
         String currDate = DateUtil.format(new Date(), "yyyyMMdd");
@@ -654,16 +770,20 @@
         taskInfo.setCreateTime(new Date());
         //任务类型 中子源
         taskInfo.setTaskType(1);
-        taskInfo.setDiscernType(1);
-        taskInfo.setTaskName(routeInfo.getRouteName() + "巡线任务");
+        taskInfo.setDiscernType(2);
+        taskInfo.setTaskName("自主巡检任务");
         //执行状态 未完成
         taskInfo.setTaskStatus(0);
         taskInfo.setStartTime(new Date());
-        taskInfo.setRouteId(routeInfo.getId());
+        taskInfo.setRouteId(null);
         taskInfo.setRobotId(robotInfo.getId());
         taskInfo.setGridHeight(robotInfo.getGridHeight());
         taskInfo.setGridWidth(robotInfo.getGridWidth());
         taskInfo.setResolution(robotInfo.getResolution());
+
+        taskInfo.setStepLen(request.getStepLen());
+        taskInfo.setMaxThreshold(request.getMaxThreshold());
+
         save(taskInfo);
         return taskInfo;
     }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/utils/AlgorithmUtils.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/utils/AlgorithmUtils.java
index 22864d1..467b9c2 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/utils/AlgorithmUtils.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/utils/AlgorithmUtils.java
@@ -1,6 +1,7 @@
 package com.casic.missiles.modular.robot.utils;
 
 import java.io.BufferedReader;
+import java.io.File;
 import java.io.IOException;
 import java.io.InputStreamReader;
 import java.util.ArrayList;
@@ -16,11 +17,13 @@
     public static List<String> invokeJob(List<String> params) {
         List<String> res = new ArrayList<>();
         try {
-
             BufferedReader br = null;
             BufferedReader brError;
             String line = null;
-            Process p = new ProcessBuilder(params).start();
+            ProcessBuilder processBuilder =  new ProcessBuilder(params);
+            processBuilder.directory(new File("D:\\workspace\\server\\keti\\casic-robot-inspection\\casic-web\\src\\main\\resources\\config\\path"));
+            Process p = processBuilder.start();
+
             br = new BufferedReader(new InputStreamReader(p.getInputStream()));
             brError = new BufferedReader(new InputStreamReader(p.getErrorStream()));
             while ((line = br.readLine()) != null || (line = brError.readLine()) != null) {
@@ -37,33 +40,29 @@
     public static void main(String[] args) {
         String path = "D:\\workspace\\server\\keti\\casic-robot-inspection\\casic-web\\src\\main\\resources\\config\\path\\";
         //算法初始化调用
-        String exePath = path + "PathPlanning_init.exe";
+        String exePath = path + "init.exe";
 
         ArrayList<String> invocation = new ArrayList<String>();
         invocation.add(exePath);
-        invocation.add("40");
-        invocation.add("20");
-        invocation.add("0.5");
+        invocation.add("10");
+        invocation.add("10");
         invocation.add("5");
-        invocation.add("5");
+        invocation.add("0.16");
+        invocation.add("Mission1");
         invokeJob(invocation);
 
         //{"count":"null","robotId":"1","taskId":"1748224487696142338","x":"8","xMax":"100","y":"2","yMax":"100"}
-        String planning = path + "PathPlanning.exe";
         //{"open":"0,0,0,0,0,0,0,0","robotId":"1","taskId":"1748250669749735425","x":"6","xMax":"100","y":"2","yMax":"100"}
         //{"open":"1,1,1,1,1,1,1,1","robotId":"1","taskId":"1748252894098178049","x":"6","xMax":"100","y":"2","yMax":"100"}
         //{"count":"6.0","open":"1,1,1,1,1,1,1,1","robotId":"1","taskId":"1750713404601159681","x":"6","xMax":"20","y":"2","yMax":"40"}
+        String planning = path + "search.exe";
         ArrayList<String> planInvocation = new ArrayList<String>();
         planInvocation.add(planning);
-        planInvocation.add("40");
-        planInvocation.add("40");
-        planInvocation.add("1,1,0,1,1,1,1,1");
-        planInvocation.add("6.0");
-        //next_X
-        planInvocation.add("6");
-        //next_Y
-        planInvocation.add("2");
-        ProcessBuilder processBuilder = new ProcessBuilder();
-        invokeJob(planInvocation);
+        planInvocation.add("10");
+        planInvocation.add("1.0");
+        planInvocation.add("0.2");
+        planInvocation.add("Mission1");
+        List<String> res = invokeJob(planInvocation);
+        System.out.println(res);
     }
 }
\ No newline at end of file

diff --git a/.gitignore b/.gitignore
index 9e20bc1..f42a9cb 100644
--- a/.gitignore
+++ b/.gitignore
@@ -2,3 +2,4 @@
 **/target/**
 logs/
 *.iml
+*.exe
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java b/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
index c7b5d28..a849528 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
@@ -28,6 +28,7 @@
     String KEY_CUR_TASK_ID = "CUR_TASK_ID_";
     String KEY_CUR_TASK_INIT = "CUR_TASK_INIT_";
 
+    String KEY_COST_MAP = "KEY_COST_MAP";
     /**
      * PSD数据ID
      */
@@ -41,6 +42,7 @@
      */
     String KEY_CUR_XY_POINT = "KEY_CUR_XY_POINT_";
     String KEY_CUR_ESTIMATE_GRID = "KEY_CUR_ESTIMATE_GRID_";
+    String KEY_PLANING_FLAG = "KEY_PLANING_FLAG_";
     /**
      * 机器人巡点栅格
      */
@@ -55,6 +57,7 @@
      * 探测器配置参数缓存
      */
     String KEY_CONFIG_PARAMS_ID = "KEY_CONFIG_PARAMS_ID_";
+    String KEY_CONFIG_COSTMAP_RESOLUTION_SET = "KEY_CONFIG_COSTMAP_RESOLUTION_SET_";
 
     /**
      * 暂停状态缓存信息
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
index c4c905d..3bb475f 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
@@ -1,9 +1,14 @@
 package com.casic.missiles.modular.robot.controller;
 
+import cn.hutool.core.lang.Assert;
 import cn.hutool.core.util.StrUtil;
 import com.casic.missiles.core.base.controller.BaseController;
+import com.casic.missiles.core.cache.CacheUtil;
 import com.casic.missiles.core.util.SpringContextHolder;
+import com.casic.missiles.model.exception.RequestValidException;
+import com.casic.missiles.model.response.ResponseData;
 import com.casic.missiles.model.response.dto.ResponseDataDTO;
+import com.casic.missiles.modular.constants.RobotDictConstants;
 import com.casic.missiles.modular.robot.IBaseRobotService;
 import com.casic.missiles.modular.robot.dto.RobotInfoSetDTO;
 import com.casic.missiles.modular.robot.model.RobotInfo;
@@ -19,10 +24,16 @@
 import io.swagger.annotations.ApiOperation;
 import io.swagger.annotations.ApiParam;
 import lombok.SneakyThrows;
+import lombok.extern.slf4j.Slf4j;
+import org.apache.ibatis.annotations.Param;
+import org.springframework.beans.factory.annotation.Value;
 import org.springframework.web.bind.annotation.*;
+import org.springframework.web.multipart.MultipartFile;
 
 import javax.servlet.http.HttpServletResponse;
+import java.io.File;
 import java.util.List;
+import java.util.Optional;
 
 /**
  * 机器人操作类
@@ -30,6 +41,7 @@
  * @author lwh
  * @date 2023-10-30
  */
+@Slf4j
 @Api(tags = "机器人控制接口")
 @RestController
 @RequestMapping("/robot/opt")
@@ -61,6 +73,28 @@
         return baseRobotService.execCmdHandler(request, InstructCodeEnums.CHANGE_PCD);
     }
 
+    @Value("${casic.file.uploadPath}")
+    private String filePath;
+
+    @SneakyThrows
+    @ApiOperation(value = "PCD上传接口")
+    @PostMapping(value = "/uploadPcd")
+    public ResponseData uploadPcd(@RequestParam("file") MultipartFile file, @ApiParam("机器人ID,url拼接传参") String robotId) {
+        Assert.isFalse(StrUtil.isEmpty(robotId), () -> new RequestValidException("机器ID不能为空", Optional.of(new Throwable().getStackTrace()[2])));
+        RobotInfo info = robotInfoService.getById(robotId);
+        if (file != null && !file.isEmpty()) {
+            File pcdFile = new File(filePath + "finalCloud_" + info.getId() + ".pcd");
+            try {
+                file.transferTo(pcdFile);
+            } catch (Exception e) {
+                log.error(e.getMessage());
+            }
+            return ResponseData.success();
+        } else {
+            return ResponseData.error("附件上传失败!");
+        }
+    }
+
     @SneakyThrows
     @ApiOperation(value = "PCD下载接口")
     @PostMapping(value = "/pcdDownload")
@@ -136,6 +170,10 @@
     @ApiOperation(value = "启动或关闭循迹功能", notes = "返回 1成功 0失败  wait 启动时是否等待，0代表循迹启动即运动，不等待；1代表循迹启动原地等待。若无此字段，默认循迹启动不等待直接运动")
     @PostMapping(value = "/startTrack")
     public ResponseDataDTO<Integer> startTrack(@RequestBody StartTrackRequest request) {
+        request.setIsTask(1);
+        if (request.getStepLen() == null) {
+            request.setStepLen(2);
+        }
         baseRobotService.startTrack(request);
         return ResponseDataDTO.success();
     }
@@ -156,6 +194,13 @@
         MessageRequestDTO<CrlNavRequest> messageDTO = new MessageRequestDTO();
         messageDTO.setMsg(request);
         ResponseDataDTO<Integer> list = baseRobotService.getRobotRouteOptService().robotCmd(info, InstructCodeEnums.CTRL_NAV, messageDTO);
+        if (request.getAction() == 1) {
+            CostMapResolutionRequest costMap = new CostMapResolutionRequest();
+            costMap.setResolution(info.getResolution().doubleValue());
+            request.setRobotId(request.getRobotId());
+            //是否下发分辨率
+            CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CONFIG_COSTMAP_RESOLUTION_SET + request.getRobotId(), costMap);
+        }
         return list;
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java
new file mode 100644
index 0000000..5317651
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java
@@ -0,0 +1,86 @@
+package com.casic.missiles.modular.robot.controller;
+
+import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
+import com.baomidou.mybatisplus.extension.plugins.pagination.Page;
+import com.casic.missiles.core.base.controller.BaseController;
+import com.casic.missiles.core.page.PageFactory;
+import com.casic.missiles.core.page.PageInfoBT;
+import com.casic.missiles.model.form.IdForms;
+import com.casic.missiles.model.response.dto.ResponseDataDTO;
+import com.casic.missiles.modular.robot.model.TaskGridInfo;
+import com.casic.missiles.modular.robot.service.ITaskGridInfoService;
+import com.casic.missiles.modular.robot.vo.TaskGridInfoVO;
+import io.swagger.annotations.Api;
+import io.swagger.annotations.ApiOperation;
+import org.springframework.web.bind.annotation.*;
+
+import java.util.List;
+
+/**
+ * 任务网格信息Controller
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+@Api(tags = "任务网格信息")
+@RestController
+@RequestMapping("/robot/taskGridInfo")
+public class TaskGridInfoController extends BaseController {
+
+    private final ITaskGridInfoService taskGridInfoService;
+
+    public TaskGridInfoController(ITaskGridInfoService taskGridInfoService) {
+        this.taskGridInfoService = taskGridInfoService;
+    }
+
+    @ApiOperation(value = "列表查询")
+    @GetMapping(value = "/list")
+    public ResponseDataDTO<List<TaskGridInfo>> list(TaskGridInfoVO taskGridInfoVO) {
+        QueryWrapper<TaskGridInfo> query = taskGridInfoVO.genQuery();
+        return ResponseDataDTO.success(taskGridInfoService.list(query));
+    }
+
+    @ApiOperation(value = "分页查询")
+    @GetMapping(value = "/listPage")
+    public ResponseDataDTO<PageInfoBT<TaskGridInfo>> listPage(TaskGridInfoVO taskGridInfoVO) {
+        Page<TaskGridInfo> page = PageFactory.defaultPage();
+        QueryWrapper<TaskGridInfo> query = taskGridInfoVO.genQuery();
+        page = taskGridInfoService.page(page, query);
+        return ResponseDataDTO.success(super.packForBT(page));
+    }
+
+
+    @ApiOperation(value = "新增接口")
+    @PostMapping(value = "/add")
+    public ResponseDataDTO add(@RequestBody TaskGridInfo taskGridInfo) {
+        taskGridInfoService.save(taskGridInfo);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "修改接口")
+    @PostMapping(value = "/update")
+    public ResponseDataDTO update(@RequestBody TaskGridInfo taskGridInfo) {
+        taskGridInfoService.updateById(taskGridInfo);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "删除接口")
+    @GetMapping(value = "/delete")
+    public ResponseDataDTO delete(String id) {
+        taskGridInfoService.removeById(id);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation("批量删除")
+    @PostMapping("/batchDelete")
+    public ResponseDataDTO batchDelete(@RequestBody IdForms ids) {
+        taskGridInfoService.removeByIds(ids.getIds());
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "详情查询")
+    @GetMapping(value = "/detail")
+    public ResponseDataDTO<TaskGridInfo> detail(String id) {
+        return ResponseDataDTO.success(taskGridInfoService.getById(id));
+    }
+}
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
index e82a792..6992274 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
@@ -98,7 +98,7 @@
     public ResponseDataDTO<AlgorithmResponse> test() {
         AlgorithmResponse response = new AlgorithmResponse();
         response.setTaskHeatMap(new TaskHeatMap());
-        algorithmService.readAlgorithmResponse("step3.txt",response,123L,null);
+        algorithmService.readAlgorithmResponse("step3.txt",null,response,123L,null);
         sendWebSocket(InstructStatusMsgEnums.HEAT_MAP, "1", response.getTaskHeatMap());
         return ResponseDataDTO.success(response);
     }
@@ -121,7 +121,7 @@
     @GetMapping(value = "/testGrid")
     public ResponseDataDTO<List<RouteInfo>> testGrid(@RequestBody PathInitDTO pathInitDTO) {
         RobotInfo robotInfo = infoService.getById(pathInitDTO.getRobotId());
-        GridPointDTO gridPointDTO = algorithmService.convertSlamToGrid(new BigDecimal(pathInitDTO.getX()), new BigDecimal(10), new BigDecimal(4), robotInfo);
+        GridPointDTO gridPointDTO = algorithmService.convertSlamToGrid(new BigDecimal(pathInitDTO.getXNow()), new BigDecimal(10), new BigDecimal(4), robotInfo);
         return ResponseDataDTO.success(algorithmService.convertGridCenterPoint(gridPointDTO, new BigDecimal(10), robotInfo));
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/TaskGridInfoMapper.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/TaskGridInfoMapper.java
new file mode 100644
index 0000000..5c3a70b
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/TaskGridInfoMapper.java
@@ -0,0 +1,23 @@
+package com.casic.missiles.modular.robot.dao;
+
+
+import com.baomidou.mybatisplus.core.mapper.BaseMapper;
+import com.casic.missiles.modular.robot.model.TaskGridInfo;
+import org.apache.ibatis.annotations.Param;
+import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
+import com.baomidou.mybatisplus.extension.plugins.pagination.Page;
+import java.util.List;
+/**
+ * <p>
+ *  任务网格信息 Mapper 接口
+ * </p>
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+public interface TaskGridInfoMapper extends BaseMapper<TaskGridInfo> {
+/**
+* 任务网格信息 分页检索
+*/
+List<TaskGridInfo> selectTaskGridInfoPage( @Param("page") Page<TaskGridInfo> page,@Param("ew" ) QueryWrapper<TaskGridInfo> query);
+}
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/mapping/TaskGridInfoMapper.xml b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/mapping/TaskGridInfoMapper.xml
new file mode 100644
index 0000000..be573e0
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/mapping/TaskGridInfoMapper.xml
@@ -0,0 +1,80 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!DOCTYPE mapper PUBLIC "-//mybatis.org//DTD Mapper 3.0//EN" "http://mybatis.org/dtd/mybatis-3-mapper.dtd">
+<mapper namespace="com.casic.missiles.modular.robot.dao.TaskGridInfoMapper">
+    <!-- 通用查询映射结果 -->
+    <resultMap id="BaseResultMap" type="com.casic.missiles.modular.robot.model.TaskGridInfo">
+        <result column="id" property="id"/>
+        <result column="create_time" property="createTime"/>
+        <result column="update_time" property="updateTime"/>
+        <result column="task_id" property="taskId"/>
+        <result column="estimate_grid_x" property="estimateGridX"/>
+        <result column="estimate_grid_y" property="estimateGridY"/>
+        <result column="neutron_count" property="neutronCount"/>
+        <result column="cur_grid_x" property="curGridX"/>
+        <result column="cur_grid_y" property="curGridY"/>
+    </resultMap>
+    <!-- 通用查询结果列 -->
+    <sql id="Base_Column_List">
+        id
+        ,
+                    create_time as createTime ,
+                    update_time as updateTime ,
+                    task_id as taskId ,
+                    estimate_grid_x as estimateGridX ,
+                    estimate_grid_y as estimateGridY ,
+                    neutron_count as neutronCount ,
+                    cur_grid_x as curGridX ,
+                    cur_grid_y as curGridY
+    </sql>
+    <sql id="Base_Column_Alias">
+        a
+        .
+        id
+        ,
+            a.create_time
+            as createTime ,
+            a.update_time
+            as updateTime ,
+            a.task_id
+            as taskId ,
+            a.estimate_grid_x
+            as estimateGridX ,
+            a.estimate_grid_y
+            as estimateGridY ,
+            a.neutron_count
+            as neutronCount ,
+            a.cur_grid_x
+            as curGridX ,
+            a.cur_grid_y
+            as curGridY
+    </sql>
+    <select id="selectTaskGridInfoPage" resultType="com.casic.missiles.modular.robot.model.TaskGridInfo">
+        select
+        <include refid="Base_Column_Alias"/>
+        from task_grid_info a
+        <where>
+            <if test="ew != null">
+                ${ew.sqlSegment}
+            </if>
+        </where>
+    </select>
+    <sql id="Base_Column_Query">
+        <if test="request!=null">
+            <if test="request.taskId!= null and request.taskId!=''">
+                and task_id = #{request.taskId}
+            </if>
+            <if test="request.estimateGridX!= null and request.estimateGridX!=''">
+                and estimate_grid_x = #{request.estimateGridX}
+            </if>
+            <if test="request.estimateGridY!= null and request.estimateGridY!=''">
+                and estimate_grid_y = #{request.estimateGridY}
+            </if>
+            <if test="request.curGridX!= null and request.curGridX!=''">
+                and cur_grid_x = #{request.curGridX}
+            </if>
+            <if test="request.curGridY!= null and request.curGridY!=''">
+                and cur_grid_y = #{request.curGridY}
+            </if>
+        </if>
+    </sql>
+</mapper>
\ No newline at end of file
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java
index 749f559..4559074 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java
@@ -13,4 +13,9 @@
      * 规划点位信息
      */
     private PointNextDTO pointNextDTO;
+    /**
+     * 寻源目标地址
+     */
+    private PointNextDTO coordinate;
+    private boolean isEnd;
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java
index 39ff2a2..cef3238 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java
@@ -12,6 +12,9 @@
     private BigDecimal y;
 
     public static GridPointDTO convertGridPoint(PointNextDTO nextDTO) {
+        if(nextDTO==null){
+            return null;
+        }
         GridPointDTO gridPointDTO = new GridPointDTO();
         gridPointDTO.setX(Convert.toBigDecimal(nextDTO.getX()));
         gridPointDTO.setY(Convert.toBigDecimal(nextDTO.getY()));
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java
index 8c88242..33be25d 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java
@@ -12,25 +12,19 @@
 public class PathInitDTO {
     //---------------------算法参数
     /**
-     * x坐标最大值（x方向栅格总数）
+     * 初始位置x坐标(栅格地图下的坐标)
      */
-    private String xMax;
+    private String xNow;
     /**
-     * y坐标最大值（y方向栅格总数）
+     * 初始位置y坐标(栅格地图下的坐标)
      */
-    private String yMax;
+    private String yNow;
     /**
      * 初始位置计数率
      */
-    private String count;
-    /**
-     * 初始位置x坐标(0<=x<x_max)
-     */
-    private String x;
-    /**
-     * 初始位置y坐标(0<=y<y_max)
-     */
-    private String y;
+    private String zNow;
+
+    private Integer stepLen;
 
     //---------------------其他执行参数
     private String robotId;
@@ -38,11 +32,11 @@
 
     public List<String> toArray() {
         List<String> list = new ArrayList<>();
-        list.add(xMax);
-        list.add(yMax);
-        list.add(count);
-        list.add(x);
-        list.add(y);
+        list.add(xNow);
+        list.add(yNow);
+        list.add(stepLen+"");
+        list.add(zNow);
+        list.add(taskId+"");
         return list;
     }
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java
index 109e95c..ada6774 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java
@@ -10,21 +10,17 @@
  */
 @Data
 public class PathPlanDTO extends PathInitDTO {
-
     /**
-     * 长度为8的数组，记录(x-1, y-1), (x-1, y), (x-1, y+1), (x, y-1), (x, y+1), (x+1, y-1), (x+1, y), (x+1, y+1)8个坐标的可通行信息。
-     * 若某坐标有障碍物（不可通行行）则记为0，否则记为1
+     * 浮点阈值(阈值，超过该值后结束巡检开始寻源)
      */
-    private String open;
-
+    private String th;
     public List<String> toArray() {
         List<String> list = new ArrayList<>();
-        list.add(getXMax());
-        list.add(getYMax());
-        list.add(open);
-        list.add(getCount());
-        list.add(getX());
-        list.add(getY());
+
+        list.add(getStepLen()+"");
+        list.add(getTh());
+        list.add(getZNow());
+        list.add(getTaskId()+"");
         return list;
     }
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskGridInfo.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskGridInfo.java
new file mode 100644
index 0000000..101c10a
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskGridInfo.java
@@ -0,0 +1,83 @@
+package com.casic.missiles.modular.robot.model;
+
+import com.baomidou.mybatisplus.annotation.TableField;
+import com.baomidou.mybatisplus.annotation.TableName;
+import com.baomidou.mybatisplus.extension.activerecord.Model;
+import io.swagger.annotations.ApiModelProperty;
+import lombok.Data;
+
+import java.math.BigDecimal;
+import java.util.Date;
+
+/**
+ * 任务网格信息对象 task_grid_info
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+@Data
+@TableName("task_grid_info")
+public class TaskGridInfo extends Model<TaskGridInfo> {
+    private static final long serialVersionUID = 1L;
+    @TableField(exist = false)
+    private Integer index;
+    @ApiModelProperty(value = "主键", dataType = "Long")
+    @TableField("id")
+    private Long id;
+
+    @ApiModelProperty(value = "创建时间", dataType = "Date")
+    @TableField("create_time")
+    private Date createTime;
+
+    @ApiModelProperty(value = "修改时间", dataType = "Date")
+    @TableField("update_time")
+    private Date updateTime;
+
+    @ApiModelProperty(value = "任务ID", dataType = "Long")
+    @TableField("task_id")
+    private Long taskId;
+
+    @ApiModelProperty(value = "估算栅格x", dataType = "Long")
+    @TableField("estimate_grid_x")
+    private String estimateGridX;
+
+    @ApiModelProperty(value = "估算栅格y", dataType = "Long")
+    @TableField("estimate_grid_y")
+    private String estimateGridY;
+
+    @ApiModelProperty(value = "中子计数率", dataType = "BigDecimal")
+    @TableField("neutron_count")
+    private BigDecimal neutronCount;
+
+    @ApiModelProperty(value = "当前栅格x", dataType = "Long")
+    @TableField("cur_grid_x")
+    private Integer curGridX;
+
+    @ApiModelProperty(value = "当前栅格y", dataType = "Long")
+    @TableField("cur_grid_y")
+    private Integer curGridY;
+
+    public String getCurPoint(){
+        return "("+curGridX+","+curGridY+")";
+    }
+    public String getEstimatePoint(){
+        if(estimateGridX==null&estimateGridY==null){
+            return "结束";
+        }
+        return "("+estimateGridX+","+estimateGridY+")";
+    }
+    @Override
+    public String toString() {
+        return "TaskGridInfo{"
+                + "id=" + id
+                + "createTime=" + createTime
+                + "updateTime=" + updateTime
+                + "taskId=" + taskId
+                + "estimateGridX=" + estimateGridX
+                + "estimateGridY=" + estimateGridY
+                + "neutronCount=" + neutronCount
+                + "curGridX=" + curGridX
+                + "curGridY=" + curGridY
+                + "}";
+    }
+}
\ No newline at end of file
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java
index edc104e..a8acb2a 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java
@@ -53,6 +53,13 @@
     @TableField("route_id")
     private Long routeId;
 
+    @ApiModelProperty(value = "寻源初始步长", dataType = "Integer")
+    @TableField("step_len")
+    private Integer stepLen;
+
+    @ApiModelProperty(value = "寻源阈值设定", dataType = "BigDecimal")
+    @TableField("max_threshold")
+    private BigDecimal maxThreshold;
     /**
      * 任务类型(1巡检任务)
      */
@@ -62,7 +69,6 @@
     @ApiModelProperty(value = "任务类型字典名称", dataType = "String")
     @TableField(exist = false)
     private String taskTypeName;
-
     @ApiModelProperty(value = "识别类型(discernType 1中子源识别)", dataType = "Integer")
     @TableField("discern_type")
     private Integer discernType;
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java
index c4bc25d..c6620f3 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java
@@ -53,7 +53,7 @@
     }),
     SET_SPEED("/set_speed" , "/set_speed_response" , "最大运动速度设置" , true, new TypeReference<RobotMsgResponseDTO<Integer>>() {
     }),
-    SET_TARGETPOINT("/set_targetpoint" , "/set_targetpoint_response" , "设置导航目标点" , false, new TypeReference<RobotMsgResponseDTO<Integer>>() {
+    SET_TARGETPOINT("/set_targetpoint" , "/set_targetpoint_response" , "设置导航目标点" , true, new TypeReference<RobotMsgResponseDTO<Integer>>() {
     }),
 
     GET_DATA_LIST("/get_data_list" , "/get_data_list_response" , "获取数据包列表" , true, new TypeReference<RobotMsgResponseDTO<List<String>>>() {
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/AbstractSocketHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/AbstractSocketHandler.java
index 83f4cb2..e47e343 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/AbstractSocketHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/AbstractSocketHandler.java
@@ -49,7 +49,7 @@
                 wsMsgBaseDTO.setMsgKey(enums.getMsgKey());
                 wsMsgBaseDTO.setData(jsonObject.get("msg"));
                 String msg = JSON.toJSONString(wsMsgBaseDTO);
-                log.info("推送{}消息：{}", enums.getRemarks(), msg);
+//                log.info("推送{}消息：{}", enums.getRemarks(), msg);
                 webSocket.sendOneMessage(robotId, msg);
             }
         } catch (Exception e) {
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/cmd/TargetPointCmdHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/cmd/TargetPointCmdHandler.java
index c1c09fa..43b4460 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/cmd/TargetPointCmdHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/cmd/TargetPointCmdHandler.java
@@ -34,7 +34,11 @@
     @Override
     public ResponseDataDTO senderCmd(TargetPointRequest request, InstructCodeEnums enums) {
         MessageRequestDTO<TargetPointRequest> messageDTO = new MessageRequestDTO();
-        //读取默认关键点信息
+        //读取默认关键点信息 {"x":"1.715708819927424","y":"-0.9100042544250901","z":"0.017993879527185876","theta":"-0.039278239414312105"}
+//        request.setX("1.715708819927424");
+//        request.setY("-0.9100042544250901");
+//        request.setZ("0.017993879527185876");
+//        request.setTheta("-0.039278239414312105");
         messageDTO.setMsg(request);
         ResponseDataDTO<Integer> list = optService.robotCmd(getRobotInfo(), enums, messageDTO);
         return list;
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/AppCostMapHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/AppCostMapHandler.java
index 0335693..88c9f79 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/AppCostMapHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/AppCostMapHandler.java
@@ -1,6 +1,11 @@
 package com.casic.missiles.modular.robot.opt.handler.msg;
 
+import com.casic.missiles.core.cache.CacheUtil;
+import com.casic.missiles.modular.constants.RobotDictConstants;
+import com.casic.missiles.modular.robot.IBaseRobotService;
+import com.casic.missiles.modular.robot.opt.enums.InstructCodeEnums;
 import com.casic.missiles.modular.robot.opt.handler.AbstractSocketHandler;
+import com.casic.missiles.modular.robot.opt.instruct.dto.CostMapResolutionRequest;
 import com.casic.missiles.modular.robot.service.IRobotStatusInfoService;
 import lombok.extern.slf4j.Slf4j;
 import org.eclipse.paho.client.mqttv3.MqttMessage;
@@ -16,14 +21,27 @@
 @Component("/app_costmap_sender")
 public class AppCostMapHandler extends AbstractSocketHandler {
     private final IRobotStatusInfoService statusInfoService;
+    private final IBaseRobotService baseRobotService;
     private static Map<String, Integer> index = new ConcurrentHashMap<>();
 
-    public AppCostMapHandler(IRobotStatusInfoService statusInfoService) {
+    public AppCostMapHandler(IRobotStatusInfoService statusInfoService, IBaseRobotService baseRobotService) {
         this.statusInfoService = statusInfoService;
+        this.baseRobotService = baseRobotService;
     }
 
     @Override
     public void process(String robotId, String topic, MqttMessage message) {
         statusInfoService.saveCostMap(robotId, message);
+
+        CostMapResolutionRequest costMapSet = CacheUtil.get(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CONFIG_COSTMAP_RESOLUTION_SET +robotId);
+        if(costMapSet != null){
+            CacheUtil.remove(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CONFIG_COSTMAP_RESOLUTION_SET +robotId);
+            new Thread(() -> {
+                costMapSet.setRobotId(Long.valueOf(robotId));
+                baseRobotService.execCmdHandler(costMapSet, InstructCodeEnums.COSTMAP_RESOLUTION);
+            }).start();
+
+        }
+
     }
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/PoseMessageMsgHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/PoseMessageMsgHandler.java
index 29f8919..6c3c3bd 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/PoseMessageMsgHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/PoseMessageMsgHandler.java
@@ -17,11 +17,9 @@
 @Component("/pose_message")
 public class PoseMessageMsgHandler extends AbstractSocketHandler {
 
-
     private final EventPublisher publisher;
 
     public PoseMessageMsgHandler(EventPublisher publisher) {
-
         this.publisher = publisher;
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/StopStateMsgHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/StopStateMsgHandler.java
index 8950815..8a71e0b 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/StopStateMsgHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/StopStateMsgHandler.java
@@ -30,7 +30,7 @@
     public void process(String robotId, String topic, MqttMessage message) {
 
         index.put(index_key, index.get("index") + 1);
-        if (index.get(index_key)>50) {
+        if (index.get(index_key)>30) {
             index.put(index_key, 0);
             log.info("主键：{},停障状态topic:{},消息内容:{}", robotId, topic, message.toString());
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/impl/RobotOptServiceImpl.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/impl/RobotOptServiceImpl.java
index 8471e7a..7940a24 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/impl/RobotOptServiceImpl.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/impl/RobotOptServiceImpl.java
@@ -146,7 +146,7 @@
     public void startTrack(StartTrackRequest request) {
         //增加循迹任务记录
         taskInfoService.reloadTask(request, robotInfoService.getById(request.getRobotId()));
-        this.execCmdHandler(request, InstructCodeEnums.START_TRACK);
+//        this.execCmdHandler(request, InstructCodeEnums.START_TRACK);
     }
 
     public IRobotRouteOptService getRobotRouteOptService() {
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/BaseRobotCmdDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/BaseRobotCmdDTO.java
index 5608f2b..06b53de 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/BaseRobotCmdDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/BaseRobotCmdDTO.java
@@ -3,11 +3,13 @@
 import io.swagger.annotations.ApiModelProperty;
 import lombok.Data;
 
+import java.io.Serializable;
+
 /**
  * 机器人指令基类
  */
 @Data
-public class BaseRobotCmdDTO {
+public class BaseRobotCmdDTO implements Serializable {
     @ApiModelProperty(value = "机器人主键", dataType = "String")
     protected Long robotId;
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/StartTrackRequest.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/StartTrackRequest.java
index a18321b..85546a7 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/StartTrackRequest.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/StartTrackRequest.java
@@ -3,6 +3,8 @@
 import io.swagger.annotations.ApiModelProperty;
 import lombok.Data;
 
+import java.math.BigDecimal;
+
 /**
  * 导航下发指令
  */
@@ -10,18 +12,19 @@
 public class StartTrackRequest extends BaseRobotCmdDTO {
     @ApiModelProperty(value = "sn编码(可不填 默认机器ID)", dataType = "String")
     private String sn;
-
     @ApiModelProperty(value = "下发状态 1:开启 0:关闭", dataType = "String")
     private Integer action;
     @ApiModelProperty(value = "等待状态 1:等待 0:不等待", dataType = "String")
     private Integer wait;
     @ApiModelProperty(value = "等待状态 0:循迹 1:任务", dataType = "isTask")
     private Integer isTask;
-    @ApiModelProperty(value = "轨迹名称", dataType = "String")
+    @ApiModelProperty(value = "轨迹名称(废弃)", dataType = "String")
     private String track_name;
     @ApiModelProperty(value = "关键点名称", dataType = "String", hidden = true)
     private String taskpoint_name;
-
+    @ApiModelProperty(value = "初始寻源步长", dataType = "Integer")
+    private Integer stepLen;
+    @ApiModelProperty(value = "寻源阈值", dataType = "BigDecimal")
+    private BigDecimal maxThreshold;
     private String obs_mode="1";
-
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/response/StopStateResponseDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/response/StopStateResponseDTO.java
index eb514bf..e0679d7 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/response/StopStateResponseDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/response/StopStateResponseDTO.java
@@ -3,11 +3,13 @@
 import io.swagger.annotations.ApiModelProperty;
 import lombok.Data;
 
+import java.io.Serializable;
+
 /**
  * 电压信息返回
  */
 @Data
-public class StopStateResponseDTO {
+public class StopStateResponseDTO implements Serializable {
     @ApiModelProperty(value = "软件急停(0：无触发 1：触发)" , dataType = "Integer")
     private Integer soft;
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IAlgorithmService.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IAlgorithmService.java
index 56b3e1d..f62523b 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IAlgorithmService.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IAlgorithmService.java
@@ -6,6 +6,7 @@
 import com.casic.missiles.modular.robot.dto.PathPlanDTO;
 import com.casic.missiles.modular.robot.model.RobotInfo;
 
+import java.io.File;
 import java.math.BigDecimal;
 import java.util.List;
 
@@ -38,5 +39,5 @@
      * @return
      */
     GridPointDTO convertGridCenterPoint(GridPointDTO pointDTO, BigDecimal resolution, RobotInfo robotInfo);
-    void readAlgorithmResponse(String fileName, AlgorithmResponse response, Long taskId, List<String> res);
+    void readAlgorithmResponse(String fileName, File repetitionFile, AlgorithmResponse response, Long taskId, List<String> res);
 }
\ No newline at end of file
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IRobotStatusInfoService.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IRobotStatusInfoService.java
index 2af210a..757f8d0 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IRobotStatusInfoService.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IRobotStatusInfoService.java
@@ -67,10 +67,9 @@
     /**
      * 更新车辆路线及任务信息
      *
-     * @param currRouteId 当前路线信息
      * @param currTaskId  当前任务信息
      */
-    void updateCurrTask(String robotId, Long currRouteId, Long currTaskId);
+    void updateCurrTask(String robotId, Long currTaskId);
 
     RobotStatusInfo getStatusByRobotId(String robotId);
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/ITaskGridInfoService.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/ITaskGridInfoService.java
new file mode 100644
index 0000000..fc0da93
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/ITaskGridInfoService.java
@@ -0,0 +1,22 @@
+package com.casic.missiles.modular.robot.service;
+
+import com.baomidou.mybatisplus.extension.service.IService;
+import  com.casic.missiles.modular.robot.model.TaskGridInfo;
+import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
+import com.baomidou.mybatisplus.extension.plugins.pagination.Page;
+import java.util.List;
+
+
+/**
+ * <p>
+ * 任务网格信息 服务类
+ * </p>
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+public interface ITaskGridInfoService extends IService<TaskGridInfo> {
+/**
+* 任务网格信息 分页检索
+*/
+List<TaskGridInfo> selectTaskGridInfoPage(Page<TaskGridInfo> page,QueryWrapper<TaskGridInfo> query);}
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/ITaskInfoService.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/ITaskInfoService.java
index 31d7ba6..b395993 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/ITaskInfoService.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/ITaskInfoService.java
@@ -51,6 +51,13 @@
      * @param dto 实体
      */
     void pointCallBack(PoseMessageResponseDTO dto, String robotId);
+
+    /**
+     * 车辆路线规划
+     * @param dto
+     * @param robotId
+     */
+    void carPathPlanning(PoseMessageResponseDTO dto, String robotId);
     void pointTestCallBack(PoseMessageResponseDTO dto, String robotId);
     /**
      * 图片路径保存
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/AlgorithmServiceImpl.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/AlgorithmServiceImpl.java
index f808fa0..a4dbe0b 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/AlgorithmServiceImpl.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/AlgorithmServiceImpl.java
@@ -29,10 +29,12 @@
 @Slf4j
 @Service
 public class AlgorithmServiceImpl implements IAlgorithmService {
-    @Value("${casic.algorithm.initPath:-1}")
-    private String initPath;
-    @Value("${casic.algorithm.planPath:-1}")
-    private String planPath;
+    @Value("${casic.algorithm.path:-1}")
+    private String path;
+    @Value("${casic.algorithm.initPathName:-1}")
+    private String initPathName;
+    @Value("${casic.algorithm.planPathName:-1}")
+    private String planPathName;
     private final ITaskHeatMapService heatMapService;
 
     public AlgorithmServiceImpl(ITaskHeatMapService heatMapService) {
@@ -42,13 +44,13 @@
     @Transactional
     @Override
     public AlgorithmResponse pathPlanningInit(PathInitDTO initDTO) {
-        if (StrUtil.isEmpty(initDTO.getCount())) {
-            initDTO.setCount("0.00");
+        if (StrUtil.isEmpty(initDTO.getZNow())) {
+            initDTO.setZNow("0.00");
         }
         log.info(JSON.toJSONString(initDTO));
         //算法路径及入参配置
         List<String> commends = new ArrayList<>();
-        commends.add(initPath);
+        commends.add(path + initPathName);
         commends.addAll(initDTO.toArray());
         List<String> res = AlgorithmUtils.invokeJob(commends);
 
@@ -63,38 +65,72 @@
     @Transactional
     @Override
     public AlgorithmResponse pathPlanning(PathPlanDTO planDTO) {
-        if (StrUtil.isEmpty(planDTO.getCount())) {
-            planDTO.setCount("0.001");
+        if (StrUtil.isEmpty(planDTO.getZNow())) {
+            planDTO.setZNow("0.001");
         }
         //算法路径及入参配置
         List<String> commends = new ArrayList<>();
-        commends.add(planPath);
+        commends.add(path + planPathName);
         commends.addAll(planDTO.toArray());
         log.info("寻源算法调用:{}", JSON.toJSONString(planDTO));
         List<String> res = AlgorithmUtils.invokeJob(commends);
         AlgorithmResponse response = getAlgorithmResponse(planDTO, res);
-        //next 点位编写
-        if (CollectionUtil.isNotEmpty(res) && res.size() == 2) {
-            response.getPointNextDTO().setX(formatBigDecimal(res.get(0).replaceAll("next_X: ", "")));
-            response.getPointNextDTO().setY(formatBigDecimal(res.get(1).replaceAll("next_Y: ", "")));
-        }
+
         return response;
     }
 
     private AlgorithmResponse getAlgorithmResponse(PathInitDTO initDTO, List<String> res) {
-        File repetitionFile = new File("repetition.txt");
+        //读取当前路径图片
+        File repetitionFileDir = new File(path + "\\" + initDTO.getTaskId() + "\\predicts");
+        File[] files = repetitionFileDir.listFiles((dir, name) -> name.endsWith(".txt") && name.startsWith("step"));
+        File repetitionFile = null;
+        //判定当前执行路径最大step2文件夹
+        if (files != null && files.length > 0) {
+             repetitionFile = files[files.length - 1];
+        }
         AlgorithmResponse response = new AlgorithmResponse();
         if (initDTO.getTaskId() != null && repetitionFile.exists()) {
-            readAlgorithmResponse("repetition.txt", response, initDTO.getTaskId(), res);
-            readAlgorithmResponse("step.txt", response, initDTO.getTaskId(), res);
+            //读取热力图
+            readAlgorithmResponse("repetition.txt",repetitionFile, response, initDTO.getTaskId(), res);
         } else {
             log.info("taskId is empty!");
         }
-        readAlgorithmResponse("xy_train.txt", response, initDTO.getTaskId(), res);
-        readAlgorithmResponse("z_train.txt", response, initDTO.getTaskId(), res);
+        //读取规划路径
+        if (CollectionUtil.isNotEmpty(res)) {
+            response.setEnd(false);
+            readAlgorithmResponse(response, res);
+        }
         return response;
     }
 
+    private void readAlgorithmResponse(AlgorithmResponse response, List<String> res) {
+        List<String> resList = new ArrayList<>();
+        if (res.size() >= 2) {
+            resList = res.subList(res.size() - 2, res.size());
+        }
+        //判定是否结束寻源
+        if (res.contains("end")) {
+            response.setEnd(true);
+            for (String s : resList) {
+                if (s.startsWith("source_coordinate:")) {
+                    s = s.trim();
+                    log.info("寻源结束,目标点位：{}",s);
+                    List<String> lists = StrUtil.split(s.replaceAll("source_coordinate:", ""), ",");
+                    PointNextDTO nextDTO = new PointNextDTO();
+                    nextDTO.setX(lists.get(0));
+                    nextDTO.setY(lists.get(1));
+                    response.setCoordinate(nextDTO);
+                }
+            }
+        } else {
+            PointNextDTO nextDTO = new PointNextDTO();
+            nextDTO.setX(resList.get(0));
+            nextDTO.setY(resList.get(1));
+            response.setPointNextDTO(nextDTO);
+        }
+
+    }
+
     /**
      * 读取算法输出数据
      *
@@ -102,49 +138,18 @@
      * @param response 结果集
      * @param taskId   任务ID
      */
-    public void readAlgorithmResponse(String fileName, AlgorithmResponse response, Long taskId, List<String> res) {
-        File file = new File(fileName);
-        if (!file.exists()&&!fileName.startsWith("step")) {
+    public void readAlgorithmResponse(String fileName,File repetitionFile, AlgorithmResponse response, Long taskId, List<String> res) {
+        if (!repetitionFile.exists() && !fileName.startsWith("step")) {
             log.info("{} file is empty!", fileName);
             return;
         }
         if (response.getPointNextDTO() == null) {
             response.setPointNextDTO(new PointNextDTO());
         }
-        switch (fileName) {
-            case "repetition.txt":
-                String msg = FileUtil.readString(file, CharsetUtil.CHARSET_UTF_8);
-                TaskHeatMap taskHeatMap = createEmptyTaskHeatMap(taskId);
-                taskHeatMap.setRepetition(msg);
-                response.setTaskHeatMap(taskHeatMap);
-                break;
-            case "xy_train.txt":
-                List<String> xyTrains = FileUtil.readLines(file, CharsetUtil.CHARSET_UTF_8);
-                if (CollectionUtil.isNotEmpty(xyTrains)) {
-                    if (CollectionUtil.isNotEmpty(xyTrains)) {
-                        List<String> list = StrUtil.split(xyTrains.get(xyTrains.size() - 1), " ");
-                        response.getPointNextDTO().setX(formatBigDecimal(list.get(0)));
-                        response.getPointNextDTO().setY(formatBigDecimal(list.get(1)));
-                    }
-                }
-                break;
-            case "z_train.txt":
-                List<String> zTrains = FileUtil.readLines(file, CharsetUtil.CHARSET_UTF_8);
-                if (CollectionUtil.isNotEmpty(zTrains)) {
-                    String zTrain = zTrains.get(zTrains.size() - 1);
-                    response.getPointNextDTO().setZ(formatBigDecimal(zTrain));
-                }
-            default:
-                if (StrUtil.isNotEmpty(fileName) && fileName.startsWith("step")) {
-                    File fileDir = new File(".");
-                    File[] files = fileDir.listFiles((dir, name) -> name.endsWith(".txt") && name.startsWith("step"));
-                    if (files != null && files.length > 0) {
-                        String steps = FileUtil.readString(files[files.length - 1], CharsetUtil.CHARSET_UTF_8);
-                        response.getTaskHeatMap().setRepetition(steps);
-                    }
-                }
-                break;
-        }
+        String msg = FileUtil.readString(repetitionFile, CharsetUtil.CHARSET_UTF_8);
+        TaskHeatMap taskHeatMap = createEmptyTaskHeatMap(taskId);
+        taskHeatMap.setRepetition(msg);
+        response.setTaskHeatMap(taskHeatMap);
     }
 
     /**
@@ -157,13 +162,11 @@
     public GridPointDTO convertSlamToGrid(BigDecimal x, BigDecimal y, BigDecimal resolution, RobotInfo robotInfo) {
         GridPointDTO pointDTO = new GridPointDTO();
         BigDecimal scale = resolution;
-
         //减去栅格原点坐标
         x = x.subtract(robotInfo.getOriginX());
         y = y.subtract(robotInfo.getOriginY());
-
-        pointDTO.setX(new BigDecimal(x.divide(scale,3,BigDecimal.ROUND_HALF_UP).intValue()+1));
-        pointDTO.setY(new BigDecimal(y.divide(scale,3,BigDecimal.ROUND_HALF_UP).intValue()+1));
+        pointDTO.setX(new BigDecimal(x.divide(scale, 3, BigDecimal.ROUND_HALF_UP).intValue()+1));
+        pointDTO.setY(new BigDecimal(y.divide(scale, 3, BigDecimal.ROUND_HALF_UP).intValue() +1));
         return pointDTO;
     }
 
@@ -174,19 +177,17 @@
      * @return
      */
     public GridPointDTO convertGridCenterPoint(GridPointDTO pointDTO, BigDecimal resolution, RobotInfo robotInfo) {
-
         GridPointDTO centerPoint = new GridPointDTO();
         BigDecimal scale = resolution;
-        //点位转换
-        BigDecimal slamX = pointDTO.getX().divide(scale, 5, BigDecimal.ROUND_HALF_UP);
-        BigDecimal slamY = pointDTO.getY().divide(scale, 5, BigDecimal.ROUND_HALF_UP);
+        //点位转换 转换后栅格－1 从1开始计算
+        BigDecimal slamX = (pointDTO.getX().subtract(new BigDecimal(1))).multiply(scale);
+        BigDecimal slamY = (pointDTO.getY().subtract(new BigDecimal(1))).multiply(scale);
         //中心栅格点位中心添加
-        centerPoint.setX(slamX.add(scale.divide(new BigDecimal(2), 5, BigDecimal.ROUND_HALF_UP)));
-        centerPoint.setY(slamY.add(scale.divide(new BigDecimal(2), 5, BigDecimal.ROUND_HALF_UP)));
+        centerPoint.setX(slamX.add(scale.divide(new BigDecimal(2), 10, BigDecimal.ROUND_HALF_UP)));
+        centerPoint.setY(slamY.add(scale.divide(new BigDecimal(2), 10, BigDecimal.ROUND_HALF_UP)));
         //加上栅格原点坐标
         centerPoint.setX(centerPoint.getX().add(robotInfo.getOriginX()));
         centerPoint.setY(centerPoint.getY().add(robotInfo.getOriginY()));
-
         return centerPoint;
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotInfoServiceImpl.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotInfoServiceImpl.java
index 043e05d..fdded64 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotInfoServiceImpl.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotInfoServiceImpl.java
@@ -104,7 +104,7 @@
             //下发障碍物栅格
             sendCostMapResolution(robotInfo.getResolution(), robotInfo.getId());
             //重新生成栅格地图
-            sendChangePcd(robotInfo.getId());
+            //sendChangePcd(robotInfo.getId());
         }
         update.eq(RobotInfo::getId, robotInfo.getId());
         update(update);
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotStatusInfoServiceImpl.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotStatusInfoServiceImpl.java
index 6d3ae91..46b5c3b 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotStatusInfoServiceImpl.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotStatusInfoServiceImpl.java
@@ -15,6 +15,7 @@
 import com.casic.missiles.modular.robot.dao.RobotStatusInfoMapper;
 import com.casic.missiles.modular.robot.model.RobotInfo;
 import com.casic.missiles.modular.robot.model.RobotStatusInfo;
+import com.casic.missiles.modular.robot.opt.enums.InstructCodeEnums;
 import com.casic.missiles.modular.robot.opt.instruct.base.dto.RobotMsgDTO;
 import com.casic.missiles.modular.robot.opt.instruct.base.dto.RobotMsgResponseDTO;
 import com.casic.missiles.modular.robot.opt.instruct.base.dto.RobotResponseDTO;
@@ -26,13 +27,18 @@
 import com.casic.missiles.modular.robot.service.IRobotInfoService;
 import com.casic.missiles.modular.robot.service.IRobotStatusInfoService;
 import lombok.extern.slf4j.Slf4j;
+import org.aspectj.util.FileUtil;
 import org.eclipse.paho.client.mqttv3.MqttMessage;
+import org.springframework.beans.factory.annotation.Value;
 import org.springframework.context.annotation.Lazy;
 import org.springframework.stereotype.Service;
 import org.springframework.transaction.annotation.Transactional;
 
+import java.io.File;
 import java.io.Serializable;
 import java.math.BigDecimal;
+import java.util.ArrayList;
+import java.util.Arrays;
 import java.util.List;
 
 
@@ -147,7 +153,6 @@
     @Override
     public void saveProcessMessage(String robotId, MqttMessage message) {
 //        String json = new String(message.getPayload());
-
         RobotResponseDTO<ProcessMessageResponseDTO> responseDTO = parseJson(message, new TypeReference<RobotResponseDTO<ProcessMessageResponseDTO>>() {
         });
         if (responseDTO.getMsg() != null) {
@@ -189,9 +194,8 @@
     }
 
     @Override
-    public void updateCurrTask(String robotId, Long currRouteId, Long currTaskId) {
+    public void updateCurrTask(String robotId, Long currTaskId) {
         UpdateWrapper<RobotStatusInfo> query = new UpdateWrapper<>();
-        query.set("curr_route_id", currRouteId);
         query.set("curr_task_id", currTaskId);
         query.eq("robot_id", robotId);
         update(query);
@@ -250,6 +254,15 @@
         }
     }
 
+    @Value("${casic.algorithm.path:-1}")
+    private String path;
+
+    /**
+     * 栅格阈值
+     */
+    @Value("${casic.algorithm.num:30}")
+    private Integer num;
+
     /**
      * 机器人栅格数据
      *
@@ -262,15 +275,41 @@
         RobotResponseDTO<AppCostmapResponseDTO> responseDTO = parseJson(message, new TypeReference<RobotResponseDTO<AppCostmapResponseDTO>>() {
         });
         if (responseDTO != null && responseDTO.getMsg() != null) {
+            //发送栅格分辨率设定
+
             cacheService.updateCostMap(robotId, responseDTO.getMsg());
             robotInfoService.updateRobotGrid(robotId, responseDTO.getMsg());
-//            log.info("--------------- json data:{}", JSON.toJSONString(responseDTO.getMsg()));
+            //log.info("--------------- json data:{}", JSON.toJSONString(responseDTO.getMsg()));
+            if (responseDTO != null && responseDTO.getMsg().getData() != null) {
+                ArrayList<Integer> dataNums = new ArrayList<>();
+                //根据阈值转换栅格
+                for (Integer dataNum : responseDTO.getMsg().getData()) {
+                    if (dataNum != -1) {
+                        if (dataNum >= num) {
+                            dataNums.add(0);
+                        } else {
+                            dataNums.add(1);
+                        }
+                    }
+                }
+                writeCache(dataNums,responseDTO.getMsg().getWidth(),responseDTO.getMsg().getHeight(),robotId);
+            }
         }
     }
 
+    private void writeCache(ArrayList<Integer> integers,Integer width,Integer height,String robotId) {
+        List<List<Integer>> arrays = CollectionUtil.split(integers,width);
+        ArrayList<ArrayList<Integer>> arrayList = new ArrayList<>();
+        for (List<Integer> array : arrays) {
+            arrayList.add((ArrayList)array);
+        }
+        CollectionUtil.reverse(arrayList);
+        CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_COST_MAP + robotId, arrayList);
+//        FileUtil.writeAsString(new File(path + "map.txt"), CollectionUtil.join(integers, " ").replaceAll("\\[", "").replaceAll("]", ""));
+    }
+
     @Override
     public void updateRobotPoint(PoseMessageResponseDTO dto, String robotId) {
-        //todo cache
         UpdateWrapper<RobotStatusInfo> query = new UpdateWrapper<>();
         query.set("postion_x", dto.getX());
         query.set("postion_y", dto.getY());
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotTrajectoryInfoServiceImpl.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotTrajectoryInfoServiceImpl.java
index 6c9625f..f1262a2 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotTrajectoryInfoServiceImpl.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotTrajectoryInfoServiceImpl.java
@@ -57,29 +57,29 @@
         });
 
         //读取导航状态,如果导航未开启则不存储
-//        RobotStatusInfo statusInfo = robotStatusInfoService.getStatusByRobotId(robotId);
-        Long routeId= cacheService.readRouteId(Long.valueOf(robotId));
-        if (routeId != null && responseDTO.getMsg() != null) {
+        //RobotStatusInfo statusInfo = robotStatusInfoService.getStatusByRobotId(robotId);
+
+        if ( responseDTO.getMsg() != null) {
             PoseMessageResponseDTO dto = responseDTO.getMsg();
             //执行循迹 保存当前轨迹信息
             //更新实时定位信息
             CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.ROBOT_POINT + robotId, dto);
-            infoService.pointTestCallBack(dto, robotId);
-            if (routeId != null) {
-                RobotTrajectoryInfo info = new RobotTrajectoryInfo();
-                info.setRobotId(Long.valueOf(robotId));
-                info.setCreateTime(new Date());
-                info.setPostionX(dto.getX());
-                info.setPostionY(dto.getY());
-                info.setPostionZ(dto.getZ());
-                info.setTheta(dto.getTheta());
-                save(info);
-                try {
-                    infoService.pointCallBack(dto, robotId);
-                } catch (Exception e) {
-                    log.error("task error", e);
-                }
+            try {
+                infoService.carPathPlanning(dto, robotId);
+            } catch (Exception e) {
+                log.error("task error", e);
             }
+//            if (routeId != null) {
+//                RobotTrajectoryInfo info = new RobotTrajectoryInfo();
+//                info.setRobotId(Long.valueOf(robotId));
+//                info.setCreateTime(new Date());
+//                info.setPostionX(dto.getX());
+//                info.setPostionY(dto.getY());
+//                info.setPostionZ(dto.getZ());
+//                info.setTheta(dto.getTheta());
+//                save(info);
+
+//            }
         }
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/TaskGridInfoServiceImpl.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/TaskGridInfoServiceImpl.java
new file mode 100644
index 0000000..cf61def
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/TaskGridInfoServiceImpl.java
@@ -0,0 +1,28 @@
+package com.casic.missiles.modular.robot.service.impl;
+
+import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
+import com.baomidou.mybatisplus.extension.plugins.pagination.Page;
+import com.baomidou.mybatisplus.extension.service.impl.ServiceImpl;
+import com.casic.missiles.modular.robot.dao.TaskGridInfoMapper;
+import com.casic.missiles.modular.robot.model.TaskGridInfo;
+import com.casic.missiles.modular.robot.service.ITaskGridInfoService;
+import org.springframework.stereotype.Service;
+
+import java.util.List;
+
+
+/**
+ * <p>
+ * 任务网格信息 服务实现类
+ * </p>
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+@Service
+public class TaskGridInfoServiceImpl extends ServiceImpl<TaskGridInfoMapper, TaskGridInfo> implements ITaskGridInfoService {
+    @Override
+    public List<TaskGridInfo> selectTaskGridInfoPage(Page<TaskGridInfo> page, QueryWrapper<TaskGridInfo> query) {
+        return this.baseMapper.selectTaskGridInfoPage(page, query);
+    }
+}
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/TaskInfoServiceImpl.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/TaskInfoServiceImpl.java
index d1417b0..fd49e2e 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/TaskInfoServiceImpl.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/TaskInfoServiceImpl.java
@@ -6,10 +6,10 @@
 import cn.hutool.core.convert.Convert;
 import cn.hutool.core.date.DateUtil;
 import cn.hutool.core.io.FileUtil;
-import cn.hutool.core.thread.ExecutorBuilder;
 import cn.hutool.core.util.StrUtil;
 import com.alibaba.fastjson.JSON;
 import com.baomidou.mybatisplus.core.conditions.Wrapper;
+import com.baomidou.mybatisplus.core.conditions.query.LambdaQueryWrapper;
 import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
 import com.baomidou.mybatisplus.core.conditions.update.UpdateWrapper;
 import com.baomidou.mybatisplus.core.metadata.IPage;
@@ -47,7 +47,6 @@
 import java.io.Serializable;
 import java.math.BigDecimal;
 import java.util.*;
-import java.util.concurrent.ThreadPoolExecutor;
 
 
 /**
@@ -70,6 +69,7 @@
     private final IRobotCacheService cacheService;
     private final IBaseRobotService baseRobotService;
     private final ITaskInfoImgService imgService;
+    private final ITaskGridInfoService taskGridInfoService;
     /**
      * 初始化线程池
      */
@@ -79,12 +79,19 @@
     @Value("${casic.doc.temp}")
     private String docTemp;
     /**
+     * 5000 ms
+     */
+    @Value("${casic.task.duration:3000}")
+    private Integer stayDuration;
+    @Value("${casic.algorithm.path:-1}")
+    private String path;
+    /**
      * 障碍物精度
      */
     @Value("${casic.obstacle:10}")
     private Integer obstacle;
 
-    public TaskInfoServiceImpl(@Lazy IRouteInfoService routeInfoService, IRobotStatusInfoService statusInfoService, AbstractDictService dictService, @Lazy IAlarmRecordService recordService, @Lazy ITaskHeatMapService taskHeatMapService, IAlgorithmService algorithmService, IRobotCacheService cacheService, @Lazy IBaseRobotService baseRobotService, ITaskInfoImgService imgService) {
+    public TaskInfoServiceImpl(@Lazy IRouteInfoService routeInfoService, IRobotStatusInfoService statusInfoService, AbstractDictService dictService, @Lazy IAlarmRecordService recordService, @Lazy ITaskHeatMapService taskHeatMapService, IAlgorithmService algorithmService, IRobotCacheService cacheService, @Lazy IBaseRobotService baseRobotService, ITaskInfoImgService imgService, ITaskGridInfoService taskGridInfoService) {
         this.routeInfoService = routeInfoService;
         this.statusInfoService = statusInfoService;
         this.dictService = dictService;
@@ -94,6 +101,7 @@
         this.cacheService = cacheService;
         this.baseRobotService = baseRobotService;
         this.imgService = imgService;
+        this.taskGridInfoService = taskGridInfoService;
     }
 
     /**
@@ -184,7 +192,7 @@
         if (CollectionUtil.isEmpty(routeInfos)) {
             throw new ServiceException(500, "路线不存在,请及时更新");
         }
-        RouteInfo routeInfo = routeInfos.get(0);
+//        RouteInfo routeInfo = routeInfos.get(0);
         Integer isTask = 0;
         //开启寻源任务
         if (request.getIsTask() == 1) {
@@ -193,22 +201,21 @@
         RobotInfo robotInfo = cacheService.getRobotInfo(Convert.toLong(robotId));
         CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_IS_TASK + robotId, isTask);
         //添加任务记录
-        TaskInfo taskInfo = addTaskByStartTrack(routeInfo, robotInfo);
+        TaskInfo taskInfo = addTaskByStartTrack( robotInfo, request);
         //No.3 更新器人执行任务信息
-        Long currRouteId = routeInfo.getId();
+//        Long currRouteId = routeInfo.getId();
         Long currTaskId = taskInfo.getId();
 
-        statusInfoService.updateCurrTask(robotId, currRouteId, currTaskId);
+        statusInfoService.updateCurrTask(robotId,  currTaskId);
         //算法初始化标志创建
         CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_TASK_ID + robotId, currTaskId);
         CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_TASK_INIT + robotId, 0);
-        CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_ROUTE_ID + robotId, routeInfo.getId());
+//        CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_ROUTE_ID + robotId, routeInfo.getId());
     }
 
     private void closeTask(StartTrackRequest request, String robotId) {
         //关闭操作
         RobotStatusInfo statusInfo = statusInfoService.getStatusByRobotId(robotId);
-
         if (statusInfo.getCurrTaskId() != null) {
             UpdateWrapper<TaskInfo> updateWrapper = new UpdateWrapper<>();
             updateWrapper.set("update_time", new Date());
@@ -221,10 +228,11 @@
 
         //清空任务缓存
         robotCacheClean(robotId);
-
+        //清楚规划任务锁
+        CacheUtil.remove(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_PLANING_FLAG + robotId);
+        CacheUtil.remove(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_ESTIMATE_GRID+ robotId);
         //清空机器当前任务
-        statusInfoService.updateCurrTask(robotId, null, null);
-
+        statusInfoService.updateCurrTask(robotId, null);
     }
 
     /**
@@ -249,7 +257,6 @@
 
         //判定任务是否已经完成初始化
         Long longId = Convert.toLong(robotId);
-        Long taskId = CacheUtil.get(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_TASK_ID + robotId);
         //缓存当前点位信息
         CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_XY_POINT + robotId, dto);
         RobotInfo robotInfo = cacheService.getRobotInfo(longId);
@@ -260,86 +267,203 @@
             log.error("no config 中子源阈值未配置");
             return;
         }
-
         //读取是否是循迹任务
         Integer isTask = cacheService.readIsTask(longId);
         if (isTask != 1) {
             log.info("循迹任务,不做处理");
             return;
         }
-        //算法初始化状态读取
-        Integer initValue = cacheService.readTaskInit(longId);
-
 
         //No.1 任务初始化
+//        pathInit(longId, robotInfo, pointDTO, taskId, robotId);
+    }
+
+    @Override
+    public void carPathPlanning(PoseMessageResponseDTO dto, String robotId) {
+        //判定任务是否已经完成初始化
+        Long longId = Convert.toLong(robotId);
+        Long taskId = CacheUtil.get(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_TASK_ID + robotId);
+        //缓存当前点位信息
+        CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_XY_POINT + robotId, dto);
+        RobotInfo robotInfo = cacheService.getRobotInfo(longId);
+        //slam经纬度转栅格
+        GridPointDTO pointDTO = algorithmService.convertSlamToGrid(dto.getX(), dto.getY(), robotInfo.getResolution(), robotInfo);
+        if (robotInfo.getAlarmThreshold() == null || robotInfo.getMaxThreshold() == null) {
+            log.error("no config 中子源阈值未配置");
+            return;
+        }
+
+        //读取是否是寻源任务
+        Integer isTask = cacheService.readIsTask(longId);
+        if (isTask != 1) {
+            log.info("循迹任务,不做处理");
+            return;
+        }
+
+        TaskInfo taskInfo = getById(taskId);
+        //No.1 任务初始化
+        pathInit(longId, robotInfo, pointDTO, taskId, robotId, taskInfo.getStepLen());
+        //No.2 路线规划
+        pathPlaning(longId, robotInfo, pointDTO, taskId, robotId, taskInfo,dto);
+    }
+
+    /**
+     * 路线规划进行加锁
+     *
+     * @param robotId 机器人ID
+     * @return
+     */
+    private synchronized boolean lockPlanIngFlag(String robotId) {
+        Boolean flag = CacheUtil.get(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_PLANING_FLAG + robotId);
+        if (flag == null) {
+            CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_PLANING_FLAG + robotId, true);
+            return true;
+        }
+        return false;
+    }
+
+    /**
+     * @param longId    机器人ID
+     * @param robotInfo 机器人信息
+     * @param pointDTO  当前点位栅格信息
+     * @param taskId    任务ID
+     * @param robotId   机器人ID
+     */
+    private void pathPlaning(Long longId, RobotInfo robotInfo, GridPointDTO pointDTO, Long taskId, String robotId, TaskInfo taskInfo,PoseMessageResponseDTO curPoint) {
+        if (taskId != null) {
+            //规划节点
+            PointNextDTO pointNextDTO = CacheUtil.get(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_ESTIMATE_GRID + robotId);
+            GridPointDTO oldEstimateGrid = GridPointDTO.convertGridPoint(pointNextDTO);
+            Boolean flag = lockPlanIngFlag(robotId);
+            //是否到达预期栅格
+            if (!flag) {
+                log.info("未获取到规划任务,直接结束：{}",pointDTO);
+                return;
+            }
+            try {
+                log.debug("oldEstimateGrid:{}", oldEstimateGrid);
+                if (pointNextDTO == null || isEquals(pointDTO, oldEstimateGrid)) {
+                    log.info("当前栅格变更......", pointDTO);
+                    CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_POINT + robotId, pointDTO);
+                    //算法调用获取热力图
+                    PathPlanDTO pathPlanDTO = createPlanDTO(robotInfo, pointDTO, taskId, robotId, taskInfo.getStepLen());
+                    if (taskInfo.getMaxThreshold() == null) {
+                        taskInfo.setMaxThreshold(robotInfo.getMaxThreshold());
+                    }
+                    pathPlanDTO.setTh(taskInfo.getMaxThreshold() + "");
+
+                    log.debug("heat map -- robotId:{},taskId:{}", robotId, taskId);
+                    pathPlanDTO.setZNow(cacheService.getNeutronCount(longId));
+
+                    //存储算法输出栅格信息
+                    AlgorithmResponse response = algorithmService.pathPlanning(pathPlanDTO);
+                    CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_ESTIMATE_GRID + robotId, response.getPointNextDTO());
+                    sendWebSocket(InstructStatusMsgEnums.HEAT_MAP, robotId, response.getTaskHeatMap());
+
+                    //存储循迹任务信息
+                    TaskGridInfo taskGridInfo = new TaskGridInfo();
+                    taskGridInfo.setCreateTime(new Date());
+                    taskGridInfo.setUpdateTime(new Date());
+                    taskGridInfo.setCurGridX(pointDTO.getX().intValue());
+                    taskGridInfo.setCurGridY(pointDTO.getY().intValue());
+                    taskGridInfo.setNeutronCount(new BigDecimal(pathPlanDTO.getZNow()));
+                    taskGridInfo.setEstimateGridX(response.getPointNextDTO().getX());
+                    taskGridInfo.setEstimateGridY(response.getPointNextDTO().getY());
+                    taskGridInfo.setTaskId(taskId);
+                    taskGridInfoService.save(taskGridInfo);
+
+                    try {
+                        //停留时间阈值
+                        Thread.sleep(stayDuration);
+                    } catch (InterruptedException e) {
+                        e.printStackTrace();
+                    }
+
+                    //是否触发停止状态
+                    if (!response.isEnd()) {
+                        log.debug("task planning-- xun dian robotId:{},taskId:{}", robotId, taskId);
+                        //机器人寻点指令下发
+                        sendTargetPoint(robotInfo, response,curPoint);
+                    } else {
+                        log.debug("task stop-- robotId:{},taskId:{}", robotId, taskId);
+                        //机器人停止指令下发
+                        SoftwareStopRequest request = new SoftwareStopRequest();
+                        request.setRobotId(robotInfo.getId());
+                        //设置为急停
+                        request.setControl(1);
+//                        baseRobotService.execCmdHandler(request, InstructCodeEnums.SOFTWARE_STOP);
+                        //循迹任务结束
+                        closeTask(null, robotId);
+                    }
+                }
+            } catch (Exception e) {
+                e.printStackTrace();
+            } finally {
+                //释放锁
+                CacheUtil.remove(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_PLANING_FLAG + robotId);
+            }
+        }
+    }
+
+    private PathPlanDTO createPlanDTO(RobotInfo robotInfo, GridPointDTO pointDTO, Long taskId, String robotId, Integer stepLen) {
+        PathPlanDTO pathPlanDTO = new PathPlanDTO();
+        pathPlanDTO.setTaskId(taskId);
+        pathPlanDTO.setRobotId(robotId);
+        pathPlanDTO.setStepLen(stepLen);
+        return pathPlanDTO;
+    }
+
+    /**
+     * 寻源初始化
+     *
+     * @param longId    机器ID
+     * @param robotInfo 机器信息
+     * @param pointDTO  当前点位信息
+     * @param taskId    任务ID
+     * @param robotId   机器ID
+     * @param stepLen   步长设置
+     */
+    private void pathInit(Long longId, RobotInfo robotInfo, GridPointDTO pointDTO, Long taskId, String robotId, Integer stepLen) {
+        //障碍物栅格上传已生成 直接读取
+        //算法初始化状态读取
+        Integer initValue = cacheService.readTaskInit(longId);
+        //No.1 任务初始化
         if (initValue != 1) {
+
             //执行初始化操作
             if (cacheService.updateTaskInit(longId, 1)) {
 
-                try {
-                    File file = new File(".");
-                    File[] files = file.listFiles((dir, name) -> (name.endsWith(".png") && name.startsWith("step"))
-                            || (name.endsWith(".txt") && name.startsWith("step")) || name.equals("repetition.txt") || name.equals("xy_train.txt") || name.equals("z_train.txt"));
-                    if (files != null) {
-                        for (int i = 0; i < files.length; i++) {
-                            FileUtil.del(files[i]);
+                List<List<Integer>> costList = CacheUtil.get(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_COST_MAP + robotId);
+                List<String> strs = new ArrayList<>();
+                List<String> strs2 = new ArrayList<>();
+                for (List<Integer> list : costList) {
+                    strs2.add(CollectionUtil.join(list, " "));
+                }
+                FileUtil.writeLines(strs2, new File(path + "map2.txt"), "utf-8");
+                //行转列 为算法输入数据
+                if(CollectionUtil.isNotEmpty(costList)){
+                    CollectionUtil.reverse(costList);
+                    for (int i = 0; i < costList.get(0).size(); i++) {
+                        ArrayList<Integer> ints = new ArrayList<>();
+                        for (List<Integer> list : costList) {
+                            ints.add(list.get(i));
                         }
+                        strs.add(CollectionUtil.join(ints, " "));
                     }
-                } catch (Exception e) {
-                    e.printStackTrace();
+//                    CollectionUtil.reverse(strs);
                 }
 
-                PathInitDTO pathInitDTO = createInitDTO(robotInfo, pointDTO, taskId, robotId);
+
+                FileUtil.writeLines(strs, new File(path + "map.txt"), "utf-8");
+
+                PathInitDTO pathInitDTO = createInitDTO(robotInfo, pointDTO, taskId, robotId, stepLen);
                 //中子源计数率读取
-                pathInitDTO.setCount(cacheService.getNeutronCount(longId));
+                pathInitDTO.setZNow(cacheService.getNeutronCount(longId));
+                log.info("init params:{}", JSON.toJSONString(pathInitDTO));
                 //执行1次算法 初始化操作
                 algorithmService.pathPlanningInit(pathInitDTO);
             }
         }
-
-        //No.2 任务执行中
-        if (taskId != null) {
-            //判断栅格变化
-            GridPointDTO oldPointDTO = CacheUtil.get(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_POINT + robotId);
-            if (oldPointDTO == null || isChange(pointDTO, oldPointDTO)) {
-                log.debug("grid change -- robotId:{},taskId:{}", robotId, taskId);
-                CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_POINT + robotId, pointDTO);
-                //算法调用获取热力图
-                PathPlanDTO pathPlanDTO = createPlanDTO(robotInfo, pointDTO, taskId, robotId);
-                //可通行标记为1 不可通行标记为0
-                String obstacle = getObstacle(robotId, pointDTO);
-                //TO-DO 障碍物判定信息
-                pathPlanDTO.setOpen(obstacle);
-                //中子源计数率读取
-                pathPlanDTO.setCount(cacheService.getNeutronCount(longId));
-                log.debug("heat map -- robotId:{},taskId:{}", robotId, taskId);
-                AlgorithmResponse response = algorithmService.pathPlanning(pathPlanDTO);
-                //存储算法输出栅格信息
-                CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_ESTIMATE_GRID + robotId, response.getPointNextDTO());
-                sendWebSocket(InstructStatusMsgEnums.HEAT_MAP, robotId, response.getTaskHeatMap());
-                //是否触发寻源操作
-                BigDecimal bigCount = new BigDecimal(pathPlanDTO.getCount());
-                if (bigCount.compareTo(robotInfo.getAlarmThreshold()) >= 0) {
-                    log.debug("task planning-- xun dian robotId:{},taskId:{}", robotId, taskId);
-                    //机器人寻点指令下发
-                    sendTargetPoint(robotInfo, response);
-                }
-
-                //是否触发停止状态
-                if (bigCount.compareTo(robotInfo.getMaxThreshold()) > 0) {
-                    log.debug("task stop-- robotId:{},taskId:{}", robotId, taskId);
-                    //机器人停止指令下发
-                    SoftwareStopRequest request = new SoftwareStopRequest();
-                    request.setRobotId(robotInfo.getId());
-                    //设置为急停
-                    request.setControl(1);
-                    baseRobotService.execCmdHandler(request, InstructCodeEnums.SOFTWARE_STOP);
-                    //循迹任务结束
-                    closeTask(null, robotId);
-                }
-
-            }
-        }
     }
 
     /**
@@ -353,7 +477,6 @@
         //判定任务是否已经完成初始化
         Long longId = Convert.toLong(robotId);
         Long taskId = CacheUtil.get(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_TASK_ID + robotId);
-
         RobotInfo robotInfo = cacheService.getRobotInfo(longId);
         //slam经纬度转栅格
         GridPointDTO pointDTO = algorithmService.convertSlamToGrid(dto.getX(), dto.getY(), robotInfo.getResolution(), robotInfo);
@@ -428,7 +551,7 @@
      * @param robotInfo 机器人信息
      * @param response  算法输出数据
      */
-    private void sendTargetPoint(RobotInfo robotInfo, AlgorithmResponse response) {
+    private void sendTargetPoint(RobotInfo robotInfo, AlgorithmResponse response,PoseMessageResponseDTO curPoint) {
         //读取已设置巡点目标信息
         PointNextDTO nextDTO = CacheUtil.get(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_TARGET_POINT_ID + robotInfo.getId());
         //巡点
@@ -439,8 +562,8 @@
             TargetPointRequest request = new TargetPointRequest();
             request.setX(Convert.toStr(gridCenterPoint.getX()));
             request.setY(Convert.toStr(gridCenterPoint.getY()));
-            request.setZ("0");
-
+            request.setZ(curPoint.getZ().toString());
+            request.setTheta(curPoint.getTheta().toString());
             request.setRobotId(robotInfo.getId());
             //执行巡点指令
             baseRobotService.execCmdHandler(request, InstructCodeEnums.SET_TARGETPOINT);
@@ -507,21 +630,20 @@
         params.put("startTime", startTime);
         params.put("endTime", endTime);
         //读取
-        List<AlarmRecord> records = recordService.selectListByTaskId(info.getId(), info.getRobotId());
         Integer index = 1;
-
-        if (CollectionUtil.isNotEmpty(records)) {
-            for (AlarmRecord record : records) {
+        LambdaQueryWrapper<TaskGridInfo> taskGridQuery = new LambdaQueryWrapper<>();
+        taskGridQuery.eq(TaskGridInfo::getTaskId,info.getId());
+        taskGridQuery.orderByAsc(TaskGridInfo::getCreateTime);
+        List<TaskGridInfo> taskGridInfos = taskGridInfoService.list(taskGridQuery);
+        if (CollectionUtil.isNotEmpty(taskGridInfos)) {
+            for (TaskGridInfo record : taskGridInfos) {
                 //栅格信息转换
-                GridPointDTO pointDTO = algorithmService.convertSlamToGrid(record.getX(), record.getY(), info.getResolution(), robotInfo);
-                record.setX(pointDTO.getX());
-                record.setY(pointDTO.getY());
                 record.setIndex(index);
                 index++;
             }
         }
 
-        params.put("records", records);
+        params.put("records", taskGridInfos);
         ImageEntity mcaImg = new ImageEntity();
         mcaImg.setHeight(600);
         mcaImg.setWidth(600);
@@ -557,7 +679,6 @@
             psdImg.setData(WordUtil.getImageBase64(uploadPath + info.getPsdImg()));
         }
         params.put("psdImg", psdImg);
-
         WordUtil.exportOutputStream(docTemp, response.getOutputStream(), params);
     }
 
@@ -589,7 +710,7 @@
     private String saveHeatImg(String taskId) {
         String newFileName = "task\\" + taskId + ".png";
         //读取当前路径图片
-        File file = new File(".");
+        File file = new File(path + "\\" + taskId + "\\figs");
         File[] files = file.listFiles((dir, name) -> name.endsWith(".png") && name.startsWith("step"));
         //判定当前执行路径最大step图片
         if (files != null && files.length > 0) {
@@ -614,33 +735,28 @@
         return true;
     }
 
-    private PathPlanDTO createPlanDTO(RobotInfo robotInfo, GridPointDTO pointDTO, Long taskId, String robotId) {
-        PathPlanDTO pathPlanDTO = new PathPlanDTO();
-        String xMax = robotInfo.getGridHeight().divide(robotInfo.getResolution(), 5, BigDecimal.ROUND_HALF_UP).intValue() + "";
-        String yMax = robotInfo.getGridWidth().divide(robotInfo.getResolution(), 5, BigDecimal.ROUND_HALF_UP).intValue() + "";
-        pathPlanDTO.setX(pointDTO.getX().intValue() + "");
-        pathPlanDTO.setY(pointDTO.getY().intValue() + "");
-        pathPlanDTO.setXMax(xMax);
-        pathPlanDTO.setYMax(yMax);
-        pathPlanDTO.setTaskId(taskId);
-        pathPlanDTO.setRobotId(robotId);
-        return pathPlanDTO;
+    private boolean isEquals(GridPointDTO pointDTO, GridPointDTO oldPointDTO) {
+        if(oldPointDTO==null||oldPointDTO.getX()==null){
+            return false;
+        }
+        if (pointDTO.getX().compareTo(oldPointDTO.getX()) == 0 && pointDTO.getY().compareTo(oldPointDTO.getY()) == 0) {
+            return true;
+        }
+        return false;
     }
 
-    private PathInitDTO createInitDTO(RobotInfo robotInfo, GridPointDTO pointDTO, Long taskId, String robotId) {
-        String xMax = robotInfo.getGridHeight().divide(robotInfo.getResolution(), 5, BigDecimal.ROUND_HALF_UP).intValue() + "";
-        String yMax = robotInfo.getGridWidth().divide(robotInfo.getResolution(), 5, BigDecimal.ROUND_HALF_UP).intValue() + "";
+
+    private PathInitDTO createInitDTO(RobotInfo robotInfo, GridPointDTO pointDTO, Long taskId, String robotId, Integer stepLen) {
         PathInitDTO pathInitDTO = new PathInitDTO();
-        pathInitDTO.setXMax(xMax);
-        pathInitDTO.setYMax(yMax);
-        pathInitDTO.setX(pointDTO.getX().intValue() + "");
-        pathInitDTO.setY(pointDTO.getY().intValue() + "");
+        pathInitDTO.setXNow(pointDTO.getX().intValue() + "");
+        pathInitDTO.setYNow(pointDTO.getY().intValue() + "");
+        pathInitDTO.setStepLen(stepLen);
         pathInitDTO.setTaskId(taskId);
         pathInitDTO.setRobotId(robotId);
         return pathInitDTO;
     }
 
-    private TaskInfo addTaskByStartTrack(RouteInfo routeInfo, RobotInfo robotInfo) {
+    private TaskInfo addTaskByStartTrack( RobotInfo robotInfo, StartTrackRequest request) {
         //No.2 新增任务信息
         TaskInfo taskInfo = new TaskInfo();
         String currDate = DateUtil.format(new Date(), "yyyyMMdd");
@@ -654,16 +770,20 @@
         taskInfo.setCreateTime(new Date());
         //任务类型 中子源
         taskInfo.setTaskType(1);
-        taskInfo.setDiscernType(1);
-        taskInfo.setTaskName(routeInfo.getRouteName() + "巡线任务");
+        taskInfo.setDiscernType(2);
+        taskInfo.setTaskName("自主巡检任务");
         //执行状态 未完成
         taskInfo.setTaskStatus(0);
         taskInfo.setStartTime(new Date());
-        taskInfo.setRouteId(routeInfo.getId());
+        taskInfo.setRouteId(null);
         taskInfo.setRobotId(robotInfo.getId());
         taskInfo.setGridHeight(robotInfo.getGridHeight());
         taskInfo.setGridWidth(robotInfo.getGridWidth());
         taskInfo.setResolution(robotInfo.getResolution());
+
+        taskInfo.setStepLen(request.getStepLen());
+        taskInfo.setMaxThreshold(request.getMaxThreshold());
+
         save(taskInfo);
         return taskInfo;
     }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/utils/AlgorithmUtils.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/utils/AlgorithmUtils.java
index 22864d1..467b9c2 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/utils/AlgorithmUtils.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/utils/AlgorithmUtils.java
@@ -1,6 +1,7 @@
 package com.casic.missiles.modular.robot.utils;
 
 import java.io.BufferedReader;
+import java.io.File;
 import java.io.IOException;
 import java.io.InputStreamReader;
 import java.util.ArrayList;
@@ -16,11 +17,13 @@
     public static List<String> invokeJob(List<String> params) {
         List<String> res = new ArrayList<>();
         try {
-
             BufferedReader br = null;
             BufferedReader brError;
             String line = null;
-            Process p = new ProcessBuilder(params).start();
+            ProcessBuilder processBuilder =  new ProcessBuilder(params);
+            processBuilder.directory(new File("D:\\workspace\\server\\keti\\casic-robot-inspection\\casic-web\\src\\main\\resources\\config\\path"));
+            Process p = processBuilder.start();
+
             br = new BufferedReader(new InputStreamReader(p.getInputStream()));
             brError = new BufferedReader(new InputStreamReader(p.getErrorStream()));
             while ((line = br.readLine()) != null || (line = brError.readLine()) != null) {
@@ -37,33 +40,29 @@
     public static void main(String[] args) {
         String path = "D:\\workspace\\server\\keti\\casic-robot-inspection\\casic-web\\src\\main\\resources\\config\\path\\";
         //算法初始化调用
-        String exePath = path + "PathPlanning_init.exe";
+        String exePath = path + "init.exe";
 
         ArrayList<String> invocation = new ArrayList<String>();
         invocation.add(exePath);
-        invocation.add("40");
-        invocation.add("20");
-        invocation.add("0.5");
+        invocation.add("10");
+        invocation.add("10");
         invocation.add("5");
-        invocation.add("5");
+        invocation.add("0.16");
+        invocation.add("Mission1");
         invokeJob(invocation);
 
         //{"count":"null","robotId":"1","taskId":"1748224487696142338","x":"8","xMax":"100","y":"2","yMax":"100"}
-        String planning = path + "PathPlanning.exe";
         //{"open":"0,0,0,0,0,0,0,0","robotId":"1","taskId":"1748250669749735425","x":"6","xMax":"100","y":"2","yMax":"100"}
         //{"open":"1,1,1,1,1,1,1,1","robotId":"1","taskId":"1748252894098178049","x":"6","xMax":"100","y":"2","yMax":"100"}
         //{"count":"6.0","open":"1,1,1,1,1,1,1,1","robotId":"1","taskId":"1750713404601159681","x":"6","xMax":"20","y":"2","yMax":"40"}
+        String planning = path + "search.exe";
         ArrayList<String> planInvocation = new ArrayList<String>();
         planInvocation.add(planning);
-        planInvocation.add("40");
-        planInvocation.add("40");
-        planInvocation.add("1,1,0,1,1,1,1,1");
-        planInvocation.add("6.0");
-        //next_X
-        planInvocation.add("6");
-        //next_Y
-        planInvocation.add("2");
-        ProcessBuilder processBuilder = new ProcessBuilder();
-        invokeJob(planInvocation);
+        planInvocation.add("10");
+        planInvocation.add("1.0");
+        planInvocation.add("0.2");
+        planInvocation.add("Mission1");
+        List<String> res = invokeJob(planInvocation);
+        System.out.println(res);
     }
 }
\ No newline at end of file
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/vo/TaskGridInfoVO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/vo/TaskGridInfoVO.java
new file mode 100644
index 0000000..0aff5bf
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/vo/TaskGridInfoVO.java
@@ -0,0 +1,58 @@
+package com.casic.missiles.modular.robot.vo;
+
+import io.swagger.annotations.ApiModelProperty;
+import lombok.Data;
+import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
+import com.casic.missiles.modular.robot.model.TaskGridInfo;
+import cn.hutool.core.util.StrUtil;
+
+import java.math.BigDecimal;
+
+/**
+ * 任务网格信息对象VO
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+@Data
+public class TaskGridInfoVO {
+    private static final long serialVersionUID = 1L;
+
+
+            @ApiModelProperty(value = "任务ID" , dataType = "Long")
+            private Long taskId;
+
+            @ApiModelProperty(value = "估算栅格x" , dataType = "Long")
+            private Long estimateGridX;
+
+            @ApiModelProperty(value = "估算栅格y" , dataType = "Long")
+            private Long estimateGridY;
+
+            @ApiModelProperty(value = "中子计数率" , dataType = "BigDecimal")
+            private BigDecimal neutronCount;
+
+            @ApiModelProperty(value = "当前栅格x" , dataType = "Long")
+            private Long curGridX;
+
+            @ApiModelProperty(value = "当前栅格y" , dataType = "Long")
+            private Long curGridY;
+
+public QueryWrapper<TaskGridInfo> genQuery() {
+QueryWrapper<TaskGridInfo> query = new QueryWrapper<>();
+if (taskId!=null) {
+        query.eq("task_id",taskId);
+}
+if (estimateGridX!=null) {
+        query.eq("estimate_grid_x",estimateGridX);
+}
+if (estimateGridY!=null) {
+        query.eq("estimate_grid_y",estimateGridY);
+}
+if (curGridX!=null) {
+        query.eq("cur_grid_x",curGridX);
+}
+if (curGridY!=null) {
+        query.eq("cur_grid_y",curGridY);
+}
+return query;
+}}

diff --git a/.gitignore b/.gitignore
index 9e20bc1..f42a9cb 100644
--- a/.gitignore
+++ b/.gitignore
@@ -2,3 +2,4 @@
 **/target/**
 logs/
 *.iml
+*.exe
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java b/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
index c7b5d28..a849528 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
@@ -28,6 +28,7 @@
     String KEY_CUR_TASK_ID = "CUR_TASK_ID_";
     String KEY_CUR_TASK_INIT = "CUR_TASK_INIT_";
 
+    String KEY_COST_MAP = "KEY_COST_MAP";
     /**
      * PSD数据ID
      */
@@ -41,6 +42,7 @@
      */
     String KEY_CUR_XY_POINT = "KEY_CUR_XY_POINT_";
     String KEY_CUR_ESTIMATE_GRID = "KEY_CUR_ESTIMATE_GRID_";
+    String KEY_PLANING_FLAG = "KEY_PLANING_FLAG_";
     /**
      * 机器人巡点栅格
      */
@@ -55,6 +57,7 @@
      * 探测器配置参数缓存
      */
     String KEY_CONFIG_PARAMS_ID = "KEY_CONFIG_PARAMS_ID_";
+    String KEY_CONFIG_COSTMAP_RESOLUTION_SET = "KEY_CONFIG_COSTMAP_RESOLUTION_SET_";
 
     /**
      * 暂停状态缓存信息
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
index c4c905d..3bb475f 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
@@ -1,9 +1,14 @@
 package com.casic.missiles.modular.robot.controller;
 
+import cn.hutool.core.lang.Assert;
 import cn.hutool.core.util.StrUtil;
 import com.casic.missiles.core.base.controller.BaseController;
+import com.casic.missiles.core.cache.CacheUtil;
 import com.casic.missiles.core.util.SpringContextHolder;
+import com.casic.missiles.model.exception.RequestValidException;
+import com.casic.missiles.model.response.ResponseData;
 import com.casic.missiles.model.response.dto.ResponseDataDTO;
+import com.casic.missiles.modular.constants.RobotDictConstants;
 import com.casic.missiles.modular.robot.IBaseRobotService;
 import com.casic.missiles.modular.robot.dto.RobotInfoSetDTO;
 import com.casic.missiles.modular.robot.model.RobotInfo;
@@ -19,10 +24,16 @@
 import io.swagger.annotations.ApiOperation;
 import io.swagger.annotations.ApiParam;
 import lombok.SneakyThrows;
+import lombok.extern.slf4j.Slf4j;
+import org.apache.ibatis.annotations.Param;
+import org.springframework.beans.factory.annotation.Value;
 import org.springframework.web.bind.annotation.*;
+import org.springframework.web.multipart.MultipartFile;
 
 import javax.servlet.http.HttpServletResponse;
+import java.io.File;
 import java.util.List;
+import java.util.Optional;
 
 /**
  * 机器人操作类
@@ -30,6 +41,7 @@
  * @author lwh
  * @date 2023-10-30
  */
+@Slf4j
 @Api(tags = "机器人控制接口")
 @RestController
 @RequestMapping("/robot/opt")
@@ -61,6 +73,28 @@
         return baseRobotService.execCmdHandler(request, InstructCodeEnums.CHANGE_PCD);
     }
 
+    @Value("${casic.file.uploadPath}")
+    private String filePath;
+
+    @SneakyThrows
+    @ApiOperation(value = "PCD上传接口")
+    @PostMapping(value = "/uploadPcd")
+    public ResponseData uploadPcd(@RequestParam("file") MultipartFile file, @ApiParam("机器人ID,url拼接传参") String robotId) {
+        Assert.isFalse(StrUtil.isEmpty(robotId), () -> new RequestValidException("机器ID不能为空", Optional.of(new Throwable().getStackTrace()[2])));
+        RobotInfo info = robotInfoService.getById(robotId);
+        if (file != null && !file.isEmpty()) {
+            File pcdFile = new File(filePath + "finalCloud_" + info.getId() + ".pcd");
+            try {
+                file.transferTo(pcdFile);
+            } catch (Exception e) {
+                log.error(e.getMessage());
+            }
+            return ResponseData.success();
+        } else {
+            return ResponseData.error("附件上传失败!");
+        }
+    }
+
     @SneakyThrows
     @ApiOperation(value = "PCD下载接口")
     @PostMapping(value = "/pcdDownload")
@@ -136,6 +170,10 @@
     @ApiOperation(value = "启动或关闭循迹功能", notes = "返回 1成功 0失败  wait 启动时是否等待，0代表循迹启动即运动，不等待；1代表循迹启动原地等待。若无此字段，默认循迹启动不等待直接运动")
     @PostMapping(value = "/startTrack")
     public ResponseDataDTO<Integer> startTrack(@RequestBody StartTrackRequest request) {
+        request.setIsTask(1);
+        if (request.getStepLen() == null) {
+            request.setStepLen(2);
+        }
         baseRobotService.startTrack(request);
         return ResponseDataDTO.success();
     }
@@ -156,6 +194,13 @@
         MessageRequestDTO<CrlNavRequest> messageDTO = new MessageRequestDTO();
         messageDTO.setMsg(request);
         ResponseDataDTO<Integer> list = baseRobotService.getRobotRouteOptService().robotCmd(info, InstructCodeEnums.CTRL_NAV, messageDTO);
+        if (request.getAction() == 1) {
+            CostMapResolutionRequest costMap = new CostMapResolutionRequest();
+            costMap.setResolution(info.getResolution().doubleValue());
+            request.setRobotId(request.getRobotId());
+            //是否下发分辨率
+            CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CONFIG_COSTMAP_RESOLUTION_SET + request.getRobotId(), costMap);
+        }
         return list;
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java
new file mode 100644
index 0000000..5317651
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java
@@ -0,0 +1,86 @@
+package com.casic.missiles.modular.robot.controller;
+
+import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
+import com.baomidou.mybatisplus.extension.plugins.pagination.Page;
+import com.casic.missiles.core.base.controller.BaseController;
+import com.casic.missiles.core.page.PageFactory;
+import com.casic.missiles.core.page.PageInfoBT;
+import com.casic.missiles.model.form.IdForms;
+import com.casic.missiles.model.response.dto.ResponseDataDTO;
+import com.casic.missiles.modular.robot.model.TaskGridInfo;
+import com.casic.missiles.modular.robot.service.ITaskGridInfoService;
+import com.casic.missiles.modular.robot.vo.TaskGridInfoVO;
+import io.swagger.annotations.Api;
+import io.swagger.annotations.ApiOperation;
+import org.springframework.web.bind.annotation.*;
+
+import java.util.List;
+
+/**
+ * 任务网格信息Controller
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+@Api(tags = "任务网格信息")
+@RestController
+@RequestMapping("/robot/taskGridInfo")
+public class TaskGridInfoController extends BaseController {
+
+    private final ITaskGridInfoService taskGridInfoService;
+
+    public TaskGridInfoController(ITaskGridInfoService taskGridInfoService) {
+        this.taskGridInfoService = taskGridInfoService;
+    }
+
+    @ApiOperation(value = "列表查询")
+    @GetMapping(value = "/list")
+    public ResponseDataDTO<List<TaskGridInfo>> list(TaskGridInfoVO taskGridInfoVO) {
+        QueryWrapper<TaskGridInfo> query = taskGridInfoVO.genQuery();
+        return ResponseDataDTO.success(taskGridInfoService.list(query));
+    }
+
+    @ApiOperation(value = "分页查询")
+    @GetMapping(value = "/listPage")
+    public ResponseDataDTO<PageInfoBT<TaskGridInfo>> listPage(TaskGridInfoVO taskGridInfoVO) {
+        Page<TaskGridInfo> page = PageFactory.defaultPage();
+        QueryWrapper<TaskGridInfo> query = taskGridInfoVO.genQuery();
+        page = taskGridInfoService.page(page, query);
+        return ResponseDataDTO.success(super.packForBT(page));
+    }
+
+
+    @ApiOperation(value = "新增接口")
+    @PostMapping(value = "/add")
+    public ResponseDataDTO add(@RequestBody TaskGridInfo taskGridInfo) {
+        taskGridInfoService.save(taskGridInfo);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "修改接口")
+    @PostMapping(value = "/update")
+    public ResponseDataDTO update(@RequestBody TaskGridInfo taskGridInfo) {
+        taskGridInfoService.updateById(taskGridInfo);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "删除接口")
+    @GetMapping(value = "/delete")
+    public ResponseDataDTO delete(String id) {
+        taskGridInfoService.removeById(id);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation("批量删除")
+    @PostMapping("/batchDelete")
+    public ResponseDataDTO batchDelete(@RequestBody IdForms ids) {
+        taskGridInfoService.removeByIds(ids.getIds());
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "详情查询")
+    @GetMapping(value = "/detail")
+    public ResponseDataDTO<TaskGridInfo> detail(String id) {
+        return ResponseDataDTO.success(taskGridInfoService.getById(id));
+    }
+}
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
index e82a792..6992274 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
@@ -98,7 +98,7 @@
     public ResponseDataDTO<AlgorithmResponse> test() {
         AlgorithmResponse response = new AlgorithmResponse();
         response.setTaskHeatMap(new TaskHeatMap());
-        algorithmService.readAlgorithmResponse("step3.txt",response,123L,null);
+        algorithmService.readAlgorithmResponse("step3.txt",null,response,123L,null);
         sendWebSocket(InstructStatusMsgEnums.HEAT_MAP, "1", response.getTaskHeatMap());
         return ResponseDataDTO.success(response);
     }
@@ -121,7 +121,7 @@
     @GetMapping(value = "/testGrid")
     public ResponseDataDTO<List<RouteInfo>> testGrid(@RequestBody PathInitDTO pathInitDTO) {
         RobotInfo robotInfo = infoService.getById(pathInitDTO.getRobotId());
-        GridPointDTO gridPointDTO = algorithmService.convertSlamToGrid(new BigDecimal(pathInitDTO.getX()), new BigDecimal(10), new BigDecimal(4), robotInfo);
+        GridPointDTO gridPointDTO = algorithmService.convertSlamToGrid(new BigDecimal(pathInitDTO.getXNow()), new BigDecimal(10), new BigDecimal(4), robotInfo);
         return ResponseDataDTO.success(algorithmService.convertGridCenterPoint(gridPointDTO, new BigDecimal(10), robotInfo));
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/TaskGridInfoMapper.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/TaskGridInfoMapper.java
new file mode 100644
index 0000000..5c3a70b
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/TaskGridInfoMapper.java
@@ -0,0 +1,23 @@
+package com.casic.missiles.modular.robot.dao;
+
+
+import com.baomidou.mybatisplus.core.mapper.BaseMapper;
+import com.casic.missiles.modular.robot.model.TaskGridInfo;
+import org.apache.ibatis.annotations.Param;
+import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
+import com.baomidou.mybatisplus.extension.plugins.pagination.Page;
+import java.util.List;
+/**
+ * <p>
+ *  任务网格信息 Mapper 接口
+ * </p>
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+public interface TaskGridInfoMapper extends BaseMapper<TaskGridInfo> {
+/**
+* 任务网格信息 分页检索
+*/
+List<TaskGridInfo> selectTaskGridInfoPage( @Param("page") Page<TaskGridInfo> page,@Param("ew" ) QueryWrapper<TaskGridInfo> query);
+}
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/mapping/TaskGridInfoMapper.xml b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/mapping/TaskGridInfoMapper.xml
new file mode 100644
index 0000000..be573e0
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/mapping/TaskGridInfoMapper.xml
@@ -0,0 +1,80 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!DOCTYPE mapper PUBLIC "-//mybatis.org//DTD Mapper 3.0//EN" "http://mybatis.org/dtd/mybatis-3-mapper.dtd">
+<mapper namespace="com.casic.missiles.modular.robot.dao.TaskGridInfoMapper">
+    <!-- 通用查询映射结果 -->
+    <resultMap id="BaseResultMap" type="com.casic.missiles.modular.robot.model.TaskGridInfo">
+        <result column="id" property="id"/>
+        <result column="create_time" property="createTime"/>
+        <result column="update_time" property="updateTime"/>
+        <result column="task_id" property="taskId"/>
+        <result column="estimate_grid_x" property="estimateGridX"/>
+        <result column="estimate_grid_y" property="estimateGridY"/>
+        <result column="neutron_count" property="neutronCount"/>
+        <result column="cur_grid_x" property="curGridX"/>
+        <result column="cur_grid_y" property="curGridY"/>
+    </resultMap>
+    <!-- 通用查询结果列 -->
+    <sql id="Base_Column_List">
+        id
+        ,
+                    create_time as createTime ,
+                    update_time as updateTime ,
+                    task_id as taskId ,
+                    estimate_grid_x as estimateGridX ,
+                    estimate_grid_y as estimateGridY ,
+                    neutron_count as neutronCount ,
+                    cur_grid_x as curGridX ,
+                    cur_grid_y as curGridY
+    </sql>
+    <sql id="Base_Column_Alias">
+        a
+        .
+        id
+        ,
+            a.create_time
+            as createTime ,
+            a.update_time
+            as updateTime ,
+            a.task_id
+            as taskId ,
+            a.estimate_grid_x
+            as estimateGridX ,
+            a.estimate_grid_y
+            as estimateGridY ,
+            a.neutron_count
+            as neutronCount ,
+            a.cur_grid_x
+            as curGridX ,
+            a.cur_grid_y
+            as curGridY
+    </sql>
+    <select id="selectTaskGridInfoPage" resultType="com.casic.missiles.modular.robot.model.TaskGridInfo">
+        select
+        <include refid="Base_Column_Alias"/>
+        from task_grid_info a
+        <where>
+            <if test="ew != null">
+                ${ew.sqlSegment}
+            </if>
+        </where>
+    </select>
+    <sql id="Base_Column_Query">
+        <if test="request!=null">
+            <if test="request.taskId!= null and request.taskId!=''">
+                and task_id = #{request.taskId}
+            </if>
+            <if test="request.estimateGridX!= null and request.estimateGridX!=''">
+                and estimate_grid_x = #{request.estimateGridX}
+            </if>
+            <if test="request.estimateGridY!= null and request.estimateGridY!=''">
+                and estimate_grid_y = #{request.estimateGridY}
+            </if>
+            <if test="request.curGridX!= null and request.curGridX!=''">
+                and cur_grid_x = #{request.curGridX}
+            </if>
+            <if test="request.curGridY!= null and request.curGridY!=''">
+                and cur_grid_y = #{request.curGridY}
+            </if>
+        </if>
+    </sql>
+</mapper>
\ No newline at end of file
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java
index 749f559..4559074 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java
@@ -13,4 +13,9 @@
      * 规划点位信息
      */
     private PointNextDTO pointNextDTO;
+    /**
+     * 寻源目标地址
+     */
+    private PointNextDTO coordinate;
+    private boolean isEnd;
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java
index 39ff2a2..cef3238 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java
@@ -12,6 +12,9 @@
     private BigDecimal y;
 
     public static GridPointDTO convertGridPoint(PointNextDTO nextDTO) {
+        if(nextDTO==null){
+            return null;
+        }
         GridPointDTO gridPointDTO = new GridPointDTO();
         gridPointDTO.setX(Convert.toBigDecimal(nextDTO.getX()));
         gridPointDTO.setY(Convert.toBigDecimal(nextDTO.getY()));
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java
index 8c88242..33be25d 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java
@@ -12,25 +12,19 @@
 public class PathInitDTO {
     //---------------------算法参数
     /**
-     * x坐标最大值（x方向栅格总数）
+     * 初始位置x坐标(栅格地图下的坐标)
      */
-    private String xMax;
+    private String xNow;
     /**
-     * y坐标最大值（y方向栅格总数）
+     * 初始位置y坐标(栅格地图下的坐标)
      */
-    private String yMax;
+    private String yNow;
     /**
      * 初始位置计数率
      */
-    private String count;
-    /**
-     * 初始位置x坐标(0<=x<x_max)
-     */
-    private String x;
-    /**
-     * 初始位置y坐标(0<=y<y_max)
-     */
-    private String y;
+    private String zNow;
+
+    private Integer stepLen;
 
     //---------------------其他执行参数
     private String robotId;
@@ -38,11 +32,11 @@
 
     public List<String> toArray() {
         List<String> list = new ArrayList<>();
-        list.add(xMax);
-        list.add(yMax);
-        list.add(count);
-        list.add(x);
-        list.add(y);
+        list.add(xNow);
+        list.add(yNow);
+        list.add(stepLen+"");
+        list.add(zNow);
+        list.add(taskId+"");
         return list;
     }
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java
index 109e95c..ada6774 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java
@@ -10,21 +10,17 @@
  */
 @Data
 public class PathPlanDTO extends PathInitDTO {
-
     /**
-     * 长度为8的数组，记录(x-1, y-1), (x-1, y), (x-1, y+1), (x, y-1), (x, y+1), (x+1, y-1), (x+1, y), (x+1, y+1)8个坐标的可通行信息。
-     * 若某坐标有障碍物（不可通行行）则记为0，否则记为1
+     * 浮点阈值(阈值，超过该值后结束巡检开始寻源)
      */
-    private String open;
-
+    private String th;
     public List<String> toArray() {
         List<String> list = new ArrayList<>();
-        list.add(getXMax());
-        list.add(getYMax());
-        list.add(open);
-        list.add(getCount());
-        list.add(getX());
-        list.add(getY());
+
+        list.add(getStepLen()+"");
+        list.add(getTh());
+        list.add(getZNow());
+        list.add(getTaskId()+"");
         return list;
     }
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskGridInfo.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskGridInfo.java
new file mode 100644
index 0000000..101c10a
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskGridInfo.java
@@ -0,0 +1,83 @@
+package com.casic.missiles.modular.robot.model;
+
+import com.baomidou.mybatisplus.annotation.TableField;
+import com.baomidou.mybatisplus.annotation.TableName;
+import com.baomidou.mybatisplus.extension.activerecord.Model;
+import io.swagger.annotations.ApiModelProperty;
+import lombok.Data;
+
+import java.math.BigDecimal;
+import java.util.Date;
+
+/**
+ * 任务网格信息对象 task_grid_info
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+@Data
+@TableName("task_grid_info")
+public class TaskGridInfo extends Model<TaskGridInfo> {
+    private static final long serialVersionUID = 1L;
+    @TableField(exist = false)
+    private Integer index;
+    @ApiModelProperty(value = "主键", dataType = "Long")
+    @TableField("id")
+    private Long id;
+
+    @ApiModelProperty(value = "创建时间", dataType = "Date")
+    @TableField("create_time")
+    private Date createTime;
+
+    @ApiModelProperty(value = "修改时间", dataType = "Date")
+    @TableField("update_time")
+    private Date updateTime;
+
+    @ApiModelProperty(value = "任务ID", dataType = "Long")
+    @TableField("task_id")
+    private Long taskId;
+
+    @ApiModelProperty(value = "估算栅格x", dataType = "Long")
+    @TableField("estimate_grid_x")
+    private String estimateGridX;
+
+    @ApiModelProperty(value = "估算栅格y", dataType = "Long")
+    @TableField("estimate_grid_y")
+    private String estimateGridY;
+
+    @ApiModelProperty(value = "中子计数率", dataType = "BigDecimal")
+    @TableField("neutron_count")
+    private BigDecimal neutronCount;
+
+    @ApiModelProperty(value = "当前栅格x", dataType = "Long")
+    @TableField("cur_grid_x")
+    private Integer curGridX;
+
+    @ApiModelProperty(value = "当前栅格y", dataType = "Long")
+    @TableField("cur_grid_y")
+    private Integer curGridY;
+
+    public String getCurPoint(){
+        return "("+curGridX+","+curGridY+")";
+    }
+    public String getEstimatePoint(){
+        if(estimateGridX==null&estimateGridY==null){
+            return "结束";
+        }
+        return "("+estimateGridX+","+estimateGridY+")";
+    }
+    @Override
+    public String toString() {
+        return "TaskGridInfo{"
+                + "id=" + id
+                + "createTime=" + createTime
+                + "updateTime=" + updateTime
+                + "taskId=" + taskId
+                + "estimateGridX=" + estimateGridX
+                + "estimateGridY=" + estimateGridY
+                + "neutronCount=" + neutronCount
+                + "curGridX=" + curGridX
+                + "curGridY=" + curGridY
+                + "}";
+    }
+}
\ No newline at end of file
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java
index edc104e..a8acb2a 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java
@@ -53,6 +53,13 @@
     @TableField("route_id")
     private Long routeId;
 
+    @ApiModelProperty(value = "寻源初始步长", dataType = "Integer")
+    @TableField("step_len")
+    private Integer stepLen;
+
+    @ApiModelProperty(value = "寻源阈值设定", dataType = "BigDecimal")
+    @TableField("max_threshold")
+    private BigDecimal maxThreshold;
     /**
      * 任务类型(1巡检任务)
      */
@@ -62,7 +69,6 @@
     @ApiModelProperty(value = "任务类型字典名称", dataType = "String")
     @TableField(exist = false)
     private String taskTypeName;
-
     @ApiModelProperty(value = "识别类型(discernType 1中子源识别)", dataType = "Integer")
     @TableField("discern_type")
     private Integer discernType;
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java
index c4bc25d..c6620f3 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java
@@ -53,7 +53,7 @@
     }),
     SET_SPEED("/set_speed" , "/set_speed_response" , "最大运动速度设置" , true, new TypeReference<RobotMsgResponseDTO<Integer>>() {
     }),
-    SET_TARGETPOINT("/set_targetpoint" , "/set_targetpoint_response" , "设置导航目标点" , false, new TypeReference<RobotMsgResponseDTO<Integer>>() {
+    SET_TARGETPOINT("/set_targetpoint" , "/set_targetpoint_response" , "设置导航目标点" , true, new TypeReference<RobotMsgResponseDTO<Integer>>() {
     }),
 
     GET_DATA_LIST("/get_data_list" , "/get_data_list_response" , "获取数据包列表" , true, new TypeReference<RobotMsgResponseDTO<List<String>>>() {
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/AbstractSocketHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/AbstractSocketHandler.java
index 83f4cb2..e47e343 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/AbstractSocketHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/AbstractSocketHandler.java
@@ -49,7 +49,7 @@
                 wsMsgBaseDTO.setMsgKey(enums.getMsgKey());
                 wsMsgBaseDTO.setData(jsonObject.get("msg"));
                 String msg = JSON.toJSONString(wsMsgBaseDTO);
-                log.info("推送{}消息：{}", enums.getRemarks(), msg);
+//                log.info("推送{}消息：{}", enums.getRemarks(), msg);
                 webSocket.sendOneMessage(robotId, msg);
             }
         } catch (Exception e) {
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/cmd/TargetPointCmdHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/cmd/TargetPointCmdHandler.java
index c1c09fa..43b4460 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/cmd/TargetPointCmdHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/cmd/TargetPointCmdHandler.java
@@ -34,7 +34,11 @@
     @Override
     public ResponseDataDTO senderCmd(TargetPointRequest request, InstructCodeEnums enums) {
         MessageRequestDTO<TargetPointRequest> messageDTO = new MessageRequestDTO();
-        //读取默认关键点信息
+        //读取默认关键点信息 {"x":"1.715708819927424","y":"-0.9100042544250901","z":"0.017993879527185876","theta":"-0.039278239414312105"}
+//        request.setX("1.715708819927424");
+//        request.setY("-0.9100042544250901");
+//        request.setZ("0.017993879527185876");
+//        request.setTheta("-0.039278239414312105");
         messageDTO.setMsg(request);
         ResponseDataDTO<Integer> list = optService.robotCmd(getRobotInfo(), enums, messageDTO);
         return list;
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/AppCostMapHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/AppCostMapHandler.java
index 0335693..88c9f79 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/AppCostMapHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/AppCostMapHandler.java
@@ -1,6 +1,11 @@
 package com.casic.missiles.modular.robot.opt.handler.msg;
 
+import com.casic.missiles.core.cache.CacheUtil;
+import com.casic.missiles.modular.constants.RobotDictConstants;
+import com.casic.missiles.modular.robot.IBaseRobotService;
+import com.casic.missiles.modular.robot.opt.enums.InstructCodeEnums;
 import com.casic.missiles.modular.robot.opt.handler.AbstractSocketHandler;
+import com.casic.missiles.modular.robot.opt.instruct.dto.CostMapResolutionRequest;
 import com.casic.missiles.modular.robot.service.IRobotStatusInfoService;
 import lombok.extern.slf4j.Slf4j;
 import org.eclipse.paho.client.mqttv3.MqttMessage;
@@ -16,14 +21,27 @@
 @Component("/app_costmap_sender")
 public class AppCostMapHandler extends AbstractSocketHandler {
     private final IRobotStatusInfoService statusInfoService;
+    private final IBaseRobotService baseRobotService;
     private static Map<String, Integer> index = new ConcurrentHashMap<>();
 
-    public AppCostMapHandler(IRobotStatusInfoService statusInfoService) {
+    public AppCostMapHandler(IRobotStatusInfoService statusInfoService, IBaseRobotService baseRobotService) {
         this.statusInfoService = statusInfoService;
+        this.baseRobotService = baseRobotService;
     }
 
     @Override
     public void process(String robotId, String topic, MqttMessage message) {
         statusInfoService.saveCostMap(robotId, message);
+
+        CostMapResolutionRequest costMapSet = CacheUtil.get(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CONFIG_COSTMAP_RESOLUTION_SET +robotId);
+        if(costMapSet != null){
+            CacheUtil.remove(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CONFIG_COSTMAP_RESOLUTION_SET +robotId);
+            new Thread(() -> {
+                costMapSet.setRobotId(Long.valueOf(robotId));
+                baseRobotService.execCmdHandler(costMapSet, InstructCodeEnums.COSTMAP_RESOLUTION);
+            }).start();
+
+        }
+
     }
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/PoseMessageMsgHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/PoseMessageMsgHandler.java
index 29f8919..6c3c3bd 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/PoseMessageMsgHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/PoseMessageMsgHandler.java
@@ -17,11 +17,9 @@
 @Component("/pose_message")
 public class PoseMessageMsgHandler extends AbstractSocketHandler {
 
-
     private final EventPublisher publisher;
 
     public PoseMessageMsgHandler(EventPublisher publisher) {
-
         this.publisher = publisher;
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/StopStateMsgHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/StopStateMsgHandler.java
index 8950815..8a71e0b 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/StopStateMsgHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/StopStateMsgHandler.java
@@ -30,7 +30,7 @@
     public void process(String robotId, String topic, MqttMessage message) {
 
         index.put(index_key, index.get("index") + 1);
-        if (index.get(index_key)>50) {
+        if (index.get(index_key)>30) {
             index.put(index_key, 0);
             log.info("主键：{},停障状态topic:{},消息内容:{}", robotId, topic, message.toString());
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/impl/RobotOptServiceImpl.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/impl/RobotOptServiceImpl.java
index 8471e7a..7940a24 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/impl/RobotOptServiceImpl.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/impl/RobotOptServiceImpl.java
@@ -146,7 +146,7 @@
     public void startTrack(StartTrackRequest request) {
         //增加循迹任务记录
         taskInfoService.reloadTask(request, robotInfoService.getById(request.getRobotId()));
-        this.execCmdHandler(request, InstructCodeEnums.START_TRACK);
+//        this.execCmdHandler(request, InstructCodeEnums.START_TRACK);
     }
 
     public IRobotRouteOptService getRobotRouteOptService() {
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/BaseRobotCmdDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/BaseRobotCmdDTO.java
index 5608f2b..06b53de 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/BaseRobotCmdDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/BaseRobotCmdDTO.java
@@ -3,11 +3,13 @@
 import io.swagger.annotations.ApiModelProperty;
 import lombok.Data;
 
+import java.io.Serializable;
+
 /**
  * 机器人指令基类
  */
 @Data
-public class BaseRobotCmdDTO {
+public class BaseRobotCmdDTO implements Serializable {
     @ApiModelProperty(value = "机器人主键", dataType = "String")
     protected Long robotId;
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/StartTrackRequest.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/StartTrackRequest.java
index a18321b..85546a7 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/StartTrackRequest.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/StartTrackRequest.java
@@ -3,6 +3,8 @@
 import io.swagger.annotations.ApiModelProperty;
 import lombok.Data;
 
+import java.math.BigDecimal;
+
 /**
  * 导航下发指令
  */
@@ -10,18 +12,19 @@
 public class StartTrackRequest extends BaseRobotCmdDTO {
     @ApiModelProperty(value = "sn编码(可不填 默认机器ID)", dataType = "String")
     private String sn;
-
     @ApiModelProperty(value = "下发状态 1:开启 0:关闭", dataType = "String")
     private Integer action;
     @ApiModelProperty(value = "等待状态 1:等待 0:不等待", dataType = "String")
     private Integer wait;
     @ApiModelProperty(value = "等待状态 0:循迹 1:任务", dataType = "isTask")
     private Integer isTask;
-    @ApiModelProperty(value = "轨迹名称", dataType = "String")
+    @ApiModelProperty(value = "轨迹名称(废弃)", dataType = "String")
     private String track_name;
     @ApiModelProperty(value = "关键点名称", dataType = "String", hidden = true)
     private String taskpoint_name;
-
+    @ApiModelProperty(value = "初始寻源步长", dataType = "Integer")
+    private Integer stepLen;
+    @ApiModelProperty(value = "寻源阈值", dataType = "BigDecimal")
+    private BigDecimal maxThreshold;
     private String obs_mode="1";
-
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/response/StopStateResponseDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/response/StopStateResponseDTO.java
index eb514bf..e0679d7 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/response/StopStateResponseDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/response/StopStateResponseDTO.java
@@ -3,11 +3,13 @@
 import io.swagger.annotations.ApiModelProperty;
 import lombok.Data;
 
+import java.io.Serializable;
+
 /**
  * 电压信息返回
  */
 @Data
-public class StopStateResponseDTO {
+public class StopStateResponseDTO implements Serializable {
     @ApiModelProperty(value = "软件急停(0：无触发 1：触发)" , dataType = "Integer")
     private Integer soft;
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IAlgorithmService.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IAlgorithmService.java
index 56b3e1d..f62523b 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IAlgorithmService.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IAlgorithmService.java
@@ -6,6 +6,7 @@
 import com.casic.missiles.modular.robot.dto.PathPlanDTO;
 import com.casic.missiles.modular.robot.model.RobotInfo;
 
+import java.io.File;
 import java.math.BigDecimal;
 import java.util.List;
 
@@ -38,5 +39,5 @@
      * @return
      */
     GridPointDTO convertGridCenterPoint(GridPointDTO pointDTO, BigDecimal resolution, RobotInfo robotInfo);
-    void readAlgorithmResponse(String fileName, AlgorithmResponse response, Long taskId, List<String> res);
+    void readAlgorithmResponse(String fileName, File repetitionFile, AlgorithmResponse response, Long taskId, List<String> res);
 }
\ No newline at end of file
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IRobotStatusInfoService.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IRobotStatusInfoService.java
index 2af210a..757f8d0 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IRobotStatusInfoService.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IRobotStatusInfoService.java
@@ -67,10 +67,9 @@
     /**
      * 更新车辆路线及任务信息
      *
-     * @param currRouteId 当前路线信息
      * @param currTaskId  当前任务信息
      */
-    void updateCurrTask(String robotId, Long currRouteId, Long currTaskId);
+    void updateCurrTask(String robotId, Long currTaskId);
 
     RobotStatusInfo getStatusByRobotId(String robotId);
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/ITaskGridInfoService.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/ITaskGridInfoService.java
new file mode 100644
index 0000000..fc0da93
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/ITaskGridInfoService.java
@@ -0,0 +1,22 @@
+package com.casic.missiles.modular.robot.service;
+
+import com.baomidou.mybatisplus.extension.service.IService;
+import  com.casic.missiles.modular.robot.model.TaskGridInfo;
+import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
+import com.baomidou.mybatisplus.extension.plugins.pagination.Page;
+import java.util.List;
+
+
+/**
+ * <p>
+ * 任务网格信息 服务类
+ * </p>
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+public interface ITaskGridInfoService extends IService<TaskGridInfo> {
+/**
+* 任务网格信息 分页检索
+*/
+List<TaskGridInfo> selectTaskGridInfoPage(Page<TaskGridInfo> page,QueryWrapper<TaskGridInfo> query);}
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/ITaskInfoService.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/ITaskInfoService.java
index 31d7ba6..b395993 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/ITaskInfoService.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/ITaskInfoService.java
@@ -51,6 +51,13 @@
      * @param dto 实体
      */
     void pointCallBack(PoseMessageResponseDTO dto, String robotId);
+
+    /**
+     * 车辆路线规划
+     * @param dto
+     * @param robotId
+     */
+    void carPathPlanning(PoseMessageResponseDTO dto, String robotId);
     void pointTestCallBack(PoseMessageResponseDTO dto, String robotId);
     /**
      * 图片路径保存
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/AlgorithmServiceImpl.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/AlgorithmServiceImpl.java
index f808fa0..a4dbe0b 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/AlgorithmServiceImpl.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/AlgorithmServiceImpl.java
@@ -29,10 +29,12 @@
 @Slf4j
 @Service
 public class AlgorithmServiceImpl implements IAlgorithmService {
-    @Value("${casic.algorithm.initPath:-1}")
-    private String initPath;
-    @Value("${casic.algorithm.planPath:-1}")
-    private String planPath;
+    @Value("${casic.algorithm.path:-1}")
+    private String path;
+    @Value("${casic.algorithm.initPathName:-1}")
+    private String initPathName;
+    @Value("${casic.algorithm.planPathName:-1}")
+    private String planPathName;
     private final ITaskHeatMapService heatMapService;
 
     public AlgorithmServiceImpl(ITaskHeatMapService heatMapService) {
@@ -42,13 +44,13 @@
     @Transactional
     @Override
     public AlgorithmResponse pathPlanningInit(PathInitDTO initDTO) {
-        if (StrUtil.isEmpty(initDTO.getCount())) {
-            initDTO.setCount("0.00");
+        if (StrUtil.isEmpty(initDTO.getZNow())) {
+            initDTO.setZNow("0.00");
         }
         log.info(JSON.toJSONString(initDTO));
         //算法路径及入参配置
         List<String> commends = new ArrayList<>();
-        commends.add(initPath);
+        commends.add(path + initPathName);
         commends.addAll(initDTO.toArray());
         List<String> res = AlgorithmUtils.invokeJob(commends);
 
@@ -63,38 +65,72 @@
     @Transactional
     @Override
     public AlgorithmResponse pathPlanning(PathPlanDTO planDTO) {
-        if (StrUtil.isEmpty(planDTO.getCount())) {
-            planDTO.setCount("0.001");
+        if (StrUtil.isEmpty(planDTO.getZNow())) {
+            planDTO.setZNow("0.001");
         }
         //算法路径及入参配置
         List<String> commends = new ArrayList<>();
-        commends.add(planPath);
+        commends.add(path + planPathName);
         commends.addAll(planDTO.toArray());
         log.info("寻源算法调用:{}", JSON.toJSONString(planDTO));
         List<String> res = AlgorithmUtils.invokeJob(commends);
         AlgorithmResponse response = getAlgorithmResponse(planDTO, res);
-        //next 点位编写
-        if (CollectionUtil.isNotEmpty(res) && res.size() == 2) {
-            response.getPointNextDTO().setX(formatBigDecimal(res.get(0).replaceAll("next_X: ", "")));
-            response.getPointNextDTO().setY(formatBigDecimal(res.get(1).replaceAll("next_Y: ", "")));
-        }
+
         return response;
     }
 
     private AlgorithmResponse getAlgorithmResponse(PathInitDTO initDTO, List<String> res) {
-        File repetitionFile = new File("repetition.txt");
+        //读取当前路径图片
+        File repetitionFileDir = new File(path + "\\" + initDTO.getTaskId() + "\\predicts");
+        File[] files = repetitionFileDir.listFiles((dir, name) -> name.endsWith(".txt") && name.startsWith("step"));
+        File repetitionFile = null;
+        //判定当前执行路径最大step2文件夹
+        if (files != null && files.length > 0) {
+             repetitionFile = files[files.length - 1];
+        }
         AlgorithmResponse response = new AlgorithmResponse();
         if (initDTO.getTaskId() != null && repetitionFile.exists()) {
-            readAlgorithmResponse("repetition.txt", response, initDTO.getTaskId(), res);
-            readAlgorithmResponse("step.txt", response, initDTO.getTaskId(), res);
+            //读取热力图
+            readAlgorithmResponse("repetition.txt",repetitionFile, response, initDTO.getTaskId(), res);
         } else {
             log.info("taskId is empty!");
         }
-        readAlgorithmResponse("xy_train.txt", response, initDTO.getTaskId(), res);
-        readAlgorithmResponse("z_train.txt", response, initDTO.getTaskId(), res);
+        //读取规划路径
+        if (CollectionUtil.isNotEmpty(res)) {
+            response.setEnd(false);
+            readAlgorithmResponse(response, res);
+        }
         return response;
     }
 
+    private void readAlgorithmResponse(AlgorithmResponse response, List<String> res) {
+        List<String> resList = new ArrayList<>();
+        if (res.size() >= 2) {
+            resList = res.subList(res.size() - 2, res.size());
+        }
+        //判定是否结束寻源
+        if (res.contains("end")) {
+            response.setEnd(true);
+            for (String s : resList) {
+                if (s.startsWith("source_coordinate:")) {
+                    s = s.trim();
+                    log.info("寻源结束,目标点位：{}",s);
+                    List<String> lists = StrUtil.split(s.replaceAll("source_coordinate:", ""), ",");
+                    PointNextDTO nextDTO = new PointNextDTO();
+                    nextDTO.setX(lists.get(0));
+                    nextDTO.setY(lists.get(1));
+                    response.setCoordinate(nextDTO);
+                }
+            }
+        } else {
+            PointNextDTO nextDTO = new PointNextDTO();
+            nextDTO.setX(resList.get(0));
+            nextDTO.setY(resList.get(1));
+            response.setPointNextDTO(nextDTO);
+        }
+
+    }
+
     /**
      * 读取算法输出数据
      *
@@ -102,49 +138,18 @@
      * @param response 结果集
      * @param taskId   任务ID
      */
-    public void readAlgorithmResponse(String fileName, AlgorithmResponse response, Long taskId, List<String> res) {
-        File file = new File(fileName);
-        if (!file.exists()&&!fileName.startsWith("step")) {
+    public void readAlgorithmResponse(String fileName,File repetitionFile, AlgorithmResponse response, Long taskId, List<String> res) {
+        if (!repetitionFile.exists() && !fileName.startsWith("step")) {
             log.info("{} file is empty!", fileName);
             return;
         }
         if (response.getPointNextDTO() == null) {
             response.setPointNextDTO(new PointNextDTO());
         }
-        switch (fileName) {
-            case "repetition.txt":
-                String msg = FileUtil.readString(file, CharsetUtil.CHARSET_UTF_8);
-                TaskHeatMap taskHeatMap = createEmptyTaskHeatMap(taskId);
-                taskHeatMap.setRepetition(msg);
-                response.setTaskHeatMap(taskHeatMap);
-                break;
-            case "xy_train.txt":
-                List<String> xyTrains = FileUtil.readLines(file, CharsetUtil.CHARSET_UTF_8);
-                if (CollectionUtil.isNotEmpty(xyTrains)) {
-                    if (CollectionUtil.isNotEmpty(xyTrains)) {
-                        List<String> list = StrUtil.split(xyTrains.get(xyTrains.size() - 1), " ");
-                        response.getPointNextDTO().setX(formatBigDecimal(list.get(0)));
-                        response.getPointNextDTO().setY(formatBigDecimal(list.get(1)));
-                    }
-                }
-                break;
-            case "z_train.txt":
-                List<String> zTrains = FileUtil.readLines(file, CharsetUtil.CHARSET_UTF_8);
-                if (CollectionUtil.isNotEmpty(zTrains)) {
-                    String zTrain = zTrains.get(zTrains.size() - 1);
-                    response.getPointNextDTO().setZ(formatBigDecimal(zTrain));
-                }
-            default:
-                if (StrUtil.isNotEmpty(fileName) && fileName.startsWith("step")) {
-                    File fileDir = new File(".");
-                    File[] files = fileDir.listFiles((dir, name) -> name.endsWith(".txt") && name.startsWith("step"));
-                    if (files != null && files.length > 0) {
-                        String steps = FileUtil.readString(files[files.length - 1], CharsetUtil.CHARSET_UTF_8);
-                        response.getTaskHeatMap().setRepetition(steps);
-                    }
-                }
-                break;
-        }
+        String msg = FileUtil.readString(repetitionFile, CharsetUtil.CHARSET_UTF_8);
+        TaskHeatMap taskHeatMap = createEmptyTaskHeatMap(taskId);
+        taskHeatMap.setRepetition(msg);
+        response.setTaskHeatMap(taskHeatMap);
     }
 
     /**
@@ -157,13 +162,11 @@
     public GridPointDTO convertSlamToGrid(BigDecimal x, BigDecimal y, BigDecimal resolution, RobotInfo robotInfo) {
         GridPointDTO pointDTO = new GridPointDTO();
         BigDecimal scale = resolution;
-
         //减去栅格原点坐标
         x = x.subtract(robotInfo.getOriginX());
         y = y.subtract(robotInfo.getOriginY());
-
-        pointDTO.setX(new BigDecimal(x.divide(scale,3,BigDecimal.ROUND_HALF_UP).intValue()+1));
-        pointDTO.setY(new BigDecimal(y.divide(scale,3,BigDecimal.ROUND_HALF_UP).intValue()+1));
+        pointDTO.setX(new BigDecimal(x.divide(scale, 3, BigDecimal.ROUND_HALF_UP).intValue()+1));
+        pointDTO.setY(new BigDecimal(y.divide(scale, 3, BigDecimal.ROUND_HALF_UP).intValue() +1));
         return pointDTO;
     }
 
@@ -174,19 +177,17 @@
      * @return
      */
     public GridPointDTO convertGridCenterPoint(GridPointDTO pointDTO, BigDecimal resolution, RobotInfo robotInfo) {
-
         GridPointDTO centerPoint = new GridPointDTO();
         BigDecimal scale = resolution;
-        //点位转换
-        BigDecimal slamX = pointDTO.getX().divide(scale, 5, BigDecimal.ROUND_HALF_UP);
-        BigDecimal slamY = pointDTO.getY().divide(scale, 5, BigDecimal.ROUND_HALF_UP);
+        //点位转换 转换后栅格－1 从1开始计算
+        BigDecimal slamX = (pointDTO.getX().subtract(new BigDecimal(1))).multiply(scale);
+        BigDecimal slamY = (pointDTO.getY().subtract(new BigDecimal(1))).multiply(scale);
         //中心栅格点位中心添加
-        centerPoint.setX(slamX.add(scale.divide(new BigDecimal(2), 5, BigDecimal.ROUND_HALF_UP)));
-        centerPoint.setY(slamY.add(scale.divide(new BigDecimal(2), 5, BigDecimal.ROUND_HALF_UP)));
+        centerPoint.setX(slamX.add(scale.divide(new BigDecimal(2), 10, BigDecimal.ROUND_HALF_UP)));
+        centerPoint.setY(slamY.add(scale.divide(new BigDecimal(2), 10, BigDecimal.ROUND_HALF_UP)));
         //加上栅格原点坐标
         centerPoint.setX(centerPoint.getX().add(robotInfo.getOriginX()));
         centerPoint.setY(centerPoint.getY().add(robotInfo.getOriginY()));
-
         return centerPoint;
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotInfoServiceImpl.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotInfoServiceImpl.java
index 043e05d..fdded64 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotInfoServiceImpl.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotInfoServiceImpl.java
@@ -104,7 +104,7 @@
             //下发障碍物栅格
             sendCostMapResolution(robotInfo.getResolution(), robotInfo.getId());
             //重新生成栅格地图
-            sendChangePcd(robotInfo.getId());
+            //sendChangePcd(robotInfo.getId());
         }
         update.eq(RobotInfo::getId, robotInfo.getId());
         update(update);
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotStatusInfoServiceImpl.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotStatusInfoServiceImpl.java
index 6d3ae91..46b5c3b 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotStatusInfoServiceImpl.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotStatusInfoServiceImpl.java
@@ -15,6 +15,7 @@
 import com.casic.missiles.modular.robot.dao.RobotStatusInfoMapper;
 import com.casic.missiles.modular.robot.model.RobotInfo;
 import com.casic.missiles.modular.robot.model.RobotStatusInfo;
+import com.casic.missiles.modular.robot.opt.enums.InstructCodeEnums;
 import com.casic.missiles.modular.robot.opt.instruct.base.dto.RobotMsgDTO;
 import com.casic.missiles.modular.robot.opt.instruct.base.dto.RobotMsgResponseDTO;
 import com.casic.missiles.modular.robot.opt.instruct.base.dto.RobotResponseDTO;
@@ -26,13 +27,18 @@
 import com.casic.missiles.modular.robot.service.IRobotInfoService;
 import com.casic.missiles.modular.robot.service.IRobotStatusInfoService;
 import lombok.extern.slf4j.Slf4j;
+import org.aspectj.util.FileUtil;
 import org.eclipse.paho.client.mqttv3.MqttMessage;
+import org.springframework.beans.factory.annotation.Value;
 import org.springframework.context.annotation.Lazy;
 import org.springframework.stereotype.Service;
 import org.springframework.transaction.annotation.Transactional;
 
+import java.io.File;
 import java.io.Serializable;
 import java.math.BigDecimal;
+import java.util.ArrayList;
+import java.util.Arrays;
 import java.util.List;
 
 
@@ -147,7 +153,6 @@
     @Override
     public void saveProcessMessage(String robotId, MqttMessage message) {
 //        String json = new String(message.getPayload());
-
         RobotResponseDTO<ProcessMessageResponseDTO> responseDTO = parseJson(message, new TypeReference<RobotResponseDTO<ProcessMessageResponseDTO>>() {
         });
         if (responseDTO.getMsg() != null) {
@@ -189,9 +194,8 @@
     }
 
     @Override
-    public void updateCurrTask(String robotId, Long currRouteId, Long currTaskId) {
+    public void updateCurrTask(String robotId, Long currTaskId) {
         UpdateWrapper<RobotStatusInfo> query = new UpdateWrapper<>();
-        query.set("curr_route_id", currRouteId);
         query.set("curr_task_id", currTaskId);
         query.eq("robot_id", robotId);
         update(query);
@@ -250,6 +254,15 @@
         }
     }
 
+    @Value("${casic.algorithm.path:-1}")
+    private String path;
+
+    /**
+     * 栅格阈值
+     */
+    @Value("${casic.algorithm.num:30}")
+    private Integer num;
+
     /**
      * 机器人栅格数据
      *
@@ -262,15 +275,41 @@
         RobotResponseDTO<AppCostmapResponseDTO> responseDTO = parseJson(message, new TypeReference<RobotResponseDTO<AppCostmapResponseDTO>>() {
         });
         if (responseDTO != null && responseDTO.getMsg() != null) {
+            //发送栅格分辨率设定
+
             cacheService.updateCostMap(robotId, responseDTO.getMsg());
             robotInfoService.updateRobotGrid(robotId, responseDTO.getMsg());
-//            log.info("--------------- json data:{}", JSON.toJSONString(responseDTO.getMsg()));
+            //log.info("--------------- json data:{}", JSON.toJSONString(responseDTO.getMsg()));
+            if (responseDTO != null && responseDTO.getMsg().getData() != null) {
+                ArrayList<Integer> dataNums = new ArrayList<>();
+                //根据阈值转换栅格
+                for (Integer dataNum : responseDTO.getMsg().getData()) {
+                    if (dataNum != -1) {
+                        if (dataNum >= num) {
+                            dataNums.add(0);
+                        } else {
+                            dataNums.add(1);
+                        }
+                    }
+                }
+                writeCache(dataNums,responseDTO.getMsg().getWidth(),responseDTO.getMsg().getHeight(),robotId);
+            }
         }
     }
 
+    private void writeCache(ArrayList<Integer> integers,Integer width,Integer height,String robotId) {
+        List<List<Integer>> arrays = CollectionUtil.split(integers,width);
+        ArrayList<ArrayList<Integer>> arrayList = new ArrayList<>();
+        for (List<Integer> array : arrays) {
+            arrayList.add((ArrayList)array);
+        }
+        CollectionUtil.reverse(arrayList);
+        CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_COST_MAP + robotId, arrayList);
+//        FileUtil.writeAsString(new File(path + "map.txt"), CollectionUtil.join(integers, " ").replaceAll("\\[", "").replaceAll("]", ""));
+    }
+
     @Override
     public void updateRobotPoint(PoseMessageResponseDTO dto, String robotId) {
-        //todo cache
         UpdateWrapper<RobotStatusInfo> query = new UpdateWrapper<>();
         query.set("postion_x", dto.getX());
         query.set("postion_y", dto.getY());
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotTrajectoryInfoServiceImpl.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotTrajectoryInfoServiceImpl.java
index 6c9625f..f1262a2 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotTrajectoryInfoServiceImpl.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotTrajectoryInfoServiceImpl.java
@@ -57,29 +57,29 @@
         });
 
         //读取导航状态,如果导航未开启则不存储
-//        RobotStatusInfo statusInfo = robotStatusInfoService.getStatusByRobotId(robotId);
-        Long routeId= cacheService.readRouteId(Long.valueOf(robotId));
-        if (routeId != null && responseDTO.getMsg() != null) {
+        //RobotStatusInfo statusInfo = robotStatusInfoService.getStatusByRobotId(robotId);
+
+        if ( responseDTO.getMsg() != null) {
             PoseMessageResponseDTO dto = responseDTO.getMsg();
             //执行循迹 保存当前轨迹信息
             //更新实时定位信息
             CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.ROBOT_POINT + robotId, dto);
-            infoService.pointTestCallBack(dto, robotId);
-            if (routeId != null) {
-                RobotTrajectoryInfo info = new RobotTrajectoryInfo();
-                info.setRobotId(Long.valueOf(robotId));
-                info.setCreateTime(new Date());
-                info.setPostionX(dto.getX());
-                info.setPostionY(dto.getY());
-                info.setPostionZ(dto.getZ());
-                info.setTheta(dto.getTheta());
-                save(info);
-                try {
-                    infoService.pointCallBack(dto, robotId);
-                } catch (Exception e) {
-                    log.error("task error", e);
-                }
+            try {
+                infoService.carPathPlanning(dto, robotId);
+            } catch (Exception e) {
+                log.error("task error", e);
             }
+//            if (routeId != null) {
+//                RobotTrajectoryInfo info = new RobotTrajectoryInfo();
+//                info.setRobotId(Long.valueOf(robotId));
+//                info.setCreateTime(new Date());
+//                info.setPostionX(dto.getX());
+//                info.setPostionY(dto.getY());
+//                info.setPostionZ(dto.getZ());
+//                info.setTheta(dto.getTheta());
+//                save(info);
+
+//            }
         }
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/TaskGridInfoServiceImpl.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/TaskGridInfoServiceImpl.java
new file mode 100644
index 0000000..cf61def
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/TaskGridInfoServiceImpl.java
@@ -0,0 +1,28 @@
+package com.casic.missiles.modular.robot.service.impl;
+
+import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
+import com.baomidou.mybatisplus.extension.plugins.pagination.Page;
+import com.baomidou.mybatisplus.extension.service.impl.ServiceImpl;
+import com.casic.missiles.modular.robot.dao.TaskGridInfoMapper;
+import com.casic.missiles.modular.robot.model.TaskGridInfo;
+import com.casic.missiles.modular.robot.service.ITaskGridInfoService;
+import org.springframework.stereotype.Service;
+
+import java.util.List;
+
+
+/**
+ * <p>
+ * 任务网格信息 服务实现类
+ * </p>
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+@Service
+public class TaskGridInfoServiceImpl extends ServiceImpl<TaskGridInfoMapper, TaskGridInfo> implements ITaskGridInfoService {
+    @Override
+    public List<TaskGridInfo> selectTaskGridInfoPage(Page<TaskGridInfo> page, QueryWrapper<TaskGridInfo> query) {
+        return this.baseMapper.selectTaskGridInfoPage(page, query);
+    }
+}
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/TaskInfoServiceImpl.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/TaskInfoServiceImpl.java
index d1417b0..fd49e2e 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/TaskInfoServiceImpl.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/TaskInfoServiceImpl.java
@@ -6,10 +6,10 @@
 import cn.hutool.core.convert.Convert;
 import cn.hutool.core.date.DateUtil;
 import cn.hutool.core.io.FileUtil;
-import cn.hutool.core.thread.ExecutorBuilder;
 import cn.hutool.core.util.StrUtil;
 import com.alibaba.fastjson.JSON;
 import com.baomidou.mybatisplus.core.conditions.Wrapper;
+import com.baomidou.mybatisplus.core.conditions.query.LambdaQueryWrapper;
 import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
 import com.baomidou.mybatisplus.core.conditions.update.UpdateWrapper;
 import com.baomidou.mybatisplus.core.metadata.IPage;
@@ -47,7 +47,6 @@
 import java.io.Serializable;
 import java.math.BigDecimal;
 import java.util.*;
-import java.util.concurrent.ThreadPoolExecutor;
 
 
 /**
@@ -70,6 +69,7 @@
     private final IRobotCacheService cacheService;
     private final IBaseRobotService baseRobotService;
     private final ITaskInfoImgService imgService;
+    private final ITaskGridInfoService taskGridInfoService;
     /**
      * 初始化线程池
      */
@@ -79,12 +79,19 @@
     @Value("${casic.doc.temp}")
     private String docTemp;
     /**
+     * 5000 ms
+     */
+    @Value("${casic.task.duration:3000}")
+    private Integer stayDuration;
+    @Value("${casic.algorithm.path:-1}")
+    private String path;
+    /**
      * 障碍物精度
      */
     @Value("${casic.obstacle:10}")
     private Integer obstacle;
 
-    public TaskInfoServiceImpl(@Lazy IRouteInfoService routeInfoService, IRobotStatusInfoService statusInfoService, AbstractDictService dictService, @Lazy IAlarmRecordService recordService, @Lazy ITaskHeatMapService taskHeatMapService, IAlgorithmService algorithmService, IRobotCacheService cacheService, @Lazy IBaseRobotService baseRobotService, ITaskInfoImgService imgService) {
+    public TaskInfoServiceImpl(@Lazy IRouteInfoService routeInfoService, IRobotStatusInfoService statusInfoService, AbstractDictService dictService, @Lazy IAlarmRecordService recordService, @Lazy ITaskHeatMapService taskHeatMapService, IAlgorithmService algorithmService, IRobotCacheService cacheService, @Lazy IBaseRobotService baseRobotService, ITaskInfoImgService imgService, ITaskGridInfoService taskGridInfoService) {
         this.routeInfoService = routeInfoService;
         this.statusInfoService = statusInfoService;
         this.dictService = dictService;
@@ -94,6 +101,7 @@
         this.cacheService = cacheService;
         this.baseRobotService = baseRobotService;
         this.imgService = imgService;
+        this.taskGridInfoService = taskGridInfoService;
     }
 
     /**
@@ -184,7 +192,7 @@
         if (CollectionUtil.isEmpty(routeInfos)) {
             throw new ServiceException(500, "路线不存在,请及时更新");
         }
-        RouteInfo routeInfo = routeInfos.get(0);
+//        RouteInfo routeInfo = routeInfos.get(0);
         Integer isTask = 0;
         //开启寻源任务
         if (request.getIsTask() == 1) {
@@ -193,22 +201,21 @@
         RobotInfo robotInfo = cacheService.getRobotInfo(Convert.toLong(robotId));
         CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_IS_TASK + robotId, isTask);
         //添加任务记录
-        TaskInfo taskInfo = addTaskByStartTrack(routeInfo, robotInfo);
+        TaskInfo taskInfo = addTaskByStartTrack( robotInfo, request);
         //No.3 更新器人执行任务信息
-        Long currRouteId = routeInfo.getId();
+//        Long currRouteId = routeInfo.getId();
         Long currTaskId = taskInfo.getId();
 
-        statusInfoService.updateCurrTask(robotId, currRouteId, currTaskId);
+        statusInfoService.updateCurrTask(robotId,  currTaskId);
         //算法初始化标志创建
         CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_TASK_ID + robotId, currTaskId);
         CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_TASK_INIT + robotId, 0);
-        CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_ROUTE_ID + robotId, routeInfo.getId());
+//        CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_ROUTE_ID + robotId, routeInfo.getId());
     }
 
     private void closeTask(StartTrackRequest request, String robotId) {
         //关闭操作
         RobotStatusInfo statusInfo = statusInfoService.getStatusByRobotId(robotId);
-
         if (statusInfo.getCurrTaskId() != null) {
             UpdateWrapper<TaskInfo> updateWrapper = new UpdateWrapper<>();
             updateWrapper.set("update_time", new Date());
@@ -221,10 +228,11 @@
 
         //清空任务缓存
         robotCacheClean(robotId);
-
+        //清楚规划任务锁
+        CacheUtil.remove(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_PLANING_FLAG + robotId);
+        CacheUtil.remove(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_ESTIMATE_GRID+ robotId);
         //清空机器当前任务
-        statusInfoService.updateCurrTask(robotId, null, null);
-
+        statusInfoService.updateCurrTask(robotId, null);
     }
 
     /**
@@ -249,7 +257,6 @@
 
         //判定任务是否已经完成初始化
         Long longId = Convert.toLong(robotId);
-        Long taskId = CacheUtil.get(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_TASK_ID + robotId);
         //缓存当前点位信息
         CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_XY_POINT + robotId, dto);
         RobotInfo robotInfo = cacheService.getRobotInfo(longId);
@@ -260,86 +267,203 @@
             log.error("no config 中子源阈值未配置");
             return;
         }
-
         //读取是否是循迹任务
         Integer isTask = cacheService.readIsTask(longId);
         if (isTask != 1) {
             log.info("循迹任务,不做处理");
             return;
         }
-        //算法初始化状态读取
-        Integer initValue = cacheService.readTaskInit(longId);
-
 
         //No.1 任务初始化
+//        pathInit(longId, robotInfo, pointDTO, taskId, robotId);
+    }
+
+    @Override
+    public void carPathPlanning(PoseMessageResponseDTO dto, String robotId) {
+        //判定任务是否已经完成初始化
+        Long longId = Convert.toLong(robotId);
+        Long taskId = CacheUtil.get(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_TASK_ID + robotId);
+        //缓存当前点位信息
+        CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_XY_POINT + robotId, dto);
+        RobotInfo robotInfo = cacheService.getRobotInfo(longId);
+        //slam经纬度转栅格
+        GridPointDTO pointDTO = algorithmService.convertSlamToGrid(dto.getX(), dto.getY(), robotInfo.getResolution(), robotInfo);
+        if (robotInfo.getAlarmThreshold() == null || robotInfo.getMaxThreshold() == null) {
+            log.error("no config 中子源阈值未配置");
+            return;
+        }
+
+        //读取是否是寻源任务
+        Integer isTask = cacheService.readIsTask(longId);
+        if (isTask != 1) {
+            log.info("循迹任务,不做处理");
+            return;
+        }
+
+        TaskInfo taskInfo = getById(taskId);
+        //No.1 任务初始化
+        pathInit(longId, robotInfo, pointDTO, taskId, robotId, taskInfo.getStepLen());
+        //No.2 路线规划
+        pathPlaning(longId, robotInfo, pointDTO, taskId, robotId, taskInfo,dto);
+    }
+
+    /**
+     * 路线规划进行加锁
+     *
+     * @param robotId 机器人ID
+     * @return
+     */
+    private synchronized boolean lockPlanIngFlag(String robotId) {
+        Boolean flag = CacheUtil.get(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_PLANING_FLAG + robotId);
+        if (flag == null) {
+            CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_PLANING_FLAG + robotId, true);
+            return true;
+        }
+        return false;
+    }
+
+    /**
+     * @param longId    机器人ID
+     * @param robotInfo 机器人信息
+     * @param pointDTO  当前点位栅格信息
+     * @param taskId    任务ID
+     * @param robotId   机器人ID
+     */
+    private void pathPlaning(Long longId, RobotInfo robotInfo, GridPointDTO pointDTO, Long taskId, String robotId, TaskInfo taskInfo,PoseMessageResponseDTO curPoint) {
+        if (taskId != null) {
+            //规划节点
+            PointNextDTO pointNextDTO = CacheUtil.get(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_ESTIMATE_GRID + robotId);
+            GridPointDTO oldEstimateGrid = GridPointDTO.convertGridPoint(pointNextDTO);
+            Boolean flag = lockPlanIngFlag(robotId);
+            //是否到达预期栅格
+            if (!flag) {
+                log.info("未获取到规划任务,直接结束：{}",pointDTO);
+                return;
+            }
+            try {
+                log.debug("oldEstimateGrid:{}", oldEstimateGrid);
+                if (pointNextDTO == null || isEquals(pointDTO, oldEstimateGrid)) {
+                    log.info("当前栅格变更......", pointDTO);
+                    CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_POINT + robotId, pointDTO);
+                    //算法调用获取热力图
+                    PathPlanDTO pathPlanDTO = createPlanDTO(robotInfo, pointDTO, taskId, robotId, taskInfo.getStepLen());
+                    if (taskInfo.getMaxThreshold() == null) {
+                        taskInfo.setMaxThreshold(robotInfo.getMaxThreshold());
+                    }
+                    pathPlanDTO.setTh(taskInfo.getMaxThreshold() + "");
+
+                    log.debug("heat map -- robotId:{},taskId:{}", robotId, taskId);
+                    pathPlanDTO.setZNow(cacheService.getNeutronCount(longId));
+
+                    //存储算法输出栅格信息
+                    AlgorithmResponse response = algorithmService.pathPlanning(pathPlanDTO);
+                    CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_ESTIMATE_GRID + robotId, response.getPointNextDTO());
+                    sendWebSocket(InstructStatusMsgEnums.HEAT_MAP, robotId, response.getTaskHeatMap());
+
+                    //存储循迹任务信息
+                    TaskGridInfo taskGridInfo = new TaskGridInfo();
+                    taskGridInfo.setCreateTime(new Date());
+                    taskGridInfo.setUpdateTime(new Date());
+                    taskGridInfo.setCurGridX(pointDTO.getX().intValue());
+                    taskGridInfo.setCurGridY(pointDTO.getY().intValue());
+                    taskGridInfo.setNeutronCount(new BigDecimal(pathPlanDTO.getZNow()));
+                    taskGridInfo.setEstimateGridX(response.getPointNextDTO().getX());
+                    taskGridInfo.setEstimateGridY(response.getPointNextDTO().getY());
+                    taskGridInfo.setTaskId(taskId);
+                    taskGridInfoService.save(taskGridInfo);
+
+                    try {
+                        //停留时间阈值
+                        Thread.sleep(stayDuration);
+                    } catch (InterruptedException e) {
+                        e.printStackTrace();
+                    }
+
+                    //是否触发停止状态
+                    if (!response.isEnd()) {
+                        log.debug("task planning-- xun dian robotId:{},taskId:{}", robotId, taskId);
+                        //机器人寻点指令下发
+                        sendTargetPoint(robotInfo, response,curPoint);
+                    } else {
+                        log.debug("task stop-- robotId:{},taskId:{}", robotId, taskId);
+                        //机器人停止指令下发
+                        SoftwareStopRequest request = new SoftwareStopRequest();
+                        request.setRobotId(robotInfo.getId());
+                        //设置为急停
+                        request.setControl(1);
+//                        baseRobotService.execCmdHandler(request, InstructCodeEnums.SOFTWARE_STOP);
+                        //循迹任务结束
+                        closeTask(null, robotId);
+                    }
+                }
+            } catch (Exception e) {
+                e.printStackTrace();
+            } finally {
+                //释放锁
+                CacheUtil.remove(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_PLANING_FLAG + robotId);
+            }
+        }
+    }
+
+    private PathPlanDTO createPlanDTO(RobotInfo robotInfo, GridPointDTO pointDTO, Long taskId, String robotId, Integer stepLen) {
+        PathPlanDTO pathPlanDTO = new PathPlanDTO();
+        pathPlanDTO.setTaskId(taskId);
+        pathPlanDTO.setRobotId(robotId);
+        pathPlanDTO.setStepLen(stepLen);
+        return pathPlanDTO;
+    }
+
+    /**
+     * 寻源初始化
+     *
+     * @param longId    机器ID
+     * @param robotInfo 机器信息
+     * @param pointDTO  当前点位信息
+     * @param taskId    任务ID
+     * @param robotId   机器ID
+     * @param stepLen   步长设置
+     */
+    private void pathInit(Long longId, RobotInfo robotInfo, GridPointDTO pointDTO, Long taskId, String robotId, Integer stepLen) {
+        //障碍物栅格上传已生成 直接读取
+        //算法初始化状态读取
+        Integer initValue = cacheService.readTaskInit(longId);
+        //No.1 任务初始化
         if (initValue != 1) {
+
             //执行初始化操作
             if (cacheService.updateTaskInit(longId, 1)) {
 
-                try {
-                    File file = new File(".");
-                    File[] files = file.listFiles((dir, name) -> (name.endsWith(".png") && name.startsWith("step"))
-                            || (name.endsWith(".txt") && name.startsWith("step")) || name.equals("repetition.txt") || name.equals("xy_train.txt") || name.equals("z_train.txt"));
-                    if (files != null) {
-                        for (int i = 0; i < files.length; i++) {
-                            FileUtil.del(files[i]);
+                List<List<Integer>> costList = CacheUtil.get(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_COST_MAP + robotId);
+                List<String> strs = new ArrayList<>();
+                List<String> strs2 = new ArrayList<>();
+                for (List<Integer> list : costList) {
+                    strs2.add(CollectionUtil.join(list, " "));
+                }
+                FileUtil.writeLines(strs2, new File(path + "map2.txt"), "utf-8");
+                //行转列 为算法输入数据
+                if(CollectionUtil.isNotEmpty(costList)){
+                    CollectionUtil.reverse(costList);
+                    for (int i = 0; i < costList.get(0).size(); i++) {
+                        ArrayList<Integer> ints = new ArrayList<>();
+                        for (List<Integer> list : costList) {
+                            ints.add(list.get(i));
                         }
+                        strs.add(CollectionUtil.join(ints, " "));
                     }
-                } catch (Exception e) {
-                    e.printStackTrace();
+//                    CollectionUtil.reverse(strs);
                 }
 
-                PathInitDTO pathInitDTO = createInitDTO(robotInfo, pointDTO, taskId, robotId);
+
+                FileUtil.writeLines(strs, new File(path + "map.txt"), "utf-8");
+
+                PathInitDTO pathInitDTO = createInitDTO(robotInfo, pointDTO, taskId, robotId, stepLen);
                 //中子源计数率读取
-                pathInitDTO.setCount(cacheService.getNeutronCount(longId));
+                pathInitDTO.setZNow(cacheService.getNeutronCount(longId));
+                log.info("init params:{}", JSON.toJSONString(pathInitDTO));
                 //执行1次算法 初始化操作
                 algorithmService.pathPlanningInit(pathInitDTO);
             }
         }
-
-        //No.2 任务执行中
-        if (taskId != null) {
-            //判断栅格变化
-            GridPointDTO oldPointDTO = CacheUtil.get(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_POINT + robotId);
-            if (oldPointDTO == null || isChange(pointDTO, oldPointDTO)) {
-                log.debug("grid change -- robotId:{},taskId:{}", robotId, taskId);
-                CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_POINT + robotId, pointDTO);
-                //算法调用获取热力图
-                PathPlanDTO pathPlanDTO = createPlanDTO(robotInfo, pointDTO, taskId, robotId);
-                //可通行标记为1 不可通行标记为0
-                String obstacle = getObstacle(robotId, pointDTO);
-                //TO-DO 障碍物判定信息
-                pathPlanDTO.setOpen(obstacle);
-                //中子源计数率读取
-                pathPlanDTO.setCount(cacheService.getNeutronCount(longId));
-                log.debug("heat map -- robotId:{},taskId:{}", robotId, taskId);
-                AlgorithmResponse response = algorithmService.pathPlanning(pathPlanDTO);
-                //存储算法输出栅格信息
-                CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_ESTIMATE_GRID + robotId, response.getPointNextDTO());
-                sendWebSocket(InstructStatusMsgEnums.HEAT_MAP, robotId, response.getTaskHeatMap());
-                //是否触发寻源操作
-                BigDecimal bigCount = new BigDecimal(pathPlanDTO.getCount());
-                if (bigCount.compareTo(robotInfo.getAlarmThreshold()) >= 0) {
-                    log.debug("task planning-- xun dian robotId:{},taskId:{}", robotId, taskId);
-                    //机器人寻点指令下发
-                    sendTargetPoint(robotInfo, response);
-                }
-
-                //是否触发停止状态
-                if (bigCount.compareTo(robotInfo.getMaxThreshold()) > 0) {
-                    log.debug("task stop-- robotId:{},taskId:{}", robotId, taskId);
-                    //机器人停止指令下发
-                    SoftwareStopRequest request = new SoftwareStopRequest();
-                    request.setRobotId(robotInfo.getId());
-                    //设置为急停
-                    request.setControl(1);
-                    baseRobotService.execCmdHandler(request, InstructCodeEnums.SOFTWARE_STOP);
-                    //循迹任务结束
-                    closeTask(null, robotId);
-                }
-
-            }
-        }
     }
 
     /**
@@ -353,7 +477,6 @@
         //判定任务是否已经完成初始化
         Long longId = Convert.toLong(robotId);
         Long taskId = CacheUtil.get(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_TASK_ID + robotId);
-
         RobotInfo robotInfo = cacheService.getRobotInfo(longId);
         //slam经纬度转栅格
         GridPointDTO pointDTO = algorithmService.convertSlamToGrid(dto.getX(), dto.getY(), robotInfo.getResolution(), robotInfo);
@@ -428,7 +551,7 @@
      * @param robotInfo 机器人信息
      * @param response  算法输出数据
      */
-    private void sendTargetPoint(RobotInfo robotInfo, AlgorithmResponse response) {
+    private void sendTargetPoint(RobotInfo robotInfo, AlgorithmResponse response,PoseMessageResponseDTO curPoint) {
         //读取已设置巡点目标信息
         PointNextDTO nextDTO = CacheUtil.get(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_TARGET_POINT_ID + robotInfo.getId());
         //巡点
@@ -439,8 +562,8 @@
             TargetPointRequest request = new TargetPointRequest();
             request.setX(Convert.toStr(gridCenterPoint.getX()));
             request.setY(Convert.toStr(gridCenterPoint.getY()));
-            request.setZ("0");
-
+            request.setZ(curPoint.getZ().toString());
+            request.setTheta(curPoint.getTheta().toString());
             request.setRobotId(robotInfo.getId());
             //执行巡点指令
             baseRobotService.execCmdHandler(request, InstructCodeEnums.SET_TARGETPOINT);
@@ -507,21 +630,20 @@
         params.put("startTime", startTime);
         params.put("endTime", endTime);
         //读取
-        List<AlarmRecord> records = recordService.selectListByTaskId(info.getId(), info.getRobotId());
         Integer index = 1;
-
-        if (CollectionUtil.isNotEmpty(records)) {
-            for (AlarmRecord record : records) {
+        LambdaQueryWrapper<TaskGridInfo> taskGridQuery = new LambdaQueryWrapper<>();
+        taskGridQuery.eq(TaskGridInfo::getTaskId,info.getId());
+        taskGridQuery.orderByAsc(TaskGridInfo::getCreateTime);
+        List<TaskGridInfo> taskGridInfos = taskGridInfoService.list(taskGridQuery);
+        if (CollectionUtil.isNotEmpty(taskGridInfos)) {
+            for (TaskGridInfo record : taskGridInfos) {
                 //栅格信息转换
-                GridPointDTO pointDTO = algorithmService.convertSlamToGrid(record.getX(), record.getY(), info.getResolution(), robotInfo);
-                record.setX(pointDTO.getX());
-                record.setY(pointDTO.getY());
                 record.setIndex(index);
                 index++;
             }
         }
 
-        params.put("records", records);
+        params.put("records", taskGridInfos);
         ImageEntity mcaImg = new ImageEntity();
         mcaImg.setHeight(600);
         mcaImg.setWidth(600);
@@ -557,7 +679,6 @@
             psdImg.setData(WordUtil.getImageBase64(uploadPath + info.getPsdImg()));
         }
         params.put("psdImg", psdImg);
-
         WordUtil.exportOutputStream(docTemp, response.getOutputStream(), params);
     }
 
@@ -589,7 +710,7 @@
     private String saveHeatImg(String taskId) {
         String newFileName = "task\\" + taskId + ".png";
         //读取当前路径图片
-        File file = new File(".");
+        File file = new File(path + "\\" + taskId + "\\figs");
         File[] files = file.listFiles((dir, name) -> name.endsWith(".png") && name.startsWith("step"));
         //判定当前执行路径最大step图片
         if (files != null && files.length > 0) {
@@ -614,33 +735,28 @@
         return true;
     }
 
-    private PathPlanDTO createPlanDTO(RobotInfo robotInfo, GridPointDTO pointDTO, Long taskId, String robotId) {
-        PathPlanDTO pathPlanDTO = new PathPlanDTO();
-        String xMax = robotInfo.getGridHeight().divide(robotInfo.getResolution(), 5, BigDecimal.ROUND_HALF_UP).intValue() + "";
-        String yMax = robotInfo.getGridWidth().divide(robotInfo.getResolution(), 5, BigDecimal.ROUND_HALF_UP).intValue() + "";
-        pathPlanDTO.setX(pointDTO.getX().intValue() + "");
-        pathPlanDTO.setY(pointDTO.getY().intValue() + "");
-        pathPlanDTO.setXMax(xMax);
-        pathPlanDTO.setYMax(yMax);
-        pathPlanDTO.setTaskId(taskId);
-        pathPlanDTO.setRobotId(robotId);
-        return pathPlanDTO;
+    private boolean isEquals(GridPointDTO pointDTO, GridPointDTO oldPointDTO) {
+        if(oldPointDTO==null||oldPointDTO.getX()==null){
+            return false;
+        }
+        if (pointDTO.getX().compareTo(oldPointDTO.getX()) == 0 && pointDTO.getY().compareTo(oldPointDTO.getY()) == 0) {
+            return true;
+        }
+        return false;
     }
 
-    private PathInitDTO createInitDTO(RobotInfo robotInfo, GridPointDTO pointDTO, Long taskId, String robotId) {
-        String xMax = robotInfo.getGridHeight().divide(robotInfo.getResolution(), 5, BigDecimal.ROUND_HALF_UP).intValue() + "";
-        String yMax = robotInfo.getGridWidth().divide(robotInfo.getResolution(), 5, BigDecimal.ROUND_HALF_UP).intValue() + "";
+
+    private PathInitDTO createInitDTO(RobotInfo robotInfo, GridPointDTO pointDTO, Long taskId, String robotId, Integer stepLen) {
         PathInitDTO pathInitDTO = new PathInitDTO();
-        pathInitDTO.setXMax(xMax);
-        pathInitDTO.setYMax(yMax);
-        pathInitDTO.setX(pointDTO.getX().intValue() + "");
-        pathInitDTO.setY(pointDTO.getY().intValue() + "");
+        pathInitDTO.setXNow(pointDTO.getX().intValue() + "");
+        pathInitDTO.setYNow(pointDTO.getY().intValue() + "");
+        pathInitDTO.setStepLen(stepLen);
         pathInitDTO.setTaskId(taskId);
         pathInitDTO.setRobotId(robotId);
         return pathInitDTO;
     }
 
-    private TaskInfo addTaskByStartTrack(RouteInfo routeInfo, RobotInfo robotInfo) {
+    private TaskInfo addTaskByStartTrack( RobotInfo robotInfo, StartTrackRequest request) {
         //No.2 新增任务信息
         TaskInfo taskInfo = new TaskInfo();
         String currDate = DateUtil.format(new Date(), "yyyyMMdd");
@@ -654,16 +770,20 @@
         taskInfo.setCreateTime(new Date());
         //任务类型 中子源
         taskInfo.setTaskType(1);
-        taskInfo.setDiscernType(1);
-        taskInfo.setTaskName(routeInfo.getRouteName() + "巡线任务");
+        taskInfo.setDiscernType(2);
+        taskInfo.setTaskName("自主巡检任务");
         //执行状态 未完成
         taskInfo.setTaskStatus(0);
         taskInfo.setStartTime(new Date());
-        taskInfo.setRouteId(routeInfo.getId());
+        taskInfo.setRouteId(null);
         taskInfo.setRobotId(robotInfo.getId());
         taskInfo.setGridHeight(robotInfo.getGridHeight());
         taskInfo.setGridWidth(robotInfo.getGridWidth());
         taskInfo.setResolution(robotInfo.getResolution());
+
+        taskInfo.setStepLen(request.getStepLen());
+        taskInfo.setMaxThreshold(request.getMaxThreshold());
+
         save(taskInfo);
         return taskInfo;
     }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/utils/AlgorithmUtils.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/utils/AlgorithmUtils.java
index 22864d1..467b9c2 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/utils/AlgorithmUtils.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/utils/AlgorithmUtils.java
@@ -1,6 +1,7 @@
 package com.casic.missiles.modular.robot.utils;
 
 import java.io.BufferedReader;
+import java.io.File;
 import java.io.IOException;
 import java.io.InputStreamReader;
 import java.util.ArrayList;
@@ -16,11 +17,13 @@
     public static List<String> invokeJob(List<String> params) {
         List<String> res = new ArrayList<>();
         try {
-
             BufferedReader br = null;
             BufferedReader brError;
             String line = null;
-            Process p = new ProcessBuilder(params).start();
+            ProcessBuilder processBuilder =  new ProcessBuilder(params);
+            processBuilder.directory(new File("D:\\workspace\\server\\keti\\casic-robot-inspection\\casic-web\\src\\main\\resources\\config\\path"));
+            Process p = processBuilder.start();
+
             br = new BufferedReader(new InputStreamReader(p.getInputStream()));
             brError = new BufferedReader(new InputStreamReader(p.getErrorStream()));
             while ((line = br.readLine()) != null || (line = brError.readLine()) != null) {
@@ -37,33 +40,29 @@
     public static void main(String[] args) {
         String path = "D:\\workspace\\server\\keti\\casic-robot-inspection\\casic-web\\src\\main\\resources\\config\\path\\";
         //算法初始化调用
-        String exePath = path + "PathPlanning_init.exe";
+        String exePath = path + "init.exe";
 
         ArrayList<String> invocation = new ArrayList<String>();
         invocation.add(exePath);
-        invocation.add("40");
-        invocation.add("20");
-        invocation.add("0.5");
+        invocation.add("10");
+        invocation.add("10");
         invocation.add("5");
-        invocation.add("5");
+        invocation.add("0.16");
+        invocation.add("Mission1");
         invokeJob(invocation);
 
         //{"count":"null","robotId":"1","taskId":"1748224487696142338","x":"8","xMax":"100","y":"2","yMax":"100"}
-        String planning = path + "PathPlanning.exe";
         //{"open":"0,0,0,0,0,0,0,0","robotId":"1","taskId":"1748250669749735425","x":"6","xMax":"100","y":"2","yMax":"100"}
         //{"open":"1,1,1,1,1,1,1,1","robotId":"1","taskId":"1748252894098178049","x":"6","xMax":"100","y":"2","yMax":"100"}
         //{"count":"6.0","open":"1,1,1,1,1,1,1,1","robotId":"1","taskId":"1750713404601159681","x":"6","xMax":"20","y":"2","yMax":"40"}
+        String planning = path + "search.exe";
         ArrayList<String> planInvocation = new ArrayList<String>();
         planInvocation.add(planning);
-        planInvocation.add("40");
-        planInvocation.add("40");
-        planInvocation.add("1,1,0,1,1,1,1,1");
-        planInvocation.add("6.0");
-        //next_X
-        planInvocation.add("6");
-        //next_Y
-        planInvocation.add("2");
-        ProcessBuilder processBuilder = new ProcessBuilder();
-        invokeJob(planInvocation);
+        planInvocation.add("10");
+        planInvocation.add("1.0");
+        planInvocation.add("0.2");
+        planInvocation.add("Mission1");
+        List<String> res = invokeJob(planInvocation);
+        System.out.println(res);
     }
 }
\ No newline at end of file
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/vo/TaskGridInfoVO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/vo/TaskGridInfoVO.java
new file mode 100644
index 0000000..0aff5bf
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/vo/TaskGridInfoVO.java
@@ -0,0 +1,58 @@
+package com.casic.missiles.modular.robot.vo;
+
+import io.swagger.annotations.ApiModelProperty;
+import lombok.Data;
+import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
+import com.casic.missiles.modular.robot.model.TaskGridInfo;
+import cn.hutool.core.util.StrUtil;
+
+import java.math.BigDecimal;
+
+/**
+ * 任务网格信息对象VO
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+@Data
+public class TaskGridInfoVO {
+    private static final long serialVersionUID = 1L;
+
+
+            @ApiModelProperty(value = "任务ID" , dataType = "Long")
+            private Long taskId;
+
+            @ApiModelProperty(value = "估算栅格x" , dataType = "Long")
+            private Long estimateGridX;
+
+            @ApiModelProperty(value = "估算栅格y" , dataType = "Long")
+            private Long estimateGridY;
+
+            @ApiModelProperty(value = "中子计数率" , dataType = "BigDecimal")
+            private BigDecimal neutronCount;
+
+            @ApiModelProperty(value = "当前栅格x" , dataType = "Long")
+            private Long curGridX;
+
+            @ApiModelProperty(value = "当前栅格y" , dataType = "Long")
+            private Long curGridY;
+
+public QueryWrapper<TaskGridInfo> genQuery() {
+QueryWrapper<TaskGridInfo> query = new QueryWrapper<>();
+if (taskId!=null) {
+        query.eq("task_id",taskId);
+}
+if (estimateGridX!=null) {
+        query.eq("estimate_grid_x",estimateGridX);
+}
+if (estimateGridY!=null) {
+        query.eq("estimate_grid_y",estimateGridY);
+}
+if (curGridX!=null) {
+        query.eq("cur_grid_x",curGridX);
+}
+if (curGridY!=null) {
+        query.eq("cur_grid_y",curGridY);
+}
+return query;
+}}
diff --git a/casic-web/src/main/resources/config/application.yml b/casic-web/src/main/resources/config/application.yml
index 04420fc..05c3633 100644
--- a/casic-web/src/main/resources/config/application.yml
+++ b/casic-web/src/main/resources/config/application.yml
@@ -42,11 +42,12 @@
   session-invalidate-time: 86400 #session失效时间(只在单机环境下生效，,多机环境在SpringSessionConfig类中配置) 单位：秒
   session-validation-interval: 900 #多久检测一次失效的session(只在单机环境下生效) 单位：秒
   no-login-urls: /user/login,/kaptcha,/config/baseConfig
-  hikSdk: D:\code\robot\casic-robot-inspection\casic-web\src\main\resources\config\hiklib\HCNetSDK.dll
-  hikPlay: D:\code\robot\casic-robot-inspection\casic-web\src\main\resources\config\hiklib\PlayCtrl.dll
+  hikSdk: D:\workspace\server\keti\casic-robot-inspection\casic-web\src\main\resources\config\hiklib\HCNetSDK.dll
+  hikPlay: D:\workspace\server\keti\casic-robot-inspection\casic-web\src\main\resources\config\hiklib\PlayCtrl.dll
   algorithm:
-    initPath: D:\workspace\server\keti\casic-robot-inspection\casic-web\src\main\resources\config\path\PathPlanning_init.exe
-    planPath: D:\workspace\server\keti\casic-robot-inspection\casic-web\src\main\resources\config\path\PathPlanning.exe
+    path:  D:\workspace\server\keti\casic-robot-inspection\casic-web\src\main\resources\config\path\
+    initPathName: init.exe
+    planPathName: search.exe
   doc:
     temp: D:\workspace\server\keti\casic-robot-inspection\casic-web\src\main\resources\config\path\report.docx
   config:

diff --git a/.gitignore b/.gitignore
index 9e20bc1..f42a9cb 100644
--- a/.gitignore
+++ b/.gitignore
@@ -2,3 +2,4 @@
 **/target/**
 logs/
 *.iml
+*.exe
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java b/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
index c7b5d28..a849528 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
@@ -28,6 +28,7 @@
     String KEY_CUR_TASK_ID = "CUR_TASK_ID_";
     String KEY_CUR_TASK_INIT = "CUR_TASK_INIT_";
 
+    String KEY_COST_MAP = "KEY_COST_MAP";
     /**
      * PSD数据ID
      */
@@ -41,6 +42,7 @@
      */
     String KEY_CUR_XY_POINT = "KEY_CUR_XY_POINT_";
     String KEY_CUR_ESTIMATE_GRID = "KEY_CUR_ESTIMATE_GRID_";
+    String KEY_PLANING_FLAG = "KEY_PLANING_FLAG_";
     /**
      * 机器人巡点栅格
      */
@@ -55,6 +57,7 @@
      * 探测器配置参数缓存
      */
     String KEY_CONFIG_PARAMS_ID = "KEY_CONFIG_PARAMS_ID_";
+    String KEY_CONFIG_COSTMAP_RESOLUTION_SET = "KEY_CONFIG_COSTMAP_RESOLUTION_SET_";
 
     /**
      * 暂停状态缓存信息
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
index c4c905d..3bb475f 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
@@ -1,9 +1,14 @@
 package com.casic.missiles.modular.robot.controller;
 
+import cn.hutool.core.lang.Assert;
 import cn.hutool.core.util.StrUtil;
 import com.casic.missiles.core.base.controller.BaseController;
+import com.casic.missiles.core.cache.CacheUtil;
 import com.casic.missiles.core.util.SpringContextHolder;
+import com.casic.missiles.model.exception.RequestValidException;
+import com.casic.missiles.model.response.ResponseData;
 import com.casic.missiles.model.response.dto.ResponseDataDTO;
+import com.casic.missiles.modular.constants.RobotDictConstants;
 import com.casic.missiles.modular.robot.IBaseRobotService;
 import com.casic.missiles.modular.robot.dto.RobotInfoSetDTO;
 import com.casic.missiles.modular.robot.model.RobotInfo;
@@ -19,10 +24,16 @@
 import io.swagger.annotations.ApiOperation;
 import io.swagger.annotations.ApiParam;
 import lombok.SneakyThrows;
+import lombok.extern.slf4j.Slf4j;
+import org.apache.ibatis.annotations.Param;
+import org.springframework.beans.factory.annotation.Value;
 import org.springframework.web.bind.annotation.*;
+import org.springframework.web.multipart.MultipartFile;
 
 import javax.servlet.http.HttpServletResponse;
+import java.io.File;
 import java.util.List;
+import java.util.Optional;
 
 /**
  * 机器人操作类
@@ -30,6 +41,7 @@
  * @author lwh
  * @date 2023-10-30
  */
+@Slf4j
 @Api(tags = "机器人控制接口")
 @RestController
 @RequestMapping("/robot/opt")
@@ -61,6 +73,28 @@
         return baseRobotService.execCmdHandler(request, InstructCodeEnums.CHANGE_PCD);
     }
 
+    @Value("${casic.file.uploadPath}")
+    private String filePath;
+
+    @SneakyThrows
+    @ApiOperation(value = "PCD上传接口")
+    @PostMapping(value = "/uploadPcd")
+    public ResponseData uploadPcd(@RequestParam("file") MultipartFile file, @ApiParam("机器人ID,url拼接传参") String robotId) {
+        Assert.isFalse(StrUtil.isEmpty(robotId), () -> new RequestValidException("机器ID不能为空", Optional.of(new Throwable().getStackTrace()[2])));
+        RobotInfo info = robotInfoService.getById(robotId);
+        if (file != null && !file.isEmpty()) {
+            File pcdFile = new File(filePath + "finalCloud_" + info.getId() + ".pcd");
+            try {
+                file.transferTo(pcdFile);
+            } catch (Exception e) {
+                log.error(e.getMessage());
+            }
+            return ResponseData.success();
+        } else {
+            return ResponseData.error("附件上传失败!");
+        }
+    }
+
     @SneakyThrows
     @ApiOperation(value = "PCD下载接口")
     @PostMapping(value = "/pcdDownload")
@@ -136,6 +170,10 @@
     @ApiOperation(value = "启动或关闭循迹功能", notes = "返回 1成功 0失败  wait 启动时是否等待，0代表循迹启动即运动，不等待；1代表循迹启动原地等待。若无此字段，默认循迹启动不等待直接运动")
     @PostMapping(value = "/startTrack")
     public ResponseDataDTO<Integer> startTrack(@RequestBody StartTrackRequest request) {
+        request.setIsTask(1);
+        if (request.getStepLen() == null) {
+            request.setStepLen(2);
+        }
         baseRobotService.startTrack(request);
         return ResponseDataDTO.success();
     }
@@ -156,6 +194,13 @@
         MessageRequestDTO<CrlNavRequest> messageDTO = new MessageRequestDTO();
         messageDTO.setMsg(request);
         ResponseDataDTO<Integer> list = baseRobotService.getRobotRouteOptService().robotCmd(info, InstructCodeEnums.CTRL_NAV, messageDTO);
+        if (request.getAction() == 1) {
+            CostMapResolutionRequest costMap = new CostMapResolutionRequest();
+            costMap.setResolution(info.getResolution().doubleValue());
+            request.setRobotId(request.getRobotId());
+            //是否下发分辨率
+            CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CONFIG_COSTMAP_RESOLUTION_SET + request.getRobotId(), costMap);
+        }
         return list;
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java
new file mode 100644
index 0000000..5317651
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java
@@ -0,0 +1,86 @@
+package com.casic.missiles.modular.robot.controller;
+
+import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
+import com.baomidou.mybatisplus.extension.plugins.pagination.Page;
+import com.casic.missiles.core.base.controller.BaseController;
+import com.casic.missiles.core.page.PageFactory;
+import com.casic.missiles.core.page.PageInfoBT;
+import com.casic.missiles.model.form.IdForms;
+import com.casic.missiles.model.response.dto.ResponseDataDTO;
+import com.casic.missiles.modular.robot.model.TaskGridInfo;
+import com.casic.missiles.modular.robot.service.ITaskGridInfoService;
+import com.casic.missiles.modular.robot.vo.TaskGridInfoVO;
+import io.swagger.annotations.Api;
+import io.swagger.annotations.ApiOperation;
+import org.springframework.web.bind.annotation.*;
+
+import java.util.List;
+
+/**
+ * 任务网格信息Controller
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+@Api(tags = "任务网格信息")
+@RestController
+@RequestMapping("/robot/taskGridInfo")
+public class TaskGridInfoController extends BaseController {
+
+    private final ITaskGridInfoService taskGridInfoService;
+
+    public TaskGridInfoController(ITaskGridInfoService taskGridInfoService) {
+        this.taskGridInfoService = taskGridInfoService;
+    }
+
+    @ApiOperation(value = "列表查询")
+    @GetMapping(value = "/list")
+    public ResponseDataDTO<List<TaskGridInfo>> list(TaskGridInfoVO taskGridInfoVO) {
+        QueryWrapper<TaskGridInfo> query = taskGridInfoVO.genQuery();
+        return ResponseDataDTO.success(taskGridInfoService.list(query));
+    }
+
+    @ApiOperation(value = "分页查询")
+    @GetMapping(value = "/listPage")
+    public ResponseDataDTO<PageInfoBT<TaskGridInfo>> listPage(TaskGridInfoVO taskGridInfoVO) {
+        Page<TaskGridInfo> page = PageFactory.defaultPage();
+        QueryWrapper<TaskGridInfo> query = taskGridInfoVO.genQuery();
+        page = taskGridInfoService.page(page, query);
+        return ResponseDataDTO.success(super.packForBT(page));
+    }
+
+
+    @ApiOperation(value = "新增接口")
+    @PostMapping(value = "/add")
+    public ResponseDataDTO add(@RequestBody TaskGridInfo taskGridInfo) {
+        taskGridInfoService.save(taskGridInfo);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "修改接口")
+    @PostMapping(value = "/update")
+    public ResponseDataDTO update(@RequestBody TaskGridInfo taskGridInfo) {
+        taskGridInfoService.updateById(taskGridInfo);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "删除接口")
+    @GetMapping(value = "/delete")
+    public ResponseDataDTO delete(String id) {
+        taskGridInfoService.removeById(id);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation("批量删除")
+    @PostMapping("/batchDelete")
+    public ResponseDataDTO batchDelete(@RequestBody IdForms ids) {
+        taskGridInfoService.removeByIds(ids.getIds());
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "详情查询")
+    @GetMapping(value = "/detail")
+    public ResponseDataDTO<TaskGridInfo> detail(String id) {
+        return ResponseDataDTO.success(taskGridInfoService.getById(id));
+    }
+}
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
index e82a792..6992274 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
@@ -98,7 +98,7 @@
     public ResponseDataDTO<AlgorithmResponse> test() {
         AlgorithmResponse response = new AlgorithmResponse();
         response.setTaskHeatMap(new TaskHeatMap());
-        algorithmService.readAlgorithmResponse("step3.txt",response,123L,null);
+        algorithmService.readAlgorithmResponse("step3.txt",null,response,123L,null);
         sendWebSocket(InstructStatusMsgEnums.HEAT_MAP, "1", response.getTaskHeatMap());
         return ResponseDataDTO.success(response);
     }
@@ -121,7 +121,7 @@
     @GetMapping(value = "/testGrid")
     public ResponseDataDTO<List<RouteInfo>> testGrid(@RequestBody PathInitDTO pathInitDTO) {
         RobotInfo robotInfo = infoService.getById(pathInitDTO.getRobotId());
-        GridPointDTO gridPointDTO = algorithmService.convertSlamToGrid(new BigDecimal(pathInitDTO.getX()), new BigDecimal(10), new BigDecimal(4), robotInfo);
+        GridPointDTO gridPointDTO = algorithmService.convertSlamToGrid(new BigDecimal(pathInitDTO.getXNow()), new BigDecimal(10), new BigDecimal(4), robotInfo);
         return ResponseDataDTO.success(algorithmService.convertGridCenterPoint(gridPointDTO, new BigDecimal(10), robotInfo));
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/TaskGridInfoMapper.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/TaskGridInfoMapper.java
new file mode 100644
index 0000000..5c3a70b
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/TaskGridInfoMapper.java
@@ -0,0 +1,23 @@
+package com.casic.missiles.modular.robot.dao;
+
+
+import com.baomidou.mybatisplus.core.mapper.BaseMapper;
+import com.casic.missiles.modular.robot.model.TaskGridInfo;
+import org.apache.ibatis.annotations.Param;
+import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
+import com.baomidou.mybatisplus.extension.plugins.pagination.Page;
+import java.util.List;
+/**
+ * <p>
+ *  任务网格信息 Mapper 接口
+ * </p>
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+public interface TaskGridInfoMapper extends BaseMapper<TaskGridInfo> {
+/**
+* 任务网格信息 分页检索
+*/
+List<TaskGridInfo> selectTaskGridInfoPage( @Param("page") Page<TaskGridInfo> page,@Param("ew" ) QueryWrapper<TaskGridInfo> query);
+}
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/mapping/TaskGridInfoMapper.xml b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/mapping/TaskGridInfoMapper.xml
new file mode 100644
index 0000000..be573e0
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/mapping/TaskGridInfoMapper.xml
@@ -0,0 +1,80 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!DOCTYPE mapper PUBLIC "-//mybatis.org//DTD Mapper 3.0//EN" "http://mybatis.org/dtd/mybatis-3-mapper.dtd">
+<mapper namespace="com.casic.missiles.modular.robot.dao.TaskGridInfoMapper">
+    <!-- 通用查询映射结果 -->
+    <resultMap id="BaseResultMap" type="com.casic.missiles.modular.robot.model.TaskGridInfo">
+        <result column="id" property="id"/>
+        <result column="create_time" property="createTime"/>
+        <result column="update_time" property="updateTime"/>
+        <result column="task_id" property="taskId"/>
+        <result column="estimate_grid_x" property="estimateGridX"/>
+        <result column="estimate_grid_y" property="estimateGridY"/>
+        <result column="neutron_count" property="neutronCount"/>
+        <result column="cur_grid_x" property="curGridX"/>
+        <result column="cur_grid_y" property="curGridY"/>
+    </resultMap>
+    <!-- 通用查询结果列 -->
+    <sql id="Base_Column_List">
+        id
+        ,
+                    create_time as createTime ,
+                    update_time as updateTime ,
+                    task_id as taskId ,
+                    estimate_grid_x as estimateGridX ,
+                    estimate_grid_y as estimateGridY ,
+                    neutron_count as neutronCount ,
+                    cur_grid_x as curGridX ,
+                    cur_grid_y as curGridY
+    </sql>
+    <sql id="Base_Column_Alias">
+        a
+        .
+        id
+        ,
+            a.create_time
+            as createTime ,
+            a.update_time
+            as updateTime ,
+            a.task_id
+            as taskId ,
+            a.estimate_grid_x
+            as estimateGridX ,
+            a.estimate_grid_y
+            as estimateGridY ,
+            a.neutron_count
+            as neutronCount ,
+            a.cur_grid_x
+            as curGridX ,
+            a.cur_grid_y
+            as curGridY
+    </sql>
+    <select id="selectTaskGridInfoPage" resultType="com.casic.missiles.modular.robot.model.TaskGridInfo">
+        select
+        <include refid="Base_Column_Alias"/>
+        from task_grid_info a
+        <where>
+            <if test="ew != null">
+                ${ew.sqlSegment}
+            </if>
+        </where>
+    </select>
+    <sql id="Base_Column_Query">
+        <if test="request!=null">
+            <if test="request.taskId!= null and request.taskId!=''">
+                and task_id = #{request.taskId}
+            </if>
+            <if test="request.estimateGridX!= null and request.estimateGridX!=''">
+                and estimate_grid_x = #{request.estimateGridX}
+            </if>
+            <if test="request.estimateGridY!= null and request.estimateGridY!=''">
+                and estimate_grid_y = #{request.estimateGridY}
+            </if>
+            <if test="request.curGridX!= null and request.curGridX!=''">
+                and cur_grid_x = #{request.curGridX}
+            </if>
+            <if test="request.curGridY!= null and request.curGridY!=''">
+                and cur_grid_y = #{request.curGridY}
+            </if>
+        </if>
+    </sql>
+</mapper>
\ No newline at end of file
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java
index 749f559..4559074 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java
@@ -13,4 +13,9 @@
      * 规划点位信息
      */
     private PointNextDTO pointNextDTO;
+    /**
+     * 寻源目标地址
+     */
+    private PointNextDTO coordinate;
+    private boolean isEnd;
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java
index 39ff2a2..cef3238 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java
@@ -12,6 +12,9 @@
     private BigDecimal y;
 
     public static GridPointDTO convertGridPoint(PointNextDTO nextDTO) {
+        if(nextDTO==null){
+            return null;
+        }
         GridPointDTO gridPointDTO = new GridPointDTO();
         gridPointDTO.setX(Convert.toBigDecimal(nextDTO.getX()));
         gridPointDTO.setY(Convert.toBigDecimal(nextDTO.getY()));
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java
index 8c88242..33be25d 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java
@@ -12,25 +12,19 @@
 public class PathInitDTO {
     //---------------------算法参数
     /**
-     * x坐标最大值（x方向栅格总数）
+     * 初始位置x坐标(栅格地图下的坐标)
      */
-    private String xMax;
+    private String xNow;
     /**
-     * y坐标最大值（y方向栅格总数）
+     * 初始位置y坐标(栅格地图下的坐标)
      */
-    private String yMax;
+    private String yNow;
     /**
      * 初始位置计数率
      */
-    private String count;
-    /**
-     * 初始位置x坐标(0<=x<x_max)
-     */
-    private String x;
-    /**
-     * 初始位置y坐标(0<=y<y_max)
-     */
-    private String y;
+    private String zNow;
+
+    private Integer stepLen;
 
     //---------------------其他执行参数
     private String robotId;
@@ -38,11 +32,11 @@
 
     public List<String> toArray() {
         List<String> list = new ArrayList<>();
-        list.add(xMax);
-        list.add(yMax);
-        list.add(count);
-        list.add(x);
-        list.add(y);
+        list.add(xNow);
+        list.add(yNow);
+        list.add(stepLen+"");
+        list.add(zNow);
+        list.add(taskId+"");
         return list;
     }
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java
index 109e95c..ada6774 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java
@@ -10,21 +10,17 @@
  */
 @Data
 public class PathPlanDTO extends PathInitDTO {
-
     /**
-     * 长度为8的数组，记录(x-1, y-1), (x-1, y), (x-1, y+1), (x, y-1), (x, y+1), (x+1, y-1), (x+1, y), (x+1, y+1)8个坐标的可通行信息。
-     * 若某坐标有障碍物（不可通行行）则记为0，否则记为1
+     * 浮点阈值(阈值，超过该值后结束巡检开始寻源)
      */
-    private String open;
-
+    private String th;
     public List<String> toArray() {
         List<String> list = new ArrayList<>();
-        list.add(getXMax());
-        list.add(getYMax());
-        list.add(open);
-        list.add(getCount());
-        list.add(getX());
-        list.add(getY());
+
+        list.add(getStepLen()+"");
+        list.add(getTh());
+        list.add(getZNow());
+        list.add(getTaskId()+"");
         return list;
     }
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskGridInfo.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskGridInfo.java
new file mode 100644
index 0000000..101c10a
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskGridInfo.java
@@ -0,0 +1,83 @@
+package com.casic.missiles.modular.robot.model;
+
+import com.baomidou.mybatisplus.annotation.TableField;
+import com.baomidou.mybatisplus.annotation.TableName;
+import com.baomidou.mybatisplus.extension.activerecord.Model;
+import io.swagger.annotations.ApiModelProperty;
+import lombok.Data;
+
+import java.math.BigDecimal;
+import java.util.Date;
+
+/**
+ * 任务网格信息对象 task_grid_info
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+@Data
+@TableName("task_grid_info")
+public class TaskGridInfo extends Model<TaskGridInfo> {
+    private static final long serialVersionUID = 1L;
+    @TableField(exist = false)
+    private Integer index;
+    @ApiModelProperty(value = "主键", dataType = "Long")
+    @TableField("id")
+    private Long id;
+
+    @ApiModelProperty(value = "创建时间", dataType = "Date")
+    @TableField("create_time")
+    private Date createTime;
+
+    @ApiModelProperty(value = "修改时间", dataType = "Date")
+    @TableField("update_time")
+    private Date updateTime;
+
+    @ApiModelProperty(value = "任务ID", dataType = "Long")
+    @TableField("task_id")
+    private Long taskId;
+
+    @ApiModelProperty(value = "估算栅格x", dataType = "Long")
+    @TableField("estimate_grid_x")
+    private String estimateGridX;
+
+    @ApiModelProperty(value = "估算栅格y", dataType = "Long")
+    @TableField("estimate_grid_y")
+    private String estimateGridY;
+
+    @ApiModelProperty(value = "中子计数率", dataType = "BigDecimal")
+    @TableField("neutron_count")
+    private BigDecimal neutronCount;
+
+    @ApiModelProperty(value = "当前栅格x", dataType = "Long")
+    @TableField("cur_grid_x")
+    private Integer curGridX;
+
+    @ApiModelProperty(value = "当前栅格y", dataType = "Long")
+    @TableField("cur_grid_y")
+    private Integer curGridY;
+
+    public String getCurPoint(){
+        return "("+curGridX+","+curGridY+")";
+    }
+    public String getEstimatePoint(){
+        if(estimateGridX==null&estimateGridY==null){
+            return "结束";
+        }
+        return "("+estimateGridX+","+estimateGridY+")";
+    }
+    @Override
+    public String toString() {
+        return "TaskGridInfo{"
+                + "id=" + id
+                + "createTime=" + createTime
+                + "updateTime=" + updateTime
+                + "taskId=" + taskId
+                + "estimateGridX=" + estimateGridX
+                + "estimateGridY=" + estimateGridY
+                + "neutronCount=" + neutronCount
+                + "curGridX=" + curGridX
+                + "curGridY=" + curGridY
+                + "}";
+    }
+}
\ No newline at end of file
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java
index edc104e..a8acb2a 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java
@@ -53,6 +53,13 @@
     @TableField("route_id")
     private Long routeId;
 
+    @ApiModelProperty(value = "寻源初始步长", dataType = "Integer")
+    @TableField("step_len")
+    private Integer stepLen;
+
+    @ApiModelProperty(value = "寻源阈值设定", dataType = "BigDecimal")
+    @TableField("max_threshold")
+    private BigDecimal maxThreshold;
     /**
      * 任务类型(1巡检任务)
      */
@@ -62,7 +69,6 @@
     @ApiModelProperty(value = "任务类型字典名称", dataType = "String")
     @TableField(exist = false)
     private String taskTypeName;
-
     @ApiModelProperty(value = "识别类型(discernType 1中子源识别)", dataType = "Integer")
     @TableField("discern_type")
     private Integer discernType;
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java
index c4bc25d..c6620f3 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java
@@ -53,7 +53,7 @@
     }),
     SET_SPEED("/set_speed" , "/set_speed_response" , "最大运动速度设置" , true, new TypeReference<RobotMsgResponseDTO<Integer>>() {
     }),
-    SET_TARGETPOINT("/set_targetpoint" , "/set_targetpoint_response" , "设置导航目标点" , false, new TypeReference<RobotMsgResponseDTO<Integer>>() {
+    SET_TARGETPOINT("/set_targetpoint" , "/set_targetpoint_response" , "设置导航目标点" , true, new TypeReference<RobotMsgResponseDTO<Integer>>() {
     }),
 
     GET_DATA_LIST("/get_data_list" , "/get_data_list_response" , "获取数据包列表" , true, new TypeReference<RobotMsgResponseDTO<List<String>>>() {
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/AbstractSocketHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/AbstractSocketHandler.java
index 83f4cb2..e47e343 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/AbstractSocketHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/AbstractSocketHandler.java
@@ -49,7 +49,7 @@
                 wsMsgBaseDTO.setMsgKey(enums.getMsgKey());
                 wsMsgBaseDTO.setData(jsonObject.get("msg"));
                 String msg = JSON.toJSONString(wsMsgBaseDTO);
-                log.info("推送{}消息：{}", enums.getRemarks(), msg);
+//                log.info("推送{}消息：{}", enums.getRemarks(), msg);
                 webSocket.sendOneMessage(robotId, msg);
             }
         } catch (Exception e) {
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/cmd/TargetPointCmdHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/cmd/TargetPointCmdHandler.java
index c1c09fa..43b4460 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/cmd/TargetPointCmdHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/cmd/TargetPointCmdHandler.java
@@ -34,7 +34,11 @@
     @Override
     public ResponseDataDTO senderCmd(TargetPointRequest request, InstructCodeEnums enums) {
         MessageRequestDTO<TargetPointRequest> messageDTO = new MessageRequestDTO();
-        //读取默认关键点信息
+        //读取默认关键点信息 {"x":"1.715708819927424","y":"-0.9100042544250901","z":"0.017993879527185876","theta":"-0.039278239414312105"}
+//        request.setX("1.715708819927424");
+//        request.setY("-0.9100042544250901");
+//        request.setZ("0.017993879527185876");
+//        request.setTheta("-0.039278239414312105");
         messageDTO.setMsg(request);
         ResponseDataDTO<Integer> list = optService.robotCmd(getRobotInfo(), enums, messageDTO);
         return list;
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/AppCostMapHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/AppCostMapHandler.java
index 0335693..88c9f79 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/AppCostMapHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/AppCostMapHandler.java
@@ -1,6 +1,11 @@
 package com.casic.missiles.modular.robot.opt.handler.msg;
 
+import com.casic.missiles.core.cache.CacheUtil;
+import com.casic.missiles.modular.constants.RobotDictConstants;
+import com.casic.missiles.modular.robot.IBaseRobotService;
+import com.casic.missiles.modular.robot.opt.enums.InstructCodeEnums;
 import com.casic.missiles.modular.robot.opt.handler.AbstractSocketHandler;
+import com.casic.missiles.modular.robot.opt.instruct.dto.CostMapResolutionRequest;
 import com.casic.missiles.modular.robot.service.IRobotStatusInfoService;
 import lombok.extern.slf4j.Slf4j;
 import org.eclipse.paho.client.mqttv3.MqttMessage;
@@ -16,14 +21,27 @@
 @Component("/app_costmap_sender")
 public class AppCostMapHandler extends AbstractSocketHandler {
     private final IRobotStatusInfoService statusInfoService;
+    private final IBaseRobotService baseRobotService;
     private static Map<String, Integer> index = new ConcurrentHashMap<>();
 
-    public AppCostMapHandler(IRobotStatusInfoService statusInfoService) {
+    public AppCostMapHandler(IRobotStatusInfoService statusInfoService, IBaseRobotService baseRobotService) {
         this.statusInfoService = statusInfoService;
+        this.baseRobotService = baseRobotService;
     }
 
     @Override
     public void process(String robotId, String topic, MqttMessage message) {
         statusInfoService.saveCostMap(robotId, message);
+
+        CostMapResolutionRequest costMapSet = CacheUtil.get(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CONFIG_COSTMAP_RESOLUTION_SET +robotId);
+        if(costMapSet != null){
+            CacheUtil.remove(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CONFIG_COSTMAP_RESOLUTION_SET +robotId);
+            new Thread(() -> {
+                costMapSet.setRobotId(Long.valueOf(robotId));
+                baseRobotService.execCmdHandler(costMapSet, InstructCodeEnums.COSTMAP_RESOLUTION);
+            }).start();
+
+        }
+
     }
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/PoseMessageMsgHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/PoseMessageMsgHandler.java
index 29f8919..6c3c3bd 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/PoseMessageMsgHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/PoseMessageMsgHandler.java
@@ -17,11 +17,9 @@
 @Component("/pose_message")
 public class PoseMessageMsgHandler extends AbstractSocketHandler {
 
-
     private final EventPublisher publisher;
 
     public PoseMessageMsgHandler(EventPublisher publisher) {
-
         this.publisher = publisher;
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/StopStateMsgHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/StopStateMsgHandler.java
index 8950815..8a71e0b 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/StopStateMsgHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/StopStateMsgHandler.java
@@ -30,7 +30,7 @@
     public void process(String robotId, String topic, MqttMessage message) {
 
         index.put(index_key, index.get("index") + 1);
-        if (index.get(index_key)>50) {
+        if (index.get(index_key)>30) {
             index.put(index_key, 0);
             log.info("主键：{},停障状态topic:{},消息内容:{}", robotId, topic, message.toString());
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/impl/RobotOptServiceImpl.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/impl/RobotOptServiceImpl.java
index 8471e7a..7940a24 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/impl/RobotOptServiceImpl.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/impl/RobotOptServiceImpl.java
@@ -146,7 +146,7 @@
     public void startTrack(StartTrackRequest request) {
         //增加循迹任务记录
         taskInfoService.reloadTask(request, robotInfoService.getById(request.getRobotId()));
-        this.execCmdHandler(request, InstructCodeEnums.START_TRACK);
+//        this.execCmdHandler(request, InstructCodeEnums.START_TRACK);
     }
 
     public IRobotRouteOptService getRobotRouteOptService() {
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/BaseRobotCmdDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/BaseRobotCmdDTO.java
index 5608f2b..06b53de 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/BaseRobotCmdDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/BaseRobotCmdDTO.java
@@ -3,11 +3,13 @@
 import io.swagger.annotations.ApiModelProperty;
 import lombok.Data;
 
+import java.io.Serializable;
+
 /**
  * 机器人指令基类
  */
 @Data
-public class BaseRobotCmdDTO {
+public class BaseRobotCmdDTO implements Serializable {
     @ApiModelProperty(value = "机器人主键", dataType = "String")
     protected Long robotId;
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/StartTrackRequest.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/StartTrackRequest.java
index a18321b..85546a7 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/StartTrackRequest.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/StartTrackRequest.java
@@ -3,6 +3,8 @@
 import io.swagger.annotations.ApiModelProperty;
 import lombok.Data;
 
+import java.math.BigDecimal;
+
 /**
  * 导航下发指令
  */
@@ -10,18 +12,19 @@
 public class StartTrackRequest extends BaseRobotCmdDTO {
     @ApiModelProperty(value = "sn编码(可不填 默认机器ID)", dataType = "String")
     private String sn;
-
     @ApiModelProperty(value = "下发状态 1:开启 0:关闭", dataType = "String")
     private Integer action;
     @ApiModelProperty(value = "等待状态 1:等待 0:不等待", dataType = "String")
     private Integer wait;
     @ApiModelProperty(value = "等待状态 0:循迹 1:任务", dataType = "isTask")
     private Integer isTask;
-    @ApiModelProperty(value = "轨迹名称", dataType = "String")
+    @ApiModelProperty(value = "轨迹名称(废弃)", dataType = "String")
     private String track_name;
     @ApiModelProperty(value = "关键点名称", dataType = "String", hidden = true)
     private String taskpoint_name;
-
+    @ApiModelProperty(value = "初始寻源步长", dataType = "Integer")
+    private Integer stepLen;
+    @ApiModelProperty(value = "寻源阈值", dataType = "BigDecimal")
+    private BigDecimal maxThreshold;
     private String obs_mode="1";
-
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/response/StopStateResponseDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/response/StopStateResponseDTO.java
index eb514bf..e0679d7 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/response/StopStateResponseDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/response/StopStateResponseDTO.java
@@ -3,11 +3,13 @@
 import io.swagger.annotations.ApiModelProperty;
 import lombok.Data;
 
+import java.io.Serializable;
+
 /**
  * 电压信息返回
  */
 @Data
-public class StopStateResponseDTO {
+public class StopStateResponseDTO implements Serializable {
     @ApiModelProperty(value = "软件急停(0：无触发 1：触发)" , dataType = "Integer")
     private Integer soft;
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IAlgorithmService.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IAlgorithmService.java
index 56b3e1d..f62523b 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IAlgorithmService.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IAlgorithmService.java
@@ -6,6 +6,7 @@
 import com.casic.missiles.modular.robot.dto.PathPlanDTO;
 import com.casic.missiles.modular.robot.model.RobotInfo;
 
+import java.io.File;
 import java.math.BigDecimal;
 import java.util.List;
 
@@ -38,5 +39,5 @@
      * @return
      */
     GridPointDTO convertGridCenterPoint(GridPointDTO pointDTO, BigDecimal resolution, RobotInfo robotInfo);
-    void readAlgorithmResponse(String fileName, AlgorithmResponse response, Long taskId, List<String> res);
+    void readAlgorithmResponse(String fileName, File repetitionFile, AlgorithmResponse response, Long taskId, List<String> res);
 }
\ No newline at end of file
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IRobotStatusInfoService.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IRobotStatusInfoService.java
index 2af210a..757f8d0 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IRobotStatusInfoService.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IRobotStatusInfoService.java
@@ -67,10 +67,9 @@
     /**
      * 更新车辆路线及任务信息
      *
-     * @param currRouteId 当前路线信息
      * @param currTaskId  当前任务信息
      */
-    void updateCurrTask(String robotId, Long currRouteId, Long currTaskId);
+    void updateCurrTask(String robotId, Long currTaskId);
 
     RobotStatusInfo getStatusByRobotId(String robotId);
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/ITaskGridInfoService.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/ITaskGridInfoService.java
new file mode 100644
index 0000000..fc0da93
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/ITaskGridInfoService.java
@@ -0,0 +1,22 @@
+package com.casic.missiles.modular.robot.service;
+
+import com.baomidou.mybatisplus.extension.service.IService;
+import  com.casic.missiles.modular.robot.model.TaskGridInfo;
+import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
+import com.baomidou.mybatisplus.extension.plugins.pagination.Page;
+import java.util.List;
+
+
+/**
+ * <p>
+ * 任务网格信息 服务类
+ * </p>
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+public interface ITaskGridInfoService extends IService<TaskGridInfo> {
+/**
+* 任务网格信息 分页检索
+*/
+List<TaskGridInfo> selectTaskGridInfoPage(Page<TaskGridInfo> page,QueryWrapper<TaskGridInfo> query);}
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/ITaskInfoService.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/ITaskInfoService.java
index 31d7ba6..b395993 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/ITaskInfoService.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/ITaskInfoService.java
@@ -51,6 +51,13 @@
      * @param dto 实体
      */
     void pointCallBack(PoseMessageResponseDTO dto, String robotId);
+
+    /**
+     * 车辆路线规划
+     * @param dto
+     * @param robotId
+     */
+    void carPathPlanning(PoseMessageResponseDTO dto, String robotId);
     void pointTestCallBack(PoseMessageResponseDTO dto, String robotId);
     /**
      * 图片路径保存
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/AlgorithmServiceImpl.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/AlgorithmServiceImpl.java
index f808fa0..a4dbe0b 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/AlgorithmServiceImpl.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/AlgorithmServiceImpl.java
@@ -29,10 +29,12 @@
 @Slf4j
 @Service
 public class AlgorithmServiceImpl implements IAlgorithmService {
-    @Value("${casic.algorithm.initPath:-1}")
-    private String initPath;
-    @Value("${casic.algorithm.planPath:-1}")
-    private String planPath;
+    @Value("${casic.algorithm.path:-1}")
+    private String path;
+    @Value("${casic.algorithm.initPathName:-1}")
+    private String initPathName;
+    @Value("${casic.algorithm.planPathName:-1}")
+    private String planPathName;
     private final ITaskHeatMapService heatMapService;
 
     public AlgorithmServiceImpl(ITaskHeatMapService heatMapService) {
@@ -42,13 +44,13 @@
     @Transactional
     @Override
     public AlgorithmResponse pathPlanningInit(PathInitDTO initDTO) {
-        if (StrUtil.isEmpty(initDTO.getCount())) {
-            initDTO.setCount("0.00");
+        if (StrUtil.isEmpty(initDTO.getZNow())) {
+            initDTO.setZNow("0.00");
         }
         log.info(JSON.toJSONString(initDTO));
         //算法路径及入参配置
         List<String> commends = new ArrayList<>();
-        commends.add(initPath);
+        commends.add(path + initPathName);
         commends.addAll(initDTO.toArray());
         List<String> res = AlgorithmUtils.invokeJob(commends);
 
@@ -63,38 +65,72 @@
     @Transactional
     @Override
     public AlgorithmResponse pathPlanning(PathPlanDTO planDTO) {
-        if (StrUtil.isEmpty(planDTO.getCount())) {
-            planDTO.setCount("0.001");
+        if (StrUtil.isEmpty(planDTO.getZNow())) {
+            planDTO.setZNow("0.001");
         }
         //算法路径及入参配置
         List<String> commends = new ArrayList<>();
-        commends.add(planPath);
+        commends.add(path + planPathName);
         commends.addAll(planDTO.toArray());
         log.info("寻源算法调用:{}", JSON.toJSONString(planDTO));
         List<String> res = AlgorithmUtils.invokeJob(commends);
         AlgorithmResponse response = getAlgorithmResponse(planDTO, res);
-        //next 点位编写
-        if (CollectionUtil.isNotEmpty(res) && res.size() == 2) {
-            response.getPointNextDTO().setX(formatBigDecimal(res.get(0).replaceAll("next_X: ", "")));
-            response.getPointNextDTO().setY(formatBigDecimal(res.get(1).replaceAll("next_Y: ", "")));
-        }
+
         return response;
     }
 
     private AlgorithmResponse getAlgorithmResponse(PathInitDTO initDTO, List<String> res) {
-        File repetitionFile = new File("repetition.txt");
+        //读取当前路径图片
+        File repetitionFileDir = new File(path + "\\" + initDTO.getTaskId() + "\\predicts");
+        File[] files = repetitionFileDir.listFiles((dir, name) -> name.endsWith(".txt") && name.startsWith("step"));
+        File repetitionFile = null;
+        //判定当前执行路径最大step2文件夹
+        if (files != null && files.length > 0) {
+             repetitionFile = files[files.length - 1];
+        }
         AlgorithmResponse response = new AlgorithmResponse();
         if (initDTO.getTaskId() != null && repetitionFile.exists()) {
-            readAlgorithmResponse("repetition.txt", response, initDTO.getTaskId(), res);
-            readAlgorithmResponse("step.txt", response, initDTO.getTaskId(), res);
+            //读取热力图
+            readAlgorithmResponse("repetition.txt",repetitionFile, response, initDTO.getTaskId(), res);
         } else {
             log.info("taskId is empty!");
         }
-        readAlgorithmResponse("xy_train.txt", response, initDTO.getTaskId(), res);
-        readAlgorithmResponse("z_train.txt", response, initDTO.getTaskId(), res);
+        //读取规划路径
+        if (CollectionUtil.isNotEmpty(res)) {
+            response.setEnd(false);
+            readAlgorithmResponse(response, res);
+        }
         return response;
     }
 
+    private void readAlgorithmResponse(AlgorithmResponse response, List<String> res) {
+        List<String> resList = new ArrayList<>();
+        if (res.size() >= 2) {
+            resList = res.subList(res.size() - 2, res.size());
+        }
+        //判定是否结束寻源
+        if (res.contains("end")) {
+            response.setEnd(true);
+            for (String s : resList) {
+                if (s.startsWith("source_coordinate:")) {
+                    s = s.trim();
+                    log.info("寻源结束,目标点位：{}",s);
+                    List<String> lists = StrUtil.split(s.replaceAll("source_coordinate:", ""), ",");
+                    PointNextDTO nextDTO = new PointNextDTO();
+                    nextDTO.setX(lists.get(0));
+                    nextDTO.setY(lists.get(1));
+                    response.setCoordinate(nextDTO);
+                }
+            }
+        } else {
+            PointNextDTO nextDTO = new PointNextDTO();
+            nextDTO.setX(resList.get(0));
+            nextDTO.setY(resList.get(1));
+            response.setPointNextDTO(nextDTO);
+        }
+
+    }
+
     /**
      * 读取算法输出数据
      *
@@ -102,49 +138,18 @@
      * @param response 结果集
      * @param taskId   任务ID
      */
-    public void readAlgorithmResponse(String fileName, AlgorithmResponse response, Long taskId, List<String> res) {
-        File file = new File(fileName);
-        if (!file.exists()&&!fileName.startsWith("step")) {
+    public void readAlgorithmResponse(String fileName,File repetitionFile, AlgorithmResponse response, Long taskId, List<String> res) {
+        if (!repetitionFile.exists() && !fileName.startsWith("step")) {
             log.info("{} file is empty!", fileName);
             return;
         }
         if (response.getPointNextDTO() == null) {
             response.setPointNextDTO(new PointNextDTO());
         }
-        switch (fileName) {
-            case "repetition.txt":
-                String msg = FileUtil.readString(file, CharsetUtil.CHARSET_UTF_8);
-                TaskHeatMap taskHeatMap = createEmptyTaskHeatMap(taskId);
-                taskHeatMap.setRepetition(msg);
-                response.setTaskHeatMap(taskHeatMap);
-                break;
-            case "xy_train.txt":
-                List<String> xyTrains = FileUtil.readLines(file, CharsetUtil.CHARSET_UTF_8);
-                if (CollectionUtil.isNotEmpty(xyTrains)) {
-                    if (CollectionUtil.isNotEmpty(xyTrains)) {
-                        List<String> list = StrUtil.split(xyTrains.get(xyTrains.size() - 1), " ");
-                        response.getPointNextDTO().setX(formatBigDecimal(list.get(0)));
-                        response.getPointNextDTO().setY(formatBigDecimal(list.get(1)));
-                    }
-                }
-                break;
-            case "z_train.txt":
-                List<String> zTrains = FileUtil.readLines(file, CharsetUtil.CHARSET_UTF_8);
-                if (CollectionUtil.isNotEmpty(zTrains)) {
-                    String zTrain = zTrains.get(zTrains.size() - 1);
-                    response.getPointNextDTO().setZ(formatBigDecimal(zTrain));
-                }
-            default:
-                if (StrUtil.isNotEmpty(fileName) && fileName.startsWith("step")) {
-                    File fileDir = new File(".");
-                    File[] files = fileDir.listFiles((dir, name) -> name.endsWith(".txt") && name.startsWith("step"));
-                    if (files != null && files.length > 0) {
-                        String steps = FileUtil.readString(files[files.length - 1], CharsetUtil.CHARSET_UTF_8);
-                        response.getTaskHeatMap().setRepetition(steps);
-                    }
-                }
-                break;
-        }
+        String msg = FileUtil.readString(repetitionFile, CharsetUtil.CHARSET_UTF_8);
+        TaskHeatMap taskHeatMap = createEmptyTaskHeatMap(taskId);
+        taskHeatMap.setRepetition(msg);
+        response.setTaskHeatMap(taskHeatMap);
     }
 
     /**
@@ -157,13 +162,11 @@
     public GridPointDTO convertSlamToGrid(BigDecimal x, BigDecimal y, BigDecimal resolution, RobotInfo robotInfo) {
         GridPointDTO pointDTO = new GridPointDTO();
         BigDecimal scale = resolution;
-
         //减去栅格原点坐标
         x = x.subtract(robotInfo.getOriginX());
         y = y.subtract(robotInfo.getOriginY());
-
-        pointDTO.setX(new BigDecimal(x.divide(scale,3,BigDecimal.ROUND_HALF_UP).intValue()+1));
-        pointDTO.setY(new BigDecimal(y.divide(scale,3,BigDecimal.ROUND_HALF_UP).intValue()+1));
+        pointDTO.setX(new BigDecimal(x.divide(scale, 3, BigDecimal.ROUND_HALF_UP).intValue()+1));
+        pointDTO.setY(new BigDecimal(y.divide(scale, 3, BigDecimal.ROUND_HALF_UP).intValue() +1));
         return pointDTO;
     }
 
@@ -174,19 +177,17 @@
      * @return
      */
     public GridPointDTO convertGridCenterPoint(GridPointDTO pointDTO, BigDecimal resolution, RobotInfo robotInfo) {
-
         GridPointDTO centerPoint = new GridPointDTO();
         BigDecimal scale = resolution;
-        //点位转换
-        BigDecimal slamX = pointDTO.getX().divide(scale, 5, BigDecimal.ROUND_HALF_UP);
-        BigDecimal slamY = pointDTO.getY().divide(scale, 5, BigDecimal.ROUND_HALF_UP);
+        //点位转换 转换后栅格－1 从1开始计算
+        BigDecimal slamX = (pointDTO.getX().subtract(new BigDecimal(1))).multiply(scale);
+        BigDecimal slamY = (pointDTO.getY().subtract(new BigDecimal(1))).multiply(scale);
         //中心栅格点位中心添加
-        centerPoint.setX(slamX.add(scale.divide(new BigDecimal(2), 5, BigDecimal.ROUND_HALF_UP)));
-        centerPoint.setY(slamY.add(scale.divide(new BigDecimal(2), 5, BigDecimal.ROUND_HALF_UP)));
+        centerPoint.setX(slamX.add(scale.divide(new BigDecimal(2), 10, BigDecimal.ROUND_HALF_UP)));
+        centerPoint.setY(slamY.add(scale.divide(new BigDecimal(2), 10, BigDecimal.ROUND_HALF_UP)));
         //加上栅格原点坐标
         centerPoint.setX(centerPoint.getX().add(robotInfo.getOriginX()));
         centerPoint.setY(centerPoint.getY().add(robotInfo.getOriginY()));
-
         return centerPoint;
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotInfoServiceImpl.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotInfoServiceImpl.java
index 043e05d..fdded64 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotInfoServiceImpl.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotInfoServiceImpl.java
@@ -104,7 +104,7 @@
             //下发障碍物栅格
             sendCostMapResolution(robotInfo.getResolution(), robotInfo.getId());
             //重新生成栅格地图
-            sendChangePcd(robotInfo.getId());
+            //sendChangePcd(robotInfo.getId());
         }
         update.eq(RobotInfo::getId, robotInfo.getId());
         update(update);
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotStatusInfoServiceImpl.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotStatusInfoServiceImpl.java
index 6d3ae91..46b5c3b 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotStatusInfoServiceImpl.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotStatusInfoServiceImpl.java
@@ -15,6 +15,7 @@
 import com.casic.missiles.modular.robot.dao.RobotStatusInfoMapper;
 import com.casic.missiles.modular.robot.model.RobotInfo;
 import com.casic.missiles.modular.robot.model.RobotStatusInfo;
+import com.casic.missiles.modular.robot.opt.enums.InstructCodeEnums;
 import com.casic.missiles.modular.robot.opt.instruct.base.dto.RobotMsgDTO;
 import com.casic.missiles.modular.robot.opt.instruct.base.dto.RobotMsgResponseDTO;
 import com.casic.missiles.modular.robot.opt.instruct.base.dto.RobotResponseDTO;
@@ -26,13 +27,18 @@
 import com.casic.missiles.modular.robot.service.IRobotInfoService;
 import com.casic.missiles.modular.robot.service.IRobotStatusInfoService;
 import lombok.extern.slf4j.Slf4j;
+import org.aspectj.util.FileUtil;
 import org.eclipse.paho.client.mqttv3.MqttMessage;
+import org.springframework.beans.factory.annotation.Value;
 import org.springframework.context.annotation.Lazy;
 import org.springframework.stereotype.Service;
 import org.springframework.transaction.annotation.Transactional;
 
+import java.io.File;
 import java.io.Serializable;
 import java.math.BigDecimal;
+import java.util.ArrayList;
+import java.util.Arrays;
 import java.util.List;
 
 
@@ -147,7 +153,6 @@
     @Override
     public void saveProcessMessage(String robotId, MqttMessage message) {
 //        String json = new String(message.getPayload());
-
         RobotResponseDTO<ProcessMessageResponseDTO> responseDTO = parseJson(message, new TypeReference<RobotResponseDTO<ProcessMessageResponseDTO>>() {
         });
         if (responseDTO.getMsg() != null) {
@@ -189,9 +194,8 @@
     }
 
     @Override
-    public void updateCurrTask(String robotId, Long currRouteId, Long currTaskId) {
+    public void updateCurrTask(String robotId, Long currTaskId) {
         UpdateWrapper<RobotStatusInfo> query = new UpdateWrapper<>();
-        query.set("curr_route_id", currRouteId);
         query.set("curr_task_id", currTaskId);
         query.eq("robot_id", robotId);
         update(query);
@@ -250,6 +254,15 @@
         }
     }
 
+    @Value("${casic.algorithm.path:-1}")
+    private String path;
+
+    /**
+     * 栅格阈值
+     */
+    @Value("${casic.algorithm.num:30}")
+    private Integer num;
+
     /**
      * 机器人栅格数据
      *
@@ -262,15 +275,41 @@
         RobotResponseDTO<AppCostmapResponseDTO> responseDTO = parseJson(message, new TypeReference<RobotResponseDTO<AppCostmapResponseDTO>>() {
         });
         if (responseDTO != null && responseDTO.getMsg() != null) {
+            //发送栅格分辨率设定
+
             cacheService.updateCostMap(robotId, responseDTO.getMsg());
             robotInfoService.updateRobotGrid(robotId, responseDTO.getMsg());
-//            log.info("--------------- json data:{}", JSON.toJSONString(responseDTO.getMsg()));
+            //log.info("--------------- json data:{}", JSON.toJSONString(responseDTO.getMsg()));
+            if (responseDTO != null && responseDTO.getMsg().getData() != null) {
+                ArrayList<Integer> dataNums = new ArrayList<>();
+                //根据阈值转换栅格
+                for (Integer dataNum : responseDTO.getMsg().getData()) {
+                    if (dataNum != -1) {
+                        if (dataNum >= num) {
+                            dataNums.add(0);
+                        } else {
+                            dataNums.add(1);
+                        }
+                    }
+                }
+                writeCache(dataNums,responseDTO.getMsg().getWidth(),responseDTO.getMsg().getHeight(),robotId);
+            }
         }
     }
 
+    private void writeCache(ArrayList<Integer> integers,Integer width,Integer height,String robotId) {
+        List<List<Integer>> arrays = CollectionUtil.split(integers,width);
+        ArrayList<ArrayList<Integer>> arrayList = new ArrayList<>();
+        for (List<Integer> array : arrays) {
+            arrayList.add((ArrayList)array);
+        }
+        CollectionUtil.reverse(arrayList);
+        CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_COST_MAP + robotId, arrayList);
+//        FileUtil.writeAsString(new File(path + "map.txt"), CollectionUtil.join(integers, " ").replaceAll("\\[", "").replaceAll("]", ""));
+    }
+
     @Override
     public void updateRobotPoint(PoseMessageResponseDTO dto, String robotId) {
-        //todo cache
         UpdateWrapper<RobotStatusInfo> query = new UpdateWrapper<>();
         query.set("postion_x", dto.getX());
         query.set("postion_y", dto.getY());
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotTrajectoryInfoServiceImpl.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotTrajectoryInfoServiceImpl.java
index 6c9625f..f1262a2 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotTrajectoryInfoServiceImpl.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotTrajectoryInfoServiceImpl.java
@@ -57,29 +57,29 @@
         });
 
         //读取导航状态,如果导航未开启则不存储
-//        RobotStatusInfo statusInfo = robotStatusInfoService.getStatusByRobotId(robotId);
-        Long routeId= cacheService.readRouteId(Long.valueOf(robotId));
-        if (routeId != null && responseDTO.getMsg() != null) {
+        //RobotStatusInfo statusInfo = robotStatusInfoService.getStatusByRobotId(robotId);
+
+        if ( responseDTO.getMsg() != null) {
             PoseMessageResponseDTO dto = responseDTO.getMsg();
             //执行循迹 保存当前轨迹信息
             //更新实时定位信息
             CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.ROBOT_POINT + robotId, dto);
-            infoService.pointTestCallBack(dto, robotId);
-            if (routeId != null) {
-                RobotTrajectoryInfo info = new RobotTrajectoryInfo();
-                info.setRobotId(Long.valueOf(robotId));
-                info.setCreateTime(new Date());
-                info.setPostionX(dto.getX());
-                info.setPostionY(dto.getY());
-                info.setPostionZ(dto.getZ());
-                info.setTheta(dto.getTheta());
-                save(info);
-                try {
-                    infoService.pointCallBack(dto, robotId);
-                } catch (Exception e) {
-                    log.error("task error", e);
-                }
+            try {
+                infoService.carPathPlanning(dto, robotId);
+            } catch (Exception e) {
+                log.error("task error", e);
             }
+//            if (routeId != null) {
+//                RobotTrajectoryInfo info = new RobotTrajectoryInfo();
+//                info.setRobotId(Long.valueOf(robotId));
+//                info.setCreateTime(new Date());
+//                info.setPostionX(dto.getX());
+//                info.setPostionY(dto.getY());
+//                info.setPostionZ(dto.getZ());
+//                info.setTheta(dto.getTheta());
+//                save(info);
+
+//            }
         }
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/TaskGridInfoServiceImpl.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/TaskGridInfoServiceImpl.java
new file mode 100644
index 0000000..cf61def
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/TaskGridInfoServiceImpl.java
@@ -0,0 +1,28 @@
+package com.casic.missiles.modular.robot.service.impl;
+
+import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
+import com.baomidou.mybatisplus.extension.plugins.pagination.Page;
+import com.baomidou.mybatisplus.extension.service.impl.ServiceImpl;
+import com.casic.missiles.modular.robot.dao.TaskGridInfoMapper;
+import com.casic.missiles.modular.robot.model.TaskGridInfo;
+import com.casic.missiles.modular.robot.service.ITaskGridInfoService;
+import org.springframework.stereotype.Service;
+
+import java.util.List;
+
+
+/**
+ * <p>
+ * 任务网格信息 服务实现类
+ * </p>
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+@Service
+public class TaskGridInfoServiceImpl extends ServiceImpl<TaskGridInfoMapper, TaskGridInfo> implements ITaskGridInfoService {
+    @Override
+    public List<TaskGridInfo> selectTaskGridInfoPage(Page<TaskGridInfo> page, QueryWrapper<TaskGridInfo> query) {
+        return this.baseMapper.selectTaskGridInfoPage(page, query);
+    }
+}
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/TaskInfoServiceImpl.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/TaskInfoServiceImpl.java
index d1417b0..fd49e2e 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/TaskInfoServiceImpl.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/TaskInfoServiceImpl.java
@@ -6,10 +6,10 @@
 import cn.hutool.core.convert.Convert;
 import cn.hutool.core.date.DateUtil;
 import cn.hutool.core.io.FileUtil;
-import cn.hutool.core.thread.ExecutorBuilder;
 import cn.hutool.core.util.StrUtil;
 import com.alibaba.fastjson.JSON;
 import com.baomidou.mybatisplus.core.conditions.Wrapper;
+import com.baomidou.mybatisplus.core.conditions.query.LambdaQueryWrapper;
 import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
 import com.baomidou.mybatisplus.core.conditions.update.UpdateWrapper;
 import com.baomidou.mybatisplus.core.metadata.IPage;
@@ -47,7 +47,6 @@
 import java.io.Serializable;
 import java.math.BigDecimal;
 import java.util.*;
-import java.util.concurrent.ThreadPoolExecutor;
 
 
 /**
@@ -70,6 +69,7 @@
     private final IRobotCacheService cacheService;
     private final IBaseRobotService baseRobotService;
     private final ITaskInfoImgService imgService;
+    private final ITaskGridInfoService taskGridInfoService;
     /**
      * 初始化线程池
      */
@@ -79,12 +79,19 @@
     @Value("${casic.doc.temp}")
     private String docTemp;
     /**
+     * 5000 ms
+     */
+    @Value("${casic.task.duration:3000}")
+    private Integer stayDuration;
+    @Value("${casic.algorithm.path:-1}")
+    private String path;
+    /**
      * 障碍物精度
      */
     @Value("${casic.obstacle:10}")
     private Integer obstacle;
 
-    public TaskInfoServiceImpl(@Lazy IRouteInfoService routeInfoService, IRobotStatusInfoService statusInfoService, AbstractDictService dictService, @Lazy IAlarmRecordService recordService, @Lazy ITaskHeatMapService taskHeatMapService, IAlgorithmService algorithmService, IRobotCacheService cacheService, @Lazy IBaseRobotService baseRobotService, ITaskInfoImgService imgService) {
+    public TaskInfoServiceImpl(@Lazy IRouteInfoService routeInfoService, IRobotStatusInfoService statusInfoService, AbstractDictService dictService, @Lazy IAlarmRecordService recordService, @Lazy ITaskHeatMapService taskHeatMapService, IAlgorithmService algorithmService, IRobotCacheService cacheService, @Lazy IBaseRobotService baseRobotService, ITaskInfoImgService imgService, ITaskGridInfoService taskGridInfoService) {
         this.routeInfoService = routeInfoService;
         this.statusInfoService = statusInfoService;
         this.dictService = dictService;
@@ -94,6 +101,7 @@
         this.cacheService = cacheService;
         this.baseRobotService = baseRobotService;
         this.imgService = imgService;
+        this.taskGridInfoService = taskGridInfoService;
     }
 
     /**
@@ -184,7 +192,7 @@
         if (CollectionUtil.isEmpty(routeInfos)) {
             throw new ServiceException(500, "路线不存在,请及时更新");
         }
-        RouteInfo routeInfo = routeInfos.get(0);
+//        RouteInfo routeInfo = routeInfos.get(0);
         Integer isTask = 0;
         //开启寻源任务
         if (request.getIsTask() == 1) {
@@ -193,22 +201,21 @@
         RobotInfo robotInfo = cacheService.getRobotInfo(Convert.toLong(robotId));
         CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_IS_TASK + robotId, isTask);
         //添加任务记录
-        TaskInfo taskInfo = addTaskByStartTrack(routeInfo, robotInfo);
+        TaskInfo taskInfo = addTaskByStartTrack( robotInfo, request);
         //No.3 更新器人执行任务信息
-        Long currRouteId = routeInfo.getId();
+//        Long currRouteId = routeInfo.getId();
         Long currTaskId = taskInfo.getId();
 
-        statusInfoService.updateCurrTask(robotId, currRouteId, currTaskId);
+        statusInfoService.updateCurrTask(robotId,  currTaskId);
         //算法初始化标志创建
         CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_TASK_ID + robotId, currTaskId);
         CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_TASK_INIT + robotId, 0);
-        CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_ROUTE_ID + robotId, routeInfo.getId());
+//        CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_ROUTE_ID + robotId, routeInfo.getId());
     }
 
     private void closeTask(StartTrackRequest request, String robotId) {
         //关闭操作
         RobotStatusInfo statusInfo = statusInfoService.getStatusByRobotId(robotId);
-
         if (statusInfo.getCurrTaskId() != null) {
             UpdateWrapper<TaskInfo> updateWrapper = new UpdateWrapper<>();
             updateWrapper.set("update_time", new Date());
@@ -221,10 +228,11 @@
 
         //清空任务缓存
         robotCacheClean(robotId);
-
+        //清楚规划任务锁
+        CacheUtil.remove(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_PLANING_FLAG + robotId);
+        CacheUtil.remove(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_ESTIMATE_GRID+ robotId);
         //清空机器当前任务
-        statusInfoService.updateCurrTask(robotId, null, null);
-
+        statusInfoService.updateCurrTask(robotId, null);
     }
 
     /**
@@ -249,7 +257,6 @@
 
         //判定任务是否已经完成初始化
         Long longId = Convert.toLong(robotId);
-        Long taskId = CacheUtil.get(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_TASK_ID + robotId);
         //缓存当前点位信息
         CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_XY_POINT + robotId, dto);
         RobotInfo robotInfo = cacheService.getRobotInfo(longId);
@@ -260,86 +267,203 @@
             log.error("no config 中子源阈值未配置");
             return;
         }
-
         //读取是否是循迹任务
         Integer isTask = cacheService.readIsTask(longId);
         if (isTask != 1) {
             log.info("循迹任务,不做处理");
             return;
         }
-        //算法初始化状态读取
-        Integer initValue = cacheService.readTaskInit(longId);
-
 
         //No.1 任务初始化
+//        pathInit(longId, robotInfo, pointDTO, taskId, robotId);
+    }
+
+    @Override
+    public void carPathPlanning(PoseMessageResponseDTO dto, String robotId) {
+        //判定任务是否已经完成初始化
+        Long longId = Convert.toLong(robotId);
+        Long taskId = CacheUtil.get(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_TASK_ID + robotId);
+        //缓存当前点位信息
+        CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_XY_POINT + robotId, dto);
+        RobotInfo robotInfo = cacheService.getRobotInfo(longId);
+        //slam经纬度转栅格
+        GridPointDTO pointDTO = algorithmService.convertSlamToGrid(dto.getX(), dto.getY(), robotInfo.getResolution(), robotInfo);
+        if (robotInfo.getAlarmThreshold() == null || robotInfo.getMaxThreshold() == null) {
+            log.error("no config 中子源阈值未配置");
+            return;
+        }
+
+        //读取是否是寻源任务
+        Integer isTask = cacheService.readIsTask(longId);
+        if (isTask != 1) {
+            log.info("循迹任务,不做处理");
+            return;
+        }
+
+        TaskInfo taskInfo = getById(taskId);
+        //No.1 任务初始化
+        pathInit(longId, robotInfo, pointDTO, taskId, robotId, taskInfo.getStepLen());
+        //No.2 路线规划
+        pathPlaning(longId, robotInfo, pointDTO, taskId, robotId, taskInfo,dto);
+    }
+
+    /**
+     * 路线规划进行加锁
+     *
+     * @param robotId 机器人ID
+     * @return
+     */
+    private synchronized boolean lockPlanIngFlag(String robotId) {
+        Boolean flag = CacheUtil.get(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_PLANING_FLAG + robotId);
+        if (flag == null) {
+            CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_PLANING_FLAG + robotId, true);
+            return true;
+        }
+        return false;
+    }
+
+    /**
+     * @param longId    机器人ID
+     * @param robotInfo 机器人信息
+     * @param pointDTO  当前点位栅格信息
+     * @param taskId    任务ID
+     * @param robotId   机器人ID
+     */
+    private void pathPlaning(Long longId, RobotInfo robotInfo, GridPointDTO pointDTO, Long taskId, String robotId, TaskInfo taskInfo,PoseMessageResponseDTO curPoint) {
+        if (taskId != null) {
+            //规划节点
+            PointNextDTO pointNextDTO = CacheUtil.get(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_ESTIMATE_GRID + robotId);
+            GridPointDTO oldEstimateGrid = GridPointDTO.convertGridPoint(pointNextDTO);
+            Boolean flag = lockPlanIngFlag(robotId);
+            //是否到达预期栅格
+            if (!flag) {
+                log.info("未获取到规划任务,直接结束：{}",pointDTO);
+                return;
+            }
+            try {
+                log.debug("oldEstimateGrid:{}", oldEstimateGrid);
+                if (pointNextDTO == null || isEquals(pointDTO, oldEstimateGrid)) {
+                    log.info("当前栅格变更......", pointDTO);
+                    CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_POINT + robotId, pointDTO);
+                    //算法调用获取热力图
+                    PathPlanDTO pathPlanDTO = createPlanDTO(robotInfo, pointDTO, taskId, robotId, taskInfo.getStepLen());
+                    if (taskInfo.getMaxThreshold() == null) {
+                        taskInfo.setMaxThreshold(robotInfo.getMaxThreshold());
+                    }
+                    pathPlanDTO.setTh(taskInfo.getMaxThreshold() + "");
+
+                    log.debug("heat map -- robotId:{},taskId:{}", robotId, taskId);
+                    pathPlanDTO.setZNow(cacheService.getNeutronCount(longId));
+
+                    //存储算法输出栅格信息
+                    AlgorithmResponse response = algorithmService.pathPlanning(pathPlanDTO);
+                    CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_ESTIMATE_GRID + robotId, response.getPointNextDTO());
+                    sendWebSocket(InstructStatusMsgEnums.HEAT_MAP, robotId, response.getTaskHeatMap());
+
+                    //存储循迹任务信息
+                    TaskGridInfo taskGridInfo = new TaskGridInfo();
+                    taskGridInfo.setCreateTime(new Date());
+                    taskGridInfo.setUpdateTime(new Date());
+                    taskGridInfo.setCurGridX(pointDTO.getX().intValue());
+                    taskGridInfo.setCurGridY(pointDTO.getY().intValue());
+                    taskGridInfo.setNeutronCount(new BigDecimal(pathPlanDTO.getZNow()));
+                    taskGridInfo.setEstimateGridX(response.getPointNextDTO().getX());
+                    taskGridInfo.setEstimateGridY(response.getPointNextDTO().getY());
+                    taskGridInfo.setTaskId(taskId);
+                    taskGridInfoService.save(taskGridInfo);
+
+                    try {
+                        //停留时间阈值
+                        Thread.sleep(stayDuration);
+                    } catch (InterruptedException e) {
+                        e.printStackTrace();
+                    }
+
+                    //是否触发停止状态
+                    if (!response.isEnd()) {
+                        log.debug("task planning-- xun dian robotId:{},taskId:{}", robotId, taskId);
+                        //机器人寻点指令下发
+                        sendTargetPoint(robotInfo, response,curPoint);
+                    } else {
+                        log.debug("task stop-- robotId:{},taskId:{}", robotId, taskId);
+                        //机器人停止指令下发
+                        SoftwareStopRequest request = new SoftwareStopRequest();
+                        request.setRobotId(robotInfo.getId());
+                        //设置为急停
+                        request.setControl(1);
+//                        baseRobotService.execCmdHandler(request, InstructCodeEnums.SOFTWARE_STOP);
+                        //循迹任务结束
+                        closeTask(null, robotId);
+                    }
+                }
+            } catch (Exception e) {
+                e.printStackTrace();
+            } finally {
+                //释放锁
+                CacheUtil.remove(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_PLANING_FLAG + robotId);
+            }
+        }
+    }
+
+    private PathPlanDTO createPlanDTO(RobotInfo robotInfo, GridPointDTO pointDTO, Long taskId, String robotId, Integer stepLen) {
+        PathPlanDTO pathPlanDTO = new PathPlanDTO();
+        pathPlanDTO.setTaskId(taskId);
+        pathPlanDTO.setRobotId(robotId);
+        pathPlanDTO.setStepLen(stepLen);
+        return pathPlanDTO;
+    }
+
+    /**
+     * 寻源初始化
+     *
+     * @param longId    机器ID
+     * @param robotInfo 机器信息
+     * @param pointDTO  当前点位信息
+     * @param taskId    任务ID
+     * @param robotId   机器ID
+     * @param stepLen   步长设置
+     */
+    private void pathInit(Long longId, RobotInfo robotInfo, GridPointDTO pointDTO, Long taskId, String robotId, Integer stepLen) {
+        //障碍物栅格上传已生成 直接读取
+        //算法初始化状态读取
+        Integer initValue = cacheService.readTaskInit(longId);
+        //No.1 任务初始化
         if (initValue != 1) {
+
             //执行初始化操作
             if (cacheService.updateTaskInit(longId, 1)) {
 
-                try {
-                    File file = new File(".");
-                    File[] files = file.listFiles((dir, name) -> (name.endsWith(".png") && name.startsWith("step"))
-                            || (name.endsWith(".txt") && name.startsWith("step")) || name.equals("repetition.txt") || name.equals("xy_train.txt") || name.equals("z_train.txt"));
-                    if (files != null) {
-                        for (int i = 0; i < files.length; i++) {
-                            FileUtil.del(files[i]);
+                List<List<Integer>> costList = CacheUtil.get(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_COST_MAP + robotId);
+                List<String> strs = new ArrayList<>();
+                List<String> strs2 = new ArrayList<>();
+                for (List<Integer> list : costList) {
+                    strs2.add(CollectionUtil.join(list, " "));
+                }
+                FileUtil.writeLines(strs2, new File(path + "map2.txt"), "utf-8");
+                //行转列 为算法输入数据
+                if(CollectionUtil.isNotEmpty(costList)){
+                    CollectionUtil.reverse(costList);
+                    for (int i = 0; i < costList.get(0).size(); i++) {
+                        ArrayList<Integer> ints = new ArrayList<>();
+                        for (List<Integer> list : costList) {
+                            ints.add(list.get(i));
                         }
+                        strs.add(CollectionUtil.join(ints, " "));
                     }
-                } catch (Exception e) {
-                    e.printStackTrace();
+//                    CollectionUtil.reverse(strs);
                 }
 
-                PathInitDTO pathInitDTO = createInitDTO(robotInfo, pointDTO, taskId, robotId);
+
+                FileUtil.writeLines(strs, new File(path + "map.txt"), "utf-8");
+
+                PathInitDTO pathInitDTO = createInitDTO(robotInfo, pointDTO, taskId, robotId, stepLen);
                 //中子源计数率读取
-                pathInitDTO.setCount(cacheService.getNeutronCount(longId));
+                pathInitDTO.setZNow(cacheService.getNeutronCount(longId));
+                log.info("init params:{}", JSON.toJSONString(pathInitDTO));
                 //执行1次算法 初始化操作
                 algorithmService.pathPlanningInit(pathInitDTO);
             }
         }
-
-        //No.2 任务执行中
-        if (taskId != null) {
-            //判断栅格变化
-            GridPointDTO oldPointDTO = CacheUtil.get(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_POINT + robotId);
-            if (oldPointDTO == null || isChange(pointDTO, oldPointDTO)) {
-                log.debug("grid change -- robotId:{},taskId:{}", robotId, taskId);
-                CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_POINT + robotId, pointDTO);
-                //算法调用获取热力图
-                PathPlanDTO pathPlanDTO = createPlanDTO(robotInfo, pointDTO, taskId, robotId);
-                //可通行标记为1 不可通行标记为0
-                String obstacle = getObstacle(robotId, pointDTO);
-                //TO-DO 障碍物判定信息
-                pathPlanDTO.setOpen(obstacle);
-                //中子源计数率读取
-                pathPlanDTO.setCount(cacheService.getNeutronCount(longId));
-                log.debug("heat map -- robotId:{},taskId:{}", robotId, taskId);
-                AlgorithmResponse response = algorithmService.pathPlanning(pathPlanDTO);
-                //存储算法输出栅格信息
-                CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_ESTIMATE_GRID + robotId, response.getPointNextDTO());
-                sendWebSocket(InstructStatusMsgEnums.HEAT_MAP, robotId, response.getTaskHeatMap());
-                //是否触发寻源操作
-                BigDecimal bigCount = new BigDecimal(pathPlanDTO.getCount());
-                if (bigCount.compareTo(robotInfo.getAlarmThreshold()) >= 0) {
-                    log.debug("task planning-- xun dian robotId:{},taskId:{}", robotId, taskId);
-                    //机器人寻点指令下发
-                    sendTargetPoint(robotInfo, response);
-                }
-
-                //是否触发停止状态
-                if (bigCount.compareTo(robotInfo.getMaxThreshold()) > 0) {
-                    log.debug("task stop-- robotId:{},taskId:{}", robotId, taskId);
-                    //机器人停止指令下发
-                    SoftwareStopRequest request = new SoftwareStopRequest();
-                    request.setRobotId(robotInfo.getId());
-                    //设置为急停
-                    request.setControl(1);
-                    baseRobotService.execCmdHandler(request, InstructCodeEnums.SOFTWARE_STOP);
-                    //循迹任务结束
-                    closeTask(null, robotId);
-                }
-
-            }
-        }
     }
 
     /**
@@ -353,7 +477,6 @@
         //判定任务是否已经完成初始化
         Long longId = Convert.toLong(robotId);
         Long taskId = CacheUtil.get(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_TASK_ID + robotId);
-
         RobotInfo robotInfo = cacheService.getRobotInfo(longId);
         //slam经纬度转栅格
         GridPointDTO pointDTO = algorithmService.convertSlamToGrid(dto.getX(), dto.getY(), robotInfo.getResolution(), robotInfo);
@@ -428,7 +551,7 @@
      * @param robotInfo 机器人信息
      * @param response  算法输出数据
      */
-    private void sendTargetPoint(RobotInfo robotInfo, AlgorithmResponse response) {
+    private void sendTargetPoint(RobotInfo robotInfo, AlgorithmResponse response,PoseMessageResponseDTO curPoint) {
         //读取已设置巡点目标信息
         PointNextDTO nextDTO = CacheUtil.get(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_TARGET_POINT_ID + robotInfo.getId());
         //巡点
@@ -439,8 +562,8 @@
             TargetPointRequest request = new TargetPointRequest();
             request.setX(Convert.toStr(gridCenterPoint.getX()));
             request.setY(Convert.toStr(gridCenterPoint.getY()));
-            request.setZ("0");
-
+            request.setZ(curPoint.getZ().toString());
+            request.setTheta(curPoint.getTheta().toString());
             request.setRobotId(robotInfo.getId());
             //执行巡点指令
             baseRobotService.execCmdHandler(request, InstructCodeEnums.SET_TARGETPOINT);
@@ -507,21 +630,20 @@
         params.put("startTime", startTime);
         params.put("endTime", endTime);
         //读取
-        List<AlarmRecord> records = recordService.selectListByTaskId(info.getId(), info.getRobotId());
         Integer index = 1;
-
-        if (CollectionUtil.isNotEmpty(records)) {
-            for (AlarmRecord record : records) {
+        LambdaQueryWrapper<TaskGridInfo> taskGridQuery = new LambdaQueryWrapper<>();
+        taskGridQuery.eq(TaskGridInfo::getTaskId,info.getId());
+        taskGridQuery.orderByAsc(TaskGridInfo::getCreateTime);
+        List<TaskGridInfo> taskGridInfos = taskGridInfoService.list(taskGridQuery);
+        if (CollectionUtil.isNotEmpty(taskGridInfos)) {
+            for (TaskGridInfo record : taskGridInfos) {
                 //栅格信息转换
-                GridPointDTO pointDTO = algorithmService.convertSlamToGrid(record.getX(), record.getY(), info.getResolution(), robotInfo);
-                record.setX(pointDTO.getX());
-                record.setY(pointDTO.getY());
                 record.setIndex(index);
                 index++;
             }
         }
 
-        params.put("records", records);
+        params.put("records", taskGridInfos);
         ImageEntity mcaImg = new ImageEntity();
         mcaImg.setHeight(600);
         mcaImg.setWidth(600);
@@ -557,7 +679,6 @@
             psdImg.setData(WordUtil.getImageBase64(uploadPath + info.getPsdImg()));
         }
         params.put("psdImg", psdImg);
-
         WordUtil.exportOutputStream(docTemp, response.getOutputStream(), params);
     }
 
@@ -589,7 +710,7 @@
     private String saveHeatImg(String taskId) {
         String newFileName = "task\\" + taskId + ".png";
         //读取当前路径图片
-        File file = new File(".");
+        File file = new File(path + "\\" + taskId + "\\figs");
         File[] files = file.listFiles((dir, name) -> name.endsWith(".png") && name.startsWith("step"));
         //判定当前执行路径最大step图片
         if (files != null && files.length > 0) {
@@ -614,33 +735,28 @@
         return true;
     }
 
-    private PathPlanDTO createPlanDTO(RobotInfo robotInfo, GridPointDTO pointDTO, Long taskId, String robotId) {
-        PathPlanDTO pathPlanDTO = new PathPlanDTO();
-        String xMax = robotInfo.getGridHeight().divide(robotInfo.getResolution(), 5, BigDecimal.ROUND_HALF_UP).intValue() + "";
-        String yMax = robotInfo.getGridWidth().divide(robotInfo.getResolution(), 5, BigDecimal.ROUND_HALF_UP).intValue() + "";
-        pathPlanDTO.setX(pointDTO.getX().intValue() + "");
-        pathPlanDTO.setY(pointDTO.getY().intValue() + "");
-        pathPlanDTO.setXMax(xMax);
-        pathPlanDTO.setYMax(yMax);
-        pathPlanDTO.setTaskId(taskId);
-        pathPlanDTO.setRobotId(robotId);
-        return pathPlanDTO;
+    private boolean isEquals(GridPointDTO pointDTO, GridPointDTO oldPointDTO) {
+        if(oldPointDTO==null||oldPointDTO.getX()==null){
+            return false;
+        }
+        if (pointDTO.getX().compareTo(oldPointDTO.getX()) == 0 && pointDTO.getY().compareTo(oldPointDTO.getY()) == 0) {
+            return true;
+        }
+        return false;
     }
 
-    private PathInitDTO createInitDTO(RobotInfo robotInfo, GridPointDTO pointDTO, Long taskId, String robotId) {
-        String xMax = robotInfo.getGridHeight().divide(robotInfo.getResolution(), 5, BigDecimal.ROUND_HALF_UP).intValue() + "";
-        String yMax = robotInfo.getGridWidth().divide(robotInfo.getResolution(), 5, BigDecimal.ROUND_HALF_UP).intValue() + "";
+
+    private PathInitDTO createInitDTO(RobotInfo robotInfo, GridPointDTO pointDTO, Long taskId, String robotId, Integer stepLen) {
         PathInitDTO pathInitDTO = new PathInitDTO();
-        pathInitDTO.setXMax(xMax);
-        pathInitDTO.setYMax(yMax);
-        pathInitDTO.setX(pointDTO.getX().intValue() + "");
-        pathInitDTO.setY(pointDTO.getY().intValue() + "");
+        pathInitDTO.setXNow(pointDTO.getX().intValue() + "");
+        pathInitDTO.setYNow(pointDTO.getY().intValue() + "");
+        pathInitDTO.setStepLen(stepLen);
         pathInitDTO.setTaskId(taskId);
         pathInitDTO.setRobotId(robotId);
         return pathInitDTO;
     }
 
-    private TaskInfo addTaskByStartTrack(RouteInfo routeInfo, RobotInfo robotInfo) {
+    private TaskInfo addTaskByStartTrack( RobotInfo robotInfo, StartTrackRequest request) {
         //No.2 新增任务信息
         TaskInfo taskInfo = new TaskInfo();
         String currDate = DateUtil.format(new Date(), "yyyyMMdd");
@@ -654,16 +770,20 @@
         taskInfo.setCreateTime(new Date());
         //任务类型 中子源
         taskInfo.setTaskType(1);
-        taskInfo.setDiscernType(1);
-        taskInfo.setTaskName(routeInfo.getRouteName() + "巡线任务");
+        taskInfo.setDiscernType(2);
+        taskInfo.setTaskName("自主巡检任务");
         //执行状态 未完成
         taskInfo.setTaskStatus(0);
         taskInfo.setStartTime(new Date());
-        taskInfo.setRouteId(routeInfo.getId());
+        taskInfo.setRouteId(null);
         taskInfo.setRobotId(robotInfo.getId());
         taskInfo.setGridHeight(robotInfo.getGridHeight());
         taskInfo.setGridWidth(robotInfo.getGridWidth());
         taskInfo.setResolution(robotInfo.getResolution());
+
+        taskInfo.setStepLen(request.getStepLen());
+        taskInfo.setMaxThreshold(request.getMaxThreshold());
+
         save(taskInfo);
         return taskInfo;
     }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/utils/AlgorithmUtils.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/utils/AlgorithmUtils.java
index 22864d1..467b9c2 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/utils/AlgorithmUtils.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/utils/AlgorithmUtils.java
@@ -1,6 +1,7 @@
 package com.casic.missiles.modular.robot.utils;
 
 import java.io.BufferedReader;
+import java.io.File;
 import java.io.IOException;
 import java.io.InputStreamReader;
 import java.util.ArrayList;
@@ -16,11 +17,13 @@
     public static List<String> invokeJob(List<String> params) {
         List<String> res = new ArrayList<>();
         try {
-
             BufferedReader br = null;
             BufferedReader brError;
             String line = null;
-            Process p = new ProcessBuilder(params).start();
+            ProcessBuilder processBuilder =  new ProcessBuilder(params);
+            processBuilder.directory(new File("D:\\workspace\\server\\keti\\casic-robot-inspection\\casic-web\\src\\main\\resources\\config\\path"));
+            Process p = processBuilder.start();
+
             br = new BufferedReader(new InputStreamReader(p.getInputStream()));
             brError = new BufferedReader(new InputStreamReader(p.getErrorStream()));
             while ((line = br.readLine()) != null || (line = brError.readLine()) != null) {
@@ -37,33 +40,29 @@
     public static void main(String[] args) {
         String path = "D:\\workspace\\server\\keti\\casic-robot-inspection\\casic-web\\src\\main\\resources\\config\\path\\";
         //算法初始化调用
-        String exePath = path + "PathPlanning_init.exe";
+        String exePath = path + "init.exe";
 
         ArrayList<String> invocation = new ArrayList<String>();
         invocation.add(exePath);
-        invocation.add("40");
-        invocation.add("20");
-        invocation.add("0.5");
+        invocation.add("10");
+        invocation.add("10");
         invocation.add("5");
-        invocation.add("5");
+        invocation.add("0.16");
+        invocation.add("Mission1");
         invokeJob(invocation);
 
         //{"count":"null","robotId":"1","taskId":"1748224487696142338","x":"8","xMax":"100","y":"2","yMax":"100"}
-        String planning = path + "PathPlanning.exe";
         //{"open":"0,0,0,0,0,0,0,0","robotId":"1","taskId":"1748250669749735425","x":"6","xMax":"100","y":"2","yMax":"100"}
         //{"open":"1,1,1,1,1,1,1,1","robotId":"1","taskId":"1748252894098178049","x":"6","xMax":"100","y":"2","yMax":"100"}
         //{"count":"6.0","open":"1,1,1,1,1,1,1,1","robotId":"1","taskId":"1750713404601159681","x":"6","xMax":"20","y":"2","yMax":"40"}
+        String planning = path + "search.exe";
         ArrayList<String> planInvocation = new ArrayList<String>();
         planInvocation.add(planning);
-        planInvocation.add("40");
-        planInvocation.add("40");
-        planInvocation.add("1,1,0,1,1,1,1,1");
-        planInvocation.add("6.0");
-        //next_X
-        planInvocation.add("6");
-        //next_Y
-        planInvocation.add("2");
-        ProcessBuilder processBuilder = new ProcessBuilder();
-        invokeJob(planInvocation);
+        planInvocation.add("10");
+        planInvocation.add("1.0");
+        planInvocation.add("0.2");
+        planInvocation.add("Mission1");
+        List<String> res = invokeJob(planInvocation);
+        System.out.println(res);
     }
 }
\ No newline at end of file
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/vo/TaskGridInfoVO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/vo/TaskGridInfoVO.java
new file mode 100644
index 0000000..0aff5bf
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/vo/TaskGridInfoVO.java
@@ -0,0 +1,58 @@
+package com.casic.missiles.modular.robot.vo;
+
+import io.swagger.annotations.ApiModelProperty;
+import lombok.Data;
+import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
+import com.casic.missiles.modular.robot.model.TaskGridInfo;
+import cn.hutool.core.util.StrUtil;
+
+import java.math.BigDecimal;
+
+/**
+ * 任务网格信息对象VO
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+@Data
+public class TaskGridInfoVO {
+    private static final long serialVersionUID = 1L;
+
+
+            @ApiModelProperty(value = "任务ID" , dataType = "Long")
+            private Long taskId;
+
+            @ApiModelProperty(value = "估算栅格x" , dataType = "Long")
+            private Long estimateGridX;
+
+            @ApiModelProperty(value = "估算栅格y" , dataType = "Long")
+            private Long estimateGridY;
+
+            @ApiModelProperty(value = "中子计数率" , dataType = "BigDecimal")
+            private BigDecimal neutronCount;
+
+            @ApiModelProperty(value = "当前栅格x" , dataType = "Long")
+            private Long curGridX;
+
+            @ApiModelProperty(value = "当前栅格y" , dataType = "Long")
+            private Long curGridY;
+
+public QueryWrapper<TaskGridInfo> genQuery() {
+QueryWrapper<TaskGridInfo> query = new QueryWrapper<>();
+if (taskId!=null) {
+        query.eq("task_id",taskId);
+}
+if (estimateGridX!=null) {
+        query.eq("estimate_grid_x",estimateGridX);
+}
+if (estimateGridY!=null) {
+        query.eq("estimate_grid_y",estimateGridY);
+}
+if (curGridX!=null) {
+        query.eq("cur_grid_x",curGridX);
+}
+if (curGridY!=null) {
+        query.eq("cur_grid_y",curGridY);
+}
+return query;
+}}
diff --git a/casic-web/src/main/resources/config/application.yml b/casic-web/src/main/resources/config/application.yml
index 04420fc..05c3633 100644
--- a/casic-web/src/main/resources/config/application.yml
+++ b/casic-web/src/main/resources/config/application.yml
@@ -42,11 +42,12 @@
   session-invalidate-time: 86400 #session失效时间(只在单机环境下生效，,多机环境在SpringSessionConfig类中配置) 单位：秒
   session-validation-interval: 900 #多久检测一次失效的session(只在单机环境下生效) 单位：秒
   no-login-urls: /user/login,/kaptcha,/config/baseConfig
-  hikSdk: D:\code\robot\casic-robot-inspection\casic-web\src\main\resources\config\hiklib\HCNetSDK.dll
-  hikPlay: D:\code\robot\casic-robot-inspection\casic-web\src\main\resources\config\hiklib\PlayCtrl.dll
+  hikSdk: D:\workspace\server\keti\casic-robot-inspection\casic-web\src\main\resources\config\hiklib\HCNetSDK.dll
+  hikPlay: D:\workspace\server\keti\casic-robot-inspection\casic-web\src\main\resources\config\hiklib\PlayCtrl.dll
   algorithm:
-    initPath: D:\workspace\server\keti\casic-robot-inspection\casic-web\src\main\resources\config\path\PathPlanning_init.exe
-    planPath: D:\workspace\server\keti\casic-robot-inspection\casic-web\src\main\resources\config\path\PathPlanning.exe
+    path:  D:\workspace\server\keti\casic-robot-inspection\casic-web\src\main\resources\config\path\
+    initPathName: init.exe
+    planPathName: search.exe
   doc:
     temp: D:\workspace\server\keti\casic-robot-inspection\casic-web\src\main\resources\config\path\report.docx
   config:
diff --git a/casic-web/src/main/resources/config/path/PathPlanning.exe b/casic-web/src/main/resources/config/path/PathPlanning.exe
deleted file mode 100644
index 6678e0f..0000000
--- a/casic-web/src/main/resources/config/path/PathPlanning.exe
+++ /dev/null
Binary files differ

diff --git a/.gitignore b/.gitignore
index 9e20bc1..f42a9cb 100644
--- a/.gitignore
+++ b/.gitignore
@@ -2,3 +2,4 @@
 **/target/**
 logs/
 *.iml
+*.exe
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java b/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
index c7b5d28..a849528 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
@@ -28,6 +28,7 @@
     String KEY_CUR_TASK_ID = "CUR_TASK_ID_";
     String KEY_CUR_TASK_INIT = "CUR_TASK_INIT_";
 
+    String KEY_COST_MAP = "KEY_COST_MAP";
     /**
      * PSD数据ID
      */
@@ -41,6 +42,7 @@
      */
     String KEY_CUR_XY_POINT = "KEY_CUR_XY_POINT_";
     String KEY_CUR_ESTIMATE_GRID = "KEY_CUR_ESTIMATE_GRID_";
+    String KEY_PLANING_FLAG = "KEY_PLANING_FLAG_";
     /**
      * 机器人巡点栅格
      */
@@ -55,6 +57,7 @@
      * 探测器配置参数缓存
      */
     String KEY_CONFIG_PARAMS_ID = "KEY_CONFIG_PARAMS_ID_";
+    String KEY_CONFIG_COSTMAP_RESOLUTION_SET = "KEY_CONFIG_COSTMAP_RESOLUTION_SET_";
 
     /**
      * 暂停状态缓存信息
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
index c4c905d..3bb475f 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
@@ -1,9 +1,14 @@
 package com.casic.missiles.modular.robot.controller;
 
+import cn.hutool.core.lang.Assert;
 import cn.hutool.core.util.StrUtil;
 import com.casic.missiles.core.base.controller.BaseController;
+import com.casic.missiles.core.cache.CacheUtil;
 import com.casic.missiles.core.util.SpringContextHolder;
+import com.casic.missiles.model.exception.RequestValidException;
+import com.casic.missiles.model.response.ResponseData;
 import com.casic.missiles.model.response.dto.ResponseDataDTO;
+import com.casic.missiles.modular.constants.RobotDictConstants;
 import com.casic.missiles.modular.robot.IBaseRobotService;
 import com.casic.missiles.modular.robot.dto.RobotInfoSetDTO;
 import com.casic.missiles.modular.robot.model.RobotInfo;
@@ -19,10 +24,16 @@
 import io.swagger.annotations.ApiOperation;
 import io.swagger.annotations.ApiParam;
 import lombok.SneakyThrows;
+import lombok.extern.slf4j.Slf4j;
+import org.apache.ibatis.annotations.Param;
+import org.springframework.beans.factory.annotation.Value;
 import org.springframework.web.bind.annotation.*;
+import org.springframework.web.multipart.MultipartFile;
 
 import javax.servlet.http.HttpServletResponse;
+import java.io.File;
 import java.util.List;
+import java.util.Optional;
 
 /**
  * 机器人操作类
@@ -30,6 +41,7 @@
  * @author lwh
  * @date 2023-10-30
  */
+@Slf4j
 @Api(tags = "机器人控制接口")
 @RestController
 @RequestMapping("/robot/opt")
@@ -61,6 +73,28 @@
         return baseRobotService.execCmdHandler(request, InstructCodeEnums.CHANGE_PCD);
     }
 
+    @Value("${casic.file.uploadPath}")
+    private String filePath;
+
+    @SneakyThrows
+    @ApiOperation(value = "PCD上传接口")
+    @PostMapping(value = "/uploadPcd")
+    public ResponseData uploadPcd(@RequestParam("file") MultipartFile file, @ApiParam("机器人ID,url拼接传参") String robotId) {
+        Assert.isFalse(StrUtil.isEmpty(robotId), () -> new RequestValidException("机器ID不能为空", Optional.of(new Throwable().getStackTrace()[2])));
+        RobotInfo info = robotInfoService.getById(robotId);
+        if (file != null && !file.isEmpty()) {
+            File pcdFile = new File(filePath + "finalCloud_" + info.getId() + ".pcd");
+            try {
+                file.transferTo(pcdFile);
+            } catch (Exception e) {
+                log.error(e.getMessage());
+            }
+            return ResponseData.success();
+        } else {
+            return ResponseData.error("附件上传失败!");
+        }
+    }
+
     @SneakyThrows
     @ApiOperation(value = "PCD下载接口")
     @PostMapping(value = "/pcdDownload")
@@ -136,6 +170,10 @@
     @ApiOperation(value = "启动或关闭循迹功能", notes = "返回 1成功 0失败  wait 启动时是否等待，0代表循迹启动即运动，不等待；1代表循迹启动原地等待。若无此字段，默认循迹启动不等待直接运动")
     @PostMapping(value = "/startTrack")
     public ResponseDataDTO<Integer> startTrack(@RequestBody StartTrackRequest request) {
+        request.setIsTask(1);
+        if (request.getStepLen() == null) {
+            request.setStepLen(2);
+        }
         baseRobotService.startTrack(request);
         return ResponseDataDTO.success();
     }
@@ -156,6 +194,13 @@
         MessageRequestDTO<CrlNavRequest> messageDTO = new MessageRequestDTO();
         messageDTO.setMsg(request);
         ResponseDataDTO<Integer> list = baseRobotService.getRobotRouteOptService().robotCmd(info, InstructCodeEnums.CTRL_NAV, messageDTO);
+        if (request.getAction() == 1) {
+            CostMapResolutionRequest costMap = new CostMapResolutionRequest();
+            costMap.setResolution(info.getResolution().doubleValue());
+            request.setRobotId(request.getRobotId());
+            //是否下发分辨率
+            CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CONFIG_COSTMAP_RESOLUTION_SET + request.getRobotId(), costMap);
+        }
         return list;
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java
new file mode 100644
index 0000000..5317651
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java
@@ -0,0 +1,86 @@
+package com.casic.missiles.modular.robot.controller;
+
+import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
+import com.baomidou.mybatisplus.extension.plugins.pagination.Page;
+import com.casic.missiles.core.base.controller.BaseController;
+import com.casic.missiles.core.page.PageFactory;
+import com.casic.missiles.core.page.PageInfoBT;
+import com.casic.missiles.model.form.IdForms;
+import com.casic.missiles.model.response.dto.ResponseDataDTO;
+import com.casic.missiles.modular.robot.model.TaskGridInfo;
+import com.casic.missiles.modular.robot.service.ITaskGridInfoService;
+import com.casic.missiles.modular.robot.vo.TaskGridInfoVO;
+import io.swagger.annotations.Api;
+import io.swagger.annotations.ApiOperation;
+import org.springframework.web.bind.annotation.*;
+
+import java.util.List;
+
+/**
+ * 任务网格信息Controller
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+@Api(tags = "任务网格信息")
+@RestController
+@RequestMapping("/robot/taskGridInfo")
+public class TaskGridInfoController extends BaseController {
+
+    private final ITaskGridInfoService taskGridInfoService;
+
+    public TaskGridInfoController(ITaskGridInfoService taskGridInfoService) {
+        this.taskGridInfoService = taskGridInfoService;
+    }
+
+    @ApiOperation(value = "列表查询")
+    @GetMapping(value = "/list")
+    public ResponseDataDTO<List<TaskGridInfo>> list(TaskGridInfoVO taskGridInfoVO) {
+        QueryWrapper<TaskGridInfo> query = taskGridInfoVO.genQuery();
+        return ResponseDataDTO.success(taskGridInfoService.list(query));
+    }
+
+    @ApiOperation(value = "分页查询")
+    @GetMapping(value = "/listPage")
+    public ResponseDataDTO<PageInfoBT<TaskGridInfo>> listPage(TaskGridInfoVO taskGridInfoVO) {
+        Page<TaskGridInfo> page = PageFactory.defaultPage();
+        QueryWrapper<TaskGridInfo> query = taskGridInfoVO.genQuery();
+        page = taskGridInfoService.page(page, query);
+        return ResponseDataDTO.success(super.packForBT(page));
+    }
+
+
+    @ApiOperation(value = "新增接口")
+    @PostMapping(value = "/add")
+    public ResponseDataDTO add(@RequestBody TaskGridInfo taskGridInfo) {
+        taskGridInfoService.save(taskGridInfo);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "修改接口")
+    @PostMapping(value = "/update")
+    public ResponseDataDTO update(@RequestBody TaskGridInfo taskGridInfo) {
+        taskGridInfoService.updateById(taskGridInfo);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "删除接口")
+    @GetMapping(value = "/delete")
+    public ResponseDataDTO delete(String id) {
+        taskGridInfoService.removeById(id);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation("批量删除")
+    @PostMapping("/batchDelete")
+    public ResponseDataDTO batchDelete(@RequestBody IdForms ids) {
+        taskGridInfoService.removeByIds(ids.getIds());
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "详情查询")
+    @GetMapping(value = "/detail")
+    public ResponseDataDTO<TaskGridInfo> detail(String id) {
+        return ResponseDataDTO.success(taskGridInfoService.getById(id));
+    }
+}
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
index e82a792..6992274 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
@@ -98,7 +98,7 @@
     public ResponseDataDTO<AlgorithmResponse> test() {
         AlgorithmResponse response = new AlgorithmResponse();
         response.setTaskHeatMap(new TaskHeatMap());
-        algorithmService.readAlgorithmResponse("step3.txt",response,123L,null);
+        algorithmService.readAlgorithmResponse("step3.txt",null,response,123L,null);
         sendWebSocket(InstructStatusMsgEnums.HEAT_MAP, "1", response.getTaskHeatMap());
         return ResponseDataDTO.success(response);
     }
@@ -121,7 +121,7 @@
     @GetMapping(value = "/testGrid")
     public ResponseDataDTO<List<RouteInfo>> testGrid(@RequestBody PathInitDTO pathInitDTO) {
         RobotInfo robotInfo = infoService.getById(pathInitDTO.getRobotId());
-        GridPointDTO gridPointDTO = algorithmService.convertSlamToGrid(new BigDecimal(pathInitDTO.getX()), new BigDecimal(10), new BigDecimal(4), robotInfo);
+        GridPointDTO gridPointDTO = algorithmService.convertSlamToGrid(new BigDecimal(pathInitDTO.getXNow()), new BigDecimal(10), new BigDecimal(4), robotInfo);
         return ResponseDataDTO.success(algorithmService.convertGridCenterPoint(gridPointDTO, new BigDecimal(10), robotInfo));
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/TaskGridInfoMapper.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/TaskGridInfoMapper.java
new file mode 100644
index 0000000..5c3a70b
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/TaskGridInfoMapper.java
@@ -0,0 +1,23 @@
+package com.casic.missiles.modular.robot.dao;
+
+
+import com.baomidou.mybatisplus.core.mapper.BaseMapper;
+import com.casic.missiles.modular.robot.model.TaskGridInfo;
+import org.apache.ibatis.annotations.Param;
+import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
+import com.baomidou.mybatisplus.extension.plugins.pagination.Page;
+import java.util.List;
+/**
+ * <p>
+ *  任务网格信息 Mapper 接口
+ * </p>
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+public interface TaskGridInfoMapper extends BaseMapper<TaskGridInfo> {
+/**
+* 任务网格信息 分页检索
+*/
+List<TaskGridInfo> selectTaskGridInfoPage( @Param("page") Page<TaskGridInfo> page,@Param("ew" ) QueryWrapper<TaskGridInfo> query);
+}
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/mapping/TaskGridInfoMapper.xml b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/mapping/TaskGridInfoMapper.xml
new file mode 100644
index 0000000..be573e0
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/mapping/TaskGridInfoMapper.xml
@@ -0,0 +1,80 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!DOCTYPE mapper PUBLIC "-//mybatis.org//DTD Mapper 3.0//EN" "http://mybatis.org/dtd/mybatis-3-mapper.dtd">
+<mapper namespace="com.casic.missiles.modular.robot.dao.TaskGridInfoMapper">
+    <!-- 通用查询映射结果 -->
+    <resultMap id="BaseResultMap" type="com.casic.missiles.modular.robot.model.TaskGridInfo">
+        <result column="id" property="id"/>
+        <result column="create_time" property="createTime"/>
+        <result column="update_time" property="updateTime"/>
+        <result column="task_id" property="taskId"/>
+        <result column="estimate_grid_x" property="estimateGridX"/>
+        <result column="estimate_grid_y" property="estimateGridY"/>
+        <result column="neutron_count" property="neutronCount"/>
+        <result column="cur_grid_x" property="curGridX"/>
+        <result column="cur_grid_y" property="curGridY"/>
+    </resultMap>
+    <!-- 通用查询结果列 -->
+    <sql id="Base_Column_List">
+        id
+        ,
+                    create_time as createTime ,
+                    update_time as updateTime ,
+                    task_id as taskId ,
+                    estimate_grid_x as estimateGridX ,
+                    estimate_grid_y as estimateGridY ,
+                    neutron_count as neutronCount ,
+                    cur_grid_x as curGridX ,
+                    cur_grid_y as curGridY
+    </sql>
+    <sql id="Base_Column_Alias">
+        a
+        .
+        id
+        ,
+            a.create_time
+            as createTime ,
+            a.update_time
+            as updateTime ,
+            a.task_id
+            as taskId ,
+            a.estimate_grid_x
+            as estimateGridX ,
+            a.estimate_grid_y
+            as estimateGridY ,
+            a.neutron_count
+            as neutronCount ,
+            a.cur_grid_x
+            as curGridX ,
+            a.cur_grid_y
+            as curGridY
+    </sql>
+    <select id="selectTaskGridInfoPage" resultType="com.casic.missiles.modular.robot.model.TaskGridInfo">
+        select
+        <include refid="Base_Column_Alias"/>
+        from task_grid_info a
+        <where>
+            <if test="ew != null">
+                ${ew.sqlSegment}
+            </if>
+        </where>
+    </select>
+    <sql id="Base_Column_Query">
+        <if test="request!=null">
+            <if test="request.taskId!= null and request.taskId!=''">
+                and task_id = #{request.taskId}
+            </if>
+            <if test="request.estimateGridX!= null and request.estimateGridX!=''">
+                and estimate_grid_x = #{request.estimateGridX}
+            </if>
+            <if test="request.estimateGridY!= null and request.estimateGridY!=''">
+                and estimate_grid_y = #{request.estimateGridY}
+            </if>
+            <if test="request.curGridX!= null and request.curGridX!=''">
+                and cur_grid_x = #{request.curGridX}
+            </if>
+            <if test="request.curGridY!= null and request.curGridY!=''">
+                and cur_grid_y = #{request.curGridY}
+            </if>
+        </if>
+    </sql>
+</mapper>
\ No newline at end of file
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java
index 749f559..4559074 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java
@@ -13,4 +13,9 @@
      * 规划点位信息
      */
     private PointNextDTO pointNextDTO;
+    /**
+     * 寻源目标地址
+     */
+    private PointNextDTO coordinate;
+    private boolean isEnd;
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java
index 39ff2a2..cef3238 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java
@@ -12,6 +12,9 @@
     private BigDecimal y;
 
     public static GridPointDTO convertGridPoint(PointNextDTO nextDTO) {
+        if(nextDTO==null){
+            return null;
+        }
         GridPointDTO gridPointDTO = new GridPointDTO();
         gridPointDTO.setX(Convert.toBigDecimal(nextDTO.getX()));
         gridPointDTO.setY(Convert.toBigDecimal(nextDTO.getY()));
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java
index 8c88242..33be25d 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java
@@ -12,25 +12,19 @@
 public class PathInitDTO {
     //---------------------算法参数
     /**
-     * x坐标最大值（x方向栅格总数）
+     * 初始位置x坐标(栅格地图下的坐标)
      */
-    private String xMax;
+    private String xNow;
     /**
-     * y坐标最大值（y方向栅格总数）
+     * 初始位置y坐标(栅格地图下的坐标)
      */
-    private String yMax;
+    private String yNow;
     /**
      * 初始位置计数率
      */
-    private String count;
-    /**
-     * 初始位置x坐标(0<=x<x_max)
-     */
-    private String x;
-    /**
-     * 初始位置y坐标(0<=y<y_max)
-     */
-    private String y;
+    private String zNow;
+
+    private Integer stepLen;
 
     //---------------------其他执行参数
     private String robotId;
@@ -38,11 +32,11 @@
 
     public List<String> toArray() {
         List<String> list = new ArrayList<>();
-        list.add(xMax);
-        list.add(yMax);
-        list.add(count);
-        list.add(x);
-        list.add(y);
+        list.add(xNow);
+        list.add(yNow);
+        list.add(stepLen+"");
+        list.add(zNow);
+        list.add(taskId+"");
         return list;
     }
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java
index 109e95c..ada6774 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java
@@ -10,21 +10,17 @@
  */
 @Data
 public class PathPlanDTO extends PathInitDTO {
-
     /**
-     * 长度为8的数组，记录(x-1, y-1), (x-1, y), (x-1, y+1), (x, y-1), (x, y+1), (x+1, y-1), (x+1, y), (x+1, y+1)8个坐标的可通行信息。
-     * 若某坐标有障碍物（不可通行行）则记为0，否则记为1
+     * 浮点阈值(阈值，超过该值后结束巡检开始寻源)
      */
-    private String open;
-
+    private String th;
     public List<String> toArray() {
         List<String> list = new ArrayList<>();
-        list.add(getXMax());
-        list.add(getYMax());
-        list.add(open);
-        list.add(getCount());
-        list.add(getX());
-        list.add(getY());
+
+        list.add(getStepLen()+"");
+        list.add(getTh());
+        list.add(getZNow());
+        list.add(getTaskId()+"");
         return list;
     }
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskGridInfo.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskGridInfo.java
new file mode 100644
index 0000000..101c10a
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskGridInfo.java
@@ -0,0 +1,83 @@
+package com.casic.missiles.modular.robot.model;
+
+import com.baomidou.mybatisplus.annotation.TableField;
+import com.baomidou.mybatisplus.annotation.TableName;
+import com.baomidou.mybatisplus.extension.activerecord.Model;
+import io.swagger.annotations.ApiModelProperty;
+import lombok.Data;
+
+import java.math.BigDecimal;
+import java.util.Date;
+
+/**
+ * 任务网格信息对象 task_grid_info
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+@Data
+@TableName("task_grid_info")
+public class TaskGridInfo extends Model<TaskGridInfo> {
+    private static final long serialVersionUID = 1L;
+    @TableField(exist = false)
+    private Integer index;
+    @ApiModelProperty(value = "主键", dataType = "Long")
+    @TableField("id")
+    private Long id;
+
+    @ApiModelProperty(value = "创建时间", dataType = "Date")
+    @TableField("create_time")
+    private Date createTime;
+
+    @ApiModelProperty(value = "修改时间", dataType = "Date")
+    @TableField("update_time")
+    private Date updateTime;
+
+    @ApiModelProperty(value = "任务ID", dataType = "Long")
+    @TableField("task_id")
+    private Long taskId;
+
+    @ApiModelProperty(value = "估算栅格x", dataType = "Long")
+    @TableField("estimate_grid_x")
+    private String estimateGridX;
+
+    @ApiModelProperty(value = "估算栅格y", dataType = "Long")
+    @TableField("estimate_grid_y")
+    private String estimateGridY;
+
+    @ApiModelProperty(value = "中子计数率", dataType = "BigDecimal")
+    @TableField("neutron_count")
+    private BigDecimal neutronCount;
+
+    @ApiModelProperty(value = "当前栅格x", dataType = "Long")
+    @TableField("cur_grid_x")
+    private Integer curGridX;
+
+    @ApiModelProperty(value = "当前栅格y", dataType = "Long")
+    @TableField("cur_grid_y")
+    private Integer curGridY;
+
+    public String getCurPoint(){
+        return "("+curGridX+","+curGridY+")";
+    }
+    public String getEstimatePoint(){
+        if(estimateGridX==null&estimateGridY==null){
+            return "结束";
+        }
+        return "("+estimateGridX+","+estimateGridY+")";
+    }
+    @Override
+    public String toString() {
+        return "TaskGridInfo{"
+                + "id=" + id
+                + "createTime=" + createTime
+                + "updateTime=" + updateTime
+                + "taskId=" + taskId
+                + "estimateGridX=" + estimateGridX
+                + "estimateGridY=" + estimateGridY
+                + "neutronCount=" + neutronCount
+                + "curGridX=" + curGridX
+                + "curGridY=" + curGridY
+                + "}";
+    }
+}
\ No newline at end of file
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java
index edc104e..a8acb2a 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java
@@ -53,6 +53,13 @@
     @TableField("route_id")
     private Long routeId;
 
+    @ApiModelProperty(value = "寻源初始步长", dataType = "Integer")
+    @TableField("step_len")
+    private Integer stepLen;
+
+    @ApiModelProperty(value = "寻源阈值设定", dataType = "BigDecimal")
+    @TableField("max_threshold")
+    private BigDecimal maxThreshold;
     /**
      * 任务类型(1巡检任务)
      */
@@ -62,7 +69,6 @@
     @ApiModelProperty(value = "任务类型字典名称", dataType = "String")
     @TableField(exist = false)
     private String taskTypeName;
-
     @ApiModelProperty(value = "识别类型(discernType 1中子源识别)", dataType = "Integer")
     @TableField("discern_type")
     private Integer discernType;
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java
index c4bc25d..c6620f3 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java
@@ -53,7 +53,7 @@
     }),
     SET_SPEED("/set_speed" , "/set_speed_response" , "最大运动速度设置" , true, new TypeReference<RobotMsgResponseDTO<Integer>>() {
     }),
-    SET_TARGETPOINT("/set_targetpoint" , "/set_targetpoint_response" , "设置导航目标点" , false, new TypeReference<RobotMsgResponseDTO<Integer>>() {
+    SET_TARGETPOINT("/set_targetpoint" , "/set_targetpoint_response" , "设置导航目标点" , true, new TypeReference<RobotMsgResponseDTO<Integer>>() {
     }),
 
     GET_DATA_LIST("/get_data_list" , "/get_data_list_response" , "获取数据包列表" , true, new TypeReference<RobotMsgResponseDTO<List<String>>>() {
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/AbstractSocketHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/AbstractSocketHandler.java
index 83f4cb2..e47e343 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/AbstractSocketHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/AbstractSocketHandler.java
@@ -49,7 +49,7 @@
                 wsMsgBaseDTO.setMsgKey(enums.getMsgKey());
                 wsMsgBaseDTO.setData(jsonObject.get("msg"));
                 String msg = JSON.toJSONString(wsMsgBaseDTO);
-                log.info("推送{}消息：{}", enums.getRemarks(), msg);
+//                log.info("推送{}消息：{}", enums.getRemarks(), msg);
                 webSocket.sendOneMessage(robotId, msg);
             }
         } catch (Exception e) {
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/cmd/TargetPointCmdHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/cmd/TargetPointCmdHandler.java
index c1c09fa..43b4460 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/cmd/TargetPointCmdHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/cmd/TargetPointCmdHandler.java
@@ -34,7 +34,11 @@
     @Override
     public ResponseDataDTO senderCmd(TargetPointRequest request, InstructCodeEnums enums) {
         MessageRequestDTO<TargetPointRequest> messageDTO = new MessageRequestDTO();
-        //读取默认关键点信息
+        //读取默认关键点信息 {"x":"1.715708819927424","y":"-0.9100042544250901","z":"0.017993879527185876","theta":"-0.039278239414312105"}
+//        request.setX("1.715708819927424");
+//        request.setY("-0.9100042544250901");
+//        request.setZ("0.017993879527185876");
+//        request.setTheta("-0.039278239414312105");
         messageDTO.setMsg(request);
         ResponseDataDTO<Integer> list = optService.robotCmd(getRobotInfo(), enums, messageDTO);
         return list;
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/AppCostMapHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/AppCostMapHandler.java
index 0335693..88c9f79 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/AppCostMapHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/AppCostMapHandler.java
@@ -1,6 +1,11 @@
 package com.casic.missiles.modular.robot.opt.handler.msg;
 
+import com.casic.missiles.core.cache.CacheUtil;
+import com.casic.missiles.modular.constants.RobotDictConstants;
+import com.casic.missiles.modular.robot.IBaseRobotService;
+import com.casic.missiles.modular.robot.opt.enums.InstructCodeEnums;
 import com.casic.missiles.modular.robot.opt.handler.AbstractSocketHandler;
+import com.casic.missiles.modular.robot.opt.instruct.dto.CostMapResolutionRequest;
 import com.casic.missiles.modular.robot.service.IRobotStatusInfoService;
 import lombok.extern.slf4j.Slf4j;
 import org.eclipse.paho.client.mqttv3.MqttMessage;
@@ -16,14 +21,27 @@
 @Component("/app_costmap_sender")
 public class AppCostMapHandler extends AbstractSocketHandler {
     private final IRobotStatusInfoService statusInfoService;
+    private final IBaseRobotService baseRobotService;
     private static Map<String, Integer> index = new ConcurrentHashMap<>();
 
-    public AppCostMapHandler(IRobotStatusInfoService statusInfoService) {
+    public AppCostMapHandler(IRobotStatusInfoService statusInfoService, IBaseRobotService baseRobotService) {
         this.statusInfoService = statusInfoService;
+        this.baseRobotService = baseRobotService;
     }
 
     @Override
     public void process(String robotId, String topic, MqttMessage message) {
         statusInfoService.saveCostMap(robotId, message);
+
+        CostMapResolutionRequest costMapSet = CacheUtil.get(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CONFIG_COSTMAP_RESOLUTION_SET +robotId);
+        if(costMapSet != null){
+            CacheUtil.remove(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CONFIG_COSTMAP_RESOLUTION_SET +robotId);
+            new Thread(() -> {
+                costMapSet.setRobotId(Long.valueOf(robotId));
+                baseRobotService.execCmdHandler(costMapSet, InstructCodeEnums.COSTMAP_RESOLUTION);
+            }).start();
+
+        }
+
     }
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/PoseMessageMsgHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/PoseMessageMsgHandler.java
index 29f8919..6c3c3bd 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/PoseMessageMsgHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/PoseMessageMsgHandler.java
@@ -17,11 +17,9 @@
 @Component("/pose_message")
 public class PoseMessageMsgHandler extends AbstractSocketHandler {
 
-
     private final EventPublisher publisher;
 
     public PoseMessageMsgHandler(EventPublisher publisher) {
-
         this.publisher = publisher;
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/StopStateMsgHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/StopStateMsgHandler.java
index 8950815..8a71e0b 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/StopStateMsgHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/StopStateMsgHandler.java
@@ -30,7 +30,7 @@
     public void process(String robotId, String topic, MqttMessage message) {
 
         index.put(index_key, index.get("index") + 1);
-        if (index.get(index_key)>50) {
+        if (index.get(index_key)>30) {
             index.put(index_key, 0);
             log.info("主键：{},停障状态topic:{},消息内容:{}", robotId, topic, message.toString());
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/impl/RobotOptServiceImpl.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/impl/RobotOptServiceImpl.java
index 8471e7a..7940a24 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/impl/RobotOptServiceImpl.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/impl/RobotOptServiceImpl.java
@@ -146,7 +146,7 @@
     public void startTrack(StartTrackRequest request) {
         //增加循迹任务记录
         taskInfoService.reloadTask(request, robotInfoService.getById(request.getRobotId()));
-        this.execCmdHandler(request, InstructCodeEnums.START_TRACK);
+//        this.execCmdHandler(request, InstructCodeEnums.START_TRACK);
     }
 
     public IRobotRouteOptService getRobotRouteOptService() {
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/BaseRobotCmdDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/BaseRobotCmdDTO.java
index 5608f2b..06b53de 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/BaseRobotCmdDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/BaseRobotCmdDTO.java
@@ -3,11 +3,13 @@
 import io.swagger.annotations.ApiModelProperty;
 import lombok.Data;
 
+import java.io.Serializable;
+
 /**
  * 机器人指令基类
  */
 @Data
-public class BaseRobotCmdDTO {
+public class BaseRobotCmdDTO implements Serializable {
     @ApiModelProperty(value = "机器人主键", dataType = "String")
     protected Long robotId;
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/StartTrackRequest.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/StartTrackRequest.java
index a18321b..85546a7 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/StartTrackRequest.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/StartTrackRequest.java
@@ -3,6 +3,8 @@
 import io.swagger.annotations.ApiModelProperty;
 import lombok.Data;
 
+import java.math.BigDecimal;
+
 /**
  * 导航下发指令
  */
@@ -10,18 +12,19 @@
 public class StartTrackRequest extends BaseRobotCmdDTO {
     @ApiModelProperty(value = "sn编码(可不填 默认机器ID)", dataType = "String")
     private String sn;
-
     @ApiModelProperty(value = "下发状态 1:开启 0:关闭", dataType = "String")
     private Integer action;
     @ApiModelProperty(value = "等待状态 1:等待 0:不等待", dataType = "String")
     private Integer wait;
     @ApiModelProperty(value = "等待状态 0:循迹 1:任务", dataType = "isTask")
     private Integer isTask;
-    @ApiModelProperty(value = "轨迹名称", dataType = "String")
+    @ApiModelProperty(value = "轨迹名称(废弃)", dataType = "String")
     private String track_name;
     @ApiModelProperty(value = "关键点名称", dataType = "String", hidden = true)
     private String taskpoint_name;
-
+    @ApiModelProperty(value = "初始寻源步长", dataType = "Integer")
+    private Integer stepLen;
+    @ApiModelProperty(value = "寻源阈值", dataType = "BigDecimal")
+    private BigDecimal maxThreshold;
     private String obs_mode="1";
-
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/response/StopStateResponseDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/response/StopStateResponseDTO.java
index eb514bf..e0679d7 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/response/StopStateResponseDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/response/StopStateResponseDTO.java
@@ -3,11 +3,13 @@
 import io.swagger.annotations.ApiModelProperty;
 import lombok.Data;
 
+import java.io.Serializable;
+
 /**
  * 电压信息返回
  */
 @Data
-public class StopStateResponseDTO {
+public class StopStateResponseDTO implements Serializable {
     @ApiModelProperty(value = "软件急停(0：无触发 1：触发)" , dataType = "Integer")
     private Integer soft;
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IAlgorithmService.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IAlgorithmService.java
index 56b3e1d..f62523b 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IAlgorithmService.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IAlgorithmService.java
@@ -6,6 +6,7 @@
 import com.casic.missiles.modular.robot.dto.PathPlanDTO;
 import com.casic.missiles.modular.robot.model.RobotInfo;
 
+import java.io.File;
 import java.math.BigDecimal;
 import java.util.List;
 
@@ -38,5 +39,5 @@
      * @return
      */
     GridPointDTO convertGridCenterPoint(GridPointDTO pointDTO, BigDecimal resolution, RobotInfo robotInfo);
-    void readAlgorithmResponse(String fileName, AlgorithmResponse response, Long taskId, List<String> res);
+    void readAlgorithmResponse(String fileName, File repetitionFile, AlgorithmResponse response, Long taskId, List<String> res);
 }
\ No newline at end of file
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IRobotStatusInfoService.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IRobotStatusInfoService.java
index 2af210a..757f8d0 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IRobotStatusInfoService.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IRobotStatusInfoService.java
@@ -67,10 +67,9 @@
     /**
      * 更新车辆路线及任务信息
      *
-     * @param currRouteId 当前路线信息
      * @param currTaskId  当前任务信息
      */
-    void updateCurrTask(String robotId, Long currRouteId, Long currTaskId);
+    void updateCurrTask(String robotId, Long currTaskId);
 
     RobotStatusInfo getStatusByRobotId(String robotId);
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/ITaskGridInfoService.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/ITaskGridInfoService.java
new file mode 100644
index 0000000..fc0da93
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/ITaskGridInfoService.java
@@ -0,0 +1,22 @@
+package com.casic.missiles.modular.robot.service;
+
+import com.baomidou.mybatisplus.extension.service.IService;
+import  com.casic.missiles.modular.robot.model.TaskGridInfo;
+import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
+import com.baomidou.mybatisplus.extension.plugins.pagination.Page;
+import java.util.List;
+
+
+/**
+ * <p>
+ * 任务网格信息 服务类
+ * </p>
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+public interface ITaskGridInfoService extends IService<TaskGridInfo> {
+/**
+* 任务网格信息 分页检索
+*/
+List<TaskGridInfo> selectTaskGridInfoPage(Page<TaskGridInfo> page,QueryWrapper<TaskGridInfo> query);}
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/ITaskInfoService.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/ITaskInfoService.java
index 31d7ba6..b395993 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/ITaskInfoService.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/ITaskInfoService.java
@@ -51,6 +51,13 @@
      * @param dto 实体
      */
     void pointCallBack(PoseMessageResponseDTO dto, String robotId);
+
+    /**
+     * 车辆路线规划
+     * @param dto
+     * @param robotId
+     */
+    void carPathPlanning(PoseMessageResponseDTO dto, String robotId);
     void pointTestCallBack(PoseMessageResponseDTO dto, String robotId);
     /**
      * 图片路径保存
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/AlgorithmServiceImpl.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/AlgorithmServiceImpl.java
index f808fa0..a4dbe0b 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/AlgorithmServiceImpl.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/AlgorithmServiceImpl.java
@@ -29,10 +29,12 @@
 @Slf4j
 @Service
 public class AlgorithmServiceImpl implements IAlgorithmService {
-    @Value("${casic.algorithm.initPath:-1}")
-    private String initPath;
-    @Value("${casic.algorithm.planPath:-1}")
-    private String planPath;
+    @Value("${casic.algorithm.path:-1}")
+    private String path;
+    @Value("${casic.algorithm.initPathName:-1}")
+    private String initPathName;
+    @Value("${casic.algorithm.planPathName:-1}")
+    private String planPathName;
     private final ITaskHeatMapService heatMapService;
 
     public AlgorithmServiceImpl(ITaskHeatMapService heatMapService) {
@@ -42,13 +44,13 @@
     @Transactional
     @Override
     public AlgorithmResponse pathPlanningInit(PathInitDTO initDTO) {
-        if (StrUtil.isEmpty(initDTO.getCount())) {
-            initDTO.setCount("0.00");
+        if (StrUtil.isEmpty(initDTO.getZNow())) {
+            initDTO.setZNow("0.00");
         }
         log.info(JSON.toJSONString(initDTO));
         //算法路径及入参配置
         List<String> commends = new ArrayList<>();
-        commends.add(initPath);
+        commends.add(path + initPathName);
         commends.addAll(initDTO.toArray());
         List<String> res = AlgorithmUtils.invokeJob(commends);
 
@@ -63,38 +65,72 @@
     @Transactional
     @Override
     public AlgorithmResponse pathPlanning(PathPlanDTO planDTO) {
-        if (StrUtil.isEmpty(planDTO.getCount())) {
-            planDTO.setCount("0.001");
+        if (StrUtil.isEmpty(planDTO.getZNow())) {
+            planDTO.setZNow("0.001");
         }
         //算法路径及入参配置
         List<String> commends = new ArrayList<>();
-        commends.add(planPath);
+        commends.add(path + planPathName);
         commends.addAll(planDTO.toArray());
         log.info("寻源算法调用:{}", JSON.toJSONString(planDTO));
         List<String> res = AlgorithmUtils.invokeJob(commends);
         AlgorithmResponse response = getAlgorithmResponse(planDTO, res);
-        //next 点位编写
-        if (CollectionUtil.isNotEmpty(res) && res.size() == 2) {
-            response.getPointNextDTO().setX(formatBigDecimal(res.get(0).replaceAll("next_X: ", "")));
-            response.getPointNextDTO().setY(formatBigDecimal(res.get(1).replaceAll("next_Y: ", "")));
-        }
+
         return response;
     }
 
     private AlgorithmResponse getAlgorithmResponse(PathInitDTO initDTO, List<String> res) {
-        File repetitionFile = new File("repetition.txt");
+        //读取当前路径图片
+        File repetitionFileDir = new File(path + "\\" + initDTO.getTaskId() + "\\predicts");
+        File[] files = repetitionFileDir.listFiles((dir, name) -> name.endsWith(".txt") && name.startsWith("step"));
+        File repetitionFile = null;
+        //判定当前执行路径最大step2文件夹
+        if (files != null && files.length > 0) {
+             repetitionFile = files[files.length - 1];
+        }
         AlgorithmResponse response = new AlgorithmResponse();
         if (initDTO.getTaskId() != null && repetitionFile.exists()) {
-            readAlgorithmResponse("repetition.txt", response, initDTO.getTaskId(), res);
-            readAlgorithmResponse("step.txt", response, initDTO.getTaskId(), res);
+            //读取热力图
+            readAlgorithmResponse("repetition.txt",repetitionFile, response, initDTO.getTaskId(), res);
         } else {
             log.info("taskId is empty!");
         }
-        readAlgorithmResponse("xy_train.txt", response, initDTO.getTaskId(), res);
-        readAlgorithmResponse("z_train.txt", response, initDTO.getTaskId(), res);
+        //读取规划路径
+        if (CollectionUtil.isNotEmpty(res)) {
+            response.setEnd(false);
+            readAlgorithmResponse(response, res);
+        }
         return response;
     }
 
+    private void readAlgorithmResponse(AlgorithmResponse response, List<String> res) {
+        List<String> resList = new ArrayList<>();
+        if (res.size() >= 2) {
+            resList = res.subList(res.size() - 2, res.size());
+        }
+        //判定是否结束寻源
+        if (res.contains("end")) {
+            response.setEnd(true);
+            for (String s : resList) {
+                if (s.startsWith("source_coordinate:")) {
+                    s = s.trim();
+                    log.info("寻源结束,目标点位：{}",s);
+                    List<String> lists = StrUtil.split(s.replaceAll("source_coordinate:", ""), ",");
+                    PointNextDTO nextDTO = new PointNextDTO();
+                    nextDTO.setX(lists.get(0));
+                    nextDTO.setY(lists.get(1));
+                    response.setCoordinate(nextDTO);
+                }
+            }
+        } else {
+            PointNextDTO nextDTO = new PointNextDTO();
+            nextDTO.setX(resList.get(0));
+            nextDTO.setY(resList.get(1));
+            response.setPointNextDTO(nextDTO);
+        }
+
+    }
+
     /**
      * 读取算法输出数据
      *
@@ -102,49 +138,18 @@
      * @param response 结果集
      * @param taskId   任务ID
      */
-    public void readAlgorithmResponse(String fileName, AlgorithmResponse response, Long taskId, List<String> res) {
-        File file = new File(fileName);
-        if (!file.exists()&&!fileName.startsWith("step")) {
+    public void readAlgorithmResponse(String fileName,File repetitionFile, AlgorithmResponse response, Long taskId, List<String> res) {
+        if (!repetitionFile.exists() && !fileName.startsWith("step")) {
             log.info("{} file is empty!", fileName);
             return;
         }
         if (response.getPointNextDTO() == null) {
             response.setPointNextDTO(new PointNextDTO());
         }
-        switch (fileName) {
-            case "repetition.txt":
-                String msg = FileUtil.readString(file, CharsetUtil.CHARSET_UTF_8);
-                TaskHeatMap taskHeatMap = createEmptyTaskHeatMap(taskId);
-                taskHeatMap.setRepetition(msg);
-                response.setTaskHeatMap(taskHeatMap);
-                break;
-            case "xy_train.txt":
-                List<String> xyTrains = FileUtil.readLines(file, CharsetUtil.CHARSET_UTF_8);
-                if (CollectionUtil.isNotEmpty(xyTrains)) {
-                    if (CollectionUtil.isNotEmpty(xyTrains)) {
-                        List<String> list = StrUtil.split(xyTrains.get(xyTrains.size() - 1), " ");
-                        response.getPointNextDTO().setX(formatBigDecimal(list.get(0)));
-                        response.getPointNextDTO().setY(formatBigDecimal(list.get(1)));
-                    }
-                }
-                break;
-            case "z_train.txt":
-                List<String> zTrains = FileUtil.readLines(file, CharsetUtil.CHARSET_UTF_8);
-                if (CollectionUtil.isNotEmpty(zTrains)) {
-                    String zTrain = zTrains.get(zTrains.size() - 1);
-                    response.getPointNextDTO().setZ(formatBigDecimal(zTrain));
-                }
-            default:
-                if (StrUtil.isNotEmpty(fileName) && fileName.startsWith("step")) {
-                    File fileDir = new File(".");
-                    File[] files = fileDir.listFiles((dir, name) -> name.endsWith(".txt") && name.startsWith("step"));
-                    if (files != null && files.length > 0) {
-                        String steps = FileUtil.readString(files[files.length - 1], CharsetUtil.CHARSET_UTF_8);
-                        response.getTaskHeatMap().setRepetition(steps);
-                    }
-                }
-                break;
-        }
+        String msg = FileUtil.readString(repetitionFile, CharsetUtil.CHARSET_UTF_8);
+        TaskHeatMap taskHeatMap = createEmptyTaskHeatMap(taskId);
+        taskHeatMap.setRepetition(msg);
+        response.setTaskHeatMap(taskHeatMap);
     }
 
     /**
@@ -157,13 +162,11 @@
     public GridPointDTO convertSlamToGrid(BigDecimal x, BigDecimal y, BigDecimal resolution, RobotInfo robotInfo) {
         GridPointDTO pointDTO = new GridPointDTO();
         BigDecimal scale = resolution;
-
         //减去栅格原点坐标
         x = x.subtract(robotInfo.getOriginX());
         y = y.subtract(robotInfo.getOriginY());
-
-        pointDTO.setX(new BigDecimal(x.divide(scale,3,BigDecimal.ROUND_HALF_UP).intValue()+1));
-        pointDTO.setY(new BigDecimal(y.divide(scale,3,BigDecimal.ROUND_HALF_UP).intValue()+1));
+        pointDTO.setX(new BigDecimal(x.divide(scale, 3, BigDecimal.ROUND_HALF_UP).intValue()+1));
+        pointDTO.setY(new BigDecimal(y.divide(scale, 3, BigDecimal.ROUND_HALF_UP).intValue() +1));
         return pointDTO;
     }
 
@@ -174,19 +177,17 @@
      * @return
      */
     public GridPointDTO convertGridCenterPoint(GridPointDTO pointDTO, BigDecimal resolution, RobotInfo robotInfo) {
-
         GridPointDTO centerPoint = new GridPointDTO();
         BigDecimal scale = resolution;
-        //点位转换
-        BigDecimal slamX = pointDTO.getX().divide(scale, 5, BigDecimal.ROUND_HALF_UP);
-        BigDecimal slamY = pointDTO.getY().divide(scale, 5, BigDecimal.ROUND_HALF_UP);
+        //点位转换 转换后栅格－1 从1开始计算
+        BigDecimal slamX = (pointDTO.getX().subtract(new BigDecimal(1))).multiply(scale);
+        BigDecimal slamY = (pointDTO.getY().subtract(new BigDecimal(1))).multiply(scale);
         //中心栅格点位中心添加
-        centerPoint.setX(slamX.add(scale.divide(new BigDecimal(2), 5, BigDecimal.ROUND_HALF_UP)));
-        centerPoint.setY(slamY.add(scale.divide(new BigDecimal(2), 5, BigDecimal.ROUND_HALF_UP)));
+        centerPoint.setX(slamX.add(scale.divide(new BigDecimal(2), 10, BigDecimal.ROUND_HALF_UP)));
+        centerPoint.setY(slamY.add(scale.divide(new BigDecimal(2), 10, BigDecimal.ROUND_HALF_UP)));
         //加上栅格原点坐标
         centerPoint.setX(centerPoint.getX().add(robotInfo.getOriginX()));
         centerPoint.setY(centerPoint.getY().add(robotInfo.getOriginY()));
-
         return centerPoint;
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotInfoServiceImpl.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotInfoServiceImpl.java
index 043e05d..fdded64 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotInfoServiceImpl.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotInfoServiceImpl.java
@@ -104,7 +104,7 @@
             //下发障碍物栅格
             sendCostMapResolution(robotInfo.getResolution(), robotInfo.getId());
             //重新生成栅格地图
-            sendChangePcd(robotInfo.getId());
+            //sendChangePcd(robotInfo.getId());
         }
         update.eq(RobotInfo::getId, robotInfo.getId());
         update(update);
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotStatusInfoServiceImpl.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotStatusInfoServiceImpl.java
index 6d3ae91..46b5c3b 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotStatusInfoServiceImpl.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotStatusInfoServiceImpl.java
@@ -15,6 +15,7 @@
 import com.casic.missiles.modular.robot.dao.RobotStatusInfoMapper;
 import com.casic.missiles.modular.robot.model.RobotInfo;
 import com.casic.missiles.modular.robot.model.RobotStatusInfo;
+import com.casic.missiles.modular.robot.opt.enums.InstructCodeEnums;
 import com.casic.missiles.modular.robot.opt.instruct.base.dto.RobotMsgDTO;
 import com.casic.missiles.modular.robot.opt.instruct.base.dto.RobotMsgResponseDTO;
 import com.casic.missiles.modular.robot.opt.instruct.base.dto.RobotResponseDTO;
@@ -26,13 +27,18 @@
 import com.casic.missiles.modular.robot.service.IRobotInfoService;
 import com.casic.missiles.modular.robot.service.IRobotStatusInfoService;
 import lombok.extern.slf4j.Slf4j;
+import org.aspectj.util.FileUtil;
 import org.eclipse.paho.client.mqttv3.MqttMessage;
+import org.springframework.beans.factory.annotation.Value;
 import org.springframework.context.annotation.Lazy;
 import org.springframework.stereotype.Service;
 import org.springframework.transaction.annotation.Transactional;
 
+import java.io.File;
 import java.io.Serializable;
 import java.math.BigDecimal;
+import java.util.ArrayList;
+import java.util.Arrays;
 import java.util.List;
 
 
@@ -147,7 +153,6 @@
     @Override
     public void saveProcessMessage(String robotId, MqttMessage message) {
 //        String json = new String(message.getPayload());
-
         RobotResponseDTO<ProcessMessageResponseDTO> responseDTO = parseJson(message, new TypeReference<RobotResponseDTO<ProcessMessageResponseDTO>>() {
         });
         if (responseDTO.getMsg() != null) {
@@ -189,9 +194,8 @@
     }
 
     @Override
-    public void updateCurrTask(String robotId, Long currRouteId, Long currTaskId) {
+    public void updateCurrTask(String robotId, Long currTaskId) {
         UpdateWrapper<RobotStatusInfo> query = new UpdateWrapper<>();
-        query.set("curr_route_id", currRouteId);
         query.set("curr_task_id", currTaskId);
         query.eq("robot_id", robotId);
         update(query);
@@ -250,6 +254,15 @@
         }
     }
 
+    @Value("${casic.algorithm.path:-1}")
+    private String path;
+
+    /**
+     * 栅格阈值
+     */
+    @Value("${casic.algorithm.num:30}")
+    private Integer num;
+
     /**
      * 机器人栅格数据
      *
@@ -262,15 +275,41 @@
         RobotResponseDTO<AppCostmapResponseDTO> responseDTO = parseJson(message, new TypeReference<RobotResponseDTO<AppCostmapResponseDTO>>() {
         });
         if (responseDTO != null && responseDTO.getMsg() != null) {
+            //发送栅格分辨率设定
+
             cacheService.updateCostMap(robotId, responseDTO.getMsg());
             robotInfoService.updateRobotGrid(robotId, responseDTO.getMsg());
-//            log.info("--------------- json data:{}", JSON.toJSONString(responseDTO.getMsg()));
+            //log.info("--------------- json data:{}", JSON.toJSONString(responseDTO.getMsg()));
+            if (responseDTO != null && responseDTO.getMsg().getData() != null) {
+                ArrayList<Integer> dataNums = new ArrayList<>();
+                //根据阈值转换栅格
+                for (Integer dataNum : responseDTO.getMsg().getData()) {
+                    if (dataNum != -1) {
+                        if (dataNum >= num) {
+                            dataNums.add(0);
+                        } else {
+                            dataNums.add(1);
+                        }
+                    }
+                }
+                writeCache(dataNums,responseDTO.getMsg().getWidth(),responseDTO.getMsg().getHeight(),robotId);
+            }
         }
     }
 
+    private void writeCache(ArrayList<Integer> integers,Integer width,Integer height,String robotId) {
+        List<List<Integer>> arrays = CollectionUtil.split(integers,width);
+        ArrayList<ArrayList<Integer>> arrayList = new ArrayList<>();
+        for (List<Integer> array : arrays) {
+            arrayList.add((ArrayList)array);
+        }
+        CollectionUtil.reverse(arrayList);
+        CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_COST_MAP + robotId, arrayList);
+//        FileUtil.writeAsString(new File(path + "map.txt"), CollectionUtil.join(integers, " ").replaceAll("\\[", "").replaceAll("]", ""));
+    }
+
     @Override
     public void updateRobotPoint(PoseMessageResponseDTO dto, String robotId) {
-        //todo cache
         UpdateWrapper<RobotStatusInfo> query = new UpdateWrapper<>();
         query.set("postion_x", dto.getX());
         query.set("postion_y", dto.getY());
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotTrajectoryInfoServiceImpl.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotTrajectoryInfoServiceImpl.java
index 6c9625f..f1262a2 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotTrajectoryInfoServiceImpl.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotTrajectoryInfoServiceImpl.java
@@ -57,29 +57,29 @@
         });
 
         //读取导航状态,如果导航未开启则不存储
-//        RobotStatusInfo statusInfo = robotStatusInfoService.getStatusByRobotId(robotId);
-        Long routeId= cacheService.readRouteId(Long.valueOf(robotId));
-        if (routeId != null && responseDTO.getMsg() != null) {
+        //RobotStatusInfo statusInfo = robotStatusInfoService.getStatusByRobotId(robotId);
+
+        if ( responseDTO.getMsg() != null) {
             PoseMessageResponseDTO dto = responseDTO.getMsg();
             //执行循迹 保存当前轨迹信息
             //更新实时定位信息
             CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.ROBOT_POINT + robotId, dto);
-            infoService.pointTestCallBack(dto, robotId);
-            if (routeId != null) {
-                RobotTrajectoryInfo info = new RobotTrajectoryInfo();
-                info.setRobotId(Long.valueOf(robotId));
-                info.setCreateTime(new Date());
-                info.setPostionX(dto.getX());
-                info.setPostionY(dto.getY());
-                info.setPostionZ(dto.getZ());
-                info.setTheta(dto.getTheta());
-                save(info);
-                try {
-                    infoService.pointCallBack(dto, robotId);
-                } catch (Exception e) {
-                    log.error("task error", e);
-                }
+            try {
+                infoService.carPathPlanning(dto, robotId);
+            } catch (Exception e) {
+                log.error("task error", e);
             }
+//            if (routeId != null) {
+//                RobotTrajectoryInfo info = new RobotTrajectoryInfo();
+//                info.setRobotId(Long.valueOf(robotId));
+//                info.setCreateTime(new Date());
+//                info.setPostionX(dto.getX());
+//                info.setPostionY(dto.getY());
+//                info.setPostionZ(dto.getZ());
+//                info.setTheta(dto.getTheta());
+//                save(info);
+
+//            }
         }
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/TaskGridInfoServiceImpl.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/TaskGridInfoServiceImpl.java
new file mode 100644
index 0000000..cf61def
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/TaskGridInfoServiceImpl.java
@@ -0,0 +1,28 @@
+package com.casic.missiles.modular.robot.service.impl;
+
+import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
+import com.baomidou.mybatisplus.extension.plugins.pagination.Page;
+import com.baomidou.mybatisplus.extension.service.impl.ServiceImpl;
+import com.casic.missiles.modular.robot.dao.TaskGridInfoMapper;
+import com.casic.missiles.modular.robot.model.TaskGridInfo;
+import com.casic.missiles.modular.robot.service.ITaskGridInfoService;
+import org.springframework.stereotype.Service;
+
+import java.util.List;
+
+
+/**
+ * <p>
+ * 任务网格信息 服务实现类
+ * </p>
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+@Service
+public class TaskGridInfoServiceImpl extends ServiceImpl<TaskGridInfoMapper, TaskGridInfo> implements ITaskGridInfoService {
+    @Override
+    public List<TaskGridInfo> selectTaskGridInfoPage(Page<TaskGridInfo> page, QueryWrapper<TaskGridInfo> query) {
+        return this.baseMapper.selectTaskGridInfoPage(page, query);
+    }
+}
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/TaskInfoServiceImpl.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/TaskInfoServiceImpl.java
index d1417b0..fd49e2e 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/TaskInfoServiceImpl.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/TaskInfoServiceImpl.java
@@ -6,10 +6,10 @@
 import cn.hutool.core.convert.Convert;
 import cn.hutool.core.date.DateUtil;
 import cn.hutool.core.io.FileUtil;
-import cn.hutool.core.thread.ExecutorBuilder;
 import cn.hutool.core.util.StrUtil;
 import com.alibaba.fastjson.JSON;
 import com.baomidou.mybatisplus.core.conditions.Wrapper;
+import com.baomidou.mybatisplus.core.conditions.query.LambdaQueryWrapper;
 import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
 import com.baomidou.mybatisplus.core.conditions.update.UpdateWrapper;
 import com.baomidou.mybatisplus.core.metadata.IPage;
@@ -47,7 +47,6 @@
 import java.io.Serializable;
 import java.math.BigDecimal;
 import java.util.*;
-import java.util.concurrent.ThreadPoolExecutor;
 
 
 /**
@@ -70,6 +69,7 @@
     private final IRobotCacheService cacheService;
     private final IBaseRobotService baseRobotService;
     private final ITaskInfoImgService imgService;
+    private final ITaskGridInfoService taskGridInfoService;
     /**
      * 初始化线程池
      */
@@ -79,12 +79,19 @@
     @Value("${casic.doc.temp}")
     private String docTemp;
     /**
+     * 5000 ms
+     */
+    @Value("${casic.task.duration:3000}")
+    private Integer stayDuration;
+    @Value("${casic.algorithm.path:-1}")
+    private String path;
+    /**
      * 障碍物精度
      */
     @Value("${casic.obstacle:10}")
     private Integer obstacle;
 
-    public TaskInfoServiceImpl(@Lazy IRouteInfoService routeInfoService, IRobotStatusInfoService statusInfoService, AbstractDictService dictService, @Lazy IAlarmRecordService recordService, @Lazy ITaskHeatMapService taskHeatMapService, IAlgorithmService algorithmService, IRobotCacheService cacheService, @Lazy IBaseRobotService baseRobotService, ITaskInfoImgService imgService) {
+    public TaskInfoServiceImpl(@Lazy IRouteInfoService routeInfoService, IRobotStatusInfoService statusInfoService, AbstractDictService dictService, @Lazy IAlarmRecordService recordService, @Lazy ITaskHeatMapService taskHeatMapService, IAlgorithmService algorithmService, IRobotCacheService cacheService, @Lazy IBaseRobotService baseRobotService, ITaskInfoImgService imgService, ITaskGridInfoService taskGridInfoService) {
         this.routeInfoService = routeInfoService;
         this.statusInfoService = statusInfoService;
         this.dictService = dictService;
@@ -94,6 +101,7 @@
         this.cacheService = cacheService;
         this.baseRobotService = baseRobotService;
         this.imgService = imgService;
+        this.taskGridInfoService = taskGridInfoService;
     }
 
     /**
@@ -184,7 +192,7 @@
         if (CollectionUtil.isEmpty(routeInfos)) {
             throw new ServiceException(500, "路线不存在,请及时更新");
         }
-        RouteInfo routeInfo = routeInfos.get(0);
+//        RouteInfo routeInfo = routeInfos.get(0);
         Integer isTask = 0;
         //开启寻源任务
         if (request.getIsTask() == 1) {
@@ -193,22 +201,21 @@
         RobotInfo robotInfo = cacheService.getRobotInfo(Convert.toLong(robotId));
         CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_IS_TASK + robotId, isTask);
         //添加任务记录
-        TaskInfo taskInfo = addTaskByStartTrack(routeInfo, robotInfo);
+        TaskInfo taskInfo = addTaskByStartTrack( robotInfo, request);
         //No.3 更新器人执行任务信息
-        Long currRouteId = routeInfo.getId();
+//        Long currRouteId = routeInfo.getId();
         Long currTaskId = taskInfo.getId();
 
-        statusInfoService.updateCurrTask(robotId, currRouteId, currTaskId);
+        statusInfoService.updateCurrTask(robotId,  currTaskId);
         //算法初始化标志创建
         CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_TASK_ID + robotId, currTaskId);
         CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_TASK_INIT + robotId, 0);
-        CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_ROUTE_ID + robotId, routeInfo.getId());
+//        CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_ROUTE_ID + robotId, routeInfo.getId());
     }
 
     private void closeTask(StartTrackRequest request, String robotId) {
         //关闭操作
         RobotStatusInfo statusInfo = statusInfoService.getStatusByRobotId(robotId);
-
         if (statusInfo.getCurrTaskId() != null) {
             UpdateWrapper<TaskInfo> updateWrapper = new UpdateWrapper<>();
             updateWrapper.set("update_time", new Date());
@@ -221,10 +228,11 @@
 
         //清空任务缓存
         robotCacheClean(robotId);
-
+        //清楚规划任务锁
+        CacheUtil.remove(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_PLANING_FLAG + robotId);
+        CacheUtil.remove(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_ESTIMATE_GRID+ robotId);
         //清空机器当前任务
-        statusInfoService.updateCurrTask(robotId, null, null);
-
+        statusInfoService.updateCurrTask(robotId, null);
     }
 
     /**
@@ -249,7 +257,6 @@
 
         //判定任务是否已经完成初始化
         Long longId = Convert.toLong(robotId);
-        Long taskId = CacheUtil.get(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_TASK_ID + robotId);
         //缓存当前点位信息
         CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_XY_POINT + robotId, dto);
         RobotInfo robotInfo = cacheService.getRobotInfo(longId);
@@ -260,86 +267,203 @@
             log.error("no config 中子源阈值未配置");
             return;
         }
-
         //读取是否是循迹任务
         Integer isTask = cacheService.readIsTask(longId);
         if (isTask != 1) {
             log.info("循迹任务,不做处理");
             return;
         }
-        //算法初始化状态读取
-        Integer initValue = cacheService.readTaskInit(longId);
-
 
         //No.1 任务初始化
+//        pathInit(longId, robotInfo, pointDTO, taskId, robotId);
+    }
+
+    @Override
+    public void carPathPlanning(PoseMessageResponseDTO dto, String robotId) {
+        //判定任务是否已经完成初始化
+        Long longId = Convert.toLong(robotId);
+        Long taskId = CacheUtil.get(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_TASK_ID + robotId);
+        //缓存当前点位信息
+        CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_XY_POINT + robotId, dto);
+        RobotInfo robotInfo = cacheService.getRobotInfo(longId);
+        //slam经纬度转栅格
+        GridPointDTO pointDTO = algorithmService.convertSlamToGrid(dto.getX(), dto.getY(), robotInfo.getResolution(), robotInfo);
+        if (robotInfo.getAlarmThreshold() == null || robotInfo.getMaxThreshold() == null) {
+            log.error("no config 中子源阈值未配置");
+            return;
+        }
+
+        //读取是否是寻源任务
+        Integer isTask = cacheService.readIsTask(longId);
+        if (isTask != 1) {
+            log.info("循迹任务,不做处理");
+            return;
+        }
+
+        TaskInfo taskInfo = getById(taskId);
+        //No.1 任务初始化
+        pathInit(longId, robotInfo, pointDTO, taskId, robotId, taskInfo.getStepLen());
+        //No.2 路线规划
+        pathPlaning(longId, robotInfo, pointDTO, taskId, robotId, taskInfo,dto);
+    }
+
+    /**
+     * 路线规划进行加锁
+     *
+     * @param robotId 机器人ID
+     * @return
+     */
+    private synchronized boolean lockPlanIngFlag(String robotId) {
+        Boolean flag = CacheUtil.get(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_PLANING_FLAG + robotId);
+        if (flag == null) {
+            CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_PLANING_FLAG + robotId, true);
+            return true;
+        }
+        return false;
+    }
+
+    /**
+     * @param longId    机器人ID
+     * @param robotInfo 机器人信息
+     * @param pointDTO  当前点位栅格信息
+     * @param taskId    任务ID
+     * @param robotId   机器人ID
+     */
+    private void pathPlaning(Long longId, RobotInfo robotInfo, GridPointDTO pointDTO, Long taskId, String robotId, TaskInfo taskInfo,PoseMessageResponseDTO curPoint) {
+        if (taskId != null) {
+            //规划节点
+            PointNextDTO pointNextDTO = CacheUtil.get(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_ESTIMATE_GRID + robotId);
+            GridPointDTO oldEstimateGrid = GridPointDTO.convertGridPoint(pointNextDTO);
+            Boolean flag = lockPlanIngFlag(robotId);
+            //是否到达预期栅格
+            if (!flag) {
+                log.info("未获取到规划任务,直接结束：{}",pointDTO);
+                return;
+            }
+            try {
+                log.debug("oldEstimateGrid:{}", oldEstimateGrid);
+                if (pointNextDTO == null || isEquals(pointDTO, oldEstimateGrid)) {
+                    log.info("当前栅格变更......", pointDTO);
+                    CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_POINT + robotId, pointDTO);
+                    //算法调用获取热力图
+                    PathPlanDTO pathPlanDTO = createPlanDTO(robotInfo, pointDTO, taskId, robotId, taskInfo.getStepLen());
+                    if (taskInfo.getMaxThreshold() == null) {
+                        taskInfo.setMaxThreshold(robotInfo.getMaxThreshold());
+                    }
+                    pathPlanDTO.setTh(taskInfo.getMaxThreshold() + "");
+
+                    log.debug("heat map -- robotId:{},taskId:{}", robotId, taskId);
+                    pathPlanDTO.setZNow(cacheService.getNeutronCount(longId));
+
+                    //存储算法输出栅格信息
+                    AlgorithmResponse response = algorithmService.pathPlanning(pathPlanDTO);
+                    CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_ESTIMATE_GRID + robotId, response.getPointNextDTO());
+                    sendWebSocket(InstructStatusMsgEnums.HEAT_MAP, robotId, response.getTaskHeatMap());
+
+                    //存储循迹任务信息
+                    TaskGridInfo taskGridInfo = new TaskGridInfo();
+                    taskGridInfo.setCreateTime(new Date());
+                    taskGridInfo.setUpdateTime(new Date());
+                    taskGridInfo.setCurGridX(pointDTO.getX().intValue());
+                    taskGridInfo.setCurGridY(pointDTO.getY().intValue());
+                    taskGridInfo.setNeutronCount(new BigDecimal(pathPlanDTO.getZNow()));
+                    taskGridInfo.setEstimateGridX(response.getPointNextDTO().getX());
+                    taskGridInfo.setEstimateGridY(response.getPointNextDTO().getY());
+                    taskGridInfo.setTaskId(taskId);
+                    taskGridInfoService.save(taskGridInfo);
+
+                    try {
+                        //停留时间阈值
+                        Thread.sleep(stayDuration);
+                    } catch (InterruptedException e) {
+                        e.printStackTrace();
+                    }
+
+                    //是否触发停止状态
+                    if (!response.isEnd()) {
+                        log.debug("task planning-- xun dian robotId:{},taskId:{}", robotId, taskId);
+                        //机器人寻点指令下发
+                        sendTargetPoint(robotInfo, response,curPoint);
+                    } else {
+                        log.debug("task stop-- robotId:{},taskId:{}", robotId, taskId);
+                        //机器人停止指令下发
+                        SoftwareStopRequest request = new SoftwareStopRequest();
+                        request.setRobotId(robotInfo.getId());
+                        //设置为急停
+                        request.setControl(1);
+//                        baseRobotService.execCmdHandler(request, InstructCodeEnums.SOFTWARE_STOP);
+                        //循迹任务结束
+                        closeTask(null, robotId);
+                    }
+                }
+            } catch (Exception e) {
+                e.printStackTrace();
+            } finally {
+                //释放锁
+                CacheUtil.remove(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_PLANING_FLAG + robotId);
+            }
+        }
+    }
+
+    private PathPlanDTO createPlanDTO(RobotInfo robotInfo, GridPointDTO pointDTO, Long taskId, String robotId, Integer stepLen) {
+        PathPlanDTO pathPlanDTO = new PathPlanDTO();
+        pathPlanDTO.setTaskId(taskId);
+        pathPlanDTO.setRobotId(robotId);
+        pathPlanDTO.setStepLen(stepLen);
+        return pathPlanDTO;
+    }
+
+    /**
+     * 寻源初始化
+     *
+     * @param longId    机器ID
+     * @param robotInfo 机器信息
+     * @param pointDTO  当前点位信息
+     * @param taskId    任务ID
+     * @param robotId   机器ID
+     * @param stepLen   步长设置
+     */
+    private void pathInit(Long longId, RobotInfo robotInfo, GridPointDTO pointDTO, Long taskId, String robotId, Integer stepLen) {
+        //障碍物栅格上传已生成 直接读取
+        //算法初始化状态读取
+        Integer initValue = cacheService.readTaskInit(longId);
+        //No.1 任务初始化
         if (initValue != 1) {
+
             //执行初始化操作
             if (cacheService.updateTaskInit(longId, 1)) {
 
-                try {
-                    File file = new File(".");
-                    File[] files = file.listFiles((dir, name) -> (name.endsWith(".png") && name.startsWith("step"))
-                            || (name.endsWith(".txt") && name.startsWith("step")) || name.equals("repetition.txt") || name.equals("xy_train.txt") || name.equals("z_train.txt"));
-                    if (files != null) {
-                        for (int i = 0; i < files.length; i++) {
-                            FileUtil.del(files[i]);
+                List<List<Integer>> costList = CacheUtil.get(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_COST_MAP + robotId);
+                List<String> strs = new ArrayList<>();
+                List<String> strs2 = new ArrayList<>();
+                for (List<Integer> list : costList) {
+                    strs2.add(CollectionUtil.join(list, " "));
+                }
+                FileUtil.writeLines(strs2, new File(path + "map2.txt"), "utf-8");
+                //行转列 为算法输入数据
+                if(CollectionUtil.isNotEmpty(costList)){
+                    CollectionUtil.reverse(costList);
+                    for (int i = 0; i < costList.get(0).size(); i++) {
+                        ArrayList<Integer> ints = new ArrayList<>();
+                        for (List<Integer> list : costList) {
+                            ints.add(list.get(i));
                         }
+                        strs.add(CollectionUtil.join(ints, " "));
                     }
-                } catch (Exception e) {
-                    e.printStackTrace();
+//                    CollectionUtil.reverse(strs);
                 }
 
-                PathInitDTO pathInitDTO = createInitDTO(robotInfo, pointDTO, taskId, robotId);
+
+                FileUtil.writeLines(strs, new File(path + "map.txt"), "utf-8");
+
+                PathInitDTO pathInitDTO = createInitDTO(robotInfo, pointDTO, taskId, robotId, stepLen);
                 //中子源计数率读取
-                pathInitDTO.setCount(cacheService.getNeutronCount(longId));
+                pathInitDTO.setZNow(cacheService.getNeutronCount(longId));
+                log.info("init params:{}", JSON.toJSONString(pathInitDTO));
                 //执行1次算法 初始化操作
                 algorithmService.pathPlanningInit(pathInitDTO);
             }
         }
-
-        //No.2 任务执行中
-        if (taskId != null) {
-            //判断栅格变化
-            GridPointDTO oldPointDTO = CacheUtil.get(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_POINT + robotId);
-            if (oldPointDTO == null || isChange(pointDTO, oldPointDTO)) {
-                log.debug("grid change -- robotId:{},taskId:{}", robotId, taskId);
-                CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_POINT + robotId, pointDTO);
-                //算法调用获取热力图
-                PathPlanDTO pathPlanDTO = createPlanDTO(robotInfo, pointDTO, taskId, robotId);
-                //可通行标记为1 不可通行标记为0
-                String obstacle = getObstacle(robotId, pointDTO);
-                //TO-DO 障碍物判定信息
-                pathPlanDTO.setOpen(obstacle);
-                //中子源计数率读取
-                pathPlanDTO.setCount(cacheService.getNeutronCount(longId));
-                log.debug("heat map -- robotId:{},taskId:{}", robotId, taskId);
-                AlgorithmResponse response = algorithmService.pathPlanning(pathPlanDTO);
-                //存储算法输出栅格信息
-                CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_ESTIMATE_GRID + robotId, response.getPointNextDTO());
-                sendWebSocket(InstructStatusMsgEnums.HEAT_MAP, robotId, response.getTaskHeatMap());
-                //是否触发寻源操作
-                BigDecimal bigCount = new BigDecimal(pathPlanDTO.getCount());
-                if (bigCount.compareTo(robotInfo.getAlarmThreshold()) >= 0) {
-                    log.debug("task planning-- xun dian robotId:{},taskId:{}", robotId, taskId);
-                    //机器人寻点指令下发
-                    sendTargetPoint(robotInfo, response);
-                }
-
-                //是否触发停止状态
-                if (bigCount.compareTo(robotInfo.getMaxThreshold()) > 0) {
-                    log.debug("task stop-- robotId:{},taskId:{}", robotId, taskId);
-                    //机器人停止指令下发
-                    SoftwareStopRequest request = new SoftwareStopRequest();
-                    request.setRobotId(robotInfo.getId());
-                    //设置为急停
-                    request.setControl(1);
-                    baseRobotService.execCmdHandler(request, InstructCodeEnums.SOFTWARE_STOP);
-                    //循迹任务结束
-                    closeTask(null, robotId);
-                }
-
-            }
-        }
     }
 
     /**
@@ -353,7 +477,6 @@
         //判定任务是否已经完成初始化
         Long longId = Convert.toLong(robotId);
         Long taskId = CacheUtil.get(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_TASK_ID + robotId);
-
         RobotInfo robotInfo = cacheService.getRobotInfo(longId);
         //slam经纬度转栅格
         GridPointDTO pointDTO = algorithmService.convertSlamToGrid(dto.getX(), dto.getY(), robotInfo.getResolution(), robotInfo);
@@ -428,7 +551,7 @@
      * @param robotInfo 机器人信息
      * @param response  算法输出数据
      */
-    private void sendTargetPoint(RobotInfo robotInfo, AlgorithmResponse response) {
+    private void sendTargetPoint(RobotInfo robotInfo, AlgorithmResponse response,PoseMessageResponseDTO curPoint) {
         //读取已设置巡点目标信息
         PointNextDTO nextDTO = CacheUtil.get(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_TARGET_POINT_ID + robotInfo.getId());
         //巡点
@@ -439,8 +562,8 @@
             TargetPointRequest request = new TargetPointRequest();
             request.setX(Convert.toStr(gridCenterPoint.getX()));
             request.setY(Convert.toStr(gridCenterPoint.getY()));
-            request.setZ("0");
-
+            request.setZ(curPoint.getZ().toString());
+            request.setTheta(curPoint.getTheta().toString());
             request.setRobotId(robotInfo.getId());
             //执行巡点指令
             baseRobotService.execCmdHandler(request, InstructCodeEnums.SET_TARGETPOINT);
@@ -507,21 +630,20 @@
         params.put("startTime", startTime);
         params.put("endTime", endTime);
         //读取
-        List<AlarmRecord> records = recordService.selectListByTaskId(info.getId(), info.getRobotId());
         Integer index = 1;
-
-        if (CollectionUtil.isNotEmpty(records)) {
-            for (AlarmRecord record : records) {
+        LambdaQueryWrapper<TaskGridInfo> taskGridQuery = new LambdaQueryWrapper<>();
+        taskGridQuery.eq(TaskGridInfo::getTaskId,info.getId());
+        taskGridQuery.orderByAsc(TaskGridInfo::getCreateTime);
+        List<TaskGridInfo> taskGridInfos = taskGridInfoService.list(taskGridQuery);
+        if (CollectionUtil.isNotEmpty(taskGridInfos)) {
+            for (TaskGridInfo record : taskGridInfos) {
                 //栅格信息转换
-                GridPointDTO pointDTO = algorithmService.convertSlamToGrid(record.getX(), record.getY(), info.getResolution(), robotInfo);
-                record.setX(pointDTO.getX());
-                record.setY(pointDTO.getY());
                 record.setIndex(index);
                 index++;
             }
         }
 
-        params.put("records", records);
+        params.put("records", taskGridInfos);
         ImageEntity mcaImg = new ImageEntity();
         mcaImg.setHeight(600);
         mcaImg.setWidth(600);
@@ -557,7 +679,6 @@
             psdImg.setData(WordUtil.getImageBase64(uploadPath + info.getPsdImg()));
         }
         params.put("psdImg", psdImg);
-
         WordUtil.exportOutputStream(docTemp, response.getOutputStream(), params);
     }
 
@@ -589,7 +710,7 @@
     private String saveHeatImg(String taskId) {
         String newFileName = "task\\" + taskId + ".png";
         //读取当前路径图片
-        File file = new File(".");
+        File file = new File(path + "\\" + taskId + "\\figs");
         File[] files = file.listFiles((dir, name) -> name.endsWith(".png") && name.startsWith("step"));
         //判定当前执行路径最大step图片
         if (files != null && files.length > 0) {
@@ -614,33 +735,28 @@
         return true;
     }
 
-    private PathPlanDTO createPlanDTO(RobotInfo robotInfo, GridPointDTO pointDTO, Long taskId, String robotId) {
-        PathPlanDTO pathPlanDTO = new PathPlanDTO();
-        String xMax = robotInfo.getGridHeight().divide(robotInfo.getResolution(), 5, BigDecimal.ROUND_HALF_UP).intValue() + "";
-        String yMax = robotInfo.getGridWidth().divide(robotInfo.getResolution(), 5, BigDecimal.ROUND_HALF_UP).intValue() + "";
-        pathPlanDTO.setX(pointDTO.getX().intValue() + "");
-        pathPlanDTO.setY(pointDTO.getY().intValue() + "");
-        pathPlanDTO.setXMax(xMax);
-        pathPlanDTO.setYMax(yMax);
-        pathPlanDTO.setTaskId(taskId);
-        pathPlanDTO.setRobotId(robotId);
-        return pathPlanDTO;
+    private boolean isEquals(GridPointDTO pointDTO, GridPointDTO oldPointDTO) {
+        if(oldPointDTO==null||oldPointDTO.getX()==null){
+            return false;
+        }
+        if (pointDTO.getX().compareTo(oldPointDTO.getX()) == 0 && pointDTO.getY().compareTo(oldPointDTO.getY()) == 0) {
+            return true;
+        }
+        return false;
     }
 
-    private PathInitDTO createInitDTO(RobotInfo robotInfo, GridPointDTO pointDTO, Long taskId, String robotId) {
-        String xMax = robotInfo.getGridHeight().divide(robotInfo.getResolution(), 5, BigDecimal.ROUND_HALF_UP).intValue() + "";
-        String yMax = robotInfo.getGridWidth().divide(robotInfo.getResolution(), 5, BigDecimal.ROUND_HALF_UP).intValue() + "";
+
+    private PathInitDTO createInitDTO(RobotInfo robotInfo, GridPointDTO pointDTO, Long taskId, String robotId, Integer stepLen) {
         PathInitDTO pathInitDTO = new PathInitDTO();
-        pathInitDTO.setXMax(xMax);
-        pathInitDTO.setYMax(yMax);
-        pathInitDTO.setX(pointDTO.getX().intValue() + "");
-        pathInitDTO.setY(pointDTO.getY().intValue() + "");
+        pathInitDTO.setXNow(pointDTO.getX().intValue() + "");
+        pathInitDTO.setYNow(pointDTO.getY().intValue() + "");
+        pathInitDTO.setStepLen(stepLen);
         pathInitDTO.setTaskId(taskId);
         pathInitDTO.setRobotId(robotId);
         return pathInitDTO;
     }
 
-    private TaskInfo addTaskByStartTrack(RouteInfo routeInfo, RobotInfo robotInfo) {
+    private TaskInfo addTaskByStartTrack( RobotInfo robotInfo, StartTrackRequest request) {
         //No.2 新增任务信息
         TaskInfo taskInfo = new TaskInfo();
         String currDate = DateUtil.format(new Date(), "yyyyMMdd");
@@ -654,16 +770,20 @@
         taskInfo.setCreateTime(new Date());
         //任务类型 中子源
         taskInfo.setTaskType(1);
-        taskInfo.setDiscernType(1);
-        taskInfo.setTaskName(routeInfo.getRouteName() + "巡线任务");
+        taskInfo.setDiscernType(2);
+        taskInfo.setTaskName("自主巡检任务");
         //执行状态 未完成
         taskInfo.setTaskStatus(0);
         taskInfo.setStartTime(new Date());
-        taskInfo.setRouteId(routeInfo.getId());
+        taskInfo.setRouteId(null);
         taskInfo.setRobotId(robotInfo.getId());
         taskInfo.setGridHeight(robotInfo.getGridHeight());
         taskInfo.setGridWidth(robotInfo.getGridWidth());
         taskInfo.setResolution(robotInfo.getResolution());
+
+        taskInfo.setStepLen(request.getStepLen());
+        taskInfo.setMaxThreshold(request.getMaxThreshold());
+
         save(taskInfo);
         return taskInfo;
     }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/utils/AlgorithmUtils.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/utils/AlgorithmUtils.java
index 22864d1..467b9c2 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/utils/AlgorithmUtils.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/utils/AlgorithmUtils.java
@@ -1,6 +1,7 @@
 package com.casic.missiles.modular.robot.utils;
 
 import java.io.BufferedReader;
+import java.io.File;
 import java.io.IOException;
 import java.io.InputStreamReader;
 import java.util.ArrayList;
@@ -16,11 +17,13 @@
     public static List<String> invokeJob(List<String> params) {
         List<String> res = new ArrayList<>();
         try {
-
             BufferedReader br = null;
             BufferedReader brError;
             String line = null;
-            Process p = new ProcessBuilder(params).start();
+            ProcessBuilder processBuilder =  new ProcessBuilder(params);
+            processBuilder.directory(new File("D:\\workspace\\server\\keti\\casic-robot-inspection\\casic-web\\src\\main\\resources\\config\\path"));
+            Process p = processBuilder.start();
+
             br = new BufferedReader(new InputStreamReader(p.getInputStream()));
             brError = new BufferedReader(new InputStreamReader(p.getErrorStream()));
             while ((line = br.readLine()) != null || (line = brError.readLine()) != null) {
@@ -37,33 +40,29 @@
     public static void main(String[] args) {
         String path = "D:\\workspace\\server\\keti\\casic-robot-inspection\\casic-web\\src\\main\\resources\\config\\path\\";
         //算法初始化调用
-        String exePath = path + "PathPlanning_init.exe";
+        String exePath = path + "init.exe";
 
         ArrayList<String> invocation = new ArrayList<String>();
         invocation.add(exePath);
-        invocation.add("40");
-        invocation.add("20");
-        invocation.add("0.5");
+        invocation.add("10");
+        invocation.add("10");
         invocation.add("5");
-        invocation.add("5");
+        invocation.add("0.16");
+        invocation.add("Mission1");
         invokeJob(invocation);
 
         //{"count":"null","robotId":"1","taskId":"1748224487696142338","x":"8","xMax":"100","y":"2","yMax":"100"}
-        String planning = path + "PathPlanning.exe";
         //{"open":"0,0,0,0,0,0,0,0","robotId":"1","taskId":"1748250669749735425","x":"6","xMax":"100","y":"2","yMax":"100"}
         //{"open":"1,1,1,1,1,1,1,1","robotId":"1","taskId":"1748252894098178049","x":"6","xMax":"100","y":"2","yMax":"100"}
         //{"count":"6.0","open":"1,1,1,1,1,1,1,1","robotId":"1","taskId":"1750713404601159681","x":"6","xMax":"20","y":"2","yMax":"40"}
+        String planning = path + "search.exe";
         ArrayList<String> planInvocation = new ArrayList<String>();
         planInvocation.add(planning);
-        planInvocation.add("40");
-        planInvocation.add("40");
-        planInvocation.add("1,1,0,1,1,1,1,1");
-        planInvocation.add("6.0");
-        //next_X
-        planInvocation.add("6");
-        //next_Y
-        planInvocation.add("2");
-        ProcessBuilder processBuilder = new ProcessBuilder();
-        invokeJob(planInvocation);
+        planInvocation.add("10");
+        planInvocation.add("1.0");
+        planInvocation.add("0.2");
+        planInvocation.add("Mission1");
+        List<String> res = invokeJob(planInvocation);
+        System.out.println(res);
     }
 }
\ No newline at end of file
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/vo/TaskGridInfoVO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/vo/TaskGridInfoVO.java
new file mode 100644
index 0000000..0aff5bf
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/vo/TaskGridInfoVO.java
@@ -0,0 +1,58 @@
+package com.casic.missiles.modular.robot.vo;
+
+import io.swagger.annotations.ApiModelProperty;
+import lombok.Data;
+import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
+import com.casic.missiles.modular.robot.model.TaskGridInfo;
+import cn.hutool.core.util.StrUtil;
+
+import java.math.BigDecimal;
+
+/**
+ * 任务网格信息对象VO
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+@Data
+public class TaskGridInfoVO {
+    private static final long serialVersionUID = 1L;
+
+
+            @ApiModelProperty(value = "任务ID" , dataType = "Long")
+            private Long taskId;
+
+            @ApiModelProperty(value = "估算栅格x" , dataType = "Long")
+            private Long estimateGridX;
+
+            @ApiModelProperty(value = "估算栅格y" , dataType = "Long")
+            private Long estimateGridY;
+
+            @ApiModelProperty(value = "中子计数率" , dataType = "BigDecimal")
+            private BigDecimal neutronCount;
+
+            @ApiModelProperty(value = "当前栅格x" , dataType = "Long")
+            private Long curGridX;
+
+            @ApiModelProperty(value = "当前栅格y" , dataType = "Long")
+            private Long curGridY;
+
+public QueryWrapper<TaskGridInfo> genQuery() {
+QueryWrapper<TaskGridInfo> query = new QueryWrapper<>();
+if (taskId!=null) {
+        query.eq("task_id",taskId);
+}
+if (estimateGridX!=null) {
+        query.eq("estimate_grid_x",estimateGridX);
+}
+if (estimateGridY!=null) {
+        query.eq("estimate_grid_y",estimateGridY);
+}
+if (curGridX!=null) {
+        query.eq("cur_grid_x",curGridX);
+}
+if (curGridY!=null) {
+        query.eq("cur_grid_y",curGridY);
+}
+return query;
+}}
diff --git a/casic-web/src/main/resources/config/application.yml b/casic-web/src/main/resources/config/application.yml
index 04420fc..05c3633 100644
--- a/casic-web/src/main/resources/config/application.yml
+++ b/casic-web/src/main/resources/config/application.yml
@@ -42,11 +42,12 @@
   session-invalidate-time: 86400 #session失效时间(只在单机环境下生效，,多机环境在SpringSessionConfig类中配置) 单位：秒
   session-validation-interval: 900 #多久检测一次失效的session(只在单机环境下生效) 单位：秒
   no-login-urls: /user/login,/kaptcha,/config/baseConfig
-  hikSdk: D:\code\robot\casic-robot-inspection\casic-web\src\main\resources\config\hiklib\HCNetSDK.dll
-  hikPlay: D:\code\robot\casic-robot-inspection\casic-web\src\main\resources\config\hiklib\PlayCtrl.dll
+  hikSdk: D:\workspace\server\keti\casic-robot-inspection\casic-web\src\main\resources\config\hiklib\HCNetSDK.dll
+  hikPlay: D:\workspace\server\keti\casic-robot-inspection\casic-web\src\main\resources\config\hiklib\PlayCtrl.dll
   algorithm:
-    initPath: D:\workspace\server\keti\casic-robot-inspection\casic-web\src\main\resources\config\path\PathPlanning_init.exe
-    planPath: D:\workspace\server\keti\casic-robot-inspection\casic-web\src\main\resources\config\path\PathPlanning.exe
+    path:  D:\workspace\server\keti\casic-robot-inspection\casic-web\src\main\resources\config\path\
+    initPathName: init.exe
+    planPathName: search.exe
   doc:
     temp: D:\workspace\server\keti\casic-robot-inspection\casic-web\src\main\resources\config\path\report.docx
   config:
diff --git a/casic-web/src/main/resources/config/path/PathPlanning.exe b/casic-web/src/main/resources/config/path/PathPlanning.exe
deleted file mode 100644
index 6678e0f..0000000
--- a/casic-web/src/main/resources/config/path/PathPlanning.exe
+++ /dev/null
Binary files differ
diff --git a/casic-web/src/main/resources/config/path/PathPlanning_init.exe b/casic-web/src/main/resources/config/path/PathPlanning_init.exe
deleted file mode 100644
index d89dc16..0000000
--- a/casic-web/src/main/resources/config/path/PathPlanning_init.exe
+++ /dev/null
Binary files differ

diff --git a/.gitignore b/.gitignore
index 9e20bc1..f42a9cb 100644
--- a/.gitignore
+++ b/.gitignore
@@ -2,3 +2,4 @@
 **/target/**
 logs/
 *.iml
+*.exe
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java b/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
index c7b5d28..a849528 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
@@ -28,6 +28,7 @@
     String KEY_CUR_TASK_ID = "CUR_TASK_ID_";
     String KEY_CUR_TASK_INIT = "CUR_TASK_INIT_";
 
+    String KEY_COST_MAP = "KEY_COST_MAP";
     /**
      * PSD数据ID
      */
@@ -41,6 +42,7 @@
      */
     String KEY_CUR_XY_POINT = "KEY_CUR_XY_POINT_";
     String KEY_CUR_ESTIMATE_GRID = "KEY_CUR_ESTIMATE_GRID_";
+    String KEY_PLANING_FLAG = "KEY_PLANING_FLAG_";
     /**
      * 机器人巡点栅格
      */
@@ -55,6 +57,7 @@
      * 探测器配置参数缓存
      */
     String KEY_CONFIG_PARAMS_ID = "KEY_CONFIG_PARAMS_ID_";
+    String KEY_CONFIG_COSTMAP_RESOLUTION_SET = "KEY_CONFIG_COSTMAP_RESOLUTION_SET_";
 
     /**
      * 暂停状态缓存信息
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
index c4c905d..3bb475f 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
@@ -1,9 +1,14 @@
 package com.casic.missiles.modular.robot.controller;
 
+import cn.hutool.core.lang.Assert;
 import cn.hutool.core.util.StrUtil;
 import com.casic.missiles.core.base.controller.BaseController;
+import com.casic.missiles.core.cache.CacheUtil;
 import com.casic.missiles.core.util.SpringContextHolder;
+import com.casic.missiles.model.exception.RequestValidException;
+import com.casic.missiles.model.response.ResponseData;
 import com.casic.missiles.model.response.dto.ResponseDataDTO;
+import com.casic.missiles.modular.constants.RobotDictConstants;
 import com.casic.missiles.modular.robot.IBaseRobotService;
 import com.casic.missiles.modular.robot.dto.RobotInfoSetDTO;
 import com.casic.missiles.modular.robot.model.RobotInfo;
@@ -19,10 +24,16 @@
 import io.swagger.annotations.ApiOperation;
 import io.swagger.annotations.ApiParam;
 import lombok.SneakyThrows;
+import lombok.extern.slf4j.Slf4j;
+import org.apache.ibatis.annotations.Param;
+import org.springframework.beans.factory.annotation.Value;
 import org.springframework.web.bind.annotation.*;
+import org.springframework.web.multipart.MultipartFile;
 
 import javax.servlet.http.HttpServletResponse;
+import java.io.File;
 import java.util.List;
+import java.util.Optional;
 
 /**
  * 机器人操作类
@@ -30,6 +41,7 @@
  * @author lwh
  * @date 2023-10-30
  */
+@Slf4j
 @Api(tags = "机器人控制接口")
 @RestController
 @RequestMapping("/robot/opt")
@@ -61,6 +73,28 @@
         return baseRobotService.execCmdHandler(request, InstructCodeEnums.CHANGE_PCD);
     }
 
+    @Value("${casic.file.uploadPath}")
+    private String filePath;
+
+    @SneakyThrows
+    @ApiOperation(value = "PCD上传接口")
+    @PostMapping(value = "/uploadPcd")
+    public ResponseData uploadPcd(@RequestParam("file") MultipartFile file, @ApiParam("机器人ID,url拼接传参") String robotId) {
+        Assert.isFalse(StrUtil.isEmpty(robotId), () -> new RequestValidException("机器ID不能为空", Optional.of(new Throwable().getStackTrace()[2])));
+        RobotInfo info = robotInfoService.getById(robotId);
+        if (file != null && !file.isEmpty()) {
+            File pcdFile = new File(filePath + "finalCloud_" + info.getId() + ".pcd");
+            try {
+                file.transferTo(pcdFile);
+            } catch (Exception e) {
+                log.error(e.getMessage());
+            }
+            return ResponseData.success();
+        } else {
+            return ResponseData.error("附件上传失败!");
+        }
+    }
+
     @SneakyThrows
     @ApiOperation(value = "PCD下载接口")
     @PostMapping(value = "/pcdDownload")
@@ -136,6 +170,10 @@
     @ApiOperation(value = "启动或关闭循迹功能", notes = "返回 1成功 0失败  wait 启动时是否等待，0代表循迹启动即运动，不等待；1代表循迹启动原地等待。若无此字段，默认循迹启动不等待直接运动")
     @PostMapping(value = "/startTrack")
     public ResponseDataDTO<Integer> startTrack(@RequestBody StartTrackRequest request) {
+        request.setIsTask(1);
+        if (request.getStepLen() == null) {
+            request.setStepLen(2);
+        }
         baseRobotService.startTrack(request);
         return ResponseDataDTO.success();
     }
@@ -156,6 +194,13 @@
         MessageRequestDTO<CrlNavRequest> messageDTO = new MessageRequestDTO();
         messageDTO.setMsg(request);
         ResponseDataDTO<Integer> list = baseRobotService.getRobotRouteOptService().robotCmd(info, InstructCodeEnums.CTRL_NAV, messageDTO);
+        if (request.getAction() == 1) {
+            CostMapResolutionRequest costMap = new CostMapResolutionRequest();
+            costMap.setResolution(info.getResolution().doubleValue());
+            request.setRobotId(request.getRobotId());
+            //是否下发分辨率
+            CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CONFIG_COSTMAP_RESOLUTION_SET + request.getRobotId(), costMap);
+        }
         return list;
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java
new file mode 100644
index 0000000..5317651
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java
@@ -0,0 +1,86 @@
+package com.casic.missiles.modular.robot.controller;
+
+import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
+import com.baomidou.mybatisplus.extension.plugins.pagination.Page;
+import com.casic.missiles.core.base.controller.BaseController;
+import com.casic.missiles.core.page.PageFactory;
+import com.casic.missiles.core.page.PageInfoBT;
+import com.casic.missiles.model.form.IdForms;
+import com.casic.missiles.model.response.dto.ResponseDataDTO;
+import com.casic.missiles.modular.robot.model.TaskGridInfo;
+import com.casic.missiles.modular.robot.service.ITaskGridInfoService;
+import com.casic.missiles.modular.robot.vo.TaskGridInfoVO;
+import io.swagger.annotations.Api;
+import io.swagger.annotations.ApiOperation;
+import org.springframework.web.bind.annotation.*;
+
+import java.util.List;
+
+/**
+ * 任务网格信息Controller
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+@Api(tags = "任务网格信息")
+@RestController
+@RequestMapping("/robot/taskGridInfo")
+public class TaskGridInfoController extends BaseController {
+
+    private final ITaskGridInfoService taskGridInfoService;
+
+    public TaskGridInfoController(ITaskGridInfoService taskGridInfoService) {
+        this.taskGridInfoService = taskGridInfoService;
+    }
+
+    @ApiOperation(value = "列表查询")
+    @GetMapping(value = "/list")
+    public ResponseDataDTO<List<TaskGridInfo>> list(TaskGridInfoVO taskGridInfoVO) {
+        QueryWrapper<TaskGridInfo> query = taskGridInfoVO.genQuery();
+        return ResponseDataDTO.success(taskGridInfoService.list(query));
+    }
+
+    @ApiOperation(value = "分页查询")
+    @GetMapping(value = "/listPage")
+    public ResponseDataDTO<PageInfoBT<TaskGridInfo>> listPage(TaskGridInfoVO taskGridInfoVO) {
+        Page<TaskGridInfo> page = PageFactory.defaultPage();
+        QueryWrapper<TaskGridInfo> query = taskGridInfoVO.genQuery();
+        page = taskGridInfoService.page(page, query);
+        return ResponseDataDTO.success(super.packForBT(page));
+    }
+
+
+    @ApiOperation(value = "新增接口")
+    @PostMapping(value = "/add")
+    public ResponseDataDTO add(@RequestBody TaskGridInfo taskGridInfo) {
+        taskGridInfoService.save(taskGridInfo);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "修改接口")
+    @PostMapping(value = "/update")
+    public ResponseDataDTO update(@RequestBody TaskGridInfo taskGridInfo) {
+        taskGridInfoService.updateById(taskGridInfo);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "删除接口")
+    @GetMapping(value = "/delete")
+    public ResponseDataDTO delete(String id) {
+        taskGridInfoService.removeById(id);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation("批量删除")
+    @PostMapping("/batchDelete")
+    public ResponseDataDTO batchDelete(@RequestBody IdForms ids) {
+        taskGridInfoService.removeByIds(ids.getIds());
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "详情查询")
+    @GetMapping(value = "/detail")
+    public ResponseDataDTO<TaskGridInfo> detail(String id) {
+        return ResponseDataDTO.success(taskGridInfoService.getById(id));
+    }
+}
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
index e82a792..6992274 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
@@ -98,7 +98,7 @@
     public ResponseDataDTO<AlgorithmResponse> test() {
         AlgorithmResponse response = new AlgorithmResponse();
         response.setTaskHeatMap(new TaskHeatMap());
-        algorithmService.readAlgorithmResponse("step3.txt",response,123L,null);
+        algorithmService.readAlgorithmResponse("step3.txt",null,response,123L,null);
         sendWebSocket(InstructStatusMsgEnums.HEAT_MAP, "1", response.getTaskHeatMap());
         return ResponseDataDTO.success(response);
     }
@@ -121,7 +121,7 @@
     @GetMapping(value = "/testGrid")
     public ResponseDataDTO<List<RouteInfo>> testGrid(@RequestBody PathInitDTO pathInitDTO) {
         RobotInfo robotInfo = infoService.getById(pathInitDTO.getRobotId());
-        GridPointDTO gridPointDTO = algorithmService.convertSlamToGrid(new BigDecimal(pathInitDTO.getX()), new BigDecimal(10), new BigDecimal(4), robotInfo);
+        GridPointDTO gridPointDTO = algorithmService.convertSlamToGrid(new BigDecimal(pathInitDTO.getXNow()), new BigDecimal(10), new BigDecimal(4), robotInfo);
         return ResponseDataDTO.success(algorithmService.convertGridCenterPoint(gridPointDTO, new BigDecimal(10), robotInfo));
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/TaskGridInfoMapper.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/TaskGridInfoMapper.java
new file mode 100644
index 0000000..5c3a70b
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/TaskGridInfoMapper.java
@@ -0,0 +1,23 @@
+package com.casic.missiles.modular.robot.dao;
+
+
+import com.baomidou.mybatisplus.core.mapper.BaseMapper;
+import com.casic.missiles.modular.robot.model.TaskGridInfo;
+import org.apache.ibatis.annotations.Param;
+import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
+import com.baomidou.mybatisplus.extension.plugins.pagination.Page;
+import java.util.List;
+/**
+ * <p>
+ *  任务网格信息 Mapper 接口
+ * </p>
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+public interface TaskGridInfoMapper extends BaseMapper<TaskGridInfo> {
+/**
+* 任务网格信息 分页检索
+*/
+List<TaskGridInfo> selectTaskGridInfoPage( @Param("page") Page<TaskGridInfo> page,@Param("ew" ) QueryWrapper<TaskGridInfo> query);
+}
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/mapping/TaskGridInfoMapper.xml b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/mapping/TaskGridInfoMapper.xml
new file mode 100644
index 0000000..be573e0
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/mapping/TaskGridInfoMapper.xml
@@ -0,0 +1,80 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!DOCTYPE mapper PUBLIC "-//mybatis.org//DTD Mapper 3.0//EN" "http://mybatis.org/dtd/mybatis-3-mapper.dtd">
+<mapper namespace="com.casic.missiles.modular.robot.dao.TaskGridInfoMapper">
+    <!-- 通用查询映射结果 -->
+    <resultMap id="BaseResultMap" type="com.casic.missiles.modular.robot.model.TaskGridInfo">
+        <result column="id" property="id"/>
+        <result column="create_time" property="createTime"/>
+        <result column="update_time" property="updateTime"/>
+        <result column="task_id" property="taskId"/>
+        <result column="estimate_grid_x" property="estimateGridX"/>
+        <result column="estimate_grid_y" property="estimateGridY"/>
+        <result column="neutron_count" property="neutronCount"/>
+        <result column="cur_grid_x" property="curGridX"/>
+        <result column="cur_grid_y" property="curGridY"/>
+    </resultMap>
+    <!-- 通用查询结果列 -->
+    <sql id="Base_Column_List">
+        id
+        ,
+                    create_time as createTime ,
+                    update_time as updateTime ,
+                    task_id as taskId ,
+                    estimate_grid_x as estimateGridX ,
+                    estimate_grid_y as estimateGridY ,
+                    neutron_count as neutronCount ,
+                    cur_grid_x as curGridX ,
+                    cur_grid_y as curGridY
+    </sql>
+    <sql id="Base_Column_Alias">
+        a
+        .
+        id
+        ,
+            a.create_time
+            as createTime ,
+            a.update_time
+            as updateTime ,
+            a.task_id
+            as taskId ,
+            a.estimate_grid_x
+            as estimateGridX ,
+            a.estimate_grid_y
+            as estimateGridY ,
+            a.neutron_count
+            as neutronCount ,
+            a.cur_grid_x
+            as curGridX ,
+            a.cur_grid_y
+            as curGridY
+    </sql>
+    <select id="selectTaskGridInfoPage" resultType="com.casic.missiles.modular.robot.model.TaskGridInfo">
+        select
+        <include refid="Base_Column_Alias"/>
+        from task_grid_info a
+        <where>
+            <if test="ew != null">
+                ${ew.sqlSegment}
+            </if>
+        </where>
+    </select>
+    <sql id="Base_Column_Query">
+        <if test="request!=null">
+            <if test="request.taskId!= null and request.taskId!=''">
+                and task_id = #{request.taskId}
+            </if>
+            <if test="request.estimateGridX!= null and request.estimateGridX!=''">
+                and estimate_grid_x = #{request.estimateGridX}
+            </if>
+            <if test="request.estimateGridY!= null and request.estimateGridY!=''">
+                and estimate_grid_y = #{request.estimateGridY}
+            </if>
+            <if test="request.curGridX!= null and request.curGridX!=''">
+                and cur_grid_x = #{request.curGridX}
+            </if>
+            <if test="request.curGridY!= null and request.curGridY!=''">
+                and cur_grid_y = #{request.curGridY}
+            </if>
+        </if>
+    </sql>
+</mapper>
\ No newline at end of file
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java
index 749f559..4559074 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java
@@ -13,4 +13,9 @@
      * 规划点位信息
      */
     private PointNextDTO pointNextDTO;
+    /**
+     * 寻源目标地址
+     */
+    private PointNextDTO coordinate;
+    private boolean isEnd;
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java
index 39ff2a2..cef3238 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java
@@ -12,6 +12,9 @@
     private BigDecimal y;
 
     public static GridPointDTO convertGridPoint(PointNextDTO nextDTO) {
+        if(nextDTO==null){
+            return null;
+        }
         GridPointDTO gridPointDTO = new GridPointDTO();
         gridPointDTO.setX(Convert.toBigDecimal(nextDTO.getX()));
         gridPointDTO.setY(Convert.toBigDecimal(nextDTO.getY()));
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java
index 8c88242..33be25d 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java
@@ -12,25 +12,19 @@
 public class PathInitDTO {
     //---------------------算法参数
     /**
-     * x坐标最大值（x方向栅格总数）
+     * 初始位置x坐标(栅格地图下的坐标)
      */
-    private String xMax;
+    private String xNow;
     /**
-     * y坐标最大值（y方向栅格总数）
+     * 初始位置y坐标(栅格地图下的坐标)
      */
-    private String yMax;
+    private String yNow;
     /**
      * 初始位置计数率
      */
-    private String count;
-    /**
-     * 初始位置x坐标(0<=x<x_max)
-     */
-    private String x;
-    /**
-     * 初始位置y坐标(0<=y<y_max)
-     */
-    private String y;
+    private String zNow;
+
+    private Integer stepLen;
 
     //---------------------其他执行参数
     private String robotId;
@@ -38,11 +32,11 @@
 
     public List<String> toArray() {
         List<String> list = new ArrayList<>();
-        list.add(xMax);
-        list.add(yMax);
-        list.add(count);
-        list.add(x);
-        list.add(y);
+        list.add(xNow);
+        list.add(yNow);
+        list.add(stepLen+"");
+        list.add(zNow);
+        list.add(taskId+"");
         return list;
     }
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java
index 109e95c..ada6774 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java
@@ -10,21 +10,17 @@
  */
 @Data
 public class PathPlanDTO extends PathInitDTO {
-
     /**
-     * 长度为8的数组，记录(x-1, y-1), (x-1, y), (x-1, y+1), (x, y-1), (x, y+1), (x+1, y-1), (x+1, y), (x+1, y+1)8个坐标的可通行信息。
-     * 若某坐标有障碍物（不可通行行）则记为0，否则记为1
+     * 浮点阈值(阈值，超过该值后结束巡检开始寻源)
      */
-    private String open;
-
+    private String th;
     public List<String> toArray() {
         List<String> list = new ArrayList<>();
-        list.add(getXMax());
-        list.add(getYMax());
-        list.add(open);
-        list.add(getCount());
-        list.add(getX());
-        list.add(getY());
+
+        list.add(getStepLen()+"");
+        list.add(getTh());
+        list.add(getZNow());
+        list.add(getTaskId()+"");
         return list;
     }
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskGridInfo.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskGridInfo.java
new file mode 100644
index 0000000..101c10a
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskGridInfo.java
@@ -0,0 +1,83 @@
+package com.casic.missiles.modular.robot.model;
+
+import com.baomidou.mybatisplus.annotation.TableField;
+import com.baomidou.mybatisplus.annotation.TableName;
+import com.baomidou.mybatisplus.extension.activerecord.Model;
+import io.swagger.annotations.ApiModelProperty;
+import lombok.Data;
+
+import java.math.BigDecimal;
+import java.util.Date;
+
+/**
+ * 任务网格信息对象 task_grid_info
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+@Data
+@TableName("task_grid_info")
+public class TaskGridInfo extends Model<TaskGridInfo> {
+    private static final long serialVersionUID = 1L;
+    @TableField(exist = false)
+    private Integer index;
+    @ApiModelProperty(value = "主键", dataType = "Long")
+    @TableField("id")
+    private Long id;
+
+    @ApiModelProperty(value = "创建时间", dataType = "Date")
+    @TableField("create_time")
+    private Date createTime;
+
+    @ApiModelProperty(value = "修改时间", dataType = "Date")
+    @TableField("update_time")
+    private Date updateTime;
+
+    @ApiModelProperty(value = "任务ID", dataType = "Long")
+    @TableField("task_id")
+    private Long taskId;
+
+    @ApiModelProperty(value = "估算栅格x", dataType = "Long")
+    @TableField("estimate_grid_x")
+    private String estimateGridX;
+
+    @ApiModelProperty(value = "估算栅格y", dataType = "Long")
+    @TableField("estimate_grid_y")
+    private String estimateGridY;
+
+    @ApiModelProperty(value = "中子计数率", dataType = "BigDecimal")
+    @TableField("neutron_count")
+    private BigDecimal neutronCount;
+
+    @ApiModelProperty(value = "当前栅格x", dataType = "Long")
+    @TableField("cur_grid_x")
+    private Integer curGridX;
+
+    @ApiModelProperty(value = "当前栅格y", dataType = "Long")
+    @TableField("cur_grid_y")
+    private Integer curGridY;
+
+    public String getCurPoint(){
+        return "("+curGridX+","+curGridY+")";
+    }
+    public String getEstimatePoint(){
+        if(estimateGridX==null&estimateGridY==null){
+            return "结束";
+        }
+        return "("+estimateGridX+","+estimateGridY+")";
+    }
+    @Override
+    public String toString() {
+        return "TaskGridInfo{"
+                + "id=" + id
+                + "createTime=" + createTime
+                + "updateTime=" + updateTime
+                + "taskId=" + taskId
+                + "estimateGridX=" + estimateGridX
+                + "estimateGridY=" + estimateGridY
+                + "neutronCount=" + neutronCount
+                + "curGridX=" + curGridX
+                + "curGridY=" + curGridY
+                + "}";
+    }
+}
\ No newline at end of file
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java
index edc104e..a8acb2a 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java
@@ -53,6 +53,13 @@
     @TableField("route_id")
     private Long routeId;
 
+    @ApiModelProperty(value = "寻源初始步长", dataType = "Integer")
+    @TableField("step_len")
+    private Integer stepLen;
+
+    @ApiModelProperty(value = "寻源阈值设定", dataType = "BigDecimal")
+    @TableField("max_threshold")
+    private BigDecimal maxThreshold;
     /**
      * 任务类型(1巡检任务)
      */
@@ -62,7 +69,6 @@
     @ApiModelProperty(value = "任务类型字典名称", dataType = "String")
     @TableField(exist = false)
     private String taskTypeName;
-
     @ApiModelProperty(value = "识别类型(discernType 1中子源识别)", dataType = "Integer")
     @TableField("discern_type")
     private Integer discernType;
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java
index c4bc25d..c6620f3 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java
@@ -53,7 +53,7 @@
     }),
     SET_SPEED("/set_speed" , "/set_speed_response" , "最大运动速度设置" , true, new TypeReference<RobotMsgResponseDTO<Integer>>() {
     }),
-    SET_TARGETPOINT("/set_targetpoint" , "/set_targetpoint_response" , "设置导航目标点" , false, new TypeReference<RobotMsgResponseDTO<Integer>>() {
+    SET_TARGETPOINT("/set_targetpoint" , "/set_targetpoint_response" , "设置导航目标点" , true, new TypeReference<RobotMsgResponseDTO<Integer>>() {
     }),
 
     GET_DATA_LIST("/get_data_list" , "/get_data_list_response" , "获取数据包列表" , true, new TypeReference<RobotMsgResponseDTO<List<String>>>() {
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/AbstractSocketHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/AbstractSocketHandler.java
index 83f4cb2..e47e343 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/AbstractSocketHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/AbstractSocketHandler.java
@@ -49,7 +49,7 @@
                 wsMsgBaseDTO.setMsgKey(enums.getMsgKey());
                 wsMsgBaseDTO.setData(jsonObject.get("msg"));
                 String msg = JSON.toJSONString(wsMsgBaseDTO);
-                log.info("推送{}消息：{}", enums.getRemarks(), msg);
+//                log.info("推送{}消息：{}", enums.getRemarks(), msg);
                 webSocket.sendOneMessage(robotId, msg);
             }
         } catch (Exception e) {
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/cmd/TargetPointCmdHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/cmd/TargetPointCmdHandler.java
index c1c09fa..43b4460 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/cmd/TargetPointCmdHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/cmd/TargetPointCmdHandler.java
@@ -34,7 +34,11 @@
     @Override
     public ResponseDataDTO senderCmd(TargetPointRequest request, InstructCodeEnums enums) {
         MessageRequestDTO<TargetPointRequest> messageDTO = new MessageRequestDTO();
-        //读取默认关键点信息
+        //读取默认关键点信息 {"x":"1.715708819927424","y":"-0.9100042544250901","z":"0.017993879527185876","theta":"-0.039278239414312105"}
+//        request.setX("1.715708819927424");
+//        request.setY("-0.9100042544250901");
+//        request.setZ("0.017993879527185876");
+//        request.setTheta("-0.039278239414312105");
         messageDTO.setMsg(request);
         ResponseDataDTO<Integer> list = optService.robotCmd(getRobotInfo(), enums, messageDTO);
         return list;
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/AppCostMapHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/AppCostMapHandler.java
index 0335693..88c9f79 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/AppCostMapHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/AppCostMapHandler.java
@@ -1,6 +1,11 @@
 package com.casic.missiles.modular.robot.opt.handler.msg;
 
+import com.casic.missiles.core.cache.CacheUtil;
+import com.casic.missiles.modular.constants.RobotDictConstants;
+import com.casic.missiles.modular.robot.IBaseRobotService;
+import com.casic.missiles.modular.robot.opt.enums.InstructCodeEnums;
 import com.casic.missiles.modular.robot.opt.handler.AbstractSocketHandler;
+import com.casic.missiles.modular.robot.opt.instruct.dto.CostMapResolutionRequest;
 import com.casic.missiles.modular.robot.service.IRobotStatusInfoService;
 import lombok.extern.slf4j.Slf4j;
 import org.eclipse.paho.client.mqttv3.MqttMessage;
@@ -16,14 +21,27 @@
 @Component("/app_costmap_sender")
 public class AppCostMapHandler extends AbstractSocketHandler {
     private final IRobotStatusInfoService statusInfoService;
+    private final IBaseRobotService baseRobotService;
     private static Map<String, Integer> index = new ConcurrentHashMap<>();
 
-    public AppCostMapHandler(IRobotStatusInfoService statusInfoService) {
+    public AppCostMapHandler(IRobotStatusInfoService statusInfoService, IBaseRobotService baseRobotService) {
         this.statusInfoService = statusInfoService;
+        this.baseRobotService = baseRobotService;
     }
 
     @Override
     public void process(String robotId, String topic, MqttMessage message) {
         statusInfoService.saveCostMap(robotId, message);
+
+        CostMapResolutionRequest costMapSet = CacheUtil.get(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CONFIG_COSTMAP_RESOLUTION_SET +robotId);
+        if(costMapSet != null){
+            CacheUtil.remove(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CONFIG_COSTMAP_RESOLUTION_SET +robotId);
+            new Thread(() -> {
+                costMapSet.setRobotId(Long.valueOf(robotId));
+                baseRobotService.execCmdHandler(costMapSet, InstructCodeEnums.COSTMAP_RESOLUTION);
+            }).start();
+
+        }
+
     }
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/PoseMessageMsgHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/PoseMessageMsgHandler.java
index 29f8919..6c3c3bd 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/PoseMessageMsgHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/PoseMessageMsgHandler.java
@@ -17,11 +17,9 @@
 @Component("/pose_message")
 public class PoseMessageMsgHandler extends AbstractSocketHandler {
 
-
     private final EventPublisher publisher;
 
     public PoseMessageMsgHandler(EventPublisher publisher) {
-
         this.publisher = publisher;
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/StopStateMsgHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/StopStateMsgHandler.java
index 8950815..8a71e0b 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/StopStateMsgHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/StopStateMsgHandler.java
@@ -30,7 +30,7 @@
     public void process(String robotId, String topic, MqttMessage message) {
 
         index.put(index_key, index.get("index") + 1);
-        if (index.get(index_key)>50) {
+        if (index.get(index_key)>30) {
             index.put(index_key, 0);
             log.info("主键：{},停障状态topic:{},消息内容:{}", robotId, topic, message.toString());
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/impl/RobotOptServiceImpl.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/impl/RobotOptServiceImpl.java
index 8471e7a..7940a24 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/impl/RobotOptServiceImpl.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/impl/RobotOptServiceImpl.java
@@ -146,7 +146,7 @@
     public void startTrack(StartTrackRequest request) {
         //增加循迹任务记录
         taskInfoService.reloadTask(request, robotInfoService.getById(request.getRobotId()));
-        this.execCmdHandler(request, InstructCodeEnums.START_TRACK);
+//        this.execCmdHandler(request, InstructCodeEnums.START_TRACK);
     }
 
     public IRobotRouteOptService getRobotRouteOptService() {
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/BaseRobotCmdDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/BaseRobotCmdDTO.java
index 5608f2b..06b53de 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/BaseRobotCmdDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/BaseRobotCmdDTO.java
@@ -3,11 +3,13 @@
 import io.swagger.annotations.ApiModelProperty;
 import lombok.Data;
 
+import java.io.Serializable;
+
 /**
  * 机器人指令基类
  */
 @Data
-public class BaseRobotCmdDTO {
+public class BaseRobotCmdDTO implements Serializable {
     @ApiModelProperty(value = "机器人主键", dataType = "String")
     protected Long robotId;
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/StartTrackRequest.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/StartTrackRequest.java
index a18321b..85546a7 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/StartTrackRequest.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/StartTrackRequest.java
@@ -3,6 +3,8 @@
 import io.swagger.annotations.ApiModelProperty;
 import lombok.Data;
 
+import java.math.BigDecimal;
+
 /**
  * 导航下发指令
  */
@@ -10,18 +12,19 @@
 public class StartTrackRequest extends BaseRobotCmdDTO {
     @ApiModelProperty(value = "sn编码(可不填 默认机器ID)", dataType = "String")
     private String sn;
-
     @ApiModelProperty(value = "下发状态 1:开启 0:关闭", dataType = "String")
     private Integer action;
     @ApiModelProperty(value = "等待状态 1:等待 0:不等待", dataType = "String")
     private Integer wait;
     @ApiModelProperty(value = "等待状态 0:循迹 1:任务", dataType = "isTask")
     private Integer isTask;
-    @ApiModelProperty(value = "轨迹名称", dataType = "String")
+    @ApiModelProperty(value = "轨迹名称(废弃)", dataType = "String")
     private String track_name;
     @ApiModelProperty(value = "关键点名称", dataType = "String", hidden = true)
     private String taskpoint_name;
-
+    @ApiModelProperty(value = "初始寻源步长", dataType = "Integer")
+    private Integer stepLen;
+    @ApiModelProperty(value = "寻源阈值", dataType = "BigDecimal")
+    private BigDecimal maxThreshold;
     private String obs_mode="1";
-
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/response/StopStateResponseDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/response/StopStateResponseDTO.java
index eb514bf..e0679d7 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/response/StopStateResponseDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/response/StopStateResponseDTO.java
@@ -3,11 +3,13 @@
 import io.swagger.annotations.ApiModelProperty;
 import lombok.Data;
 
+import java.io.Serializable;
+
 /**
  * 电压信息返回
  */
 @Data
-public class StopStateResponseDTO {
+public class StopStateResponseDTO implements Serializable {
     @ApiModelProperty(value = "软件急停(0：无触发 1：触发)" , dataType = "Integer")
     private Integer soft;
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IAlgorithmService.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IAlgorithmService.java
index 56b3e1d..f62523b 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IAlgorithmService.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IAlgorithmService.java
@@ -6,6 +6,7 @@
 import com.casic.missiles.modular.robot.dto.PathPlanDTO;
 import com.casic.missiles.modular.robot.model.RobotInfo;
 
+import java.io.File;
 import java.math.BigDecimal;
 import java.util.List;
 
@@ -38,5 +39,5 @@
      * @return
      */
     GridPointDTO convertGridCenterPoint(GridPointDTO pointDTO, BigDecimal resolution, RobotInfo robotInfo);
-    void readAlgorithmResponse(String fileName, AlgorithmResponse response, Long taskId, List<String> res);
+    void readAlgorithmResponse(String fileName, File repetitionFile, AlgorithmResponse response, Long taskId, List<String> res);
 }
\ No newline at end of file
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IRobotStatusInfoService.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IRobotStatusInfoService.java
index 2af210a..757f8d0 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IRobotStatusInfoService.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IRobotStatusInfoService.java
@@ -67,10 +67,9 @@
     /**
      * 更新车辆路线及任务信息
      *
-     * @param currRouteId 当前路线信息
      * @param currTaskId  当前任务信息
      */
-    void updateCurrTask(String robotId, Long currRouteId, Long currTaskId);
+    void updateCurrTask(String robotId, Long currTaskId);
 
     RobotStatusInfo getStatusByRobotId(String robotId);
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/ITaskGridInfoService.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/ITaskGridInfoService.java
new file mode 100644
index 0000000..fc0da93
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/ITaskGridInfoService.java
@@ -0,0 +1,22 @@
+package com.casic.missiles.modular.robot.service;
+
+import com.baomidou.mybatisplus.extension.service.IService;
+import  com.casic.missiles.modular.robot.model.TaskGridInfo;
+import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
+import com.baomidou.mybatisplus.extension.plugins.pagination.Page;
+import java.util.List;
+
+
+/**
+ * <p>
+ * 任务网格信息 服务类
+ * </p>
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+public interface ITaskGridInfoService extends IService<TaskGridInfo> {
+/**
+* 任务网格信息 分页检索
+*/
+List<TaskGridInfo> selectTaskGridInfoPage(Page<TaskGridInfo> page,QueryWrapper<TaskGridInfo> query);}
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/ITaskInfoService.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/ITaskInfoService.java
index 31d7ba6..b395993 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/ITaskInfoService.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/ITaskInfoService.java
@@ -51,6 +51,13 @@
      * @param dto 实体
      */
     void pointCallBack(PoseMessageResponseDTO dto, String robotId);
+
+    /**
+     * 车辆路线规划
+     * @param dto
+     * @param robotId
+     */
+    void carPathPlanning(PoseMessageResponseDTO dto, String robotId);
     void pointTestCallBack(PoseMessageResponseDTO dto, String robotId);
     /**
      * 图片路径保存
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/AlgorithmServiceImpl.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/AlgorithmServiceImpl.java
index f808fa0..a4dbe0b 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/AlgorithmServiceImpl.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/AlgorithmServiceImpl.java
@@ -29,10 +29,12 @@
 @Slf4j
 @Service
 public class AlgorithmServiceImpl implements IAlgorithmService {
-    @Value("${casic.algorithm.initPath:-1}")
-    private String initPath;
-    @Value("${casic.algorithm.planPath:-1}")
-    private String planPath;
+    @Value("${casic.algorithm.path:-1}")
+    private String path;
+    @Value("${casic.algorithm.initPathName:-1}")
+    private String initPathName;
+    @Value("${casic.algorithm.planPathName:-1}")
+    private String planPathName;
     private final ITaskHeatMapService heatMapService;
 
     public AlgorithmServiceImpl(ITaskHeatMapService heatMapService) {
@@ -42,13 +44,13 @@
     @Transactional
     @Override
     public AlgorithmResponse pathPlanningInit(PathInitDTO initDTO) {
-        if (StrUtil.isEmpty(initDTO.getCount())) {
-            initDTO.setCount("0.00");
+        if (StrUtil.isEmpty(initDTO.getZNow())) {
+            initDTO.setZNow("0.00");
         }
         log.info(JSON.toJSONString(initDTO));
         //算法路径及入参配置
         List<String> commends = new ArrayList<>();
-        commends.add(initPath);
+        commends.add(path + initPathName);
         commends.addAll(initDTO.toArray());
         List<String> res = AlgorithmUtils.invokeJob(commends);
 
@@ -63,38 +65,72 @@
     @Transactional
     @Override
     public AlgorithmResponse pathPlanning(PathPlanDTO planDTO) {
-        if (StrUtil.isEmpty(planDTO.getCount())) {
-            planDTO.setCount("0.001");
+        if (StrUtil.isEmpty(planDTO.getZNow())) {
+            planDTO.setZNow("0.001");
         }
         //算法路径及入参配置
         List<String> commends = new ArrayList<>();
-        commends.add(planPath);
+        commends.add(path + planPathName);
         commends.addAll(planDTO.toArray());
         log.info("寻源算法调用:{}", JSON.toJSONString(planDTO));
         List<String> res = AlgorithmUtils.invokeJob(commends);
         AlgorithmResponse response = getAlgorithmResponse(planDTO, res);
-        //next 点位编写
-        if (CollectionUtil.isNotEmpty(res) && res.size() == 2) {
-            response.getPointNextDTO().setX(formatBigDecimal(res.get(0).replaceAll("next_X: ", "")));
-            response.getPointNextDTO().setY(formatBigDecimal(res.get(1).replaceAll("next_Y: ", "")));
-        }
+
         return response;
     }
 
     private AlgorithmResponse getAlgorithmResponse(PathInitDTO initDTO, List<String> res) {
-        File repetitionFile = new File("repetition.txt");
+        //读取当前路径图片
+        File repetitionFileDir = new File(path + "\\" + initDTO.getTaskId() + "\\predicts");
+        File[] files = repetitionFileDir.listFiles((dir, name) -> name.endsWith(".txt") && name.startsWith("step"));
+        File repetitionFile = null;
+        //判定当前执行路径最大step2文件夹
+        if (files != null && files.length > 0) {
+             repetitionFile = files[files.length - 1];
+        }
         AlgorithmResponse response = new AlgorithmResponse();
         if (initDTO.getTaskId() != null && repetitionFile.exists()) {
-            readAlgorithmResponse("repetition.txt", response, initDTO.getTaskId(), res);
-            readAlgorithmResponse("step.txt", response, initDTO.getTaskId(), res);
+            //读取热力图
+            readAlgorithmResponse("repetition.txt",repetitionFile, response, initDTO.getTaskId(), res);
         } else {
             log.info("taskId is empty!");
         }
-        readAlgorithmResponse("xy_train.txt", response, initDTO.getTaskId(), res);
-        readAlgorithmResponse("z_train.txt", response, initDTO.getTaskId(), res);
+        //读取规划路径
+        if (CollectionUtil.isNotEmpty(res)) {
+            response.setEnd(false);
+            readAlgorithmResponse(response, res);
+        }
         return response;
     }
 
+    private void readAlgorithmResponse(AlgorithmResponse response, List<String> res) {
+        List<String> resList = new ArrayList<>();
+        if (res.size() >= 2) {
+            resList = res.subList(res.size() - 2, res.size());
+        }
+        //判定是否结束寻源
+        if (res.contains("end")) {
+            response.setEnd(true);
+            for (String s : resList) {
+                if (s.startsWith("source_coordinate:")) {
+                    s = s.trim();
+                    log.info("寻源结束,目标点位：{}",s);
+                    List<String> lists = StrUtil.split(s.replaceAll("source_coordinate:", ""), ",");
+                    PointNextDTO nextDTO = new PointNextDTO();
+                    nextDTO.setX(lists.get(0));
+                    nextDTO.setY(lists.get(1));
+                    response.setCoordinate(nextDTO);
+                }
+            }
+        } else {
+            PointNextDTO nextDTO = new PointNextDTO();
+            nextDTO.setX(resList.get(0));
+            nextDTO.setY(resList.get(1));
+            response.setPointNextDTO(nextDTO);
+        }
+
+    }
+
     /**
      * 读取算法输出数据
      *
@@ -102,49 +138,18 @@
      * @param response 结果集
      * @param taskId   任务ID
      */
-    public void readAlgorithmResponse(String fileName, AlgorithmResponse response, Long taskId, List<String> res) {
-        File file = new File(fileName);
-        if (!file.exists()&&!fileName.startsWith("step")) {
+    public void readAlgorithmResponse(String fileName,File repetitionFile, AlgorithmResponse response, Long taskId, List<String> res) {
+        if (!repetitionFile.exists() && !fileName.startsWith("step")) {
             log.info("{} file is empty!", fileName);
             return;
         }
         if (response.getPointNextDTO() == null) {
             response.setPointNextDTO(new PointNextDTO());
         }
-        switch (fileName) {
-            case "repetition.txt":
-                String msg = FileUtil.readString(file, CharsetUtil.CHARSET_UTF_8);
-                TaskHeatMap taskHeatMap = createEmptyTaskHeatMap(taskId);
-                taskHeatMap.setRepetition(msg);
-                response.setTaskHeatMap(taskHeatMap);
-                break;
-            case "xy_train.txt":
-                List<String> xyTrains = FileUtil.readLines(file, CharsetUtil.CHARSET_UTF_8);
-                if (CollectionUtil.isNotEmpty(xyTrains)) {
-                    if (CollectionUtil.isNotEmpty(xyTrains)) {
-                        List<String> list = StrUtil.split(xyTrains.get(xyTrains.size() - 1), " ");
-                        response.getPointNextDTO().setX(formatBigDecimal(list.get(0)));
-                        response.getPointNextDTO().setY(formatBigDecimal(list.get(1)));
-                    }
-                }
-                break;
-            case "z_train.txt":
-                List<String> zTrains = FileUtil.readLines(file, CharsetUtil.CHARSET_UTF_8);
-                if (CollectionUtil.isNotEmpty(zTrains)) {
-                    String zTrain = zTrains.get(zTrains.size() - 1);
-                    response.getPointNextDTO().setZ(formatBigDecimal(zTrain));
-                }
-            default:
-                if (StrUtil.isNotEmpty(fileName) && fileName.startsWith("step")) {
-                    File fileDir = new File(".");
-                    File[] files = fileDir.listFiles((dir, name) -> name.endsWith(".txt") && name.startsWith("step"));
-                    if (files != null && files.length > 0) {
-                        String steps = FileUtil.readString(files[files.length - 1], CharsetUtil.CHARSET_UTF_8);
-                        response.getTaskHeatMap().setRepetition(steps);
-                    }
-                }
-                break;
-        }
+        String msg = FileUtil.readString(repetitionFile, CharsetUtil.CHARSET_UTF_8);
+        TaskHeatMap taskHeatMap = createEmptyTaskHeatMap(taskId);
+        taskHeatMap.setRepetition(msg);
+        response.setTaskHeatMap(taskHeatMap);
     }
 
     /**
@@ -157,13 +162,11 @@
     public GridPointDTO convertSlamToGrid(BigDecimal x, BigDecimal y, BigDecimal resolution, RobotInfo robotInfo) {
         GridPointDTO pointDTO = new GridPointDTO();
         BigDecimal scale = resolution;
-
         //减去栅格原点坐标
         x = x.subtract(robotInfo.getOriginX());
         y = y.subtract(robotInfo.getOriginY());
-
-        pointDTO.setX(new BigDecimal(x.divide(scale,3,BigDecimal.ROUND_HALF_UP).intValue()+1));
-        pointDTO.setY(new BigDecimal(y.divide(scale,3,BigDecimal.ROUND_HALF_UP).intValue()+1));
+        pointDTO.setX(new BigDecimal(x.divide(scale, 3, BigDecimal.ROUND_HALF_UP).intValue()+1));
+        pointDTO.setY(new BigDecimal(y.divide(scale, 3, BigDecimal.ROUND_HALF_UP).intValue() +1));
         return pointDTO;
     }
 
@@ -174,19 +177,17 @@
      * @return
      */
     public GridPointDTO convertGridCenterPoint(GridPointDTO pointDTO, BigDecimal resolution, RobotInfo robotInfo) {
-
         GridPointDTO centerPoint = new GridPointDTO();
         BigDecimal scale = resolution;
-        //点位转换
-        BigDecimal slamX = pointDTO.getX().divide(scale, 5, BigDecimal.ROUND_HALF_UP);
-        BigDecimal slamY = pointDTO.getY().divide(scale, 5, BigDecimal.ROUND_HALF_UP);
+        //点位转换 转换后栅格－1 从1开始计算
+        BigDecimal slamX = (pointDTO.getX().subtract(new BigDecimal(1))).multiply(scale);
+        BigDecimal slamY = (pointDTO.getY().subtract(new BigDecimal(1))).multiply(scale);
         //中心栅格点位中心添加
-        centerPoint.setX(slamX.add(scale.divide(new BigDecimal(2), 5, BigDecimal.ROUND_HALF_UP)));
-        centerPoint.setY(slamY.add(scale.divide(new BigDecimal(2), 5, BigDecimal.ROUND_HALF_UP)));
+        centerPoint.setX(slamX.add(scale.divide(new BigDecimal(2), 10, BigDecimal.ROUND_HALF_UP)));
+        centerPoint.setY(slamY.add(scale.divide(new BigDecimal(2), 10, BigDecimal.ROUND_HALF_UP)));
         //加上栅格原点坐标
         centerPoint.setX(centerPoint.getX().add(robotInfo.getOriginX()));
         centerPoint.setY(centerPoint.getY().add(robotInfo.getOriginY()));
-
         return centerPoint;
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotInfoServiceImpl.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotInfoServiceImpl.java
index 043e05d..fdded64 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotInfoServiceImpl.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotInfoServiceImpl.java
@@ -104,7 +104,7 @@
             //下发障碍物栅格
             sendCostMapResolution(robotInfo.getResolution(), robotInfo.getId());
             //重新生成栅格地图
-            sendChangePcd(robotInfo.getId());
+            //sendChangePcd(robotInfo.getId());
         }
         update.eq(RobotInfo::getId, robotInfo.getId());
         update(update);
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotStatusInfoServiceImpl.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotStatusInfoServiceImpl.java
index 6d3ae91..46b5c3b 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotStatusInfoServiceImpl.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotStatusInfoServiceImpl.java
@@ -15,6 +15,7 @@
 import com.casic.missiles.modular.robot.dao.RobotStatusInfoMapper;
 import com.casic.missiles.modular.robot.model.RobotInfo;
 import com.casic.missiles.modular.robot.model.RobotStatusInfo;
+import com.casic.missiles.modular.robot.opt.enums.InstructCodeEnums;
 import com.casic.missiles.modular.robot.opt.instruct.base.dto.RobotMsgDTO;
 import com.casic.missiles.modular.robot.opt.instruct.base.dto.RobotMsgResponseDTO;
 import com.casic.missiles.modular.robot.opt.instruct.base.dto.RobotResponseDTO;
@@ -26,13 +27,18 @@
 import com.casic.missiles.modular.robot.service.IRobotInfoService;
 import com.casic.missiles.modular.robot.service.IRobotStatusInfoService;
 import lombok.extern.slf4j.Slf4j;
+import org.aspectj.util.FileUtil;
 import org.eclipse.paho.client.mqttv3.MqttMessage;
+import org.springframework.beans.factory.annotation.Value;
 import org.springframework.context.annotation.Lazy;
 import org.springframework.stereotype.Service;
 import org.springframework.transaction.annotation.Transactional;
 
+import java.io.File;
 import java.io.Serializable;
 import java.math.BigDecimal;
+import java.util.ArrayList;
+import java.util.Arrays;
 import java.util.List;
 
 
@@ -147,7 +153,6 @@
     @Override
     public void saveProcessMessage(String robotId, MqttMessage message) {
 //        String json = new String(message.getPayload());
-
         RobotResponseDTO<ProcessMessageResponseDTO> responseDTO = parseJson(message, new TypeReference<RobotResponseDTO<ProcessMessageResponseDTO>>() {
         });
         if (responseDTO.getMsg() != null) {
@@ -189,9 +194,8 @@
     }
 
     @Override
-    public void updateCurrTask(String robotId, Long currRouteId, Long currTaskId) {
+    public void updateCurrTask(String robotId, Long currTaskId) {
         UpdateWrapper<RobotStatusInfo> query = new UpdateWrapper<>();
-        query.set("curr_route_id", currRouteId);
         query.set("curr_task_id", currTaskId);
         query.eq("robot_id", robotId);
         update(query);
@@ -250,6 +254,15 @@
         }
     }
 
+    @Value("${casic.algorithm.path:-1}")
+    private String path;
+
+    /**
+     * 栅格阈值
+     */
+    @Value("${casic.algorithm.num:30}")
+    private Integer num;
+
     /**
      * 机器人栅格数据
      *
@@ -262,15 +275,41 @@
         RobotResponseDTO<AppCostmapResponseDTO> responseDTO = parseJson(message, new TypeReference<RobotResponseDTO<AppCostmapResponseDTO>>() {
         });
         if (responseDTO != null && responseDTO.getMsg() != null) {
+            //发送栅格分辨率设定
+
             cacheService.updateCostMap(robotId, responseDTO.getMsg());
             robotInfoService.updateRobotGrid(robotId, responseDTO.getMsg());
-//            log.info("--------------- json data:{}", JSON.toJSONString(responseDTO.getMsg()));
+            //log.info("--------------- json data:{}", JSON.toJSONString(responseDTO.getMsg()));
+            if (responseDTO != null && responseDTO.getMsg().getData() != null) {
+                ArrayList<Integer> dataNums = new ArrayList<>();
+                //根据阈值转换栅格
+                for (Integer dataNum : responseDTO.getMsg().getData()) {
+                    if (dataNum != -1) {
+                        if (dataNum >= num) {
+                            dataNums.add(0);
+                        } else {
+                            dataNums.add(1);
+                        }
+                    }
+                }
+                writeCache(dataNums,responseDTO.getMsg().getWidth(),responseDTO.getMsg().getHeight(),robotId);
+            }
         }
     }
 
+    private void writeCache(ArrayList<Integer> integers,Integer width,Integer height,String robotId) {
+        List<List<Integer>> arrays = CollectionUtil.split(integers,width);
+        ArrayList<ArrayList<Integer>> arrayList = new ArrayList<>();
+        for (List<Integer> array : arrays) {
+            arrayList.add((ArrayList)array);
+        }
+        CollectionUtil.reverse(arrayList);
+        CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_COST_MAP + robotId, arrayList);
+//        FileUtil.writeAsString(new File(path + "map.txt"), CollectionUtil.join(integers, " ").replaceAll("\\[", "").replaceAll("]", ""));
+    }
+
     @Override
     public void updateRobotPoint(PoseMessageResponseDTO dto, String robotId) {
-        //todo cache
         UpdateWrapper<RobotStatusInfo> query = new UpdateWrapper<>();
         query.set("postion_x", dto.getX());
         query.set("postion_y", dto.getY());
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotTrajectoryInfoServiceImpl.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotTrajectoryInfoServiceImpl.java
index 6c9625f..f1262a2 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotTrajectoryInfoServiceImpl.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotTrajectoryInfoServiceImpl.java
@@ -57,29 +57,29 @@
         });
 
         //读取导航状态,如果导航未开启则不存储
-//        RobotStatusInfo statusInfo = robotStatusInfoService.getStatusByRobotId(robotId);
-        Long routeId= cacheService.readRouteId(Long.valueOf(robotId));
-        if (routeId != null && responseDTO.getMsg() != null) {
+        //RobotStatusInfo statusInfo = robotStatusInfoService.getStatusByRobotId(robotId);
+
+        if ( responseDTO.getMsg() != null) {
             PoseMessageResponseDTO dto = responseDTO.getMsg();
             //执行循迹 保存当前轨迹信息
             //更新实时定位信息
             CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.ROBOT_POINT + robotId, dto);
-            infoService.pointTestCallBack(dto, robotId);
-            if (routeId != null) {
-                RobotTrajectoryInfo info = new RobotTrajectoryInfo();
-                info.setRobotId(Long.valueOf(robotId));
-                info.setCreateTime(new Date());
-                info.setPostionX(dto.getX());
-                info.setPostionY(dto.getY());
-                info.setPostionZ(dto.getZ());
-                info.setTheta(dto.getTheta());
-                save(info);
-                try {
-                    infoService.pointCallBack(dto, robotId);
-                } catch (Exception e) {
-                    log.error("task error", e);
-                }
+            try {
+                infoService.carPathPlanning(dto, robotId);
+            } catch (Exception e) {
+                log.error("task error", e);
             }
+//            if (routeId != null) {
+//                RobotTrajectoryInfo info = new RobotTrajectoryInfo();
+//                info.setRobotId(Long.valueOf(robotId));
+//                info.setCreateTime(new Date());
+//                info.setPostionX(dto.getX());
+//                info.setPostionY(dto.getY());
+//                info.setPostionZ(dto.getZ());
+//                info.setTheta(dto.getTheta());
+//                save(info);
+
+//            }
         }
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/TaskGridInfoServiceImpl.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/TaskGridInfoServiceImpl.java
new file mode 100644
index 0000000..cf61def
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/TaskGridInfoServiceImpl.java
@@ -0,0 +1,28 @@
+package com.casic.missiles.modular.robot.service.impl;
+
+import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
+import com.baomidou.mybatisplus.extension.plugins.pagination.Page;
+import com.baomidou.mybatisplus.extension.service.impl.ServiceImpl;
+import com.casic.missiles.modular.robot.dao.TaskGridInfoMapper;
+import com.casic.missiles.modular.robot.model.TaskGridInfo;
+import com.casic.missiles.modular.robot.service.ITaskGridInfoService;
+import org.springframework.stereotype.Service;
+
+import java.util.List;
+
+
+/**
+ * <p>
+ * 任务网格信息 服务实现类
+ * </p>
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+@Service
+public class TaskGridInfoServiceImpl extends ServiceImpl<TaskGridInfoMapper, TaskGridInfo> implements ITaskGridInfoService {
+    @Override
+    public List<TaskGridInfo> selectTaskGridInfoPage(Page<TaskGridInfo> page, QueryWrapper<TaskGridInfo> query) {
+        return this.baseMapper.selectTaskGridInfoPage(page, query);
+    }
+}
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/TaskInfoServiceImpl.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/TaskInfoServiceImpl.java
index d1417b0..fd49e2e 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/TaskInfoServiceImpl.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/TaskInfoServiceImpl.java
@@ -6,10 +6,10 @@
 import cn.hutool.core.convert.Convert;
 import cn.hutool.core.date.DateUtil;
 import cn.hutool.core.io.FileUtil;
-import cn.hutool.core.thread.ExecutorBuilder;
 import cn.hutool.core.util.StrUtil;
 import com.alibaba.fastjson.JSON;
 import com.baomidou.mybatisplus.core.conditions.Wrapper;
+import com.baomidou.mybatisplus.core.conditions.query.LambdaQueryWrapper;
 import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
 import com.baomidou.mybatisplus.core.conditions.update.UpdateWrapper;
 import com.baomidou.mybatisplus.core.metadata.IPage;
@@ -47,7 +47,6 @@
 import java.io.Serializable;
 import java.math.BigDecimal;
 import java.util.*;
-import java.util.concurrent.ThreadPoolExecutor;
 
 
 /**
@@ -70,6 +69,7 @@
     private final IRobotCacheService cacheService;
     private final IBaseRobotService baseRobotService;
     private final ITaskInfoImgService imgService;
+    private final ITaskGridInfoService taskGridInfoService;
     /**
      * 初始化线程池
      */
@@ -79,12 +79,19 @@
     @Value("${casic.doc.temp}")
     private String docTemp;
     /**
+     * 5000 ms
+     */
+    @Value("${casic.task.duration:3000}")
+    private Integer stayDuration;
+    @Value("${casic.algorithm.path:-1}")
+    private String path;
+    /**
      * 障碍物精度
      */
     @Value("${casic.obstacle:10}")
     private Integer obstacle;
 
-    public TaskInfoServiceImpl(@Lazy IRouteInfoService routeInfoService, IRobotStatusInfoService statusInfoService, AbstractDictService dictService, @Lazy IAlarmRecordService recordService, @Lazy ITaskHeatMapService taskHeatMapService, IAlgorithmService algorithmService, IRobotCacheService cacheService, @Lazy IBaseRobotService baseRobotService, ITaskInfoImgService imgService) {
+    public TaskInfoServiceImpl(@Lazy IRouteInfoService routeInfoService, IRobotStatusInfoService statusInfoService, AbstractDictService dictService, @Lazy IAlarmRecordService recordService, @Lazy ITaskHeatMapService taskHeatMapService, IAlgorithmService algorithmService, IRobotCacheService cacheService, @Lazy IBaseRobotService baseRobotService, ITaskInfoImgService imgService, ITaskGridInfoService taskGridInfoService) {
         this.routeInfoService = routeInfoService;
         this.statusInfoService = statusInfoService;
         this.dictService = dictService;
@@ -94,6 +101,7 @@
         this.cacheService = cacheService;
         this.baseRobotService = baseRobotService;
         this.imgService = imgService;
+        this.taskGridInfoService = taskGridInfoService;
     }
 
     /**
@@ -184,7 +192,7 @@
         if (CollectionUtil.isEmpty(routeInfos)) {
             throw new ServiceException(500, "路线不存在,请及时更新");
         }
-        RouteInfo routeInfo = routeInfos.get(0);
+//        RouteInfo routeInfo = routeInfos.get(0);
         Integer isTask = 0;
         //开启寻源任务
         if (request.getIsTask() == 1) {
@@ -193,22 +201,21 @@
         RobotInfo robotInfo = cacheService.getRobotInfo(Convert.toLong(robotId));
         CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_IS_TASK + robotId, isTask);
         //添加任务记录
-        TaskInfo taskInfo = addTaskByStartTrack(routeInfo, robotInfo);
+        TaskInfo taskInfo = addTaskByStartTrack( robotInfo, request);
         //No.3 更新器人执行任务信息
-        Long currRouteId = routeInfo.getId();
+//        Long currRouteId = routeInfo.getId();
         Long currTaskId = taskInfo.getId();
 
-        statusInfoService.updateCurrTask(robotId, currRouteId, currTaskId);
+        statusInfoService.updateCurrTask(robotId,  currTaskId);
         //算法初始化标志创建
         CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_TASK_ID + robotId, currTaskId);
         CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_TASK_INIT + robotId, 0);
-        CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_ROUTE_ID + robotId, routeInfo.getId());
+//        CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_ROUTE_ID + robotId, routeInfo.getId());
     }
 
     private void closeTask(StartTrackRequest request, String robotId) {
         //关闭操作
         RobotStatusInfo statusInfo = statusInfoService.getStatusByRobotId(robotId);
-
         if (statusInfo.getCurrTaskId() != null) {
             UpdateWrapper<TaskInfo> updateWrapper = new UpdateWrapper<>();
             updateWrapper.set("update_time", new Date());
@@ -221,10 +228,11 @@
 
         //清空任务缓存
         robotCacheClean(robotId);
-
+        //清楚规划任务锁
+        CacheUtil.remove(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_PLANING_FLAG + robotId);
+        CacheUtil.remove(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_ESTIMATE_GRID+ robotId);
         //清空机器当前任务
-        statusInfoService.updateCurrTask(robotId, null, null);
-
+        statusInfoService.updateCurrTask(robotId, null);
     }
 
     /**
@@ -249,7 +257,6 @@
 
         //判定任务是否已经完成初始化
         Long longId = Convert.toLong(robotId);
-        Long taskId = CacheUtil.get(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_TASK_ID + robotId);
         //缓存当前点位信息
         CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_XY_POINT + robotId, dto);
         RobotInfo robotInfo = cacheService.getRobotInfo(longId);
@@ -260,86 +267,203 @@
             log.error("no config 中子源阈值未配置");
             return;
         }
-
         //读取是否是循迹任务
         Integer isTask = cacheService.readIsTask(longId);
         if (isTask != 1) {
             log.info("循迹任务,不做处理");
             return;
         }
-        //算法初始化状态读取
-        Integer initValue = cacheService.readTaskInit(longId);
-
 
         //No.1 任务初始化
+//        pathInit(longId, robotInfo, pointDTO, taskId, robotId);
+    }
+
+    @Override
+    public void carPathPlanning(PoseMessageResponseDTO dto, String robotId) {
+        //判定任务是否已经完成初始化
+        Long longId = Convert.toLong(robotId);
+        Long taskId = CacheUtil.get(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_TASK_ID + robotId);
+        //缓存当前点位信息
+        CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_XY_POINT + robotId, dto);
+        RobotInfo robotInfo = cacheService.getRobotInfo(longId);
+        //slam经纬度转栅格
+        GridPointDTO pointDTO = algorithmService.convertSlamToGrid(dto.getX(), dto.getY(), robotInfo.getResolution(), robotInfo);
+        if (robotInfo.getAlarmThreshold() == null || robotInfo.getMaxThreshold() == null) {
+            log.error("no config 中子源阈值未配置");
+            return;
+        }
+
+        //读取是否是寻源任务
+        Integer isTask = cacheService.readIsTask(longId);
+        if (isTask != 1) {
+            log.info("循迹任务,不做处理");
+            return;
+        }
+
+        TaskInfo taskInfo = getById(taskId);
+        //No.1 任务初始化
+        pathInit(longId, robotInfo, pointDTO, taskId, robotId, taskInfo.getStepLen());
+        //No.2 路线规划
+        pathPlaning(longId, robotInfo, pointDTO, taskId, robotId, taskInfo,dto);
+    }
+
+    /**
+     * 路线规划进行加锁
+     *
+     * @param robotId 机器人ID
+     * @return
+     */
+    private synchronized boolean lockPlanIngFlag(String robotId) {
+        Boolean flag = CacheUtil.get(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_PLANING_FLAG + robotId);
+        if (flag == null) {
+            CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_PLANING_FLAG + robotId, true);
+            return true;
+        }
+        return false;
+    }
+
+    /**
+     * @param longId    机器人ID
+     * @param robotInfo 机器人信息
+     * @param pointDTO  当前点位栅格信息
+     * @param taskId    任务ID
+     * @param robotId   机器人ID
+     */
+    private void pathPlaning(Long longId, RobotInfo robotInfo, GridPointDTO pointDTO, Long taskId, String robotId, TaskInfo taskInfo,PoseMessageResponseDTO curPoint) {
+        if (taskId != null) {
+            //规划节点
+            PointNextDTO pointNextDTO = CacheUtil.get(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_ESTIMATE_GRID + robotId);
+            GridPointDTO oldEstimateGrid = GridPointDTO.convertGridPoint(pointNextDTO);
+            Boolean flag = lockPlanIngFlag(robotId);
+            //是否到达预期栅格
+            if (!flag) {
+                log.info("未获取到规划任务,直接结束：{}",pointDTO);
+                return;
+            }
+            try {
+                log.debug("oldEstimateGrid:{}", oldEstimateGrid);
+                if (pointNextDTO == null || isEquals(pointDTO, oldEstimateGrid)) {
+                    log.info("当前栅格变更......", pointDTO);
+                    CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_POINT + robotId, pointDTO);
+                    //算法调用获取热力图
+                    PathPlanDTO pathPlanDTO = createPlanDTO(robotInfo, pointDTO, taskId, robotId, taskInfo.getStepLen());
+                    if (taskInfo.getMaxThreshold() == null) {
+                        taskInfo.setMaxThreshold(robotInfo.getMaxThreshold());
+                    }
+                    pathPlanDTO.setTh(taskInfo.getMaxThreshold() + "");
+
+                    log.debug("heat map -- robotId:{},taskId:{}", robotId, taskId);
+                    pathPlanDTO.setZNow(cacheService.getNeutronCount(longId));
+
+                    //存储算法输出栅格信息
+                    AlgorithmResponse response = algorithmService.pathPlanning(pathPlanDTO);
+                    CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_ESTIMATE_GRID + robotId, response.getPointNextDTO());
+                    sendWebSocket(InstructStatusMsgEnums.HEAT_MAP, robotId, response.getTaskHeatMap());
+
+                    //存储循迹任务信息
+                    TaskGridInfo taskGridInfo = new TaskGridInfo();
+                    taskGridInfo.setCreateTime(new Date());
+                    taskGridInfo.setUpdateTime(new Date());
+                    taskGridInfo.setCurGridX(pointDTO.getX().intValue());
+                    taskGridInfo.setCurGridY(pointDTO.getY().intValue());
+                    taskGridInfo.setNeutronCount(new BigDecimal(pathPlanDTO.getZNow()));
+                    taskGridInfo.setEstimateGridX(response.getPointNextDTO().getX());
+                    taskGridInfo.setEstimateGridY(response.getPointNextDTO().getY());
+                    taskGridInfo.setTaskId(taskId);
+                    taskGridInfoService.save(taskGridInfo);
+
+                    try {
+                        //停留时间阈值
+                        Thread.sleep(stayDuration);
+                    } catch (InterruptedException e) {
+                        e.printStackTrace();
+                    }
+
+                    //是否触发停止状态
+                    if (!response.isEnd()) {
+                        log.debug("task planning-- xun dian robotId:{},taskId:{}", robotId, taskId);
+                        //机器人寻点指令下发
+                        sendTargetPoint(robotInfo, response,curPoint);
+                    } else {
+                        log.debug("task stop-- robotId:{},taskId:{}", robotId, taskId);
+                        //机器人停止指令下发
+                        SoftwareStopRequest request = new SoftwareStopRequest();
+                        request.setRobotId(robotInfo.getId());
+                        //设置为急停
+                        request.setControl(1);
+//                        baseRobotService.execCmdHandler(request, InstructCodeEnums.SOFTWARE_STOP);
+                        //循迹任务结束
+                        closeTask(null, robotId);
+                    }
+                }
+            } catch (Exception e) {
+                e.printStackTrace();
+            } finally {
+                //释放锁
+                CacheUtil.remove(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_PLANING_FLAG + robotId);
+            }
+        }
+    }
+
+    private PathPlanDTO createPlanDTO(RobotInfo robotInfo, GridPointDTO pointDTO, Long taskId, String robotId, Integer stepLen) {
+        PathPlanDTO pathPlanDTO = new PathPlanDTO();
+        pathPlanDTO.setTaskId(taskId);
+        pathPlanDTO.setRobotId(robotId);
+        pathPlanDTO.setStepLen(stepLen);
+        return pathPlanDTO;
+    }
+
+    /**
+     * 寻源初始化
+     *
+     * @param longId    机器ID
+     * @param robotInfo 机器信息
+     * @param pointDTO  当前点位信息
+     * @param taskId    任务ID
+     * @param robotId   机器ID
+     * @param stepLen   步长设置
+     */
+    private void pathInit(Long longId, RobotInfo robotInfo, GridPointDTO pointDTO, Long taskId, String robotId, Integer stepLen) {
+        //障碍物栅格上传已生成 直接读取
+        //算法初始化状态读取
+        Integer initValue = cacheService.readTaskInit(longId);
+        //No.1 任务初始化
         if (initValue != 1) {
+
             //执行初始化操作
             if (cacheService.updateTaskInit(longId, 1)) {
 
-                try {
-                    File file = new File(".");
-                    File[] files = file.listFiles((dir, name) -> (name.endsWith(".png") && name.startsWith("step"))
-                            || (name.endsWith(".txt") && name.startsWith("step")) || name.equals("repetition.txt") || name.equals("xy_train.txt") || name.equals("z_train.txt"));
-                    if (files != null) {
-                        for (int i = 0; i < files.length; i++) {
-                            FileUtil.del(files[i]);
+                List<List<Integer>> costList = CacheUtil.get(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_COST_MAP + robotId);
+                List<String> strs = new ArrayList<>();
+                List<String> strs2 = new ArrayList<>();
+                for (List<Integer> list : costList) {
+                    strs2.add(CollectionUtil.join(list, " "));
+                }
+                FileUtil.writeLines(strs2, new File(path + "map2.txt"), "utf-8");
+                //行转列 为算法输入数据
+                if(CollectionUtil.isNotEmpty(costList)){
+                    CollectionUtil.reverse(costList);
+                    for (int i = 0; i < costList.get(0).size(); i++) {
+                        ArrayList<Integer> ints = new ArrayList<>();
+                        for (List<Integer> list : costList) {
+                            ints.add(list.get(i));
                         }
+                        strs.add(CollectionUtil.join(ints, " "));
                     }
-                } catch (Exception e) {
-                    e.printStackTrace();
+//                    CollectionUtil.reverse(strs);
                 }
 
-                PathInitDTO pathInitDTO = createInitDTO(robotInfo, pointDTO, taskId, robotId);
+
+                FileUtil.writeLines(strs, new File(path + "map.txt"), "utf-8");
+
+                PathInitDTO pathInitDTO = createInitDTO(robotInfo, pointDTO, taskId, robotId, stepLen);
                 //中子源计数率读取
-                pathInitDTO.setCount(cacheService.getNeutronCount(longId));
+                pathInitDTO.setZNow(cacheService.getNeutronCount(longId));
+                log.info("init params:{}", JSON.toJSONString(pathInitDTO));
                 //执行1次算法 初始化操作
                 algorithmService.pathPlanningInit(pathInitDTO);
             }
         }
-
-        //No.2 任务执行中
-        if (taskId != null) {
-            //判断栅格变化
-            GridPointDTO oldPointDTO = CacheUtil.get(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_POINT + robotId);
-            if (oldPointDTO == null || isChange(pointDTO, oldPointDTO)) {
-                log.debug("grid change -- robotId:{},taskId:{}", robotId, taskId);
-                CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_POINT + robotId, pointDTO);
-                //算法调用获取热力图
-                PathPlanDTO pathPlanDTO = createPlanDTO(robotInfo, pointDTO, taskId, robotId);
-                //可通行标记为1 不可通行标记为0
-                String obstacle = getObstacle(robotId, pointDTO);
-                //TO-DO 障碍物判定信息
-                pathPlanDTO.setOpen(obstacle);
-                //中子源计数率读取
-                pathPlanDTO.setCount(cacheService.getNeutronCount(longId));
-                log.debug("heat map -- robotId:{},taskId:{}", robotId, taskId);
-                AlgorithmResponse response = algorithmService.pathPlanning(pathPlanDTO);
-                //存储算法输出栅格信息
-                CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_ESTIMATE_GRID + robotId, response.getPointNextDTO());
-                sendWebSocket(InstructStatusMsgEnums.HEAT_MAP, robotId, response.getTaskHeatMap());
-                //是否触发寻源操作
-                BigDecimal bigCount = new BigDecimal(pathPlanDTO.getCount());
-                if (bigCount.compareTo(robotInfo.getAlarmThreshold()) >= 0) {
-                    log.debug("task planning-- xun dian robotId:{},taskId:{}", robotId, taskId);
-                    //机器人寻点指令下发
-                    sendTargetPoint(robotInfo, response);
-                }
-
-                //是否触发停止状态
-                if (bigCount.compareTo(robotInfo.getMaxThreshold()) > 0) {
-                    log.debug("task stop-- robotId:{},taskId:{}", robotId, taskId);
-                    //机器人停止指令下发
-                    SoftwareStopRequest request = new SoftwareStopRequest();
-                    request.setRobotId(robotInfo.getId());
-                    //设置为急停
-                    request.setControl(1);
-                    baseRobotService.execCmdHandler(request, InstructCodeEnums.SOFTWARE_STOP);
-                    //循迹任务结束
-                    closeTask(null, robotId);
-                }
-
-            }
-        }
     }
 
     /**
@@ -353,7 +477,6 @@
         //判定任务是否已经完成初始化
         Long longId = Convert.toLong(robotId);
         Long taskId = CacheUtil.get(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_TASK_ID + robotId);
-
         RobotInfo robotInfo = cacheService.getRobotInfo(longId);
         //slam经纬度转栅格
         GridPointDTO pointDTO = algorithmService.convertSlamToGrid(dto.getX(), dto.getY(), robotInfo.getResolution(), robotInfo);
@@ -428,7 +551,7 @@
      * @param robotInfo 机器人信息
      * @param response  算法输出数据
      */
-    private void sendTargetPoint(RobotInfo robotInfo, AlgorithmResponse response) {
+    private void sendTargetPoint(RobotInfo robotInfo, AlgorithmResponse response,PoseMessageResponseDTO curPoint) {
         //读取已设置巡点目标信息
         PointNextDTO nextDTO = CacheUtil.get(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_TARGET_POINT_ID + robotInfo.getId());
         //巡点
@@ -439,8 +562,8 @@
             TargetPointRequest request = new TargetPointRequest();
             request.setX(Convert.toStr(gridCenterPoint.getX()));
             request.setY(Convert.toStr(gridCenterPoint.getY()));
-            request.setZ("0");
-
+            request.setZ(curPoint.getZ().toString());
+            request.setTheta(curPoint.getTheta().toString());
             request.setRobotId(robotInfo.getId());
             //执行巡点指令
             baseRobotService.execCmdHandler(request, InstructCodeEnums.SET_TARGETPOINT);
@@ -507,21 +630,20 @@
         params.put("startTime", startTime);
         params.put("endTime", endTime);
         //读取
-        List<AlarmRecord> records = recordService.selectListByTaskId(info.getId(), info.getRobotId());
         Integer index = 1;
-
-        if (CollectionUtil.isNotEmpty(records)) {
-            for (AlarmRecord record : records) {
+        LambdaQueryWrapper<TaskGridInfo> taskGridQuery = new LambdaQueryWrapper<>();
+        taskGridQuery.eq(TaskGridInfo::getTaskId,info.getId());
+        taskGridQuery.orderByAsc(TaskGridInfo::getCreateTime);
+        List<TaskGridInfo> taskGridInfos = taskGridInfoService.list(taskGridQuery);
+        if (CollectionUtil.isNotEmpty(taskGridInfos)) {
+            for (TaskGridInfo record : taskGridInfos) {
                 //栅格信息转换
-                GridPointDTO pointDTO = algorithmService.convertSlamToGrid(record.getX(), record.getY(), info.getResolution(), robotInfo);
-                record.setX(pointDTO.getX());
-                record.setY(pointDTO.getY());
                 record.setIndex(index);
                 index++;
             }
         }
 
-        params.put("records", records);
+        params.put("records", taskGridInfos);
         ImageEntity mcaImg = new ImageEntity();
         mcaImg.setHeight(600);
         mcaImg.setWidth(600);
@@ -557,7 +679,6 @@
             psdImg.setData(WordUtil.getImageBase64(uploadPath + info.getPsdImg()));
         }
         params.put("psdImg", psdImg);
-
         WordUtil.exportOutputStream(docTemp, response.getOutputStream(), params);
     }
 
@@ -589,7 +710,7 @@
     private String saveHeatImg(String taskId) {
         String newFileName = "task\\" + taskId + ".png";
         //读取当前路径图片
-        File file = new File(".");
+        File file = new File(path + "\\" + taskId + "\\figs");
         File[] files = file.listFiles((dir, name) -> name.endsWith(".png") && name.startsWith("step"));
         //判定当前执行路径最大step图片
         if (files != null && files.length > 0) {
@@ -614,33 +735,28 @@
         return true;
     }
 
-    private PathPlanDTO createPlanDTO(RobotInfo robotInfo, GridPointDTO pointDTO, Long taskId, String robotId) {
-        PathPlanDTO pathPlanDTO = new PathPlanDTO();
-        String xMax = robotInfo.getGridHeight().divide(robotInfo.getResolution(), 5, BigDecimal.ROUND_HALF_UP).intValue() + "";
-        String yMax = robotInfo.getGridWidth().divide(robotInfo.getResolution(), 5, BigDecimal.ROUND_HALF_UP).intValue() + "";
-        pathPlanDTO.setX(pointDTO.getX().intValue() + "");
-        pathPlanDTO.setY(pointDTO.getY().intValue() + "");
-        pathPlanDTO.setXMax(xMax);
-        pathPlanDTO.setYMax(yMax);
-        pathPlanDTO.setTaskId(taskId);
-        pathPlanDTO.setRobotId(robotId);
-        return pathPlanDTO;
+    private boolean isEquals(GridPointDTO pointDTO, GridPointDTO oldPointDTO) {
+        if(oldPointDTO==null||oldPointDTO.getX()==null){
+            return false;
+        }
+        if (pointDTO.getX().compareTo(oldPointDTO.getX()) == 0 && pointDTO.getY().compareTo(oldPointDTO.getY()) == 0) {
+            return true;
+        }
+        return false;
     }
 
-    private PathInitDTO createInitDTO(RobotInfo robotInfo, GridPointDTO pointDTO, Long taskId, String robotId) {
-        String xMax = robotInfo.getGridHeight().divide(robotInfo.getResolution(), 5, BigDecimal.ROUND_HALF_UP).intValue() + "";
-        String yMax = robotInfo.getGridWidth().divide(robotInfo.getResolution(), 5, BigDecimal.ROUND_HALF_UP).intValue() + "";
+
+    private PathInitDTO createInitDTO(RobotInfo robotInfo, GridPointDTO pointDTO, Long taskId, String robotId, Integer stepLen) {
         PathInitDTO pathInitDTO = new PathInitDTO();
-        pathInitDTO.setXMax(xMax);
-        pathInitDTO.setYMax(yMax);
-        pathInitDTO.setX(pointDTO.getX().intValue() + "");
-        pathInitDTO.setY(pointDTO.getY().intValue() + "");
+        pathInitDTO.setXNow(pointDTO.getX().intValue() + "");
+        pathInitDTO.setYNow(pointDTO.getY().intValue() + "");
+        pathInitDTO.setStepLen(stepLen);
         pathInitDTO.setTaskId(taskId);
         pathInitDTO.setRobotId(robotId);
         return pathInitDTO;
     }
 
-    private TaskInfo addTaskByStartTrack(RouteInfo routeInfo, RobotInfo robotInfo) {
+    private TaskInfo addTaskByStartTrack( RobotInfo robotInfo, StartTrackRequest request) {
         //No.2 新增任务信息
         TaskInfo taskInfo = new TaskInfo();
         String currDate = DateUtil.format(new Date(), "yyyyMMdd");
@@ -654,16 +770,20 @@
         taskInfo.setCreateTime(new Date());
         //任务类型 中子源
         taskInfo.setTaskType(1);
-        taskInfo.setDiscernType(1);
-        taskInfo.setTaskName(routeInfo.getRouteName() + "巡线任务");
+        taskInfo.setDiscernType(2);
+        taskInfo.setTaskName("自主巡检任务");
         //执行状态 未完成
         taskInfo.setTaskStatus(0);
         taskInfo.setStartTime(new Date());
-        taskInfo.setRouteId(routeInfo.getId());
+        taskInfo.setRouteId(null);
         taskInfo.setRobotId(robotInfo.getId());
         taskInfo.setGridHeight(robotInfo.getGridHeight());
         taskInfo.setGridWidth(robotInfo.getGridWidth());
         taskInfo.setResolution(robotInfo.getResolution());
+
+        taskInfo.setStepLen(request.getStepLen());
+        taskInfo.setMaxThreshold(request.getMaxThreshold());
+
         save(taskInfo);
         return taskInfo;
     }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/utils/AlgorithmUtils.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/utils/AlgorithmUtils.java
index 22864d1..467b9c2 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/utils/AlgorithmUtils.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/utils/AlgorithmUtils.java
@@ -1,6 +1,7 @@
 package com.casic.missiles.modular.robot.utils;
 
 import java.io.BufferedReader;
+import java.io.File;
 import java.io.IOException;
 import java.io.InputStreamReader;
 import java.util.ArrayList;
@@ -16,11 +17,13 @@
     public static List<String> invokeJob(List<String> params) {
         List<String> res = new ArrayList<>();
         try {
-
             BufferedReader br = null;
             BufferedReader brError;
             String line = null;
-            Process p = new ProcessBuilder(params).start();
+            ProcessBuilder processBuilder =  new ProcessBuilder(params);
+            processBuilder.directory(new File("D:\\workspace\\server\\keti\\casic-robot-inspection\\casic-web\\src\\main\\resources\\config\\path"));
+            Process p = processBuilder.start();
+
             br = new BufferedReader(new InputStreamReader(p.getInputStream()));
             brError = new BufferedReader(new InputStreamReader(p.getErrorStream()));
             while ((line = br.readLine()) != null || (line = brError.readLine()) != null) {
@@ -37,33 +40,29 @@
     public static void main(String[] args) {
         String path = "D:\\workspace\\server\\keti\\casic-robot-inspection\\casic-web\\src\\main\\resources\\config\\path\\";
         //算法初始化调用
-        String exePath = path + "PathPlanning_init.exe";
+        String exePath = path + "init.exe";
 
         ArrayList<String> invocation = new ArrayList<String>();
         invocation.add(exePath);
-        invocation.add("40");
-        invocation.add("20");
-        invocation.add("0.5");
+        invocation.add("10");
+        invocation.add("10");
         invocation.add("5");
-        invocation.add("5");
+        invocation.add("0.16");
+        invocation.add("Mission1");
         invokeJob(invocation);
 
         //{"count":"null","robotId":"1","taskId":"1748224487696142338","x":"8","xMax":"100","y":"2","yMax":"100"}
-        String planning = path + "PathPlanning.exe";
         //{"open":"0,0,0,0,0,0,0,0","robotId":"1","taskId":"1748250669749735425","x":"6","xMax":"100","y":"2","yMax":"100"}
         //{"open":"1,1,1,1,1,1,1,1","robotId":"1","taskId":"1748252894098178049","x":"6","xMax":"100","y":"2","yMax":"100"}
         //{"count":"6.0","open":"1,1,1,1,1,1,1,1","robotId":"1","taskId":"1750713404601159681","x":"6","xMax":"20","y":"2","yMax":"40"}
+        String planning = path + "search.exe";
         ArrayList<String> planInvocation = new ArrayList<String>();
         planInvocation.add(planning);
-        planInvocation.add("40");
-        planInvocation.add("40");
-        planInvocation.add("1,1,0,1,1,1,1,1");
-        planInvocation.add("6.0");
-        //next_X
-        planInvocation.add("6");
-        //next_Y
-        planInvocation.add("2");
-        ProcessBuilder processBuilder = new ProcessBuilder();
-        invokeJob(planInvocation);
+        planInvocation.add("10");
+        planInvocation.add("1.0");
+        planInvocation.add("0.2");
+        planInvocation.add("Mission1");
+        List<String> res = invokeJob(planInvocation);
+        System.out.println(res);
     }
 }
\ No newline at end of file
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/vo/TaskGridInfoVO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/vo/TaskGridInfoVO.java
new file mode 100644
index 0000000..0aff5bf
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/vo/TaskGridInfoVO.java
@@ -0,0 +1,58 @@
+package com.casic.missiles.modular.robot.vo;
+
+import io.swagger.annotations.ApiModelProperty;
+import lombok.Data;
+import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
+import com.casic.missiles.modular.robot.model.TaskGridInfo;
+import cn.hutool.core.util.StrUtil;
+
+import java.math.BigDecimal;
+
+/**
+ * 任务网格信息对象VO
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+@Data
+public class TaskGridInfoVO {
+    private static final long serialVersionUID = 1L;
+
+
+            @ApiModelProperty(value = "任务ID" , dataType = "Long")
+            private Long taskId;
+
+            @ApiModelProperty(value = "估算栅格x" , dataType = "Long")
+            private Long estimateGridX;
+
+            @ApiModelProperty(value = "估算栅格y" , dataType = "Long")
+            private Long estimateGridY;
+
+            @ApiModelProperty(value = "中子计数率" , dataType = "BigDecimal")
+            private BigDecimal neutronCount;
+
+            @ApiModelProperty(value = "当前栅格x" , dataType = "Long")
+            private Long curGridX;
+
+            @ApiModelProperty(value = "当前栅格y" , dataType = "Long")
+            private Long curGridY;
+
+public QueryWrapper<TaskGridInfo> genQuery() {
+QueryWrapper<TaskGridInfo> query = new QueryWrapper<>();
+if (taskId!=null) {
+        query.eq("task_id",taskId);
+}
+if (estimateGridX!=null) {
+        query.eq("estimate_grid_x",estimateGridX);
+}
+if (estimateGridY!=null) {
+        query.eq("estimate_grid_y",estimateGridY);
+}
+if (curGridX!=null) {
+        query.eq("cur_grid_x",curGridX);
+}
+if (curGridY!=null) {
+        query.eq("cur_grid_y",curGridY);
+}
+return query;
+}}
diff --git a/casic-web/src/main/resources/config/application.yml b/casic-web/src/main/resources/config/application.yml
index 04420fc..05c3633 100644
--- a/casic-web/src/main/resources/config/application.yml
+++ b/casic-web/src/main/resources/config/application.yml
@@ -42,11 +42,12 @@
   session-invalidate-time: 86400 #session失效时间(只在单机环境下生效，,多机环境在SpringSessionConfig类中配置) 单位：秒
   session-validation-interval: 900 #多久检测一次失效的session(只在单机环境下生效) 单位：秒
   no-login-urls: /user/login,/kaptcha,/config/baseConfig
-  hikSdk: D:\code\robot\casic-robot-inspection\casic-web\src\main\resources\config\hiklib\HCNetSDK.dll
-  hikPlay: D:\code\robot\casic-robot-inspection\casic-web\src\main\resources\config\hiklib\PlayCtrl.dll
+  hikSdk: D:\workspace\server\keti\casic-robot-inspection\casic-web\src\main\resources\config\hiklib\HCNetSDK.dll
+  hikPlay: D:\workspace\server\keti\casic-robot-inspection\casic-web\src\main\resources\config\hiklib\PlayCtrl.dll
   algorithm:
-    initPath: D:\workspace\server\keti\casic-robot-inspection\casic-web\src\main\resources\config\path\PathPlanning_init.exe
-    planPath: D:\workspace\server\keti\casic-robot-inspection\casic-web\src\main\resources\config\path\PathPlanning.exe
+    path:  D:\workspace\server\keti\casic-robot-inspection\casic-web\src\main\resources\config\path\
+    initPathName: init.exe
+    planPathName: search.exe
   doc:
     temp: D:\workspace\server\keti\casic-robot-inspection\casic-web\src\main\resources\config\path\report.docx
   config:
diff --git a/casic-web/src/main/resources/config/path/PathPlanning.exe b/casic-web/src/main/resources/config/path/PathPlanning.exe
deleted file mode 100644
index 6678e0f..0000000
--- a/casic-web/src/main/resources/config/path/PathPlanning.exe
+++ /dev/null
Binary files differ
diff --git a/casic-web/src/main/resources/config/path/PathPlanning_init.exe b/casic-web/src/main/resources/config/path/PathPlanning_init.exe
deleted file mode 100644
index d89dc16..0000000
--- a/casic-web/src/main/resources/config/path/PathPlanning_init.exe
+++ /dev/null
Binary files differ
diff --git a/casic-web/src/main/resources/config/path/map.txt b/casic-web/src/main/resources/config/path/map.txt
new file mode 100644
index 0000000..61e2959
--- /dev/null
+++ b/casic-web/src/main/resources/config/path/map.txt
@@ -0,0 +1,36 @@
+0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
+0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
+0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 0 0
+0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 1 0 0
+0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0
+0 0 0 0 0 0 0 0 1 1 1 1 0 0 0 0 0 0
+0 0 0 0 0 0 1 1 1 1 1 1 1 0 0 0 0 0
+0 0 0 0 0 0 1 1 1 1 1 1 1 1 1 0 0 0
+0 0 0 0 1 1 1 1 1 1 1 1 1 1 1 0 0 0
+0 0 0 0 1 1 1 1 1 1 1 1 1 1 1 0 0 0
+0 0 0 0 1 1 1 1 1 1 1 1 1 1 1 0 0 0
+0 0 0 0 1 1 1 1 1 1 1 1 1 1 1 0 0 0
+0 0 0 0 1 1 1 1 1 1 1 1 1 1 1 0 0 0
+0 0 0 0 1 1 1 1 1 1 1 1 1 1 1 0 0 0
+0 0 0 0 1 1 1 1 1 1 1 1 1 1 0 0 0 0
+0 0 0 0 1 1 1 1 1 1 1 1 1 1 0 0 0 0
+0 0 0 0 1 1 1 1 1 1 1 1 1 1 0 0 0 0
+0 0 0 0 1 1 1 1 1 1 1 1 1 1 0 0 0 0
+0 0 0 0 0 0 0 0 1 1 1 1 1 0 0 0 0 0
+0 0 0 0 0 0 0 0 0 0 0 1 1 0 0 0 0 0
+0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
+0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
+0 0 0 1 1 1 1 1 1 1 1 1 1 1 0 0 0 0
+0 0 0 1 1 1 1 1 1 1 1 1 1 1 0 0 0 0
+0 0 0 0 1 1 1 1 1 1 1 1 1 1 0 0 0 0
+0 0 0 0 1 1 1 1 1 1 1 1 0 0 0 0 0 1
+0 0 0 1 1 1 1 1 1 1 1 1 0 0 0 0 0 1
+0 0 1 1 1 1 1 1 1 1 1 1 1 1 0 0 0 1
+0 0 0 1 1 1 1 1 1 1 1 1 1 1 0 0 0 1
+0 0 0 1 1 1 1 1 1 1 1 1 1 1 0 0 0 1
+0 0 0 1 1 1 1 1 1 1 1 1 1 1 0 0 0 1
+0 0 0 1 1 1 1 1 1 1 1 1 1 1 1 0 0 1
+0 0 1 1 1 1 1 1 1 1 1 1 1 1 1 0 0 1
+0 0 1 1 1 1 1 1 1 1 1 1 1 1 1 0 0 1
+0 0 1 1 1 1 1 1 1 1 1 1 1 1 1 0 0 1
+0 0 1 1 1 1 1 1 1 1 1 1 1 1 1 0 0 1

diff --git a/.gitignore b/.gitignore
index 9e20bc1..f42a9cb 100644
--- a/.gitignore
+++ b/.gitignore
@@ -2,3 +2,4 @@
 **/target/**
 logs/
 *.iml
+*.exe
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java b/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
index c7b5d28..a849528 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/constants/RobotDictConstants.java
@@ -28,6 +28,7 @@
     String KEY_CUR_TASK_ID = "CUR_TASK_ID_";
     String KEY_CUR_TASK_INIT = "CUR_TASK_INIT_";
 
+    String KEY_COST_MAP = "KEY_COST_MAP";
     /**
      * PSD数据ID
      */
@@ -41,6 +42,7 @@
      */
     String KEY_CUR_XY_POINT = "KEY_CUR_XY_POINT_";
     String KEY_CUR_ESTIMATE_GRID = "KEY_CUR_ESTIMATE_GRID_";
+    String KEY_PLANING_FLAG = "KEY_PLANING_FLAG_";
     /**
      * 机器人巡点栅格
      */
@@ -55,6 +57,7 @@
      * 探测器配置参数缓存
      */
     String KEY_CONFIG_PARAMS_ID = "KEY_CONFIG_PARAMS_ID_";
+    String KEY_CONFIG_COSTMAP_RESOLUTION_SET = "KEY_CONFIG_COSTMAP_RESOLUTION_SET_";
 
     /**
      * 暂停状态缓存信息
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
index c4c905d..3bb475f 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/RobotOptController.java
@@ -1,9 +1,14 @@
 package com.casic.missiles.modular.robot.controller;
 
+import cn.hutool.core.lang.Assert;
 import cn.hutool.core.util.StrUtil;
 import com.casic.missiles.core.base.controller.BaseController;
+import com.casic.missiles.core.cache.CacheUtil;
 import com.casic.missiles.core.util.SpringContextHolder;
+import com.casic.missiles.model.exception.RequestValidException;
+import com.casic.missiles.model.response.ResponseData;
 import com.casic.missiles.model.response.dto.ResponseDataDTO;
+import com.casic.missiles.modular.constants.RobotDictConstants;
 import com.casic.missiles.modular.robot.IBaseRobotService;
 import com.casic.missiles.modular.robot.dto.RobotInfoSetDTO;
 import com.casic.missiles.modular.robot.model.RobotInfo;
@@ -19,10 +24,16 @@
 import io.swagger.annotations.ApiOperation;
 import io.swagger.annotations.ApiParam;
 import lombok.SneakyThrows;
+import lombok.extern.slf4j.Slf4j;
+import org.apache.ibatis.annotations.Param;
+import org.springframework.beans.factory.annotation.Value;
 import org.springframework.web.bind.annotation.*;
+import org.springframework.web.multipart.MultipartFile;
 
 import javax.servlet.http.HttpServletResponse;
+import java.io.File;
 import java.util.List;
+import java.util.Optional;
 
 /**
  * 机器人操作类
@@ -30,6 +41,7 @@
  * @author lwh
  * @date 2023-10-30
  */
+@Slf4j
 @Api(tags = "机器人控制接口")
 @RestController
 @RequestMapping("/robot/opt")
@@ -61,6 +73,28 @@
         return baseRobotService.execCmdHandler(request, InstructCodeEnums.CHANGE_PCD);
     }
 
+    @Value("${casic.file.uploadPath}")
+    private String filePath;
+
+    @SneakyThrows
+    @ApiOperation(value = "PCD上传接口")
+    @PostMapping(value = "/uploadPcd")
+    public ResponseData uploadPcd(@RequestParam("file") MultipartFile file, @ApiParam("机器人ID,url拼接传参") String robotId) {
+        Assert.isFalse(StrUtil.isEmpty(robotId), () -> new RequestValidException("机器ID不能为空", Optional.of(new Throwable().getStackTrace()[2])));
+        RobotInfo info = robotInfoService.getById(robotId);
+        if (file != null && !file.isEmpty()) {
+            File pcdFile = new File(filePath + "finalCloud_" + info.getId() + ".pcd");
+            try {
+                file.transferTo(pcdFile);
+            } catch (Exception e) {
+                log.error(e.getMessage());
+            }
+            return ResponseData.success();
+        } else {
+            return ResponseData.error("附件上传失败!");
+        }
+    }
+
     @SneakyThrows
     @ApiOperation(value = "PCD下载接口")
     @PostMapping(value = "/pcdDownload")
@@ -136,6 +170,10 @@
     @ApiOperation(value = "启动或关闭循迹功能", notes = "返回 1成功 0失败  wait 启动时是否等待，0代表循迹启动即运动，不等待；1代表循迹启动原地等待。若无此字段，默认循迹启动不等待直接运动")
     @PostMapping(value = "/startTrack")
     public ResponseDataDTO<Integer> startTrack(@RequestBody StartTrackRequest request) {
+        request.setIsTask(1);
+        if (request.getStepLen() == null) {
+            request.setStepLen(2);
+        }
         baseRobotService.startTrack(request);
         return ResponseDataDTO.success();
     }
@@ -156,6 +194,13 @@
         MessageRequestDTO<CrlNavRequest> messageDTO = new MessageRequestDTO();
         messageDTO.setMsg(request);
         ResponseDataDTO<Integer> list = baseRobotService.getRobotRouteOptService().robotCmd(info, InstructCodeEnums.CTRL_NAV, messageDTO);
+        if (request.getAction() == 1) {
+            CostMapResolutionRequest costMap = new CostMapResolutionRequest();
+            costMap.setResolution(info.getResolution().doubleValue());
+            request.setRobotId(request.getRobotId());
+            //是否下发分辨率
+            CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CONFIG_COSTMAP_RESOLUTION_SET + request.getRobotId(), costMap);
+        }
         return list;
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java
new file mode 100644
index 0000000..5317651
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskGridInfoController.java
@@ -0,0 +1,86 @@
+package com.casic.missiles.modular.robot.controller;
+
+import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
+import com.baomidou.mybatisplus.extension.plugins.pagination.Page;
+import com.casic.missiles.core.base.controller.BaseController;
+import com.casic.missiles.core.page.PageFactory;
+import com.casic.missiles.core.page.PageInfoBT;
+import com.casic.missiles.model.form.IdForms;
+import com.casic.missiles.model.response.dto.ResponseDataDTO;
+import com.casic.missiles.modular.robot.model.TaskGridInfo;
+import com.casic.missiles.modular.robot.service.ITaskGridInfoService;
+import com.casic.missiles.modular.robot.vo.TaskGridInfoVO;
+import io.swagger.annotations.Api;
+import io.swagger.annotations.ApiOperation;
+import org.springframework.web.bind.annotation.*;
+
+import java.util.List;
+
+/**
+ * 任务网格信息Controller
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+@Api(tags = "任务网格信息")
+@RestController
+@RequestMapping("/robot/taskGridInfo")
+public class TaskGridInfoController extends BaseController {
+
+    private final ITaskGridInfoService taskGridInfoService;
+
+    public TaskGridInfoController(ITaskGridInfoService taskGridInfoService) {
+        this.taskGridInfoService = taskGridInfoService;
+    }
+
+    @ApiOperation(value = "列表查询")
+    @GetMapping(value = "/list")
+    public ResponseDataDTO<List<TaskGridInfo>> list(TaskGridInfoVO taskGridInfoVO) {
+        QueryWrapper<TaskGridInfo> query = taskGridInfoVO.genQuery();
+        return ResponseDataDTO.success(taskGridInfoService.list(query));
+    }
+
+    @ApiOperation(value = "分页查询")
+    @GetMapping(value = "/listPage")
+    public ResponseDataDTO<PageInfoBT<TaskGridInfo>> listPage(TaskGridInfoVO taskGridInfoVO) {
+        Page<TaskGridInfo> page = PageFactory.defaultPage();
+        QueryWrapper<TaskGridInfo> query = taskGridInfoVO.genQuery();
+        page = taskGridInfoService.page(page, query);
+        return ResponseDataDTO.success(super.packForBT(page));
+    }
+
+
+    @ApiOperation(value = "新增接口")
+    @PostMapping(value = "/add")
+    public ResponseDataDTO add(@RequestBody TaskGridInfo taskGridInfo) {
+        taskGridInfoService.save(taskGridInfo);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "修改接口")
+    @PostMapping(value = "/update")
+    public ResponseDataDTO update(@RequestBody TaskGridInfo taskGridInfo) {
+        taskGridInfoService.updateById(taskGridInfo);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "删除接口")
+    @GetMapping(value = "/delete")
+    public ResponseDataDTO delete(String id) {
+        taskGridInfoService.removeById(id);
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation("批量删除")
+    @PostMapping("/batchDelete")
+    public ResponseDataDTO batchDelete(@RequestBody IdForms ids) {
+        taskGridInfoService.removeByIds(ids.getIds());
+        return ResponseDataDTO.success();
+    }
+
+    @ApiOperation(value = "详情查询")
+    @GetMapping(value = "/detail")
+    public ResponseDataDTO<TaskGridInfo> detail(String id) {
+        return ResponseDataDTO.success(taskGridInfoService.getById(id));
+    }
+}
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
index e82a792..6992274 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/controller/TaskInfoController.java
@@ -98,7 +98,7 @@
     public ResponseDataDTO<AlgorithmResponse> test() {
         AlgorithmResponse response = new AlgorithmResponse();
         response.setTaskHeatMap(new TaskHeatMap());
-        algorithmService.readAlgorithmResponse("step3.txt",response,123L,null);
+        algorithmService.readAlgorithmResponse("step3.txt",null,response,123L,null);
         sendWebSocket(InstructStatusMsgEnums.HEAT_MAP, "1", response.getTaskHeatMap());
         return ResponseDataDTO.success(response);
     }
@@ -121,7 +121,7 @@
     @GetMapping(value = "/testGrid")
     public ResponseDataDTO<List<RouteInfo>> testGrid(@RequestBody PathInitDTO pathInitDTO) {
         RobotInfo robotInfo = infoService.getById(pathInitDTO.getRobotId());
-        GridPointDTO gridPointDTO = algorithmService.convertSlamToGrid(new BigDecimal(pathInitDTO.getX()), new BigDecimal(10), new BigDecimal(4), robotInfo);
+        GridPointDTO gridPointDTO = algorithmService.convertSlamToGrid(new BigDecimal(pathInitDTO.getXNow()), new BigDecimal(10), new BigDecimal(4), robotInfo);
         return ResponseDataDTO.success(algorithmService.convertGridCenterPoint(gridPointDTO, new BigDecimal(10), robotInfo));
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/TaskGridInfoMapper.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/TaskGridInfoMapper.java
new file mode 100644
index 0000000..5c3a70b
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/TaskGridInfoMapper.java
@@ -0,0 +1,23 @@
+package com.casic.missiles.modular.robot.dao;
+
+
+import com.baomidou.mybatisplus.core.mapper.BaseMapper;
+import com.casic.missiles.modular.robot.model.TaskGridInfo;
+import org.apache.ibatis.annotations.Param;
+import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
+import com.baomidou.mybatisplus.extension.plugins.pagination.Page;
+import java.util.List;
+/**
+ * <p>
+ *  任务网格信息 Mapper 接口
+ * </p>
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+public interface TaskGridInfoMapper extends BaseMapper<TaskGridInfo> {
+/**
+* 任务网格信息 分页检索
+*/
+List<TaskGridInfo> selectTaskGridInfoPage( @Param("page") Page<TaskGridInfo> page,@Param("ew" ) QueryWrapper<TaskGridInfo> query);
+}
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/mapping/TaskGridInfoMapper.xml b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/mapping/TaskGridInfoMapper.xml
new file mode 100644
index 0000000..be573e0
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dao/mapping/TaskGridInfoMapper.xml
@@ -0,0 +1,80 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!DOCTYPE mapper PUBLIC "-//mybatis.org//DTD Mapper 3.0//EN" "http://mybatis.org/dtd/mybatis-3-mapper.dtd">
+<mapper namespace="com.casic.missiles.modular.robot.dao.TaskGridInfoMapper">
+    <!-- 通用查询映射结果 -->
+    <resultMap id="BaseResultMap" type="com.casic.missiles.modular.robot.model.TaskGridInfo">
+        <result column="id" property="id"/>
+        <result column="create_time" property="createTime"/>
+        <result column="update_time" property="updateTime"/>
+        <result column="task_id" property="taskId"/>
+        <result column="estimate_grid_x" property="estimateGridX"/>
+        <result column="estimate_grid_y" property="estimateGridY"/>
+        <result column="neutron_count" property="neutronCount"/>
+        <result column="cur_grid_x" property="curGridX"/>
+        <result column="cur_grid_y" property="curGridY"/>
+    </resultMap>
+    <!-- 通用查询结果列 -->
+    <sql id="Base_Column_List">
+        id
+        ,
+                    create_time as createTime ,
+                    update_time as updateTime ,
+                    task_id as taskId ,
+                    estimate_grid_x as estimateGridX ,
+                    estimate_grid_y as estimateGridY ,
+                    neutron_count as neutronCount ,
+                    cur_grid_x as curGridX ,
+                    cur_grid_y as curGridY
+    </sql>
+    <sql id="Base_Column_Alias">
+        a
+        .
+        id
+        ,
+            a.create_time
+            as createTime ,
+            a.update_time
+            as updateTime ,
+            a.task_id
+            as taskId ,
+            a.estimate_grid_x
+            as estimateGridX ,
+            a.estimate_grid_y
+            as estimateGridY ,
+            a.neutron_count
+            as neutronCount ,
+            a.cur_grid_x
+            as curGridX ,
+            a.cur_grid_y
+            as curGridY
+    </sql>
+    <select id="selectTaskGridInfoPage" resultType="com.casic.missiles.modular.robot.model.TaskGridInfo">
+        select
+        <include refid="Base_Column_Alias"/>
+        from task_grid_info a
+        <where>
+            <if test="ew != null">
+                ${ew.sqlSegment}
+            </if>
+        </where>
+    </select>
+    <sql id="Base_Column_Query">
+        <if test="request!=null">
+            <if test="request.taskId!= null and request.taskId!=''">
+                and task_id = #{request.taskId}
+            </if>
+            <if test="request.estimateGridX!= null and request.estimateGridX!=''">
+                and estimate_grid_x = #{request.estimateGridX}
+            </if>
+            <if test="request.estimateGridY!= null and request.estimateGridY!=''">
+                and estimate_grid_y = #{request.estimateGridY}
+            </if>
+            <if test="request.curGridX!= null and request.curGridX!=''">
+                and cur_grid_x = #{request.curGridX}
+            </if>
+            <if test="request.curGridY!= null and request.curGridY!=''">
+                and cur_grid_y = #{request.curGridY}
+            </if>
+        </if>
+    </sql>
+</mapper>
\ No newline at end of file
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java
index 749f559..4559074 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/AlgorithmResponse.java
@@ -13,4 +13,9 @@
      * 规划点位信息
      */
     private PointNextDTO pointNextDTO;
+    /**
+     * 寻源目标地址
+     */
+    private PointNextDTO coordinate;
+    private boolean isEnd;
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java
index 39ff2a2..cef3238 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/GridPointDTO.java
@@ -12,6 +12,9 @@
     private BigDecimal y;
 
     public static GridPointDTO convertGridPoint(PointNextDTO nextDTO) {
+        if(nextDTO==null){
+            return null;
+        }
         GridPointDTO gridPointDTO = new GridPointDTO();
         gridPointDTO.setX(Convert.toBigDecimal(nextDTO.getX()));
         gridPointDTO.setY(Convert.toBigDecimal(nextDTO.getY()));
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java
index 8c88242..33be25d 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathInitDTO.java
@@ -12,25 +12,19 @@
 public class PathInitDTO {
     //---------------------算法参数
     /**
-     * x坐标最大值（x方向栅格总数）
+     * 初始位置x坐标(栅格地图下的坐标)
      */
-    private String xMax;
+    private String xNow;
     /**
-     * y坐标最大值（y方向栅格总数）
+     * 初始位置y坐标(栅格地图下的坐标)
      */
-    private String yMax;
+    private String yNow;
     /**
      * 初始位置计数率
      */
-    private String count;
-    /**
-     * 初始位置x坐标(0<=x<x_max)
-     */
-    private String x;
-    /**
-     * 初始位置y坐标(0<=y<y_max)
-     */
-    private String y;
+    private String zNow;
+
+    private Integer stepLen;
 
     //---------------------其他执行参数
     private String robotId;
@@ -38,11 +32,11 @@
 
     public List<String> toArray() {
         List<String> list = new ArrayList<>();
-        list.add(xMax);
-        list.add(yMax);
-        list.add(count);
-        list.add(x);
-        list.add(y);
+        list.add(xNow);
+        list.add(yNow);
+        list.add(stepLen+"");
+        list.add(zNow);
+        list.add(taskId+"");
         return list;
     }
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java
index 109e95c..ada6774 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/dto/PathPlanDTO.java
@@ -10,21 +10,17 @@
  */
 @Data
 public class PathPlanDTO extends PathInitDTO {
-
     /**
-     * 长度为8的数组，记录(x-1, y-1), (x-1, y), (x-1, y+1), (x, y-1), (x, y+1), (x+1, y-1), (x+1, y), (x+1, y+1)8个坐标的可通行信息。
-     * 若某坐标有障碍物（不可通行行）则记为0，否则记为1
+     * 浮点阈值(阈值，超过该值后结束巡检开始寻源)
      */
-    private String open;
-
+    private String th;
     public List<String> toArray() {
         List<String> list = new ArrayList<>();
-        list.add(getXMax());
-        list.add(getYMax());
-        list.add(open);
-        list.add(getCount());
-        list.add(getX());
-        list.add(getY());
+
+        list.add(getStepLen()+"");
+        list.add(getTh());
+        list.add(getZNow());
+        list.add(getTaskId()+"");
         return list;
     }
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskGridInfo.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskGridInfo.java
new file mode 100644
index 0000000..101c10a
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskGridInfo.java
@@ -0,0 +1,83 @@
+package com.casic.missiles.modular.robot.model;
+
+import com.baomidou.mybatisplus.annotation.TableField;
+import com.baomidou.mybatisplus.annotation.TableName;
+import com.baomidou.mybatisplus.extension.activerecord.Model;
+import io.swagger.annotations.ApiModelProperty;
+import lombok.Data;
+
+import java.math.BigDecimal;
+import java.util.Date;
+
+/**
+ * 任务网格信息对象 task_grid_info
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+@Data
+@TableName("task_grid_info")
+public class TaskGridInfo extends Model<TaskGridInfo> {
+    private static final long serialVersionUID = 1L;
+    @TableField(exist = false)
+    private Integer index;
+    @ApiModelProperty(value = "主键", dataType = "Long")
+    @TableField("id")
+    private Long id;
+
+    @ApiModelProperty(value = "创建时间", dataType = "Date")
+    @TableField("create_time")
+    private Date createTime;
+
+    @ApiModelProperty(value = "修改时间", dataType = "Date")
+    @TableField("update_time")
+    private Date updateTime;
+
+    @ApiModelProperty(value = "任务ID", dataType = "Long")
+    @TableField("task_id")
+    private Long taskId;
+
+    @ApiModelProperty(value = "估算栅格x", dataType = "Long")
+    @TableField("estimate_grid_x")
+    private String estimateGridX;
+
+    @ApiModelProperty(value = "估算栅格y", dataType = "Long")
+    @TableField("estimate_grid_y")
+    private String estimateGridY;
+
+    @ApiModelProperty(value = "中子计数率", dataType = "BigDecimal")
+    @TableField("neutron_count")
+    private BigDecimal neutronCount;
+
+    @ApiModelProperty(value = "当前栅格x", dataType = "Long")
+    @TableField("cur_grid_x")
+    private Integer curGridX;
+
+    @ApiModelProperty(value = "当前栅格y", dataType = "Long")
+    @TableField("cur_grid_y")
+    private Integer curGridY;
+
+    public String getCurPoint(){
+        return "("+curGridX+","+curGridY+")";
+    }
+    public String getEstimatePoint(){
+        if(estimateGridX==null&estimateGridY==null){
+            return "结束";
+        }
+        return "("+estimateGridX+","+estimateGridY+")";
+    }
+    @Override
+    public String toString() {
+        return "TaskGridInfo{"
+                + "id=" + id
+                + "createTime=" + createTime
+                + "updateTime=" + updateTime
+                + "taskId=" + taskId
+                + "estimateGridX=" + estimateGridX
+                + "estimateGridY=" + estimateGridY
+                + "neutronCount=" + neutronCount
+                + "curGridX=" + curGridX
+                + "curGridY=" + curGridY
+                + "}";
+    }
+}
\ No newline at end of file
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java
index edc104e..a8acb2a 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/model/TaskInfo.java
@@ -53,6 +53,13 @@
     @TableField("route_id")
     private Long routeId;
 
+    @ApiModelProperty(value = "寻源初始步长", dataType = "Integer")
+    @TableField("step_len")
+    private Integer stepLen;
+
+    @ApiModelProperty(value = "寻源阈值设定", dataType = "BigDecimal")
+    @TableField("max_threshold")
+    private BigDecimal maxThreshold;
     /**
      * 任务类型(1巡检任务)
      */
@@ -62,7 +69,6 @@
     @ApiModelProperty(value = "任务类型字典名称", dataType = "String")
     @TableField(exist = false)
     private String taskTypeName;
-
     @ApiModelProperty(value = "识别类型(discernType 1中子源识别)", dataType = "Integer")
     @TableField("discern_type")
     private Integer discernType;
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java
index c4bc25d..c6620f3 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/enums/InstructCodeEnums.java
@@ -53,7 +53,7 @@
     }),
     SET_SPEED("/set_speed" , "/set_speed_response" , "最大运动速度设置" , true, new TypeReference<RobotMsgResponseDTO<Integer>>() {
     }),
-    SET_TARGETPOINT("/set_targetpoint" , "/set_targetpoint_response" , "设置导航目标点" , false, new TypeReference<RobotMsgResponseDTO<Integer>>() {
+    SET_TARGETPOINT("/set_targetpoint" , "/set_targetpoint_response" , "设置导航目标点" , true, new TypeReference<RobotMsgResponseDTO<Integer>>() {
     }),
 
     GET_DATA_LIST("/get_data_list" , "/get_data_list_response" , "获取数据包列表" , true, new TypeReference<RobotMsgResponseDTO<List<String>>>() {
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/AbstractSocketHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/AbstractSocketHandler.java
index 83f4cb2..e47e343 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/AbstractSocketHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/AbstractSocketHandler.java
@@ -49,7 +49,7 @@
                 wsMsgBaseDTO.setMsgKey(enums.getMsgKey());
                 wsMsgBaseDTO.setData(jsonObject.get("msg"));
                 String msg = JSON.toJSONString(wsMsgBaseDTO);
-                log.info("推送{}消息：{}", enums.getRemarks(), msg);
+//                log.info("推送{}消息：{}", enums.getRemarks(), msg);
                 webSocket.sendOneMessage(robotId, msg);
             }
         } catch (Exception e) {
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/cmd/TargetPointCmdHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/cmd/TargetPointCmdHandler.java
index c1c09fa..43b4460 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/cmd/TargetPointCmdHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/cmd/TargetPointCmdHandler.java
@@ -34,7 +34,11 @@
     @Override
     public ResponseDataDTO senderCmd(TargetPointRequest request, InstructCodeEnums enums) {
         MessageRequestDTO<TargetPointRequest> messageDTO = new MessageRequestDTO();
-        //读取默认关键点信息
+        //读取默认关键点信息 {"x":"1.715708819927424","y":"-0.9100042544250901","z":"0.017993879527185876","theta":"-0.039278239414312105"}
+//        request.setX("1.715708819927424");
+//        request.setY("-0.9100042544250901");
+//        request.setZ("0.017993879527185876");
+//        request.setTheta("-0.039278239414312105");
         messageDTO.setMsg(request);
         ResponseDataDTO<Integer> list = optService.robotCmd(getRobotInfo(), enums, messageDTO);
         return list;
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/AppCostMapHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/AppCostMapHandler.java
index 0335693..88c9f79 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/AppCostMapHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/AppCostMapHandler.java
@@ -1,6 +1,11 @@
 package com.casic.missiles.modular.robot.opt.handler.msg;
 
+import com.casic.missiles.core.cache.CacheUtil;
+import com.casic.missiles.modular.constants.RobotDictConstants;
+import com.casic.missiles.modular.robot.IBaseRobotService;
+import com.casic.missiles.modular.robot.opt.enums.InstructCodeEnums;
 import com.casic.missiles.modular.robot.opt.handler.AbstractSocketHandler;
+import com.casic.missiles.modular.robot.opt.instruct.dto.CostMapResolutionRequest;
 import com.casic.missiles.modular.robot.service.IRobotStatusInfoService;
 import lombok.extern.slf4j.Slf4j;
 import org.eclipse.paho.client.mqttv3.MqttMessage;
@@ -16,14 +21,27 @@
 @Component("/app_costmap_sender")
 public class AppCostMapHandler extends AbstractSocketHandler {
     private final IRobotStatusInfoService statusInfoService;
+    private final IBaseRobotService baseRobotService;
     private static Map<String, Integer> index = new ConcurrentHashMap<>();
 
-    public AppCostMapHandler(IRobotStatusInfoService statusInfoService) {
+    public AppCostMapHandler(IRobotStatusInfoService statusInfoService, IBaseRobotService baseRobotService) {
         this.statusInfoService = statusInfoService;
+        this.baseRobotService = baseRobotService;
     }
 
     @Override
     public void process(String robotId, String topic, MqttMessage message) {
         statusInfoService.saveCostMap(robotId, message);
+
+        CostMapResolutionRequest costMapSet = CacheUtil.get(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CONFIG_COSTMAP_RESOLUTION_SET +robotId);
+        if(costMapSet != null){
+            CacheUtil.remove(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CONFIG_COSTMAP_RESOLUTION_SET +robotId);
+            new Thread(() -> {
+                costMapSet.setRobotId(Long.valueOf(robotId));
+                baseRobotService.execCmdHandler(costMapSet, InstructCodeEnums.COSTMAP_RESOLUTION);
+            }).start();
+
+        }
+
     }
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/PoseMessageMsgHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/PoseMessageMsgHandler.java
index 29f8919..6c3c3bd 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/PoseMessageMsgHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/PoseMessageMsgHandler.java
@@ -17,11 +17,9 @@
 @Component("/pose_message")
 public class PoseMessageMsgHandler extends AbstractSocketHandler {
 
-
     private final EventPublisher publisher;
 
     public PoseMessageMsgHandler(EventPublisher publisher) {
-
         this.publisher = publisher;
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/StopStateMsgHandler.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/StopStateMsgHandler.java
index 8950815..8a71e0b 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/StopStateMsgHandler.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/handler/msg/StopStateMsgHandler.java
@@ -30,7 +30,7 @@
     public void process(String robotId, String topic, MqttMessage message) {
 
         index.put(index_key, index.get("index") + 1);
-        if (index.get(index_key)>50) {
+        if (index.get(index_key)>30) {
             index.put(index_key, 0);
             log.info("主键：{},停障状态topic:{},消息内容:{}", robotId, topic, message.toString());
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/impl/RobotOptServiceImpl.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/impl/RobotOptServiceImpl.java
index 8471e7a..7940a24 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/impl/RobotOptServiceImpl.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/impl/RobotOptServiceImpl.java
@@ -146,7 +146,7 @@
     public void startTrack(StartTrackRequest request) {
         //增加循迹任务记录
         taskInfoService.reloadTask(request, robotInfoService.getById(request.getRobotId()));
-        this.execCmdHandler(request, InstructCodeEnums.START_TRACK);
+//        this.execCmdHandler(request, InstructCodeEnums.START_TRACK);
     }
 
     public IRobotRouteOptService getRobotRouteOptService() {
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/BaseRobotCmdDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/BaseRobotCmdDTO.java
index 5608f2b..06b53de 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/BaseRobotCmdDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/BaseRobotCmdDTO.java
@@ -3,11 +3,13 @@
 import io.swagger.annotations.ApiModelProperty;
 import lombok.Data;
 
+import java.io.Serializable;
+
 /**
  * 机器人指令基类
  */
 @Data
-public class BaseRobotCmdDTO {
+public class BaseRobotCmdDTO implements Serializable {
     @ApiModelProperty(value = "机器人主键", dataType = "String")
     protected Long robotId;
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/StartTrackRequest.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/StartTrackRequest.java
index a18321b..85546a7 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/StartTrackRequest.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/dto/StartTrackRequest.java
@@ -3,6 +3,8 @@
 import io.swagger.annotations.ApiModelProperty;
 import lombok.Data;
 
+import java.math.BigDecimal;
+
 /**
  * 导航下发指令
  */
@@ -10,18 +12,19 @@
 public class StartTrackRequest extends BaseRobotCmdDTO {
     @ApiModelProperty(value = "sn编码(可不填 默认机器ID)", dataType = "String")
     private String sn;
-
     @ApiModelProperty(value = "下发状态 1:开启 0:关闭", dataType = "String")
     private Integer action;
     @ApiModelProperty(value = "等待状态 1:等待 0:不等待", dataType = "String")
     private Integer wait;
     @ApiModelProperty(value = "等待状态 0:循迹 1:任务", dataType = "isTask")
     private Integer isTask;
-    @ApiModelProperty(value = "轨迹名称", dataType = "String")
+    @ApiModelProperty(value = "轨迹名称(废弃)", dataType = "String")
     private String track_name;
     @ApiModelProperty(value = "关键点名称", dataType = "String", hidden = true)
     private String taskpoint_name;
-
+    @ApiModelProperty(value = "初始寻源步长", dataType = "Integer")
+    private Integer stepLen;
+    @ApiModelProperty(value = "寻源阈值", dataType = "BigDecimal")
+    private BigDecimal maxThreshold;
     private String obs_mode="1";
-
 }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/response/StopStateResponseDTO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/response/StopStateResponseDTO.java
index eb514bf..e0679d7 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/response/StopStateResponseDTO.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/opt/instruct/response/StopStateResponseDTO.java
@@ -3,11 +3,13 @@
 import io.swagger.annotations.ApiModelProperty;
 import lombok.Data;
 
+import java.io.Serializable;
+
 /**
  * 电压信息返回
  */
 @Data
-public class StopStateResponseDTO {
+public class StopStateResponseDTO implements Serializable {
     @ApiModelProperty(value = "软件急停(0：无触发 1：触发)" , dataType = "Integer")
     private Integer soft;
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IAlgorithmService.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IAlgorithmService.java
index 56b3e1d..f62523b 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IAlgorithmService.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IAlgorithmService.java
@@ -6,6 +6,7 @@
 import com.casic.missiles.modular.robot.dto.PathPlanDTO;
 import com.casic.missiles.modular.robot.model.RobotInfo;
 
+import java.io.File;
 import java.math.BigDecimal;
 import java.util.List;
 
@@ -38,5 +39,5 @@
      * @return
      */
     GridPointDTO convertGridCenterPoint(GridPointDTO pointDTO, BigDecimal resolution, RobotInfo robotInfo);
-    void readAlgorithmResponse(String fileName, AlgorithmResponse response, Long taskId, List<String> res);
+    void readAlgorithmResponse(String fileName, File repetitionFile, AlgorithmResponse response, Long taskId, List<String> res);
 }
\ No newline at end of file
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IRobotStatusInfoService.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IRobotStatusInfoService.java
index 2af210a..757f8d0 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IRobotStatusInfoService.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/IRobotStatusInfoService.java
@@ -67,10 +67,9 @@
     /**
      * 更新车辆路线及任务信息
      *
-     * @param currRouteId 当前路线信息
      * @param currTaskId  当前任务信息
      */
-    void updateCurrTask(String robotId, Long currRouteId, Long currTaskId);
+    void updateCurrTask(String robotId, Long currTaskId);
 
     RobotStatusInfo getStatusByRobotId(String robotId);
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/ITaskGridInfoService.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/ITaskGridInfoService.java
new file mode 100644
index 0000000..fc0da93
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/ITaskGridInfoService.java
@@ -0,0 +1,22 @@
+package com.casic.missiles.modular.robot.service;
+
+import com.baomidou.mybatisplus.extension.service.IService;
+import  com.casic.missiles.modular.robot.model.TaskGridInfo;
+import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
+import com.baomidou.mybatisplus.extension.plugins.pagination.Page;
+import java.util.List;
+
+
+/**
+ * <p>
+ * 任务网格信息 服务类
+ * </p>
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+public interface ITaskGridInfoService extends IService<TaskGridInfo> {
+/**
+* 任务网格信息 分页检索
+*/
+List<TaskGridInfo> selectTaskGridInfoPage(Page<TaskGridInfo> page,QueryWrapper<TaskGridInfo> query);}
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/ITaskInfoService.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/ITaskInfoService.java
index 31d7ba6..b395993 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/ITaskInfoService.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/ITaskInfoService.java
@@ -51,6 +51,13 @@
      * @param dto 实体
      */
     void pointCallBack(PoseMessageResponseDTO dto, String robotId);
+
+    /**
+     * 车辆路线规划
+     * @param dto
+     * @param robotId
+     */
+    void carPathPlanning(PoseMessageResponseDTO dto, String robotId);
     void pointTestCallBack(PoseMessageResponseDTO dto, String robotId);
     /**
      * 图片路径保存
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/AlgorithmServiceImpl.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/AlgorithmServiceImpl.java
index f808fa0..a4dbe0b 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/AlgorithmServiceImpl.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/AlgorithmServiceImpl.java
@@ -29,10 +29,12 @@
 @Slf4j
 @Service
 public class AlgorithmServiceImpl implements IAlgorithmService {
-    @Value("${casic.algorithm.initPath:-1}")
-    private String initPath;
-    @Value("${casic.algorithm.planPath:-1}")
-    private String planPath;
+    @Value("${casic.algorithm.path:-1}")
+    private String path;
+    @Value("${casic.algorithm.initPathName:-1}")
+    private String initPathName;
+    @Value("${casic.algorithm.planPathName:-1}")
+    private String planPathName;
     private final ITaskHeatMapService heatMapService;
 
     public AlgorithmServiceImpl(ITaskHeatMapService heatMapService) {
@@ -42,13 +44,13 @@
     @Transactional
     @Override
     public AlgorithmResponse pathPlanningInit(PathInitDTO initDTO) {
-        if (StrUtil.isEmpty(initDTO.getCount())) {
-            initDTO.setCount("0.00");
+        if (StrUtil.isEmpty(initDTO.getZNow())) {
+            initDTO.setZNow("0.00");
         }
         log.info(JSON.toJSONString(initDTO));
         //算法路径及入参配置
         List<String> commends = new ArrayList<>();
-        commends.add(initPath);
+        commends.add(path + initPathName);
         commends.addAll(initDTO.toArray());
         List<String> res = AlgorithmUtils.invokeJob(commends);
 
@@ -63,38 +65,72 @@
     @Transactional
     @Override
     public AlgorithmResponse pathPlanning(PathPlanDTO planDTO) {
-        if (StrUtil.isEmpty(planDTO.getCount())) {
-            planDTO.setCount("0.001");
+        if (StrUtil.isEmpty(planDTO.getZNow())) {
+            planDTO.setZNow("0.001");
         }
         //算法路径及入参配置
         List<String> commends = new ArrayList<>();
-        commends.add(planPath);
+        commends.add(path + planPathName);
         commends.addAll(planDTO.toArray());
         log.info("寻源算法调用:{}", JSON.toJSONString(planDTO));
         List<String> res = AlgorithmUtils.invokeJob(commends);
         AlgorithmResponse response = getAlgorithmResponse(planDTO, res);
-        //next 点位编写
-        if (CollectionUtil.isNotEmpty(res) && res.size() == 2) {
-            response.getPointNextDTO().setX(formatBigDecimal(res.get(0).replaceAll("next_X: ", "")));
-            response.getPointNextDTO().setY(formatBigDecimal(res.get(1).replaceAll("next_Y: ", "")));
-        }
+
         return response;
     }
 
     private AlgorithmResponse getAlgorithmResponse(PathInitDTO initDTO, List<String> res) {
-        File repetitionFile = new File("repetition.txt");
+        //读取当前路径图片
+        File repetitionFileDir = new File(path + "\\" + initDTO.getTaskId() + "\\predicts");
+        File[] files = repetitionFileDir.listFiles((dir, name) -> name.endsWith(".txt") && name.startsWith("step"));
+        File repetitionFile = null;
+        //判定当前执行路径最大step2文件夹
+        if (files != null && files.length > 0) {
+             repetitionFile = files[files.length - 1];
+        }
         AlgorithmResponse response = new AlgorithmResponse();
         if (initDTO.getTaskId() != null && repetitionFile.exists()) {
-            readAlgorithmResponse("repetition.txt", response, initDTO.getTaskId(), res);
-            readAlgorithmResponse("step.txt", response, initDTO.getTaskId(), res);
+            //读取热力图
+            readAlgorithmResponse("repetition.txt",repetitionFile, response, initDTO.getTaskId(), res);
         } else {
             log.info("taskId is empty!");
         }
-        readAlgorithmResponse("xy_train.txt", response, initDTO.getTaskId(), res);
-        readAlgorithmResponse("z_train.txt", response, initDTO.getTaskId(), res);
+        //读取规划路径
+        if (CollectionUtil.isNotEmpty(res)) {
+            response.setEnd(false);
+            readAlgorithmResponse(response, res);
+        }
         return response;
     }
 
+    private void readAlgorithmResponse(AlgorithmResponse response, List<String> res) {
+        List<String> resList = new ArrayList<>();
+        if (res.size() >= 2) {
+            resList = res.subList(res.size() - 2, res.size());
+        }
+        //判定是否结束寻源
+        if (res.contains("end")) {
+            response.setEnd(true);
+            for (String s : resList) {
+                if (s.startsWith("source_coordinate:")) {
+                    s = s.trim();
+                    log.info("寻源结束,目标点位：{}",s);
+                    List<String> lists = StrUtil.split(s.replaceAll("source_coordinate:", ""), ",");
+                    PointNextDTO nextDTO = new PointNextDTO();
+                    nextDTO.setX(lists.get(0));
+                    nextDTO.setY(lists.get(1));
+                    response.setCoordinate(nextDTO);
+                }
+            }
+        } else {
+            PointNextDTO nextDTO = new PointNextDTO();
+            nextDTO.setX(resList.get(0));
+            nextDTO.setY(resList.get(1));
+            response.setPointNextDTO(nextDTO);
+        }
+
+    }
+
     /**
      * 读取算法输出数据
      *
@@ -102,49 +138,18 @@
      * @param response 结果集
      * @param taskId   任务ID
      */
-    public void readAlgorithmResponse(String fileName, AlgorithmResponse response, Long taskId, List<String> res) {
-        File file = new File(fileName);
-        if (!file.exists()&&!fileName.startsWith("step")) {
+    public void readAlgorithmResponse(String fileName,File repetitionFile, AlgorithmResponse response, Long taskId, List<String> res) {
+        if (!repetitionFile.exists() && !fileName.startsWith("step")) {
             log.info("{} file is empty!", fileName);
             return;
         }
         if (response.getPointNextDTO() == null) {
             response.setPointNextDTO(new PointNextDTO());
         }
-        switch (fileName) {
-            case "repetition.txt":
-                String msg = FileUtil.readString(file, CharsetUtil.CHARSET_UTF_8);
-                TaskHeatMap taskHeatMap = createEmptyTaskHeatMap(taskId);
-                taskHeatMap.setRepetition(msg);
-                response.setTaskHeatMap(taskHeatMap);
-                break;
-            case "xy_train.txt":
-                List<String> xyTrains = FileUtil.readLines(file, CharsetUtil.CHARSET_UTF_8);
-                if (CollectionUtil.isNotEmpty(xyTrains)) {
-                    if (CollectionUtil.isNotEmpty(xyTrains)) {
-                        List<String> list = StrUtil.split(xyTrains.get(xyTrains.size() - 1), " ");
-                        response.getPointNextDTO().setX(formatBigDecimal(list.get(0)));
-                        response.getPointNextDTO().setY(formatBigDecimal(list.get(1)));
-                    }
-                }
-                break;
-            case "z_train.txt":
-                List<String> zTrains = FileUtil.readLines(file, CharsetUtil.CHARSET_UTF_8);
-                if (CollectionUtil.isNotEmpty(zTrains)) {
-                    String zTrain = zTrains.get(zTrains.size() - 1);
-                    response.getPointNextDTO().setZ(formatBigDecimal(zTrain));
-                }
-            default:
-                if (StrUtil.isNotEmpty(fileName) && fileName.startsWith("step")) {
-                    File fileDir = new File(".");
-                    File[] files = fileDir.listFiles((dir, name) -> name.endsWith(".txt") && name.startsWith("step"));
-                    if (files != null && files.length > 0) {
-                        String steps = FileUtil.readString(files[files.length - 1], CharsetUtil.CHARSET_UTF_8);
-                        response.getTaskHeatMap().setRepetition(steps);
-                    }
-                }
-                break;
-        }
+        String msg = FileUtil.readString(repetitionFile, CharsetUtil.CHARSET_UTF_8);
+        TaskHeatMap taskHeatMap = createEmptyTaskHeatMap(taskId);
+        taskHeatMap.setRepetition(msg);
+        response.setTaskHeatMap(taskHeatMap);
     }
 
     /**
@@ -157,13 +162,11 @@
     public GridPointDTO convertSlamToGrid(BigDecimal x, BigDecimal y, BigDecimal resolution, RobotInfo robotInfo) {
         GridPointDTO pointDTO = new GridPointDTO();
         BigDecimal scale = resolution;
-
         //减去栅格原点坐标
         x = x.subtract(robotInfo.getOriginX());
         y = y.subtract(robotInfo.getOriginY());
-
-        pointDTO.setX(new BigDecimal(x.divide(scale,3,BigDecimal.ROUND_HALF_UP).intValue()+1));
-        pointDTO.setY(new BigDecimal(y.divide(scale,3,BigDecimal.ROUND_HALF_UP).intValue()+1));
+        pointDTO.setX(new BigDecimal(x.divide(scale, 3, BigDecimal.ROUND_HALF_UP).intValue()+1));
+        pointDTO.setY(new BigDecimal(y.divide(scale, 3, BigDecimal.ROUND_HALF_UP).intValue() +1));
         return pointDTO;
     }
 
@@ -174,19 +177,17 @@
      * @return
      */
     public GridPointDTO convertGridCenterPoint(GridPointDTO pointDTO, BigDecimal resolution, RobotInfo robotInfo) {
-
         GridPointDTO centerPoint = new GridPointDTO();
         BigDecimal scale = resolution;
-        //点位转换
-        BigDecimal slamX = pointDTO.getX().divide(scale, 5, BigDecimal.ROUND_HALF_UP);
-        BigDecimal slamY = pointDTO.getY().divide(scale, 5, BigDecimal.ROUND_HALF_UP);
+        //点位转换 转换后栅格－1 从1开始计算
+        BigDecimal slamX = (pointDTO.getX().subtract(new BigDecimal(1))).multiply(scale);
+        BigDecimal slamY = (pointDTO.getY().subtract(new BigDecimal(1))).multiply(scale);
         //中心栅格点位中心添加
-        centerPoint.setX(slamX.add(scale.divide(new BigDecimal(2), 5, BigDecimal.ROUND_HALF_UP)));
-        centerPoint.setY(slamY.add(scale.divide(new BigDecimal(2), 5, BigDecimal.ROUND_HALF_UP)));
+        centerPoint.setX(slamX.add(scale.divide(new BigDecimal(2), 10, BigDecimal.ROUND_HALF_UP)));
+        centerPoint.setY(slamY.add(scale.divide(new BigDecimal(2), 10, BigDecimal.ROUND_HALF_UP)));
         //加上栅格原点坐标
         centerPoint.setX(centerPoint.getX().add(robotInfo.getOriginX()));
         centerPoint.setY(centerPoint.getY().add(robotInfo.getOriginY()));
-
         return centerPoint;
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotInfoServiceImpl.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotInfoServiceImpl.java
index 043e05d..fdded64 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotInfoServiceImpl.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotInfoServiceImpl.java
@@ -104,7 +104,7 @@
             //下发障碍物栅格
             sendCostMapResolution(robotInfo.getResolution(), robotInfo.getId());
             //重新生成栅格地图
-            sendChangePcd(robotInfo.getId());
+            //sendChangePcd(robotInfo.getId());
         }
         update.eq(RobotInfo::getId, robotInfo.getId());
         update(update);
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotStatusInfoServiceImpl.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotStatusInfoServiceImpl.java
index 6d3ae91..46b5c3b 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotStatusInfoServiceImpl.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotStatusInfoServiceImpl.java
@@ -15,6 +15,7 @@
 import com.casic.missiles.modular.robot.dao.RobotStatusInfoMapper;
 import com.casic.missiles.modular.robot.model.RobotInfo;
 import com.casic.missiles.modular.robot.model.RobotStatusInfo;
+import com.casic.missiles.modular.robot.opt.enums.InstructCodeEnums;
 import com.casic.missiles.modular.robot.opt.instruct.base.dto.RobotMsgDTO;
 import com.casic.missiles.modular.robot.opt.instruct.base.dto.RobotMsgResponseDTO;
 import com.casic.missiles.modular.robot.opt.instruct.base.dto.RobotResponseDTO;
@@ -26,13 +27,18 @@
 import com.casic.missiles.modular.robot.service.IRobotInfoService;
 import com.casic.missiles.modular.robot.service.IRobotStatusInfoService;
 import lombok.extern.slf4j.Slf4j;
+import org.aspectj.util.FileUtil;
 import org.eclipse.paho.client.mqttv3.MqttMessage;
+import org.springframework.beans.factory.annotation.Value;
 import org.springframework.context.annotation.Lazy;
 import org.springframework.stereotype.Service;
 import org.springframework.transaction.annotation.Transactional;
 
+import java.io.File;
 import java.io.Serializable;
 import java.math.BigDecimal;
+import java.util.ArrayList;
+import java.util.Arrays;
 import java.util.List;
 
 
@@ -147,7 +153,6 @@
     @Override
     public void saveProcessMessage(String robotId, MqttMessage message) {
 //        String json = new String(message.getPayload());
-
         RobotResponseDTO<ProcessMessageResponseDTO> responseDTO = parseJson(message, new TypeReference<RobotResponseDTO<ProcessMessageResponseDTO>>() {
         });
         if (responseDTO.getMsg() != null) {
@@ -189,9 +194,8 @@
     }
 
     @Override
-    public void updateCurrTask(String robotId, Long currRouteId, Long currTaskId) {
+    public void updateCurrTask(String robotId, Long currTaskId) {
         UpdateWrapper<RobotStatusInfo> query = new UpdateWrapper<>();
-        query.set("curr_route_id", currRouteId);
         query.set("curr_task_id", currTaskId);
         query.eq("robot_id", robotId);
         update(query);
@@ -250,6 +254,15 @@
         }
     }
 
+    @Value("${casic.algorithm.path:-1}")
+    private String path;
+
+    /**
+     * 栅格阈值
+     */
+    @Value("${casic.algorithm.num:30}")
+    private Integer num;
+
     /**
      * 机器人栅格数据
      *
@@ -262,15 +275,41 @@
         RobotResponseDTO<AppCostmapResponseDTO> responseDTO = parseJson(message, new TypeReference<RobotResponseDTO<AppCostmapResponseDTO>>() {
         });
         if (responseDTO != null && responseDTO.getMsg() != null) {
+            //发送栅格分辨率设定
+
             cacheService.updateCostMap(robotId, responseDTO.getMsg());
             robotInfoService.updateRobotGrid(robotId, responseDTO.getMsg());
-//            log.info("--------------- json data:{}", JSON.toJSONString(responseDTO.getMsg()));
+            //log.info("--------------- json data:{}", JSON.toJSONString(responseDTO.getMsg()));
+            if (responseDTO != null && responseDTO.getMsg().getData() != null) {
+                ArrayList<Integer> dataNums = new ArrayList<>();
+                //根据阈值转换栅格
+                for (Integer dataNum : responseDTO.getMsg().getData()) {
+                    if (dataNum != -1) {
+                        if (dataNum >= num) {
+                            dataNums.add(0);
+                        } else {
+                            dataNums.add(1);
+                        }
+                    }
+                }
+                writeCache(dataNums,responseDTO.getMsg().getWidth(),responseDTO.getMsg().getHeight(),robotId);
+            }
         }
     }
 
+    private void writeCache(ArrayList<Integer> integers,Integer width,Integer height,String robotId) {
+        List<List<Integer>> arrays = CollectionUtil.split(integers,width);
+        ArrayList<ArrayList<Integer>> arrayList = new ArrayList<>();
+        for (List<Integer> array : arrays) {
+            arrayList.add((ArrayList)array);
+        }
+        CollectionUtil.reverse(arrayList);
+        CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_COST_MAP + robotId, arrayList);
+//        FileUtil.writeAsString(new File(path + "map.txt"), CollectionUtil.join(integers, " ").replaceAll("\\[", "").replaceAll("]", ""));
+    }
+
     @Override
     public void updateRobotPoint(PoseMessageResponseDTO dto, String robotId) {
-        //todo cache
         UpdateWrapper<RobotStatusInfo> query = new UpdateWrapper<>();
         query.set("postion_x", dto.getX());
         query.set("postion_y", dto.getY());
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotTrajectoryInfoServiceImpl.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotTrajectoryInfoServiceImpl.java
index 6c9625f..f1262a2 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotTrajectoryInfoServiceImpl.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/RobotTrajectoryInfoServiceImpl.java
@@ -57,29 +57,29 @@
         });
 
         //读取导航状态,如果导航未开启则不存储
-//        RobotStatusInfo statusInfo = robotStatusInfoService.getStatusByRobotId(robotId);
-        Long routeId= cacheService.readRouteId(Long.valueOf(robotId));
-        if (routeId != null && responseDTO.getMsg() != null) {
+        //RobotStatusInfo statusInfo = robotStatusInfoService.getStatusByRobotId(robotId);
+
+        if ( responseDTO.getMsg() != null) {
             PoseMessageResponseDTO dto = responseDTO.getMsg();
             //执行循迹 保存当前轨迹信息
             //更新实时定位信息
             CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.ROBOT_POINT + robotId, dto);
-            infoService.pointTestCallBack(dto, robotId);
-            if (routeId != null) {
-                RobotTrajectoryInfo info = new RobotTrajectoryInfo();
-                info.setRobotId(Long.valueOf(robotId));
-                info.setCreateTime(new Date());
-                info.setPostionX(dto.getX());
-                info.setPostionY(dto.getY());
-                info.setPostionZ(dto.getZ());
-                info.setTheta(dto.getTheta());
-                save(info);
-                try {
-                    infoService.pointCallBack(dto, robotId);
-                } catch (Exception e) {
-                    log.error("task error", e);
-                }
+            try {
+                infoService.carPathPlanning(dto, robotId);
+            } catch (Exception e) {
+                log.error("task error", e);
             }
+//            if (routeId != null) {
+//                RobotTrajectoryInfo info = new RobotTrajectoryInfo();
+//                info.setRobotId(Long.valueOf(robotId));
+//                info.setCreateTime(new Date());
+//                info.setPostionX(dto.getX());
+//                info.setPostionY(dto.getY());
+//                info.setPostionZ(dto.getZ());
+//                info.setTheta(dto.getTheta());
+//                save(info);
+
+//            }
         }
     }
 
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/TaskGridInfoServiceImpl.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/TaskGridInfoServiceImpl.java
new file mode 100644
index 0000000..cf61def
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/TaskGridInfoServiceImpl.java
@@ -0,0 +1,28 @@
+package com.casic.missiles.modular.robot.service.impl;
+
+import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
+import com.baomidou.mybatisplus.extension.plugins.pagination.Page;
+import com.baomidou.mybatisplus.extension.service.impl.ServiceImpl;
+import com.casic.missiles.modular.robot.dao.TaskGridInfoMapper;
+import com.casic.missiles.modular.robot.model.TaskGridInfo;
+import com.casic.missiles.modular.robot.service.ITaskGridInfoService;
+import org.springframework.stereotype.Service;
+
+import java.util.List;
+
+
+/**
+ * <p>
+ * 任务网格信息 服务实现类
+ * </p>
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+@Service
+public class TaskGridInfoServiceImpl extends ServiceImpl<TaskGridInfoMapper, TaskGridInfo> implements ITaskGridInfoService {
+    @Override
+    public List<TaskGridInfo> selectTaskGridInfoPage(Page<TaskGridInfo> page, QueryWrapper<TaskGridInfo> query) {
+        return this.baseMapper.selectTaskGridInfoPage(page, query);
+    }
+}
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/TaskInfoServiceImpl.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/TaskInfoServiceImpl.java
index d1417b0..fd49e2e 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/TaskInfoServiceImpl.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/service/impl/TaskInfoServiceImpl.java
@@ -6,10 +6,10 @@
 import cn.hutool.core.convert.Convert;
 import cn.hutool.core.date.DateUtil;
 import cn.hutool.core.io.FileUtil;
-import cn.hutool.core.thread.ExecutorBuilder;
 import cn.hutool.core.util.StrUtil;
 import com.alibaba.fastjson.JSON;
 import com.baomidou.mybatisplus.core.conditions.Wrapper;
+import com.baomidou.mybatisplus.core.conditions.query.LambdaQueryWrapper;
 import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
 import com.baomidou.mybatisplus.core.conditions.update.UpdateWrapper;
 import com.baomidou.mybatisplus.core.metadata.IPage;
@@ -47,7 +47,6 @@
 import java.io.Serializable;
 import java.math.BigDecimal;
 import java.util.*;
-import java.util.concurrent.ThreadPoolExecutor;
 
 
 /**
@@ -70,6 +69,7 @@
     private final IRobotCacheService cacheService;
     private final IBaseRobotService baseRobotService;
     private final ITaskInfoImgService imgService;
+    private final ITaskGridInfoService taskGridInfoService;
     /**
      * 初始化线程池
      */
@@ -79,12 +79,19 @@
     @Value("${casic.doc.temp}")
     private String docTemp;
     /**
+     * 5000 ms
+     */
+    @Value("${casic.task.duration:3000}")
+    private Integer stayDuration;
+    @Value("${casic.algorithm.path:-1}")
+    private String path;
+    /**
      * 障碍物精度
      */
     @Value("${casic.obstacle:10}")
     private Integer obstacle;
 
-    public TaskInfoServiceImpl(@Lazy IRouteInfoService routeInfoService, IRobotStatusInfoService statusInfoService, AbstractDictService dictService, @Lazy IAlarmRecordService recordService, @Lazy ITaskHeatMapService taskHeatMapService, IAlgorithmService algorithmService, IRobotCacheService cacheService, @Lazy IBaseRobotService baseRobotService, ITaskInfoImgService imgService) {
+    public TaskInfoServiceImpl(@Lazy IRouteInfoService routeInfoService, IRobotStatusInfoService statusInfoService, AbstractDictService dictService, @Lazy IAlarmRecordService recordService, @Lazy ITaskHeatMapService taskHeatMapService, IAlgorithmService algorithmService, IRobotCacheService cacheService, @Lazy IBaseRobotService baseRobotService, ITaskInfoImgService imgService, ITaskGridInfoService taskGridInfoService) {
         this.routeInfoService = routeInfoService;
         this.statusInfoService = statusInfoService;
         this.dictService = dictService;
@@ -94,6 +101,7 @@
         this.cacheService = cacheService;
         this.baseRobotService = baseRobotService;
         this.imgService = imgService;
+        this.taskGridInfoService = taskGridInfoService;
     }
 
     /**
@@ -184,7 +192,7 @@
         if (CollectionUtil.isEmpty(routeInfos)) {
             throw new ServiceException(500, "路线不存在,请及时更新");
         }
-        RouteInfo routeInfo = routeInfos.get(0);
+//        RouteInfo routeInfo = routeInfos.get(0);
         Integer isTask = 0;
         //开启寻源任务
         if (request.getIsTask() == 1) {
@@ -193,22 +201,21 @@
         RobotInfo robotInfo = cacheService.getRobotInfo(Convert.toLong(robotId));
         CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_IS_TASK + robotId, isTask);
         //添加任务记录
-        TaskInfo taskInfo = addTaskByStartTrack(routeInfo, robotInfo);
+        TaskInfo taskInfo = addTaskByStartTrack( robotInfo, request);
         //No.3 更新器人执行任务信息
-        Long currRouteId = routeInfo.getId();
+//        Long currRouteId = routeInfo.getId();
         Long currTaskId = taskInfo.getId();
 
-        statusInfoService.updateCurrTask(robotId, currRouteId, currTaskId);
+        statusInfoService.updateCurrTask(robotId,  currTaskId);
         //算法初始化标志创建
         CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_TASK_ID + robotId, currTaskId);
         CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_TASK_INIT + robotId, 0);
-        CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_ROUTE_ID + robotId, routeInfo.getId());
+//        CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_ROUTE_ID + robotId, routeInfo.getId());
     }
 
     private void closeTask(StartTrackRequest request, String robotId) {
         //关闭操作
         RobotStatusInfo statusInfo = statusInfoService.getStatusByRobotId(robotId);
-
         if (statusInfo.getCurrTaskId() != null) {
             UpdateWrapper<TaskInfo> updateWrapper = new UpdateWrapper<>();
             updateWrapper.set("update_time", new Date());
@@ -221,10 +228,11 @@
 
         //清空任务缓存
         robotCacheClean(robotId);
-
+        //清楚规划任务锁
+        CacheUtil.remove(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_PLANING_FLAG + robotId);
+        CacheUtil.remove(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_ESTIMATE_GRID+ robotId);
         //清空机器当前任务
-        statusInfoService.updateCurrTask(robotId, null, null);
-
+        statusInfoService.updateCurrTask(robotId, null);
     }
 
     /**
@@ -249,7 +257,6 @@
 
         //判定任务是否已经完成初始化
         Long longId = Convert.toLong(robotId);
-        Long taskId = CacheUtil.get(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_TASK_ID + robotId);
         //缓存当前点位信息
         CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_XY_POINT + robotId, dto);
         RobotInfo robotInfo = cacheService.getRobotInfo(longId);
@@ -260,86 +267,203 @@
             log.error("no config 中子源阈值未配置");
             return;
         }
-
         //读取是否是循迹任务
         Integer isTask = cacheService.readIsTask(longId);
         if (isTask != 1) {
             log.info("循迹任务,不做处理");
             return;
         }
-        //算法初始化状态读取
-        Integer initValue = cacheService.readTaskInit(longId);
-
 
         //No.1 任务初始化
+//        pathInit(longId, robotInfo, pointDTO, taskId, robotId);
+    }
+
+    @Override
+    public void carPathPlanning(PoseMessageResponseDTO dto, String robotId) {
+        //判定任务是否已经完成初始化
+        Long longId = Convert.toLong(robotId);
+        Long taskId = CacheUtil.get(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_TASK_ID + robotId);
+        //缓存当前点位信息
+        CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_XY_POINT + robotId, dto);
+        RobotInfo robotInfo = cacheService.getRobotInfo(longId);
+        //slam经纬度转栅格
+        GridPointDTO pointDTO = algorithmService.convertSlamToGrid(dto.getX(), dto.getY(), robotInfo.getResolution(), robotInfo);
+        if (robotInfo.getAlarmThreshold() == null || robotInfo.getMaxThreshold() == null) {
+            log.error("no config 中子源阈值未配置");
+            return;
+        }
+
+        //读取是否是寻源任务
+        Integer isTask = cacheService.readIsTask(longId);
+        if (isTask != 1) {
+            log.info("循迹任务,不做处理");
+            return;
+        }
+
+        TaskInfo taskInfo = getById(taskId);
+        //No.1 任务初始化
+        pathInit(longId, robotInfo, pointDTO, taskId, robotId, taskInfo.getStepLen());
+        //No.2 路线规划
+        pathPlaning(longId, robotInfo, pointDTO, taskId, robotId, taskInfo,dto);
+    }
+
+    /**
+     * 路线规划进行加锁
+     *
+     * @param robotId 机器人ID
+     * @return
+     */
+    private synchronized boolean lockPlanIngFlag(String robotId) {
+        Boolean flag = CacheUtil.get(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_PLANING_FLAG + robotId);
+        if (flag == null) {
+            CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_PLANING_FLAG + robotId, true);
+            return true;
+        }
+        return false;
+    }
+
+    /**
+     * @param longId    机器人ID
+     * @param robotInfo 机器人信息
+     * @param pointDTO  当前点位栅格信息
+     * @param taskId    任务ID
+     * @param robotId   机器人ID
+     */
+    private void pathPlaning(Long longId, RobotInfo robotInfo, GridPointDTO pointDTO, Long taskId, String robotId, TaskInfo taskInfo,PoseMessageResponseDTO curPoint) {
+        if (taskId != null) {
+            //规划节点
+            PointNextDTO pointNextDTO = CacheUtil.get(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_ESTIMATE_GRID + robotId);
+            GridPointDTO oldEstimateGrid = GridPointDTO.convertGridPoint(pointNextDTO);
+            Boolean flag = lockPlanIngFlag(robotId);
+            //是否到达预期栅格
+            if (!flag) {
+                log.info("未获取到规划任务,直接结束：{}",pointDTO);
+                return;
+            }
+            try {
+                log.debug("oldEstimateGrid:{}", oldEstimateGrid);
+                if (pointNextDTO == null || isEquals(pointDTO, oldEstimateGrid)) {
+                    log.info("当前栅格变更......", pointDTO);
+                    CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_POINT + robotId, pointDTO);
+                    //算法调用获取热力图
+                    PathPlanDTO pathPlanDTO = createPlanDTO(robotInfo, pointDTO, taskId, robotId, taskInfo.getStepLen());
+                    if (taskInfo.getMaxThreshold() == null) {
+                        taskInfo.setMaxThreshold(robotInfo.getMaxThreshold());
+                    }
+                    pathPlanDTO.setTh(taskInfo.getMaxThreshold() + "");
+
+                    log.debug("heat map -- robotId:{},taskId:{}", robotId, taskId);
+                    pathPlanDTO.setZNow(cacheService.getNeutronCount(longId));
+
+                    //存储算法输出栅格信息
+                    AlgorithmResponse response = algorithmService.pathPlanning(pathPlanDTO);
+                    CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_ESTIMATE_GRID + robotId, response.getPointNextDTO());
+                    sendWebSocket(InstructStatusMsgEnums.HEAT_MAP, robotId, response.getTaskHeatMap());
+
+                    //存储循迹任务信息
+                    TaskGridInfo taskGridInfo = new TaskGridInfo();
+                    taskGridInfo.setCreateTime(new Date());
+                    taskGridInfo.setUpdateTime(new Date());
+                    taskGridInfo.setCurGridX(pointDTO.getX().intValue());
+                    taskGridInfo.setCurGridY(pointDTO.getY().intValue());
+                    taskGridInfo.setNeutronCount(new BigDecimal(pathPlanDTO.getZNow()));
+                    taskGridInfo.setEstimateGridX(response.getPointNextDTO().getX());
+                    taskGridInfo.setEstimateGridY(response.getPointNextDTO().getY());
+                    taskGridInfo.setTaskId(taskId);
+                    taskGridInfoService.save(taskGridInfo);
+
+                    try {
+                        //停留时间阈值
+                        Thread.sleep(stayDuration);
+                    } catch (InterruptedException e) {
+                        e.printStackTrace();
+                    }
+
+                    //是否触发停止状态
+                    if (!response.isEnd()) {
+                        log.debug("task planning-- xun dian robotId:{},taskId:{}", robotId, taskId);
+                        //机器人寻点指令下发
+                        sendTargetPoint(robotInfo, response,curPoint);
+                    } else {
+                        log.debug("task stop-- robotId:{},taskId:{}", robotId, taskId);
+                        //机器人停止指令下发
+                        SoftwareStopRequest request = new SoftwareStopRequest();
+                        request.setRobotId(robotInfo.getId());
+                        //设置为急停
+                        request.setControl(1);
+//                        baseRobotService.execCmdHandler(request, InstructCodeEnums.SOFTWARE_STOP);
+                        //循迹任务结束
+                        closeTask(null, robotId);
+                    }
+                }
+            } catch (Exception e) {
+                e.printStackTrace();
+            } finally {
+                //释放锁
+                CacheUtil.remove(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_PLANING_FLAG + robotId);
+            }
+        }
+    }
+
+    private PathPlanDTO createPlanDTO(RobotInfo robotInfo, GridPointDTO pointDTO, Long taskId, String robotId, Integer stepLen) {
+        PathPlanDTO pathPlanDTO = new PathPlanDTO();
+        pathPlanDTO.setTaskId(taskId);
+        pathPlanDTO.setRobotId(robotId);
+        pathPlanDTO.setStepLen(stepLen);
+        return pathPlanDTO;
+    }
+
+    /**
+     * 寻源初始化
+     *
+     * @param longId    机器ID
+     * @param robotInfo 机器信息
+     * @param pointDTO  当前点位信息
+     * @param taskId    任务ID
+     * @param robotId   机器ID
+     * @param stepLen   步长设置
+     */
+    private void pathInit(Long longId, RobotInfo robotInfo, GridPointDTO pointDTO, Long taskId, String robotId, Integer stepLen) {
+        //障碍物栅格上传已生成 直接读取
+        //算法初始化状态读取
+        Integer initValue = cacheService.readTaskInit(longId);
+        //No.1 任务初始化
         if (initValue != 1) {
+
             //执行初始化操作
             if (cacheService.updateTaskInit(longId, 1)) {
 
-                try {
-                    File file = new File(".");
-                    File[] files = file.listFiles((dir, name) -> (name.endsWith(".png") && name.startsWith("step"))
-                            || (name.endsWith(".txt") && name.startsWith("step")) || name.equals("repetition.txt") || name.equals("xy_train.txt") || name.equals("z_train.txt"));
-                    if (files != null) {
-                        for (int i = 0; i < files.length; i++) {
-                            FileUtil.del(files[i]);
+                List<List<Integer>> costList = CacheUtil.get(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_COST_MAP + robotId);
+                List<String> strs = new ArrayList<>();
+                List<String> strs2 = new ArrayList<>();
+                for (List<Integer> list : costList) {
+                    strs2.add(CollectionUtil.join(list, " "));
+                }
+                FileUtil.writeLines(strs2, new File(path + "map2.txt"), "utf-8");
+                //行转列 为算法输入数据
+                if(CollectionUtil.isNotEmpty(costList)){
+                    CollectionUtil.reverse(costList);
+                    for (int i = 0; i < costList.get(0).size(); i++) {
+                        ArrayList<Integer> ints = new ArrayList<>();
+                        for (List<Integer> list : costList) {
+                            ints.add(list.get(i));
                         }
+                        strs.add(CollectionUtil.join(ints, " "));
                     }
-                } catch (Exception e) {
-                    e.printStackTrace();
+//                    CollectionUtil.reverse(strs);
                 }
 
-                PathInitDTO pathInitDTO = createInitDTO(robotInfo, pointDTO, taskId, robotId);
+
+                FileUtil.writeLines(strs, new File(path + "map.txt"), "utf-8");
+
+                PathInitDTO pathInitDTO = createInitDTO(robotInfo, pointDTO, taskId, robotId, stepLen);
                 //中子源计数率读取
-                pathInitDTO.setCount(cacheService.getNeutronCount(longId));
+                pathInitDTO.setZNow(cacheService.getNeutronCount(longId));
+                log.info("init params:{}", JSON.toJSONString(pathInitDTO));
                 //执行1次算法 初始化操作
                 algorithmService.pathPlanningInit(pathInitDTO);
             }
         }
-
-        //No.2 任务执行中
-        if (taskId != null) {
-            //判断栅格变化
-            GridPointDTO oldPointDTO = CacheUtil.get(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_POINT + robotId);
-            if (oldPointDTO == null || isChange(pointDTO, oldPointDTO)) {
-                log.debug("grid change -- robotId:{},taskId:{}", robotId, taskId);
-                CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_POINT + robotId, pointDTO);
-                //算法调用获取热力图
-                PathPlanDTO pathPlanDTO = createPlanDTO(robotInfo, pointDTO, taskId, robotId);
-                //可通行标记为1 不可通行标记为0
-                String obstacle = getObstacle(robotId, pointDTO);
-                //TO-DO 障碍物判定信息
-                pathPlanDTO.setOpen(obstacle);
-                //中子源计数率读取
-                pathPlanDTO.setCount(cacheService.getNeutronCount(longId));
-                log.debug("heat map -- robotId:{},taskId:{}", robotId, taskId);
-                AlgorithmResponse response = algorithmService.pathPlanning(pathPlanDTO);
-                //存储算法输出栅格信息
-                CacheUtil.put(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_ESTIMATE_GRID + robotId, response.getPointNextDTO());
-                sendWebSocket(InstructStatusMsgEnums.HEAT_MAP, robotId, response.getTaskHeatMap());
-                //是否触发寻源操作
-                BigDecimal bigCount = new BigDecimal(pathPlanDTO.getCount());
-                if (bigCount.compareTo(robotInfo.getAlarmThreshold()) >= 0) {
-                    log.debug("task planning-- xun dian robotId:{},taskId:{}", robotId, taskId);
-                    //机器人寻点指令下发
-                    sendTargetPoint(robotInfo, response);
-                }
-
-                //是否触发停止状态
-                if (bigCount.compareTo(robotInfo.getMaxThreshold()) > 0) {
-                    log.debug("task stop-- robotId:{},taskId:{}", robotId, taskId);
-                    //机器人停止指令下发
-                    SoftwareStopRequest request = new SoftwareStopRequest();
-                    request.setRobotId(robotInfo.getId());
-                    //设置为急停
-                    request.setControl(1);
-                    baseRobotService.execCmdHandler(request, InstructCodeEnums.SOFTWARE_STOP);
-                    //循迹任务结束
-                    closeTask(null, robotId);
-                }
-
-            }
-        }
     }
 
     /**
@@ -353,7 +477,6 @@
         //判定任务是否已经完成初始化
         Long longId = Convert.toLong(robotId);
         Long taskId = CacheUtil.get(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_TASK_ID + robotId);
-
         RobotInfo robotInfo = cacheService.getRobotInfo(longId);
         //slam经纬度转栅格
         GridPointDTO pointDTO = algorithmService.convertSlamToGrid(dto.getX(), dto.getY(), robotInfo.getResolution(), robotInfo);
@@ -428,7 +551,7 @@
      * @param robotInfo 机器人信息
      * @param response  算法输出数据
      */
-    private void sendTargetPoint(RobotInfo robotInfo, AlgorithmResponse response) {
+    private void sendTargetPoint(RobotInfo robotInfo, AlgorithmResponse response,PoseMessageResponseDTO curPoint) {
         //读取已设置巡点目标信息
         PointNextDTO nextDTO = CacheUtil.get(RobotDictConstants.CACHE_SCHEMA, RobotDictConstants.KEY_CUR_TARGET_POINT_ID + robotInfo.getId());
         //巡点
@@ -439,8 +562,8 @@
             TargetPointRequest request = new TargetPointRequest();
             request.setX(Convert.toStr(gridCenterPoint.getX()));
             request.setY(Convert.toStr(gridCenterPoint.getY()));
-            request.setZ("0");
-
+            request.setZ(curPoint.getZ().toString());
+            request.setTheta(curPoint.getTheta().toString());
             request.setRobotId(robotInfo.getId());
             //执行巡点指令
             baseRobotService.execCmdHandler(request, InstructCodeEnums.SET_TARGETPOINT);
@@ -507,21 +630,20 @@
         params.put("startTime", startTime);
         params.put("endTime", endTime);
         //读取
-        List<AlarmRecord> records = recordService.selectListByTaskId(info.getId(), info.getRobotId());
         Integer index = 1;
-
-        if (CollectionUtil.isNotEmpty(records)) {
-            for (AlarmRecord record : records) {
+        LambdaQueryWrapper<TaskGridInfo> taskGridQuery = new LambdaQueryWrapper<>();
+        taskGridQuery.eq(TaskGridInfo::getTaskId,info.getId());
+        taskGridQuery.orderByAsc(TaskGridInfo::getCreateTime);
+        List<TaskGridInfo> taskGridInfos = taskGridInfoService.list(taskGridQuery);
+        if (CollectionUtil.isNotEmpty(taskGridInfos)) {
+            for (TaskGridInfo record : taskGridInfos) {
                 //栅格信息转换
-                GridPointDTO pointDTO = algorithmService.convertSlamToGrid(record.getX(), record.getY(), info.getResolution(), robotInfo);
-                record.setX(pointDTO.getX());
-                record.setY(pointDTO.getY());
                 record.setIndex(index);
                 index++;
             }
         }
 
-        params.put("records", records);
+        params.put("records", taskGridInfos);
         ImageEntity mcaImg = new ImageEntity();
         mcaImg.setHeight(600);
         mcaImg.setWidth(600);
@@ -557,7 +679,6 @@
             psdImg.setData(WordUtil.getImageBase64(uploadPath + info.getPsdImg()));
         }
         params.put("psdImg", psdImg);
-
         WordUtil.exportOutputStream(docTemp, response.getOutputStream(), params);
     }
 
@@ -589,7 +710,7 @@
     private String saveHeatImg(String taskId) {
         String newFileName = "task\\" + taskId + ".png";
         //读取当前路径图片
-        File file = new File(".");
+        File file = new File(path + "\\" + taskId + "\\figs");
         File[] files = file.listFiles((dir, name) -> name.endsWith(".png") && name.startsWith("step"));
         //判定当前执行路径最大step图片
         if (files != null && files.length > 0) {
@@ -614,33 +735,28 @@
         return true;
     }
 
-    private PathPlanDTO createPlanDTO(RobotInfo robotInfo, GridPointDTO pointDTO, Long taskId, String robotId) {
-        PathPlanDTO pathPlanDTO = new PathPlanDTO();
-        String xMax = robotInfo.getGridHeight().divide(robotInfo.getResolution(), 5, BigDecimal.ROUND_HALF_UP).intValue() + "";
-        String yMax = robotInfo.getGridWidth().divide(robotInfo.getResolution(), 5, BigDecimal.ROUND_HALF_UP).intValue() + "";
-        pathPlanDTO.setX(pointDTO.getX().intValue() + "");
-        pathPlanDTO.setY(pointDTO.getY().intValue() + "");
-        pathPlanDTO.setXMax(xMax);
-        pathPlanDTO.setYMax(yMax);
-        pathPlanDTO.setTaskId(taskId);
-        pathPlanDTO.setRobotId(robotId);
-        return pathPlanDTO;
+    private boolean isEquals(GridPointDTO pointDTO, GridPointDTO oldPointDTO) {
+        if(oldPointDTO==null||oldPointDTO.getX()==null){
+            return false;
+        }
+        if (pointDTO.getX().compareTo(oldPointDTO.getX()) == 0 && pointDTO.getY().compareTo(oldPointDTO.getY()) == 0) {
+            return true;
+        }
+        return false;
     }
 
-    private PathInitDTO createInitDTO(RobotInfo robotInfo, GridPointDTO pointDTO, Long taskId, String robotId) {
-        String xMax = robotInfo.getGridHeight().divide(robotInfo.getResolution(), 5, BigDecimal.ROUND_HALF_UP).intValue() + "";
-        String yMax = robotInfo.getGridWidth().divide(robotInfo.getResolution(), 5, BigDecimal.ROUND_HALF_UP).intValue() + "";
+
+    private PathInitDTO createInitDTO(RobotInfo robotInfo, GridPointDTO pointDTO, Long taskId, String robotId, Integer stepLen) {
         PathInitDTO pathInitDTO = new PathInitDTO();
-        pathInitDTO.setXMax(xMax);
-        pathInitDTO.setYMax(yMax);
-        pathInitDTO.setX(pointDTO.getX().intValue() + "");
-        pathInitDTO.setY(pointDTO.getY().intValue() + "");
+        pathInitDTO.setXNow(pointDTO.getX().intValue() + "");
+        pathInitDTO.setYNow(pointDTO.getY().intValue() + "");
+        pathInitDTO.setStepLen(stepLen);
         pathInitDTO.setTaskId(taskId);
         pathInitDTO.setRobotId(robotId);
         return pathInitDTO;
     }
 
-    private TaskInfo addTaskByStartTrack(RouteInfo routeInfo, RobotInfo robotInfo) {
+    private TaskInfo addTaskByStartTrack( RobotInfo robotInfo, StartTrackRequest request) {
         //No.2 新增任务信息
         TaskInfo taskInfo = new TaskInfo();
         String currDate = DateUtil.format(new Date(), "yyyyMMdd");
@@ -654,16 +770,20 @@
         taskInfo.setCreateTime(new Date());
         //任务类型 中子源
         taskInfo.setTaskType(1);
-        taskInfo.setDiscernType(1);
-        taskInfo.setTaskName(routeInfo.getRouteName() + "巡线任务");
+        taskInfo.setDiscernType(2);
+        taskInfo.setTaskName("自主巡检任务");
         //执行状态 未完成
         taskInfo.setTaskStatus(0);
         taskInfo.setStartTime(new Date());
-        taskInfo.setRouteId(routeInfo.getId());
+        taskInfo.setRouteId(null);
         taskInfo.setRobotId(robotInfo.getId());
         taskInfo.setGridHeight(robotInfo.getGridHeight());
         taskInfo.setGridWidth(robotInfo.getGridWidth());
         taskInfo.setResolution(robotInfo.getResolution());
+
+        taskInfo.setStepLen(request.getStepLen());
+        taskInfo.setMaxThreshold(request.getMaxThreshold());
+
         save(taskInfo);
         return taskInfo;
     }
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/utils/AlgorithmUtils.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/utils/AlgorithmUtils.java
index 22864d1..467b9c2 100644
--- a/casic-server/src/main/java/com/casic/missiles/modular/robot/utils/AlgorithmUtils.java
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/utils/AlgorithmUtils.java
@@ -1,6 +1,7 @@
 package com.casic.missiles.modular.robot.utils;
 
 import java.io.BufferedReader;
+import java.io.File;
 import java.io.IOException;
 import java.io.InputStreamReader;
 import java.util.ArrayList;
@@ -16,11 +17,13 @@
     public static List<String> invokeJob(List<String> params) {
         List<String> res = new ArrayList<>();
         try {
-
             BufferedReader br = null;
             BufferedReader brError;
             String line = null;
-            Process p = new ProcessBuilder(params).start();
+            ProcessBuilder processBuilder =  new ProcessBuilder(params);
+            processBuilder.directory(new File("D:\\workspace\\server\\keti\\casic-robot-inspection\\casic-web\\src\\main\\resources\\config\\path"));
+            Process p = processBuilder.start();
+
             br = new BufferedReader(new InputStreamReader(p.getInputStream()));
             brError = new BufferedReader(new InputStreamReader(p.getErrorStream()));
             while ((line = br.readLine()) != null || (line = brError.readLine()) != null) {
@@ -37,33 +40,29 @@
     public static void main(String[] args) {
         String path = "D:\\workspace\\server\\keti\\casic-robot-inspection\\casic-web\\src\\main\\resources\\config\\path\\";
         //算法初始化调用
-        String exePath = path + "PathPlanning_init.exe";
+        String exePath = path + "init.exe";
 
         ArrayList<String> invocation = new ArrayList<String>();
         invocation.add(exePath);
-        invocation.add("40");
-        invocation.add("20");
-        invocation.add("0.5");
+        invocation.add("10");
+        invocation.add("10");
         invocation.add("5");
-        invocation.add("5");
+        invocation.add("0.16");
+        invocation.add("Mission1");
         invokeJob(invocation);
 
         //{"count":"null","robotId":"1","taskId":"1748224487696142338","x":"8","xMax":"100","y":"2","yMax":"100"}
-        String planning = path + "PathPlanning.exe";
         //{"open":"0,0,0,0,0,0,0,0","robotId":"1","taskId":"1748250669749735425","x":"6","xMax":"100","y":"2","yMax":"100"}
         //{"open":"1,1,1,1,1,1,1,1","robotId":"1","taskId":"1748252894098178049","x":"6","xMax":"100","y":"2","yMax":"100"}
         //{"count":"6.0","open":"1,1,1,1,1,1,1,1","robotId":"1","taskId":"1750713404601159681","x":"6","xMax":"20","y":"2","yMax":"40"}
+        String planning = path + "search.exe";
         ArrayList<String> planInvocation = new ArrayList<String>();
         planInvocation.add(planning);
-        planInvocation.add("40");
-        planInvocation.add("40");
-        planInvocation.add("1,1,0,1,1,1,1,1");
-        planInvocation.add("6.0");
-        //next_X
-        planInvocation.add("6");
-        //next_Y
-        planInvocation.add("2");
-        ProcessBuilder processBuilder = new ProcessBuilder();
-        invokeJob(planInvocation);
+        planInvocation.add("10");
+        planInvocation.add("1.0");
+        planInvocation.add("0.2");
+        planInvocation.add("Mission1");
+        List<String> res = invokeJob(planInvocation);
+        System.out.println(res);
     }
 }
\ No newline at end of file
diff --git a/casic-server/src/main/java/com/casic/missiles/modular/robot/vo/TaskGridInfoVO.java b/casic-server/src/main/java/com/casic/missiles/modular/robot/vo/TaskGridInfoVO.java
new file mode 100644
index 0000000..0aff5bf
--- /dev/null
+++ b/casic-server/src/main/java/com/casic/missiles/modular/robot/vo/TaskGridInfoVO.java
@@ -0,0 +1,58 @@
+package com.casic.missiles.modular.robot.vo;
+
+import io.swagger.annotations.ApiModelProperty;
+import lombok.Data;
+import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
+import com.casic.missiles.modular.robot.model.TaskGridInfo;
+import cn.hutool.core.util.StrUtil;
+
+import java.math.BigDecimal;
+
+/**
+ * 任务网格信息对象VO
+ *
+ * @author lwh
+ * @date 2024-03-08
+ */
+@Data
+public class TaskGridInfoVO {
+    private static final long serialVersionUID = 1L;
+
+
+            @ApiModelProperty(value = "任务ID" , dataType = "Long")
+            private Long taskId;
+
+            @ApiModelProperty(value = "估算栅格x" , dataType = "Long")
+            private Long estimateGridX;
+
+            @ApiModelProperty(value = "估算栅格y" , dataType = "Long")
+            private Long estimateGridY;
+
+            @ApiModelProperty(value = "中子计数率" , dataType = "BigDecimal")
+            private BigDecimal neutronCount;
+
+            @ApiModelProperty(value = "当前栅格x" , dataType = "Long")
+            private Long curGridX;
+
+            @ApiModelProperty(value = "当前栅格y" , dataType = "Long")
+            private Long curGridY;
+
+public QueryWrapper<TaskGridInfo> genQuery() {
+QueryWrapper<TaskGridInfo> query = new QueryWrapper<>();
+if (taskId!=null) {
+        query.eq("task_id",taskId);
+}
+if (estimateGridX!=null) {
+        query.eq("estimate_grid_x",estimateGridX);
+}
+if (estimateGridY!=null) {
+        query.eq("estimate_grid_y",estimateGridY);
+}
+if (curGridX!=null) {
+        query.eq("cur_grid_x",curGridX);
+}
+if (curGridY!=null) {
+        query.eq("cur_grid_y",curGridY);
+}
+return query;
+}}
diff --git a/casic-web/src/main/resources/config/application.yml b/casic-web/src/main/resources/config/application.yml
index 04420fc..05c3633 100644
--- a/casic-web/src/main/resources/config/application.yml
+++ b/casic-web/src/main/resources/config/application.yml
@@ -42,11 +42,12 @@
   session-invalidate-time: 86400 #session失效时间(只在单机环境下生效，,多机环境在SpringSessionConfig类中配置) 单位：秒
   session-validation-interval: 900 #多久检测一次失效的session(只在单机环境下生效) 单位：秒
   no-login-urls: /user/login,/kaptcha,/config/baseConfig
-  hikSdk: D:\code\robot\casic-robot-inspection\casic-web\src\main\resources\config\hiklib\HCNetSDK.dll
-  hikPlay: D:\code\robot\casic-robot-inspection\casic-web\src\main\resources\config\hiklib\PlayCtrl.dll
+  hikSdk: D:\workspace\server\keti\casic-robot-inspection\casic-web\src\main\resources\config\hiklib\HCNetSDK.dll
+  hikPlay: D:\workspace\server\keti\casic-robot-inspection\casic-web\src\main\resources\config\hiklib\PlayCtrl.dll
   algorithm:
-    initPath: D:\workspace\server\keti\casic-robot-inspection\casic-web\src\main\resources\config\path\PathPlanning_init.exe
-    planPath: D:\workspace\server\keti\casic-robot-inspection\casic-web\src\main\resources\config\path\PathPlanning.exe
+    path:  D:\workspace\server\keti\casic-robot-inspection\casic-web\src\main\resources\config\path\
+    initPathName: init.exe
+    planPathName: search.exe
   doc:
     temp: D:\workspace\server\keti\casic-robot-inspection\casic-web\src\main\resources\config\path\report.docx
   config:
diff --git a/casic-web/src/main/resources/config/path/PathPlanning.exe b/casic-web/src/main/resources/config/path/PathPlanning.exe
deleted file mode 100644
index 6678e0f..0000000
--- a/casic-web/src/main/resources/config/path/PathPlanning.exe
+++ /dev/null
Binary files differ
diff --git a/casic-web/src/main/resources/config/path/PathPlanning_init.exe b/casic-web/src/main/resources/config/path/PathPlanning_init.exe
deleted file mode 100644
index d89dc16..0000000
--- a/casic-web/src/main/resources/config/path/PathPlanning_init.exe
+++ /dev/null
Binary files differ
diff --git a/casic-web/src/main/resources/config/path/map.txt b/casic-web/src/main/resources/config/path/map.txt
new file mode 100644
index 0000000..61e2959
--- /dev/null
+++ b/casic-web/src/main/resources/config/path/map.txt
@@ -0,0 +1,36 @@
+0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
+0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
+0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 0 0
+0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 1 0 0
+0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0
+0 0 0 0 0 0 0 0 1 1 1 1 0 0 0 0 0 0
+0 0 0 0 0 0 1 1 1 1 1 1 1 0 0 0 0 0
+0 0 0 0 0 0 1 1 1 1 1 1 1 1 1 0 0 0
+0 0 0 0 1 1 1 1 1 1 1 1 1 1 1 0 0 0
+0 0 0 0 1 1 1 1 1 1 1 1 1 1 1 0 0 0
+0 0 0 0 1 1 1 1 1 1 1 1 1 1 1 0 0 0
+0 0 0 0 1 1 1 1 1 1 1 1 1 1 1 0 0 0
+0 0 0 0 1 1 1 1 1 1 1 1 1 1 1 0 0 0
+0 0 0 0 1 1 1 1 1 1 1 1 1 1 1 0 0 0
+0 0 0 0 1 1 1 1 1 1 1 1 1 1 0 0 0 0
+0 0 0 0 1 1 1 1 1 1 1 1 1 1 0 0 0 0
+0 0 0 0 1 1 1 1 1 1 1 1 1 1 0 0 0 0
+0 0 0 0 1 1 1 1 1 1 1 1 1 1 0 0 0 0
+0 0 0 0 0 0 0 0 1 1 1 1 1 0 0 0 0 0
+0 0 0 0 0 0 0 0 0 0 0 1 1 0 0 0 0 0
+0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
+0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
+0 0 0 1 1 1 1 1 1 1 1 1 1 1 0 0 0 0
+0 0 0 1 1 1 1 1 1 1 1 1 1 1 0 0 0 0
+0 0 0 0 1 1 1 1 1 1 1 1 1 1 0 0 0 0
+0 0 0 0 1 1 1 1 1 1 1 1 0 0 0 0 0 1
+0 0 0 1 1 1 1 1 1 1 1 1 0 0 0 0 0 1
+0 0 1 1 1 1 1 1 1 1 1 1 1 1 0 0 0 1
+0 0 0 1 1 1 1 1 1 1 1 1 1 1 0 0 0 1
+0 0 0 1 1 1 1 1 1 1 1 1 1 1 0 0 0 1
+0 0 0 1 1 1 1 1 1 1 1 1 1 1 0 0 0 1
+0 0 0 1 1 1 1 1 1 1 1 1 1 1 1 0 0 1
+0 0 1 1 1 1 1 1 1 1 1 1 1 1 1 0 0 1
+0 0 1 1 1 1 1 1 1 1 1 1 1 1 1 0 0 1
+0 0 1 1 1 1 1 1 1 1 1 1 1 1 1 0 0 1
+0 0 1 1 1 1 1 1 1 1 1 1 1 1 1 0 0 1
diff --git a/casic-web/src/main/resources/config/path/report.docx b/casic-web/src/main/resources/config/path/report.docx
index d0f2073..bb8b0a2 100644
--- a/casic-web/src/main/resources/config/path/report.docx
+++ b/casic-web/src/main/resources/config/path/report.docx
Binary files differ