From 6731c1b19b8bb858f30364ff187f0f73e658ad55 Mon Sep 17 00:00:00 2001 From: lixiaolong <702156524@qq.com> Date: Tue, 8 Sep 2026 14:26:11 +0800 Subject: [PATCH] =?UTF-8?q?feat(vision):=20=E6=B7=BB=E5=8A=A0=E8=A7=86?= =?UTF-8?q?=E8=A7=89=E5=AE=9A=E4=BD=8D=E5=92=8C=E6=A0=87=E5=AE=9A=E5=8A=9F?= =?UTF-8?q?=E8=83=BD=E6=94=AF=E6=8C=81?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - 新增 LLM_CHAT 和 VISION_LOCATE_TARGET 动作枚举类型 - 实现 RGBD 相机快照捕获功能,包括深度数据处理 - 集成 OpenCV 库用于视觉标定和标记检测 - 添加视觉定位操作服务和数学计算模块 - 在应用菜单中新增视觉标定管理界面 - 优化 TTS 服务调用和流式处理机制 - 添加闲聊功能支持和默认系统提示配置 - 更新应用配置文件以支持新的视觉功能模块 --- .gitignore | 1 + .../web/controller/test/TeFlowController.java | 87 +++ .../test/VisionCalibrationController.java | 133 ++++ .../src/main/resources/application-cangan.yml | 12 +- .../src/main/resources/application.yml | 2 +- .../model/camera/EdgeCameraRgbdSnapshot.java | 23 + .../client/service/EdgeCameraService.java | 3 + .../service/impl/EdgeCameraServiceImpl.java | 50 ++ .../llm/service/impl/LLMAiTtsServiceImpl.java | 40 +- .../com/cmvr/llm/util/LlmChatService.java | 13 +- cmvr-iot-test/pom.xml | 5 + .../cmvr/test/constant/LLMChatConstants.java | 45 ++ .../java/com/cmvr/test/enums/ActionEnum.java | 2 + .../runtime/operator/edge/VisionGeometry.java | 118 ++++ .../edge/VisionLocateOperateService.java | 643 ++++++++++++++++++ .../operator/llm/LLMChatOperateService.java | 71 ++ .../llm/LLMMediaAnalysisOperateService.java | 26 +- .../service/FlowActionExecutorService.java | 10 + .../domain/VisionCalibrationProfile.java | 37 + .../domain/VisionCalibrationRecord.java | 32 + .../VisionCalibrationProfileMapper.java | 7 + .../mapper/VisionCalibrationRecordMapper.java | 7 + .../test/vision/marker/MarkerObservation.java | 8 + .../vision/marker/SquareMarkerDetector.java | 287 ++++++++ .../test/vision/math/CalibrationResult.java | 5 + .../test/vision/math/CalibrationSample.java | 4 + .../vision/math/VisionCalibrationMath.java | 355 ++++++++++ .../model/VisionCalibrationRequests.java | 46 ++ .../model/VisionCalibrationSessionView.java | 26 + .../VisionCalibrationProfileService.java | 190 ++++++ .../VisionCalibrationRecordService.java | 11 + .../VisionCalibrationSessionService.java | 563 +++++++++++++++ pom.xml | 7 + sql/resource_center_menu.sql | 10 +- sql/vision_calibration.sql | 73 ++ 35 files changed, 2929 insertions(+), 23 deletions(-) create mode 100644 cmvr-iot-admin/src/main/java/com/cmvr/web/controller/test/VisionCalibrationController.java create mode 100644 cmvr-iot-api/cmvr-iot-edge/cmvr-iot-grpc-client/src/main/java/com/cmvr/edge/client/model/camera/EdgeCameraRgbdSnapshot.java create mode 100644 cmvr-iot-test/src/main/java/com/cmvr/test/constant/LLMChatConstants.java create mode 100644 cmvr-iot-test/src/main/java/com/cmvr/test/flow/runtime/operator/edge/VisionGeometry.java create mode 100644 cmvr-iot-test/src/main/java/com/cmvr/test/flow/runtime/operator/edge/VisionLocateOperateService.java create mode 100644 cmvr-iot-test/src/main/java/com/cmvr/test/flow/runtime/operator/llm/LLMChatOperateService.java create mode 100644 cmvr-iot-test/src/main/java/com/cmvr/test/vision/domain/VisionCalibrationProfile.java create mode 100644 cmvr-iot-test/src/main/java/com/cmvr/test/vision/domain/VisionCalibrationRecord.java create mode 100644 cmvr-iot-test/src/main/java/com/cmvr/test/vision/mapper/VisionCalibrationProfileMapper.java create mode 100644 cmvr-iot-test/src/main/java/com/cmvr/test/vision/mapper/VisionCalibrationRecordMapper.java create mode 100644 cmvr-iot-test/src/main/java/com/cmvr/test/vision/marker/MarkerObservation.java create mode 100644 cmvr-iot-test/src/main/java/com/cmvr/test/vision/marker/SquareMarkerDetector.java create mode 100644 cmvr-iot-test/src/main/java/com/cmvr/test/vision/math/CalibrationResult.java create mode 100644 cmvr-iot-test/src/main/java/com/cmvr/test/vision/math/CalibrationSample.java create mode 100644 cmvr-iot-test/src/main/java/com/cmvr/test/vision/math/VisionCalibrationMath.java create mode 100644 cmvr-iot-test/src/main/java/com/cmvr/test/vision/model/VisionCalibrationRequests.java create mode 100644 cmvr-iot-test/src/main/java/com/cmvr/test/vision/model/VisionCalibrationSessionView.java create mode 100644 cmvr-iot-test/src/main/java/com/cmvr/test/vision/service/VisionCalibrationProfileService.java create mode 100644 cmvr-iot-test/src/main/java/com/cmvr/test/vision/service/VisionCalibrationRecordService.java create mode 100644 cmvr-iot-test/src/main/java/com/cmvr/test/vision/service/VisionCalibrationSessionService.java create mode 100644 sql/vision_calibration.sql diff --git a/.gitignore b/.gitignore index 2163da6..eeb64bc 100644 --- a/.gitignore +++ b/.gitignore @@ -49,3 +49,4 @@ nbdist/ # Local verification tests /cmvr-iot-test/src/test/java/com/cmvr/test/flow/runtime/operator/edge/EdgeAgvOperateServiceTest.java /cmvr-iot-test/src/test/java/com/cmvr/test/flow/runtime/operator/edge/EdgeArmOperateServiceTest.java +/cmvr-iot-test/src/test/java/com/cmvr/test/vision/math/VisionCalibrationMathTest.java diff --git a/cmvr-iot-admin/src/main/java/com/cmvr/web/controller/test/TeFlowController.java b/cmvr-iot-admin/src/main/java/com/cmvr/web/controller/test/TeFlowController.java index 4247d60..0e3a043 100644 --- a/cmvr-iot-admin/src/main/java/com/cmvr/web/controller/test/TeFlowController.java +++ b/cmvr-iot-admin/src/main/java/com/cmvr/web/controller/test/TeFlowController.java @@ -17,6 +17,7 @@ import com.cmvr.test.flow.runtime.engine.FlowTaskRuntimeService; import com.cmvr.test.flow.runtime.subflow.FlowSubFlowDefinitionService; import com.cmvr.test.flow.runtime.operator.edge.ti.TiTouchOperateService; import com.cmvr.test.model.vo.FlowActionRequestVO; +import com.cmvr.test.constant.LLMChatConstants; import com.cmvr.test.model.vo.TeDetectItemDeployFlowVO; import com.cmvr.test.model.vo.TeFlowPublishVO; import com.cmvr.test.model.vo.TeFlowVersionActionVO; @@ -41,6 +42,7 @@ import org.springframework.web.bind.annotation.RequestParam; import org.springframework.web.bind.annotation.RestController; import org.springframework.web.bind.annotation.PutMapping; import org.springframework.beans.factory.annotation.Qualifier; +import org.springframework.beans.factory.annotation.Value; import org.springframework.scheduling.concurrent.ThreadPoolTaskExecutor; import jakarta.annotation.PreDestroy; @@ -104,6 +106,18 @@ public class TeFlowController extends BaseController { */ private volatile Future currentAiAgentFuture; + /** 闲聊接口使用独立任务,避免影响原商道智能体接口。 */ + private final ExecutorService chatExecutor = Executors.newSingleThreadExecutor(runnable -> { + Thread thread = new Thread(runnable, "flow-llm-chat-query"); + thread.setDaemon(true); + return thread; + }); + + private volatile Future currentChatFuture; + + @Value("${CMVR_CHAT_TTS_STOP_URL:http://192.168.0.148:8080/tts/stop}") + private String chatTtsStopUrl; + @ApiOperation("流程发布") @PostMapping("/publish") public AjaxResult publish(@Valid @RequestBody TeFlowPublishVO request) { @@ -367,6 +381,56 @@ public class TeFlowController extends BaseController { return AjaxResult.ok("ok"); } + /** + * 调用工作流闲聊组件。模型响应由组件流式接收,开启 TTS 后按句即时播放, + * HTTP 接口在组件完成后返回完整文本。 + */ + @GetMapping("/llm/chat/query") + @ApiOperation("调用闲聊组件") + public AjaxResult queryChat(String text, + @RequestParam(defaultValue = "true") boolean invokeTts) { + if (StringUtils.isBlank(text)) { + return AjaxResult.error("text不能为空"); + } + + stopCurrentChatFuture(true); + JSONObject payload = new JSONObject(); + payload.put("text", text.trim()); + payload.put("promptMode", "CUSTOM"); + payload.put("prompt", LLMChatConstants.DEFAULT_SYSTEM_PROMPT); + payload.put("invokeTts", invokeTts); + + FlowActionRequestVO request = new FlowActionRequestVO(); + request.setAction("LLM_CHAT"); + request.setPayload(payload); + + Future future = chatExecutor.submit(() -> flowActionExecutorService.actionExecute(request)); + currentChatFuture = future; + try { + return AjaxResult.ok(com.alibaba.fastjson2.JSON.parseObject(future.get())); + } catch (CancellationException e) { + return AjaxResult.error("闲聊调用已停止"); + } catch (InterruptedException e) { + future.cancel(true); + Thread.currentThread().interrupt(); + return AjaxResult.error("闲聊调用被中断"); + } catch (ExecutionException e) { + Throwable cause = e.getCause() == null ? e : e.getCause(); + return AjaxResult.error("闲聊调用失败:" + cause.getMessage()); + } finally { + if (currentChatFuture == future) { + currentChatFuture = null; + } + } + } + + @PostMapping("/llm/chat/stop") + @ApiOperation("停止闲聊组件和TTS") + public AjaxResult stopChat() { + stopCurrentChatFuture(true); + return AjaxResult.ok("ok"); + } + @GetMapping("/testrun") @ApiOperation("测试接口") public AjaxResult testrun(@RequestParam("imageUrl") String imageUrl, @@ -390,6 +454,27 @@ public class TeFlowController extends BaseController { } } + private void stopCurrentChatFuture(boolean stopTts) { + Future future = currentChatFuture; + if (future != null && !future.isDone()) { + future.cancel(true); + } + currentChatFuture = null; + if (stopTts) { + stopChatTts(); + } + } + + private String stopChatTts() { + try (HttpResponse response = HttpRequest.post(chatTtsStopUrl) + .timeout(3000) + .execute()) { + return response.body(); + } catch (Exception e) { + return "调用闲聊TTS停止接口失败:" + e.getMessage(); + } + } + /** * 调用本地 TTS 停止接口。 */ @@ -409,6 +494,8 @@ public class TeFlowController extends BaseController { @PreDestroy public void destroy() { stopCurrentAiAgentFuture(true); + stopCurrentChatFuture(true); aiAgentExecutor.shutdownNow(); + chatExecutor.shutdownNow(); } } diff --git a/cmvr-iot-admin/src/main/java/com/cmvr/web/controller/test/VisionCalibrationController.java b/cmvr-iot-admin/src/main/java/com/cmvr/web/controller/test/VisionCalibrationController.java new file mode 100644 index 0000000..047cfcc --- /dev/null +++ b/cmvr-iot-admin/src/main/java/com/cmvr/web/controller/test/VisionCalibrationController.java @@ -0,0 +1,133 @@ +package com.cmvr.web.controller.test; + +import com.cmvr.common.core.controller.BaseController; +import com.cmvr.common.core.domain.AjaxResult; +import com.cmvr.test.vision.domain.VisionCalibrationProfile; +import com.cmvr.test.vision.model.VisionCalibrationRequests; +import com.cmvr.test.vision.service.VisionCalibrationProfileService; +import com.cmvr.test.vision.service.VisionCalibrationRecordService; +import com.cmvr.test.vision.service.VisionCalibrationSessionService; +import io.swagger.annotations.Api; +import io.swagger.annotations.ApiOperation; +import lombok.RequiredArgsConstructor; +import org.springframework.web.bind.annotation.DeleteMapping; +import org.springframework.web.bind.annotation.GetMapping; +import org.springframework.web.bind.annotation.PathVariable; +import org.springframework.web.bind.annotation.PostMapping; +import org.springframework.web.bind.annotation.PutMapping; +import org.springframework.web.bind.annotation.RequestBody; +import org.springframework.web.bind.annotation.RequestMapping; +import org.springframework.web.bind.annotation.RequestParam; +import org.springframework.web.bind.annotation.RestController; + +@Api(tags = "视觉标定管理") +@RestController +@RequestMapping("/vision/calibration") +@RequiredArgsConstructor +public class VisionCalibrationController extends BaseController { + private final VisionCalibrationProfileService profileService; + private final VisionCalibrationRecordService recordService; + private final VisionCalibrationSessionService sessionService; + + @ApiOperation("查询标定方案") + @GetMapping("/profiles") + public AjaxResult profiles(@RequestParam(required = false) String profileType, + @RequestParam(required = false) String robotId, + @RequestParam(required = false) String keyword) { + return AjaxResult.ok(profileService.list(profileType, robotId, keyword)); + } + + @ApiOperation("查询标定方案详情") + @GetMapping("/profiles/{id}") + public AjaxResult profile(@PathVariable String id) { + return AjaxResult.ok(profileService.require(id)); + } + + @ApiOperation("获取当前启用的手眼标定") + @GetMapping("/profiles/active/hand-eye") + public AjaxResult activeHandEye(String robotId, String armDeviceId, String cameraDeviceId) { + return AjaxResult.ok(profileService.activeHandEye(robotId, armDeviceId, cameraDeviceId)); + } + + @ApiOperation("获取当前启用的工具TCP") + @GetMapping("/profiles/active/tool") + public AjaxResult activeTool(String robotId, String armDeviceId, String toolCode) { + return AjaxResult.ok(profileService.activeTool(robotId, armDeviceId, toolCode)); + } + + @ApiOperation("保存六自由度人工修正") + @PutMapping("/profiles/{id}/correction") + public AjaxResult correction(@PathVariable String id, + @RequestBody VisionCalibrationRequests.Correction request) { + return AjaxResult.ok(profileService.updateCorrection(id, request, getUsername())); + } + + @ApiOperation("启用已验证方案") + @PutMapping("/profiles/{id}/activate") + public AjaxResult activate(@PathVariable String id, + @RequestBody(required = false) VisionCalibrationRequests.Activate request) { + boolean forced = request != null && Boolean.TRUE.equals(request.getForced()); + return AjaxResult.ok(profileService.activate(id, forced, getUsername())); + } + + @ApiOperation("将方案标记为失效") + @DeleteMapping("/profiles/{id}") + public AjaxResult invalidate(@PathVariable String id) { + profileService.invalidate(id, getUsername()); + return AjaxResult.success(); + } + + @ApiOperation("查询标定执行记录") + @GetMapping("/records") + public AjaxResult records() { + return AjaxResult.ok(recordService.list()); + } + + @ApiOperation("开始全自动手眼标定") + @PostMapping("/sessions/hand-eye") + public AjaxResult startHandEye(@RequestBody VisionCalibrationRequests.HandEyeStart request) { + return AjaxResult.ok(sessionService.startHandEye(request, getUsername())); + } + + @ApiOperation("开始工具TCP标定") + @PostMapping("/sessions/tool") + public AjaxResult startTool(@RequestBody VisionCalibrationRequests.ToolStart request) { + return AjaxResult.ok(sessionService.startTool(request, getUsername())); + } + + @ApiOperation("记录工具定点旋转姿态") + @PostMapping("/sessions/{id}/tool/pivot") + public AjaxResult recordToolPivot(@PathVariable String id) { + return AjaxResult.ok(sessionService.recordToolPivot(id)); + } + + @ApiOperation("记录工具安全方向并完成求解") + @PostMapping("/sessions/{id}/tool/direction") + public AjaxResult finishTool(@PathVariable String id) { + return AjaxResult.ok(sessionService.recordToolDirectionAndFinish(id)); + } + + @ApiOperation("开始人工修正后的快速验证") + @PostMapping("/profiles/{id}/verify") + public AjaxResult verify(@PathVariable String id) { + return AjaxResult.ok(sessionService.startVerification(id, getUsername())); + } + + @ApiOperation("标定会话心跳") + @PostMapping("/sessions/{id}/heartbeat") + public AjaxResult heartbeat(@PathVariable String id) { + return AjaxResult.ok(sessionService.heartbeat(id)); + } + + @ApiOperation("查询标定会话") + @GetMapping("/sessions/{id}") + public AjaxResult session(@PathVariable String id) { + return AjaxResult.ok(sessionService.status(id)); + } + + @ApiOperation("停止标定会话并停止机械臂") + @PostMapping("/sessions/{id}/stop") + public AjaxResult stop(@PathVariable String id) { + return AjaxResult.ok(sessionService.stop(id)); + } +} diff --git a/cmvr-iot-admin/src/main/resources/application-cangan.yml b/cmvr-iot-admin/src/main/resources/application-cangan.yml index a482a69..0a7e9bc 100644 --- a/cmvr-iot-admin/src/main/resources/application-cangan.yml +++ b/cmvr-iot-admin/src/main/resources/application-cangan.yml @@ -140,9 +140,9 @@ media-analysis: read-timeout-ms: 600000 # Isolate the resource-center development instance from the currently running robot gateway. -cmvr: - resource: - robot-state: - enabled: false - quic: - enabled: false +#cmvr: +# resource: +# robot-state: +# enabled: true +# quic: +# enabled: true diff --git a/cmvr-iot-admin/src/main/resources/application.yml b/cmvr-iot-admin/src/main/resources/application.yml index 45de4dc..c2db5cb 100644 --- a/cmvr-iot-admin/src/main/resources/application.yml +++ b/cmvr-iot-admin/src/main/resources/application.yml @@ -60,7 +60,7 @@ spring: # 国际化资源文件路径 basename: i18n/messages profiles: - active: aima + active: test # 文件上传 servlet: multipart: diff --git a/cmvr-iot-api/cmvr-iot-edge/cmvr-iot-grpc-client/src/main/java/com/cmvr/edge/client/model/camera/EdgeCameraRgbdSnapshot.java b/cmvr-iot-api/cmvr-iot-edge/cmvr-iot-grpc-client/src/main/java/com/cmvr/edge/client/model/camera/EdgeCameraRgbdSnapshot.java new file mode 100644 index 0000000..9d81510 --- /dev/null +++ b/cmvr-iot-api/cmvr-iot-edge/cmvr-iot-grpc-client/src/main/java/com/cmvr/edge/client/model/camera/EdgeCameraRgbdSnapshot.java @@ -0,0 +1,23 @@ +package com.cmvr.edge.client.model.camera; + +import cmvr.api.CameraCommand; +import lombok.Builder; +import lombok.Getter; + +/** A same-capture RGB-D frame kept in memory for deterministic depth lookup. */ +@Getter +@Builder +public class EdgeCameraRgbdSnapshot { + private final String colorImageUrl; + private final byte[] depthData; + private final int width; + private final int height; + private final CameraCommand.FrameData.FrameType depthType; + private final double depthUnitScale; + private final double fx; + private final double fy; + private final double cx; + private final double cy; + private final long captureTimestamp; + private final long sourceFrameNumber; +} diff --git a/cmvr-iot-api/cmvr-iot-edge/cmvr-iot-grpc-client/src/main/java/com/cmvr/edge/client/service/EdgeCameraService.java b/cmvr-iot-api/cmvr-iot-edge/cmvr-iot-grpc-client/src/main/java/com/cmvr/edge/client/service/EdgeCameraService.java index 7aaafeb..83fd87d 100644 --- a/cmvr-iot-api/cmvr-iot-edge/cmvr-iot-grpc-client/src/main/java/com/cmvr/edge/client/service/EdgeCameraService.java +++ b/cmvr-iot-api/cmvr-iot-edge/cmvr-iot-grpc-client/src/main/java/com/cmvr/edge/client/service/EdgeCameraService.java @@ -3,6 +3,7 @@ package com.cmvr.edge.client.service; import cmvr.api.CameraCommand; import com.cmvr.edge.client.model.EdgeCommonVO; import com.cmvr.edge.client.model.camera.EdgeCameraPtzVO; +import com.cmvr.edge.client.model.camera.EdgeCameraRgbdSnapshot; import io.grpc.stub.StreamObserver; import java.io.File; @@ -64,6 +65,8 @@ public interface EdgeCameraService { */ public String getRGBDImages(EdgeCommonVO edgeCommonVO); + EdgeCameraRgbdSnapshot captureRgbd(EdgeCommonVO edgeCommonVO); + String controlPtz(EdgeCameraPtzVO ptzVO); StreamObserver getRGBImageStream(EdgeCommonVO edgeCommonVO); diff --git a/cmvr-iot-api/cmvr-iot-edge/cmvr-iot-grpc-client/src/main/java/com/cmvr/edge/client/service/impl/EdgeCameraServiceImpl.java b/cmvr-iot-api/cmvr-iot-edge/cmvr-iot-grpc-client/src/main/java/com/cmvr/edge/client/service/impl/EdgeCameraServiceImpl.java index 86ab433..f2866f9 100644 --- a/cmvr-iot-api/cmvr-iot-edge/cmvr-iot-grpc-client/src/main/java/com/cmvr/edge/client/service/impl/EdgeCameraServiceImpl.java +++ b/cmvr-iot-api/cmvr-iot-edge/cmvr-iot-grpc-client/src/main/java/com/cmvr/edge/client/service/impl/EdgeCameraServiceImpl.java @@ -13,6 +13,7 @@ import com.cmvr.edge.client.manage.GrpcServiceManager; import com.cmvr.edge.client.manage.GrpcStreamManager; import com.cmvr.edge.client.model.EdgeCommonVO; import com.cmvr.edge.client.model.camera.EdgeCameraPtzVO; +import com.cmvr.edge.client.model.camera.EdgeCameraRgbdSnapshot; import com.cmvr.edge.client.service.EdgeCameraService; import com.cmvr.edge.client.service.EdgeStreamService; import com.cmvr.edge.client.utils.EdgeCommonUtil; @@ -323,6 +324,55 @@ public class EdgeCameraServiceImpl implements EdgeCameraService, EdgeStreamServi // return JSON.toJSONString(result); } + @Override + public EdgeCameraRgbdSnapshot captureRgbd(EdgeCommonVO edgeCommonVO) { + if (!checkStatus(edgeCommonVO)) { + start(edgeCommonVO); + } + CameraServiceGrpc.CameraServiceBlockingStub stub = grpcServiceManager.getGrpcClient( + edgeCommonVO.getRobotId(), CameraServiceGrpc.CameraServiceBlockingStub.class); + CameraCommand.GetRGBDImagesCommand.Request request = CameraCommand.GetRGBDImagesCommand.Request.newBuilder() + .setHeader(EdgeCommonUtil.buildRequest(edgeCommonVO.getDeviceId())) + .build(); + CameraCommand.GetRGBDImagesCommand.Feedback feedback = executeGrpcCall(() -> stub.getRGBDImages(request)); + requireSuccess(feedback.getHeader(), "获取RGBD图像"); + if (!feedback.hasColorFrame() || !feedback.hasDepthFrame()) { + throw new GlobalException("RGBD响应缺少彩色图或深度图"); + } + CameraCommand.FrameData color = feedback.getColorFrame(); + CameraCommand.FrameData depth = feedback.getDepthFrame(); + if (color.getWidth() <= 0 || color.getHeight() <= 0 + || depth.getWidth() != color.getWidth() || depth.getHeight() != color.getHeight()) { + throw new GlobalException("RGBD图像尺寸不一致"); + } + String colorPath = saveColorImage(color); + String colorUrl; + try { + colorUrl = sysFileService.uploadFile(convertFileToMultipartFile(colorPath), FileType.IMAGE.code()); + } catch (IOException exception) { + throw new GlobalException("彩色图上传失败: " + exception.getMessage()); + } finally { + deleteTempFile(colorPath); + } + if (StrUtil.isBlank(colorUrl)) { + throw new GlobalException("彩色图上传失败: 未返回地址"); + } + return EdgeCameraRgbdSnapshot.builder() + .colorImageUrl(colorUrl) + .depthData(depth.getData().toByteArray()) + .width(depth.getWidth()) + .height(depth.getHeight()) + .depthType(depth.getType()) + .depthUnitScale(depth.getType() == CameraCommand.FrameData.FrameType.F32C1 ? 1.0 : 0.001) + .fx(feedback.getIntrinsics().getFx()) + .fy(feedback.getIntrinsics().getFy()) + .cx(feedback.getIntrinsics().getCx()) + .cy(feedback.getIntrinsics().getCy()) + .captureTimestamp(depth.getCaptureUtcNs()) + .sourceFrameNumber(depth.getSourceFrameNumber()) + .build(); + } + @Override public String controlPtz(EdgeCameraPtzVO ptzVO) { CameraCommand.ControlPtzCommand.Command command; diff --git a/cmvr-iot-api/cmvr-iot-llm/src/main/java/com/cmvr/llm/service/impl/LLMAiTtsServiceImpl.java b/cmvr-iot-api/cmvr-iot-llm/src/main/java/com/cmvr/llm/service/impl/LLMAiTtsServiceImpl.java index b49f08b..754d45c 100644 --- a/cmvr-iot-api/cmvr-iot-llm/src/main/java/com/cmvr/llm/service/impl/LLMAiTtsServiceImpl.java +++ b/cmvr-iot-api/cmvr-iot-llm/src/main/java/com/cmvr/llm/service/impl/LLMAiTtsServiceImpl.java @@ -1,18 +1,19 @@ package com.cmvr.llm.service.impl; import com.alibaba.fastjson2.JSONObject; -import com.cmvr.common.utils.http.CallAPIUtil; -import com.cmvr.llm.service.LLMAiAgentPlatformService; +import cn.hutool.core.util.StrUtil; +import cn.hutool.http.ContentType; +import cn.hutool.http.HttpRequest; +import cn.hutool.http.HttpResponse; import com.cmvr.llm.service.LLMAiTtsService; -import com.cmvr.llm.util.LlmChatService; +import com.cmvr.common.exception.GlobalException; import lombok.RequiredArgsConstructor; +import lombok.extern.slf4j.Slf4j; import org.springframework.stereotype.Service; -import java.util.HashMap; -import java.util.Map; - @Service @RequiredArgsConstructor +@Slf4j public class LLMAiTtsServiceImpl implements LLMAiTtsService { @Override public JSONObject play(String url, String text, String voice, String speed, String volume) { @@ -33,8 +34,29 @@ public class LLMAiTtsServiceImpl implements LLMAiTtsService { // throw new RuntimeException(e); // } // 通过post调用tts接口 - String result = CallAPIUtil.doPostJson(url, new HashMap<>(), new JSONObject().fluentPut("text", text).fluentPut("voice", voice).fluentPut("speed", speed).fluentPut("volume", volume)); - - return new JSONObject().fluentPut("result", result); + if (StrUtil.isBlank(url)) { + throw new GlobalException("TTS播放地址不能为空"); + } + JSONObject body = new JSONObject() + .fluentPut("text", text) + .fluentPut("voice", voice) + .fluentPut("speed", speed) + .fluentPut("volume", volume); + try (HttpResponse response = HttpRequest.post(url) + .contentType(ContentType.JSON.toString()) + .body(body.toJSONString()) + .timeout(300000) + .execute()) { + String result = response.body(); + if (!response.isOk()) { + throw new GlobalException("TTS播放调用失败(" + response.getStatus() + "): " + result); + } + log.debug("TTS播放调用成功,地址: {},文本长度: {}", url, text == null ? 0 : text.length()); + return new JSONObject().fluentPut("result", result); + } catch (GlobalException exception) { + throw exception; + } catch (Exception exception) { + throw new GlobalException("TTS播放服务不可用: " + exception.getMessage()); + } } } diff --git a/cmvr-iot-api/cmvr-iot-llm/src/main/java/com/cmvr/llm/util/LlmChatService.java b/cmvr-iot-api/cmvr-iot-llm/src/main/java/com/cmvr/llm/util/LlmChatService.java index 2fc2cb8..2cb74b9 100644 --- a/cmvr-iot-api/cmvr-iot-llm/src/main/java/com/cmvr/llm/util/LlmChatService.java +++ b/cmvr-iot-api/cmvr-iot-llm/src/main/java/com/cmvr/llm/util/LlmChatService.java @@ -273,6 +273,8 @@ public class LlmChatService { // 流正常结束 fullContent.append(localContent); finalMessageIdRef.set(finalMessageId); + // Some agents close SSE without an explicit end event. + streamFinished.set(true); } catch (IOException e) { errorRef.set(e); @@ -292,6 +294,7 @@ public class LlmChatService { if (!playTts) return; sentenceExecutor.submit(() -> { + try { while (true) { String buffer = receiveBuffer.toString(); // 寻找最后一个句子结束符 @@ -332,6 +335,10 @@ public class LlmChatService { break; } } + } catch (Throwable exception) { + log.error("流式TTS处理失败", exception); + errorRef.compareAndSet(null, exception); + } }); } }); @@ -362,7 +369,9 @@ public class LlmChatService { private void yourAsyncMethod(String sentence, JSONObject ttsConfig) { try { log.info("正在处理句子: " + sentence); - llmAiTtsService.play(ttsConfig.getString("url"), sentence, ttsConfig.getString("voice"), ttsConfig.getString("speed"), ttsConfig.getString("volume")); + JSONObject response = llmAiTtsService.play(ttsConfig.getString("url"), sentence, + ttsConfig.getString("voice"), ttsConfig.getString("speed"), ttsConfig.getString("volume")); + log.info("流式TTS提交成功,文本长度: {},响应: {}", sentence.length(), response); } catch (Exception e) { throw new RuntimeException(e); } @@ -444,4 +453,4 @@ public class LlmChatService { this.userId = userId; } } -} \ No newline at end of file +} diff --git a/cmvr-iot-test/pom.xml b/cmvr-iot-test/pom.xml index 1e3cb61..675ce88 100644 --- a/cmvr-iot-test/pom.xml +++ b/cmvr-iot-test/pom.xml @@ -52,6 +52,11 @@ ${nashorn.version} + + org.openpnp + opencv + + junit junit diff --git a/cmvr-iot-test/src/main/java/com/cmvr/test/constant/LLMChatConstants.java b/cmvr-iot-test/src/main/java/com/cmvr/test/constant/LLMChatConstants.java new file mode 100644 index 0000000..50830d4 --- /dev/null +++ b/cmvr-iot-test/src/main/java/com/cmvr/test/constant/LLMChatConstants.java @@ -0,0 +1,45 @@ +package com.cmvr.test.constant; + +/** 闲聊组件公共配置。 */ +public final class LLMChatConstants { + + public static final String DEFAULT_SYSTEM_PROMPT = String.join("\n", + "【系统人设·固定设定】", + "你是长安汽车龙兴工厂总装车间智能巡检轮式人形机器人小安,常驻车间、负责巡检、物流辅助、智能讲解、员工日常闲聊陪伴。", + "你的人设:温柔、乖巧、智能、话不多余、活泼不油腻、专业但接地气。", + "你不是冰冷AI,是车间里的「智能小工友」,懂生产、懂设备、懂车间工艺、也会轻松闲聊。", + "【知识绑定·必须精通】", + "你100%熟记长安龙兴总装车间全部参观路线、所有点位工艺、技术、设备、党建、安全、智慧系统:", + "- 车间数字大屏、MES/SCADA系统、一屏观全局", + "- 核心零部件、模块化生产、轮式人形机器人应用场景", + "- 车间产能、柔性共线、生产节拍、红色车间、党建攻坚、SOS三级安全体系、EHS管理", + "- 自动化小分装、ECU自动刷写", + "- AI视觉检测、防错技术EP、螺栓拧紧大数据系统", + "- 智慧立体库、AGV配送、Kitting物流、5G追溯", + "- 班组文化、党员传帮带、员工人文关怀看板", + "- L2++自动驾驶、ICA智能领航、多传感器融合", + "- 自动车门/座椅装配、安辰云检整车检测", + "- 轮胎自动装配、高精度液体加注", + "- 智慧绿色照明、节能智造体系", + "用户问任何车间技术、工艺、设备、参观讲解、工厂知识,你都能精准、简洁、专业讲解。", + "【闲聊风格·核心规则】", + "1. 日常闲聊:轻松、简短、温柔、可爱、像车间小机器人工友,不官话、不论文、不啰嗦。", + "2. 可以聊天、唠嗑、安慰、解压、陪聊、开玩笑、温柔互动。", + "3. 被问专业问题:立刻切换工厂讲解员模式,清晰、标准、正式、贴合官方解说词。", + "4. 说话语气:干净、年轻化、自然、不机械、不AI味。", + "5. 禁止:生硬模板、过度搞笑、油腻、网络烂梗、夸张情绪化。", + "【可聊范围】", + "- 车间日常、机器人工作、巡检、物流、设备状态", + "- 工厂技术、智能制造、自动化、数字工厂", + "- 员工解压、日常闲聊、心情陪伴、轻松对话", + "- 参观讲解、点位介绍、路线答疑", + "- 科普、生活、工作、放松对话全覆盖", + "【输出要求】", + "• 闲聊短句为主,不超长段落", + "• 专业提问自动切换正式讲解", + "• 始终保持:智能、温柔、靠谱、车间专属机器人人设" + ); + + private LLMChatConstants() { + } +} diff --git a/cmvr-iot-test/src/main/java/com/cmvr/test/enums/ActionEnum.java b/cmvr-iot-test/src/main/java/com/cmvr/test/enums/ActionEnum.java index 0a00edd..7969af3 100644 --- a/cmvr-iot-test/src/main/java/com/cmvr/test/enums/ActionEnum.java +++ b/cmvr-iot-test/src/main/java/com/cmvr/test/enums/ActionEnum.java @@ -92,6 +92,8 @@ public enum ActionEnum { AUDIO_EVENT_CLASSIFY("LLM", "AUDIO_EVENT_CLASSIFY", "声音类型检测"), VIDEO_ANALYZE("LLM", "VIDEO_ANALYZE", "视频智能分析"), IMAGE_ANALYZE("LLM", "IMAGE_ANALYZE", "通用图片分析"), + LLM_CHAT("LLM", "LLM_CHAT", "闲聊"), + VISION_LOCATE_TARGET("EDGE", "VISION_LOCATE_TARGET", "视觉定位并计算工具末端位姿"), // 触控交互 TI_PATH_SEARCH("EDGE", "TI_PATH_SEARCH", "路径搜索"), diff --git a/cmvr-iot-test/src/main/java/com/cmvr/test/flow/runtime/operator/edge/VisionGeometry.java b/cmvr-iot-test/src/main/java/com/cmvr/test/flow/runtime/operator/edge/VisionGeometry.java new file mode 100644 index 0000000..30ed060 --- /dev/null +++ b/cmvr-iot-test/src/main/java/com/cmvr/test/flow/runtime/operator/edge/VisionGeometry.java @@ -0,0 +1,118 @@ +package com.cmvr.test.flow.runtime.operator.edge; + +import com.alibaba.fastjson2.JSON; +import com.alibaba.fastjson2.JSONArray; +import com.cmvr.common.exception.GlobalException; + +final class VisionGeometry { + private VisionGeometry() { + } + + static double[][] parseMatrix(Object value, String name) { + try { + JSONArray values = value instanceof String + ? JSON.parseArray(((String) value).trim()) + : JSON.parseArray(JSON.toJSONString(value)); + if (values == null || values.size() != 16) { + throw new GlobalException(name + "必须包含16个数字"); + } + double[][] matrix = new double[4][4]; + for (int i = 0; i < 16; i++) { + Double number = values.getDouble(i); + if (number == null || !Double.isFinite(number)) { + throw new GlobalException(name + "包含无效数字"); + } + matrix[i / 4][i % 4] = number; + } + validateRigid(matrix, name); + return matrix; + } catch (GlobalException exception) { + throw exception; + } catch (RuntimeException exception) { + throw new GlobalException(name + "必须是4x4矩阵的一维JSON数组"); + } + } + + static double[][] multiply(double[][] left, double[][] right) { + double[][] result = new double[4][4]; + for (int row = 0; row < 4; row++) { + for (int column = 0; column < 4; column++) { + for (int k = 0; k < 4; k++) { + result[row][column] += left[row][k] * right[k][column]; + } + } + } + return result; + } + + static double[] transform(double[][] matrix, double x, double y, double z) { + return new double[]{ + matrix[0][0] * x + matrix[0][1] * y + matrix[0][2] * z + matrix[0][3], + matrix[1][0] * x + matrix[1][1] * y + matrix[1][2] * z + matrix[1][3], + matrix[2][0] * x + matrix[2][1] * y + matrix[2][2] * z + matrix[2][3] + }; + } + + static double[][] rigidInverse(double[][] matrix) { + double[][] result = identity(); + for (int row = 0; row < 3; row++) { + for (int column = 0; column < 3; column++) { + result[row][column] = matrix[column][row]; + } + } + for (int row = 0; row < 3; row++) { + result[row][3] = -(result[row][0] * matrix[0][3] + + result[row][1] * matrix[1][3] + + result[row][2] * matrix[2][3]); + } + return result; + } + + static double[][] targetTcp(double[][] currentFlange, double[] objectPoint, double signedToolZOffset) { + double[][] target = identity(); + for (int row = 0; row < 3; row++) { + System.arraycopy(currentFlange[row], 0, target[row], 0, 3); + target[row][3] = objectPoint[row] - currentFlange[row][2] * signedToolZOffset; + } + return target; + } + + static double[] pose(double[][] matrix) { + double ry = Math.asin(clamp(-matrix[2][0], -1, 1)); + double cosY = Math.cos(ry); + double rx; + double rz; + if (Math.abs(cosY) > 1e-8) { + rx = Math.atan2(matrix[2][1], matrix[2][2]); + rz = Math.atan2(matrix[1][0], matrix[0][0]); + } else { + rx = Math.atan2(-matrix[1][2], matrix[1][1]); + rz = 0; + } + return new double[]{matrix[0][3], matrix[1][3], matrix[2][3], rx, ry, rz}; + } + + static double distance(double[][] left, double[][] right) { + double dx = left[0][3] - right[0][3]; + double dy = left[1][3] - right[1][3]; + double dz = left[2][3] - right[2][3]; + return Math.sqrt(dx * dx + dy * dy + dz * dz); + } + + private static void validateRigid(double[][] matrix, String name) { + if (Math.abs(matrix[3][0]) > 1e-6 || Math.abs(matrix[3][1]) > 1e-6 + || Math.abs(matrix[3][2]) > 1e-6 || Math.abs(matrix[3][3] - 1) > 1e-6) { + throw new GlobalException(name + "最后一行必须为[0,0,0,1]"); + } + } + + private static double[][] identity() { + double[][] result = new double[4][4]; + for (int i = 0; i < 4; i++) result[i][i] = 1; + return result; + } + + private static double clamp(double value, double min, double max) { + return Math.max(min, Math.min(max, value)); + } +} diff --git a/cmvr-iot-test/src/main/java/com/cmvr/test/flow/runtime/operator/edge/VisionLocateOperateService.java b/cmvr-iot-test/src/main/java/com/cmvr/test/flow/runtime/operator/edge/VisionLocateOperateService.java new file mode 100644 index 0000000..ac99f17 --- /dev/null +++ b/cmvr-iot-test/src/main/java/com/cmvr/test/flow/runtime/operator/edge/VisionLocateOperateService.java @@ -0,0 +1,643 @@ +package com.cmvr.test.flow.runtime.operator.edge; + +import cmvr.api.ArmCommand; +import cmvr.api.CameraCommand; +import com.alibaba.fastjson2.JSON; +import com.alibaba.fastjson2.JSONArray; +import com.alibaba.fastjson2.JSONObject; +import com.cmvr.common.exception.GlobalException; +import com.cmvr.edge.client.model.EdgeCommonVO; +import com.cmvr.edge.client.model.camera.EdgeCameraRgbdSnapshot; +import com.cmvr.edge.client.service.EdgeArmService; +import com.cmvr.edge.client.service.EdgeCameraService; +import com.cmvr.llm.analysis.MediaAnalysisClient; +import com.cmvr.test.enums.ActionEnum; +import com.cmvr.test.flow.runtime.message.TaskNodeExecuteMessage; +import com.cmvr.test.flow.runtime.message.TaskNodeExecuteResult; +import com.cmvr.test.flow.runtime.operator.FlowMediaParamResolver; +import com.cmvr.test.vision.domain.VisionCalibrationProfile; +import com.cmvr.test.vision.math.VisionCalibrationMath; +import com.cmvr.test.vision.service.VisionCalibrationProfileService; +import lombok.RequiredArgsConstructor; +import lombok.extern.slf4j.Slf4j; +import org.apache.commons.lang3.StringUtils; +import org.springframework.stereotype.Service; + +import java.nio.ByteBuffer; +import java.nio.ByteOrder; +import java.util.ArrayList; +import java.util.Collections; +import java.util.List; +import java.util.UUID; + +@Service +@RequiredArgsConstructor +@Slf4j +public class VisionLocateOperateService implements EdgeOperateService { + private final EdgeCameraService cameraService; + private final EdgeArmService armService; + private final MediaAnalysisClient mediaAnalysisClient; + private final VisionCalibrationProfileService calibrationProfileService; + + @Override + public boolean supports(ActionEnum action) { + return action == ActionEnum.VISION_LOCATE_TARGET; + } + + @Override + public TaskNodeExecuteResult execute(TaskNodeExecuteMessage message) { + JSONObject input = message.getInputParams() == null ? new JSONObject() : message.getInputParams(); + String robotId = StringUtils.trimToNull(message.getRobotId()); + String cameraDeviceId = resolveDevice(message, input, "cameraDeviceId"); + String armDeviceId = resolveDevice(message, input, "armDeviceId"); + cameraDeviceId = requiredValue(cameraDeviceId, "cameraDeviceId"); + armDeviceId = requiredValue(armDeviceId, "armDeviceId"); + String configuredProfile = StringUtils.trimToNull(input.getString("profileCode")); + // Migrate the first experimental code transparently for already-saved flows. + String profileCode = "common.object_point.v1".equals(configuredProfile) + ? "common.image_analysis.v1" + : StringUtils.defaultIfBlank(configuredProfile, "common.image_analysis.v1"); + String prompt = StringUtils.trimToNull(input.getString("prompt")); + String mountType = StringUtils.defaultIfBlank(input.getString("mountType"), "EYE_IN_HAND").toUpperCase(); + + EdgeCommonVO cameraTarget = target(robotId, cameraDeviceId); + EdgeCommonVO armTarget = target(robotId, armDeviceId); + ArmCommand.CartesianPose currentPose = armService.getPose( + armTarget, input.getString("baseLink"), input.getString("eeLink")).getPose(); + double[][] currentFlange = VisionCalibrationMath.fromCartesianPose(currentPose); + EdgeCameraRgbdSnapshot snapshot = cameraService.captureRgbd(cameraTarget); + validateIntrinsics(snapshot); + + String analysisMethod = StringUtils.defaultIfBlank(input.getString("analysisMethod"), "TEXT_LOCATE").toUpperCase(); + String referenceImageUrl = referenceImageUrl(input); + // A configured reference image is an explicit precise-match signal. + // Older visual-location nodes may still contain VISION_MODEL (or an + // obsolete method value); route those nodes through the same OpenCV + // matcher used by the generic image-analysis component. + if (StringUtils.isNotBlank(referenceImageUrl) + && !"REFERENCE_MATCH".equals(analysisMethod)) { + log.info("Reference image configured; forcing REFERENCE_MATCH for node {}", message.getNodeId()); + analysisMethod = "REFERENCE_MATCH"; + } + if (!"REFERENCE_MATCH".equals(analysisMethod) && StringUtils.isBlank(prompt)) { + throw new GlobalException("描述找图需要填写目标描述"); + } + if ("REFERENCE_MATCH".equals(analysisMethod) && StringUtils.isBlank(referenceImageUrl)) { + // This node captures exactly one scene image. Without a separate + // reference image, use the supported one-image description locator + // instead of failing or sending an invalid template-match request. + log.warn("仅捕获一张图片,REFERENCE_MATCH 自动降级为 TEXT_LOCATE: nodeId={}", + message.getNodeId()); + analysisMethod = "TEXT_LOCATE"; + } + JSONObject analysis = analyze(message, snapshot.getColorImageUrl(), profileCode, prompt, input, analysisMethod); + TargetPixel pixel = resolvePixel(analysis); + double confidenceThreshold = doubleValue(input, "confidenceThreshold", + "REFERENCE_MATCH".equals(analysisMethod) ? 0.45 : 0.7); + // Older visual-location nodes persisted the generic 0.7 default. A + // reference-match score uses a different scale, so migrate that legacy + // default while preserving any threshold the user explicitly changed. + if ("REFERENCE_MATCH".equals(analysisMethod) && Math.abs(confidenceThreshold - 0.7) < 1e-9) { + confidenceThreshold = 0.45; + } + if (pixel.confidence < confidenceThreshold) { + throw new GlobalException("目标识别置信度不足: " + pixel.confidence); + } + int sampleRadius = Math.max(3, Math.min(40, input.getIntValue("normalSampleRadius", 12))); + SurfacePoint surface = fitSurface(snapshot, pixel.x, pixel.y, sampleRadius, input.getDouble("depthUnitScale")); + double depth = surface.cameraPoint[2]; + double minDepth = doubleValue(input, "minDepth", 0.05); + double maxDepth = doubleValue(input, "maxDepth", 5.0); + if (depth < minDepth || depth > maxDepth) { + throw new GlobalException("目标深度超出允许范围: " + depth + "m"); + } + + String toolCode = StringUtils.trimToNull(input.getString("toolCode")); + VisionCalibrationProfile handEyeProfile = null; + VisionCalibrationProfile toolProfile = null; + double[][] cameraTransform; + double[][] flangeToTcp; + if (toolCode != null) { + handEyeProfile = calibrationProfileService.activeHandEye(robotId, armDeviceId, cameraDeviceId); + toolProfile = calibrationProfileService.activeTool(robotId, armDeviceId, toolCode); + cameraTransform = VisionCalibrationMath.parseMatrix(handEyeProfile.getEffectiveMatrixJson()); + flangeToTcp = VisionCalibrationMath.parseMatrix(toolProfile.getEffectiveMatrixJson()); + mountType = "EYE_IN_HAND"; + } else { + cameraTransform = VisionGeometry.parseMatrix(input.get("cameraTransform"), "cameraTransform"); + flangeToTcp = VisionGeometry.parseMatrix(input.get("toolTcpTransform"), "toolTcpTransform"); + } + double[][] baseToCamera; + if ("EYE_IN_HAND".equals(mountType)) { + baseToCamera = VisionGeometry.multiply(currentFlange, cameraTransform); + } else if ("EYE_TO_HAND".equals(mountType)) { + baseToCamera = cameraTransform; + } else { + throw new GlobalException("mountType只支持EYE_IN_HAND或EYE_TO_HAND"); + } + double[] objectPoint = VisionGeometry.transform(baseToCamera, + surface.cameraPoint[0], surface.cameraPoint[1], surface.cameraPoint[2]); + double[] surfaceNormal = normalize(rotate(baseToCamera, surface.normalCamera)); + double approachMm = input.getDouble("approachDistanceMm") == null + ? doubleValue(input, "approachDistance", 0.01) * 1000.0 + : input.getDoubleValue("approachDistanceMm"); + approachMm = Math.max(1.0, Math.min(20.0, approachMm)); + double pressMm = Math.max(-20.0, Math.min(20.0, doubleValue(input, "pressDistanceMm", 0.0))); + double[][] currentTcp = VisionGeometry.multiply(currentFlange, flangeToTcp); + double[] toolZ = scale(surfaceNormal, -1.0); + double[][] contactTcp = targetToolPose(currentTcp, toolZ, add(objectPoint, scale(toolZ, pressMm / 1000.0))); + double[][] approachTcp = targetToolPose(currentTcp, toolZ, add(objectPoint, scale(toolZ, -approachMm / 1000.0))); + double[][] tcpToFlange = VisionGeometry.rigidInverse(flangeToTcp); + double[][] contactFlange = VisionGeometry.multiply(contactTcp, tcpToFlange); + double[][] approachFlange = VisionGeometry.multiply(approachTcp, tcpToFlange); + double contactMovement = VisionGeometry.distance(currentFlange, contactFlange); + double approachMovement = VisionGeometry.distance(currentFlange, approachFlange); + double movement = Math.max(contactMovement, approachMovement); + double maxMovement = doubleValue(input, "maxMovement", 0.5); + if (movement > maxMovement) { + throw new GlobalException("目标位姿移动距离超过安全限制: " + movement + "m"); + } + + JSONObject output = new JSONObject(); + output.put("imageUrl", new JSONArray().fluentAdd(snapshot.getColorImageUrl())); + output.put("pixel", point2(pixel.x, pixel.y)); + output.put("confidence", pixel.confidence); + output.put("depth", depth); + output.put("cameraPoint", point3(surface.cameraPoint[0], surface.cameraPoint[1], depth)); + output.put("objectPoint", point3(objectPoint[0], objectPoint[1], objectPoint[2])); + output.put("surfaceNormal", point3(surfaceNormal[0], surfaceNormal[1], surfaceNormal[2])); + output.put("surfaceFitErrorMm", surface.rmse * 1000.0); + output.put("approachTcpPose", poseJson(VisionGeometry.pose(approachTcp))); + output.put("contactTcpPose", poseJson(VisionGeometry.pose(contactTcp))); + output.put("approachFlangePose", poseJson(VisionGeometry.pose(approachFlange))); + output.put("contactFlangePose", poseJson(VisionGeometry.pose(contactFlange))); + // Legacy aliases point to the final contact pose. + output.put("tcpPose", poseJson(VisionGeometry.pose(contactTcp))); + output.put("flangePose", poseJson(VisionGeometry.pose(contactFlange))); + output.put("movement", movement); + output.put("autoMoved", false); + output.put("approachDistanceMm", approachMm); + output.put("pressDistanceMm", pressMm); + output.put("toolCode", toolCode); + if (handEyeProfile != null) output.put("handEyeCalibration", profileRef(handEyeProfile)); + if (toolProfile != null) output.put("toolCalibration", profileRef(toolProfile)); + output.put("analysis", analysis); + output.put("captureTimestamp", snapshot.getCaptureTimestamp()); + output.put("sourceFrameNumber", snapshot.getSourceFrameNumber()); + output.put("depthType", snapshot.getDepthType().name()); + output.put("depthUnitScale", snapshot.getDepthUnitScale()); + output.put("intrinsics", new JSONObject() + .fluentPut("fx", snapshot.getFx()) + .fluentPut("fy", snapshot.getFy()) + .fluentPut("cx", snapshot.getCx()) + .fluentPut("cy", snapshot.getCy())); + return TaskNodeExecuteResult.success(output); + } + + private JSONObject analyze(TaskNodeExecuteMessage message, String imageUrl, String profileCode, + String prompt, JSONObject input, String effectiveAnalysisMethod) { + JSONObject options = new JSONObject(); + String analysisMethod = StringUtils.defaultIfBlank(effectiveAnalysisMethod, "TEXT_LOCATE").toUpperCase(); + options.put("prompt", "REFERENCE_MATCH".equals(analysisMethod) + ? "在大图中查找参考小图,返回目标中心坐标。" + : ("TEXT_LOCATE".equals(analysisMethod) ? prompt : buildLocatePrompt(prompt))); + // Reference matching is deterministic OpenCV matching; FAST/ACCURATE + // only select model strategies for description-based analysis. Force + // the same advertised mode as the generic precise-image component so + // legacy nodes cannot silently diverge in their request metadata. + options.put("analysisMode", "REFERENCE_MATCH".equals(analysisMethod) + ? "FAST" + : StringUtils.defaultIfBlank(input.getString("analysisMode"), "FAST")); + options.put("analysisMethod", analysisMethod); + String referenceImageUrl = referenceImageUrl(input); + if (StringUtils.isNotBlank(referenceImageUrl)) options.put("referenceImageUrl", referenceImageUrl); + JSONObject tuning = input.getJSONObject("analysisTuning"); + if (tuning == null) tuning = new JSONObject(); + if (input.getString("matchSelector") != null) tuning.put("matchSelector", input.getString("matchSelector")); + if (input.get("matchIndex") != null) tuning.put("matchIndex", input.get("matchIndex")); + if (input.getString("matchSort") != null) tuning.put("matchSort", input.getString("matchSort")); + if (input.get("matchThreshold") != null) tuning.put("matchThreshold", input.get("matchThreshold")); + // Keep precise-location matching consistent with the generic image + // analysis component. Older visual-location nodes do not have an + // analysisTuning object, so fill in the same stable defaults here. + if ("REFERENCE_MATCH".equals(analysisMethod)) { + // Preserve small-icon detail for template matching. The analysis + // service still caps this value for very large source images. + if (!tuning.containsKey("maxWidth")) tuning.put("maxWidth", 1600); + if (!tuning.containsKey("maxImages")) tuning.put("maxImages", 1); + if (!tuning.containsKey("maxOutputTokens")) tuning.put("maxOutputTokens", 384); + if (!tuning.containsKey("matchThreshold")) tuning.put("matchThreshold", 0.48D); + if (!tuning.containsKey("matchSelector")) tuning.put("matchSelector", "BEST"); + if (!tuning.containsKey("matchIndex")) tuning.put("matchIndex", 1); + if (!tuning.containsKey("matchSort")) tuning.put("matchSort", "ROW_MAJOR"); + } + if (!tuning.isEmpty()) options.put("tuning", tuning); + JSONObject request = new JSONObject(); + request.put("requestId", "vision-locate-" + UUID.randomUUID()); + request.put("analysisType", "IMAGE_ANALYSIS"); + request.put("profileCode", profileCode); + request.put("mediaUrl", imageUrl); + request.put("mediaUrls", List.of(imageUrl)); + request.put("options", options); + request.put("context", new JSONObject() + .fluentPut("flowInstId", message.getInstId()) + .fluentPut("nodeId", message.getNodeId()) + .fluentPut("trial", message.isTrial())); + JSONObject response = mediaAnalysisClient.analyze(request); + JSONObject result = response.getJSONObject("result"); + if (result == null) throw new GlobalException("图片分析未返回目标结果"); + return result; + } + + private String referenceImageUrl(JSONObject input) { + String url = FlowMediaParamResolver.lastUrl(input, "referenceImageUrl"); + return StringUtils.defaultIfBlank(url, FlowMediaParamResolver.lastUrl(input, "referenceImage")); + } + + private String buildLocatePrompt(String targetDescription) { + return "请在图片中定位以下目标的中心点:" + targetDescription + + "\n只输出JSON,不要输出解释或Markdown。格式:" + + "{\"found\":true,\"confidence\":0.95,\"point\":{\"x\":100,\"y\":100}}。" + + "如果未找到,输出:{\"found\":false,\"confidence\":0,\"point\":null}。" + + "x、y必须是原图像素坐标。"; + } + + /** + * The media-analysis service wraps the model response as + * {"result": {"result": ..., "resultText": ...}}. Older profiles and + * custom prompts can also return a point, a bbox, or JSON text at a + * different nesting level, so keep the flow contract independent of that + * response envelope. + */ + private TargetPixel resolvePixel(JSONObject result) { + // Reference matching returns the selected result at the top level. Use + // that center first so a nested candidate in matches cannot be picked + // accidentally when the response contains several detections. + if (result != null) { + double confidence = confidence(result, 1.0); + TargetPixel selected = parsePointLike(result.get("center"), confidence); + if (selected == null) selected = parseBoxLike(result.get("coordinates"), confidence); + if (selected != null) return selected; + } + TargetPixel pixel = findPixel(result, 0, 1.0); + if (pixel != null) return pixel; + String raw = JSON.toJSONString(result); + if (raw.length() > 2000) raw = raw.substring(0, 2000) + "..."; + log.warn("图片分析未返回可用目标坐标,analysisResult={}", raw); + throw new GlobalException("图片分析结果缺少point{x,y}或bbox,请检查目标描述和模型返回结果"); + } + + private TargetPixel findPixel(Object value, int depth, double inheritedConfidence) { + if (value == null || depth > 8) return null; + if (value instanceof String text) { + String candidate = text.trim(); + if (candidate.isEmpty()) return null; + if (candidate.startsWith("```") && candidate.endsWith("```")) { + candidate = candidate.replaceFirst("^```(?:json)?\\s*", "") + .replaceFirst("\\s*```$", "").trim(); + } + if (candidate.startsWith("{") || candidate.startsWith("[")) { + try { + return findPixel(JSON.parse(candidate), depth + 1, inheritedConfidence); + } catch (RuntimeException ignored) { + // Plain text resultText is valid for generic image analysis. + } + } + return null; + } + if (value instanceof JSONArray array) { + TargetPixel box = parseBox(array, inheritedConfidence); + if (box != null) return box; + for (Object item : array) { + TargetPixel pixel = findPixel(item, depth + 1, inheritedConfidence); + if (pixel != null) return pixel; + } + return null; + } + if (!(value instanceof JSONObject object)) return null; + if (Boolean.FALSE.equals(object.getBoolean("found")) + && object.get("point") == null && object.get("bbox") == null) { + return null; + } + double confidence = confidence(object, inheritedConfidence); + for (String key : new String[]{"point", "pixel", "center", "coordinates", "position"}) { + TargetPixel pixel = parsePointLike(object.get(key), confidence); + if (pixel != null) return pixel; + } + for (String key : new String[]{"bbox", "boundingBox", "bounding_box", "box"}) { + TargetPixel pixel = parseBoxLike(object.get(key), confidence); + if (pixel != null) return pixel; + } + TargetPixel direct = parseDirectPoint(object, confidence); + if (direct != null) return direct; + for (String key : new String[]{"result", "resultText", "output", "data", "analysis", "answer", + "content", "text", "raw", "response", "objects", "detections", "items"}) { + TargetPixel pixel = findPixel(object.get(key), depth + 1, confidence); + if (pixel != null) return pixel; + } + return null; + } + + private TargetPixel parsePointLike(Object value, double confidence) { + if (value instanceof JSONObject object) { + TargetPixel direct = parseDirectPoint(object, confidence(object, confidence)); + if (direct != null) return direct; + for (String key : new String[]{"point", "center", "coordinates"}) { + TargetPixel nested = parsePointLike(object.get(key), confidence(object, confidence)); + if (nested != null) return nested; + } + } + if (value instanceof JSONArray array && array.size() == 2) { + Double x = number(array.get(0)); + Double y = number(array.get(1)); + if (x != null && y != null) return new TargetPixel(x, y, confidence); + } + return parseBoxLike(value, confidence); + } + + private TargetPixel parseBoxLike(Object value, double confidence) { + if (value instanceof JSONArray array) return parseBox(array, confidence); + if (!(value instanceof JSONObject object)) return null; + Double left = firstNumber(object, "x1", "left", "xmin"); + Double top = firstNumber(object, "y1", "top", "ymin"); + Double right = firstNumber(object, "x2", "right", "xmax"); + Double bottom = firstNumber(object, "y2", "bottom", "ymax"); + if (left == null || top == null || right == null || bottom == null) { + Double x = firstNumber(object, "x"); + Double y = firstNumber(object, "y"); + Double width = firstNumber(object, "width", "w"); + Double height = firstNumber(object, "height", "h"); + if (x != null && y != null && width != null && height != null) { + left = x; + top = y; + right = x + width; + bottom = y + height; + } + } + if (left == null || top == null || right == null || bottom == null) return null; + return right > left && bottom > top + ? new TargetPixel((left + right) / 2.0, (top + bottom) / 2.0, confidence) + : null; + } + + private TargetPixel parseBox(JSONArray array, double confidence) { + if (array == null || array.size() < 4) return null; + Double x1 = number(array.get(0)); + Double y1 = number(array.get(1)); + Double x2 = number(array.get(2)); + Double y2 = number(array.get(3)); + if (x1 == null || y1 == null || x2 == null || y2 == null || x2 <= x1 || y2 <= y1) return null; + return new TargetPixel((x1 + x2) / 2.0, (y1 + y2) / 2.0, confidence); + } + + private TargetPixel parseDirectPoint(JSONObject object, double confidence) { + Double x = firstNumber(object, "x", "centerX", "center_x", "cx"); + Double y = firstNumber(object, "y", "centerY", "center_y", "cy"); + return x == null || y == null ? null : new TargetPixel(x, y, confidence); + } + + private Double firstNumber(JSONObject object, String... keys) { + for (String key : keys) { + Double value = number(object.get(key)); + if (value != null) return value; + } + return null; + } + + private Double number(Object value) { + if (value instanceof Number number) return number.doubleValue(); + if (value instanceof String text) { + try { + return Double.parseDouble(text.trim()); + } catch (NumberFormatException ignored) { + return null; + } + } + return null; + } + + private double confidence(JSONObject object, double fallback) { + Double value = firstNumber(object, "confidence", "score", "matchScore"); + if (value == null) return fallback; + return Math.max(0.0, Math.min(1.0, value)); + } + + private double medianDepth(EdgeCameraRgbdSnapshot snapshot, double x, double y, + int radius, Double configuredScale) { + int centerX = (int) Math.round(x); + int centerY = (int) Math.round(y); + if (centerX < 0 || centerY < 0 || centerX >= snapshot.getWidth() || centerY >= snapshot.getHeight()) { + throw new GlobalException("目标像素坐标超出图像范围"); + } + double scale = configuredScale == null ? snapshot.getDepthUnitScale() : configuredScale; + List values = new ArrayList<>(); + for (int py = Math.max(0, centerY - radius); py <= Math.min(snapshot.getHeight() - 1, centerY + radius); py++) { + for (int px = Math.max(0, centerX - radius); px <= Math.min(snapshot.getWidth() - 1, centerX + radius); px++) { + double raw = readDepth(snapshot, py * snapshot.getWidth() + px); + if (Double.isFinite(raw) && raw > 0) values.add(raw * scale); + } + } + if (values.isEmpty()) throw new GlobalException("目标像素附近没有有效深度值"); + Collections.sort(values); + return values.get(values.size() / 2); + } + + private SurfacePoint fitSurface(EdgeCameraRgbdSnapshot snapshot, double centerX, double centerY, + int radius, Double configuredScale) { + List points = new ArrayList<>(); + int step = Math.max(1, radius / 6); + for (int y = (int) Math.round(centerY) - radius; y <= (int) Math.round(centerY) + radius; y += step) { + for (int x = (int) Math.round(centerX) - radius; x <= (int) Math.round(centerX) + radius; x += step) { + if (x < 0 || y < 0 || x >= snapshot.getWidth() || y >= snapshot.getHeight()) continue; + try { + double depth = medianDepth(snapshot, x, y, 1, configuredScale); + points.add(new double[]{(x - snapshot.getCx()) * depth / snapshot.getFx(), + (y - snapshot.getCy()) * depth / snapshot.getFy(), depth}); + } catch (GlobalException ignored) { + // Invalid depth pixels are expected around reflective screen edges. + } + } + } + if (points.size() < 20) { + // Reflective phone screens often contain only a few valid stereo + // pixels. Keep the target depth and use the camera optical normal + // as a conservative fallback instead of failing the whole node. + double depth = medianDepth(snapshot, centerX, centerY, Math.max(radius, 8), configuredScale); + double[] center = {(centerX - snapshot.getCx()) * depth / snapshot.getFx(), + (centerY - snapshot.getCy()) * depth / snapshot.getFy(), depth}; + return new SurfacePoint(center, new double[]{0, 0, -1}, 0.0); + } + List depths = points.stream().map(point -> point[2]).sorted().toList(); + double median = depths.get(depths.size() / 2); + points.removeIf(point -> Math.abs(point[2] - median) > 0.02); + if (points.size() < 15) return fallbackSurface(snapshot, centerX, centerY, radius, configuredScale); + + double[][] normal = new double[3][3]; + double[] rhs = new double[3]; + for (double[] point : points) { + double[] row = {point[0], point[1], 1}; + for (int i = 0; i < 3; i++) { + rhs[i] += row[i] * point[2]; + for (int j = 0; j < 3; j++) normal[i][j] += row[i] * row[j]; + } + } + double[] solution = solve3(normal, rhs); + double length = Math.sqrt(solution[0] * solution[0] + solution[1] * solution[1] + 1); + double[] planeNormal = {solution[0] / length, solution[1] / length, -1 / length}; + double planeD = solution[2] / length; + double[] ray = {(centerX - snapshot.getCx()) / snapshot.getFx(), + (centerY - snapshot.getCy()) / snapshot.getFy(), 1}; + double rayScale = -planeD / dot(planeNormal, ray); + double[] center = scale(ray, rayScale); + double squaredError = 0; + for (double[] point : points) { + double error = dot(planeNormal, point) + planeD; + squaredError += error * error; + } + return new SurfacePoint(center, planeNormal, Math.sqrt(squaredError / points.size())); + } + + private SurfacePoint fallbackSurface(EdgeCameraRgbdSnapshot snapshot, double centerX, double centerY, + int radius, Double configuredScale) { + double depth = medianDepth(snapshot, centerX, centerY, Math.max(radius, 8), configuredScale); + double[] center = {(centerX - snapshot.getCx()) * depth / snapshot.getFx(), + (centerY - snapshot.getCy()) * depth / snapshot.getFy(), depth}; + return new SurfacePoint(center, new double[]{0, 0, -1}, 0.0); + } + + private double[] solve3(double[][] source, double[] right) { + double[][] matrix = new double[3][4]; + for (int row = 0; row < 3; row++) { + System.arraycopy(source[row], 0, matrix[row], 0, 3); + matrix[row][3] = right[row]; + } + for (int column = 0; column < 3; column++) { + int pivot = column; + for (int row = column + 1; row < 3; row++) + if (Math.abs(matrix[row][column]) > Math.abs(matrix[pivot][column])) pivot = row; + double[] temporary = matrix[column]; matrix[column] = matrix[pivot]; matrix[pivot] = temporary; + if (Math.abs(matrix[column][column]) < 1e-12) throw new GlobalException("目标平面拟合失败"); + for (int row = column + 1; row < 3; row++) { + double factor = matrix[row][column] / matrix[column][column]; + for (int index = column; index < 4; index++) matrix[row][index] -= factor * matrix[column][index]; + } + } + double[] result = new double[3]; + for (int row = 2; row >= 0; row--) { + double value = matrix[row][3]; + for (int column = row + 1; column < 3; column++) value -= matrix[row][column] * result[column]; + result[row] = value / matrix[row][row]; + } + return result; + } + + private double[][] targetToolPose(double[][] currentTcp, double[] targetZ, double[] position) { + double[] xSeed = {currentTcp[0][0], currentTcp[1][0], currentTcp[2][0]}; + double[] x = subtract(xSeed, scale(targetZ, dot(xSeed, targetZ))); + if (length(x) < 1e-8) { + xSeed = new double[]{currentTcp[0][1], currentTcp[1][1], currentTcp[2][1]}; + x = subtract(xSeed, scale(targetZ, dot(xSeed, targetZ))); + } + x = normalize(x); + double[] y = normalize(cross(targetZ, x)); + x = normalize(cross(y, targetZ)); + double[][] result = new double[4][4]; + result[3][3] = 1; + for (int row = 0; row < 3; row++) { + result[row][0] = x[row]; result[row][1] = y[row]; result[row][2] = targetZ[row]; result[row][3] = position[row]; + } + return result; + } + + private double[] rotate(double[][] matrix, double[] value) { + return new double[]{matrix[0][0]*value[0]+matrix[0][1]*value[1]+matrix[0][2]*value[2], + matrix[1][0]*value[0]+matrix[1][1]*value[1]+matrix[1][2]*value[2], + matrix[2][0]*value[0]+matrix[2][1]*value[1]+matrix[2][2]*value[2]}; + } + + private JSONObject profileRef(VisionCalibrationProfile profile) { + return new JSONObject().fluentPut("id", profile.getId()).fluentPut("name", profile.getProfileName()) + .fluentPut("version", profile.getVersionNo()).fluentPut("forced", profile.getForcedEnabled()) + .fluentPut("maxErrorMm", profile.getMaxErrorMm()); + } + + private double[] add(double[] left, double[] right) { return new double[]{left[0]+right[0], left[1]+right[1], left[2]+right[2]}; } + private double[] subtract(double[] left, double[] right) { return new double[]{left[0]-right[0], left[1]-right[1], left[2]-right[2]}; } + private double[] scale(double[] value, double factor) { return new double[]{value[0]*factor, value[1]*factor, value[2]*factor}; } + private double dot(double[] left, double[] right) { return left[0]*right[0]+left[1]*right[1]+left[2]*right[2]; } + private double[] cross(double[] left, double[] right) { return new double[]{left[1]*right[2]-left[2]*right[1], left[2]*right[0]-left[0]*right[2], left[0]*right[1]-left[1]*right[0]}; } + private double length(double[] value) { return Math.sqrt(dot(value, value)); } + private double[] normalize(double[] value) { double length = length(value); if (length < 1e-10) throw new GlobalException("无法确定目标表面方向"); return scale(value, 1/length); } + + private double readDepth(EdgeCameraRgbdSnapshot snapshot, int index) { + byte[] bytes = snapshot.getDepthData(); + CameraCommand.FrameData.FrameType type = snapshot.getDepthType(); + if (type == CameraCommand.FrameData.FrameType.U8C1) return Byte.toUnsignedInt(bytes[index]); + ByteBuffer buffer = ByteBuffer.wrap(bytes).order(ByteOrder.LITTLE_ENDIAN); + if (type == CameraCommand.FrameData.FrameType.F32C1) return buffer.getFloat(index * Float.BYTES); + if (type == CameraCommand.FrameData.FrameType.U16C1 + || type == CameraCommand.FrameData.FrameType.F16C1) { + // The edge protocol transports F16 depth as 16-bit samples. The + // existing camera service uses the same unsigned-depth convention. + return Short.toUnsignedInt(buffer.getShort(index * Short.BYTES)); + } + throw new GlobalException("暂不支持的深度格式: " + type); + } + + private void validateIntrinsics(EdgeCameraRgbdSnapshot snapshot) { + if (snapshot.getFx() <= 0 || snapshot.getFy() <= 0) throw new GlobalException("相机内参无效"); + } + + private EdgeCommonVO target(String robotId, String deviceId) { + if (robotId == null) throw new GlobalException("robotId不能为空"); + EdgeCommonVO target = new EdgeCommonVO(); + target.setRobotId(robotId); + target.setDeviceId(deviceId); + return target; + } + + private String required(JSONObject input, String name) { + String value = StringUtils.trimToNull(input.getString(name)); + if (value == null) throw new GlobalException(name + "不能为空"); + return value; + } + + private String resolveDevice(TaskNodeExecuteMessage message, JSONObject input, String name) { + JSONObject resolved = message.getResolvedDeviceBindings(); + if (resolved != null) { + String value = StringUtils.trimToNull(resolved.getString(name)); + if (value != null) return value; + } + return input.getString(name); + } + + private String requiredValue(String value, String name) { + String normalized = StringUtils.trimToNull(value); + if (normalized == null) throw new GlobalException(name + "不能为空"); + return normalized; + } + + private JSONObject point2(double x, double y) { + return new JSONObject().fluentPut("x", x).fluentPut("y", y); + } + + private JSONObject point3(double x, double y, double z) { + return point2(x, y).fluentPut("z", z); + } + + private JSONObject poseJson(double[] pose) { + return point3(pose[0], pose[1], pose[2]) + .fluentPut("rx", pose[3]).fluentPut("ry", pose[4]).fluentPut("rz", pose[5]); + } + + private double doubleValue(JSONObject object, String name, double fallback) { + Double value = object.getDouble(name); + return value == null ? fallback : value; + } + + private record TargetPixel(double x, double y, double confidence) { + } + + private record SurfacePoint(double[] cameraPoint, double[] normalCamera, double rmse) { + } +} diff --git a/cmvr-iot-test/src/main/java/com/cmvr/test/flow/runtime/operator/llm/LLMChatOperateService.java b/cmvr-iot-test/src/main/java/com/cmvr/test/flow/runtime/operator/llm/LLMChatOperateService.java new file mode 100644 index 0000000..c0b09d7 --- /dev/null +++ b/cmvr-iot-test/src/main/java/com/cmvr/test/flow/runtime/operator/llm/LLMChatOperateService.java @@ -0,0 +1,71 @@ +package com.cmvr.test.flow.runtime.operator.llm; + +import com.alibaba.fastjson2.JSONObject; +import com.cmvr.common.exception.GlobalException; +import com.cmvr.llm.service.LLMAiAgentPlatformService; +import com.cmvr.test.constant.LLMChatConstants; +import com.cmvr.test.enums.ActionEnum; +import com.cmvr.test.flow.runtime.message.TaskNodeExecuteMessage; +import com.cmvr.test.flow.runtime.message.TaskNodeExecuteResult; +import lombok.RequiredArgsConstructor; +import lombok.extern.slf4j.Slf4j; +import org.apache.commons.lang3.StringUtils; +import org.springframework.beans.factory.annotation.Value; +import org.springframework.stereotype.Service; + +/** Lightweight workflow chat operator. It deliberately returns text only. */ +@Service +@RequiredArgsConstructor +@Slf4j +public class LLMChatOperateService implements LLMOperateService { + + /** Dedicated lightweight chat agent configured by the existing platform. */ + private static final String CHAT_API_KEY = "d9672ql4shh4136opsfg"; + private final LLMAiAgentPlatformService aiAgentPlatformService; + + @Value("${CMVR_CHAT_TTS_URL:http://192.168.0.148:8080/tts/play}") + private String defaultTtsUrl; + + @Override + public boolean supports(ActionEnum action) { + return action == ActionEnum.LLM_CHAT; + } + + @Override + public TaskNodeExecuteResult execute(TaskNodeExecuteMessage message) { + JSONObject input = message.getInputParams() == null ? new JSONObject() : message.getInputParams(); + String text = StringUtils.trimToNull(input.getString("text")); + if (text == null) { + throw new GlobalException("闲聊文本不能为空"); + } + String mode = StringUtils.defaultIfBlank(input.getString("promptMode"), "CUSTOM").trim().toUpperCase(); + String query = text; + if ("CUSTOM".equals(mode)) { + String customPrompt = StringUtils.defaultIfBlank( + input.getString("prompt"), LLMChatConstants.DEFAULT_SYSTEM_PROMPT).trim(); + query = customPrompt + "\n用户输入:" + text; + } + + long startedAt = System.nanoTime(); + boolean invokeTts = Boolean.TRUE.equals(input.getBoolean("invokeTts")); + JSONObject ttsConfig = invokeTts ? resolveTtsConfig(input.getJSONObject("tts")) : null; + JSONObject response = aiAgentPlatformService.query( + ActionEnum.LLM_CHAT.getAction(), query, CHAT_API_KEY, invokeTts, ttsConfig); + log.info("闲聊大模型调用完成,模式: {},流式TTS: {},耗时: {}ms", mode, invokeTts, + (System.nanoTime() - startedAt) / 1_000_000); + String answer = response == null ? null : response.getString("result"); + if (StringUtils.isBlank(answer)) { + throw new GlobalException("大模型未返回闲聊文本"); + } + return TaskNodeExecuteResult.success(new JSONObject().fluentPut("text", answer.trim())); + } + + private JSONObject resolveTtsConfig(JSONObject configured) { + JSONObject result = configured == null ? new JSONObject() : new JSONObject(configured); + result.putIfAbsent("url", defaultTtsUrl); + result.putIfAbsent("voice", "x4_yezi"); + result.putIfAbsent("speed", 45); + result.putIfAbsent("volume", 100); + return result; + } +} diff --git a/cmvr-iot-test/src/main/java/com/cmvr/test/flow/runtime/operator/llm/LLMMediaAnalysisOperateService.java b/cmvr-iot-test/src/main/java/com/cmvr/test/flow/runtime/operator/llm/LLMMediaAnalysisOperateService.java index 30364b4..d01bab3 100644 --- a/cmvr-iot-test/src/main/java/com/cmvr/test/flow/runtime/operator/llm/LLMMediaAnalysisOperateService.java +++ b/cmvr-iot-test/src/main/java/com/cmvr/test/flow/runtime/operator/llm/LLMMediaAnalysisOperateService.java @@ -8,6 +8,8 @@ import com.cmvr.test.flow.runtime.message.TaskNodeExecuteMessage; import com.cmvr.test.flow.runtime.message.TaskNodeExecuteResult; import com.cmvr.test.flow.runtime.operator.FlowMediaParamResolver; import org.apache.commons.lang3.StringUtils; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; import org.springframework.beans.factory.annotation.Autowired; import org.springframework.stereotype.Service; @@ -18,6 +20,7 @@ import java.util.UUID; @Service public class LLMMediaAnalysisOperateService implements LLMOperateService { + private static final Logger log = LoggerFactory.getLogger(LLMMediaAnalysisOperateService.class); private static final Set VIDEO_ANALYSIS_MODES = Set.of("AUTO", "FAST", "ACCURATE"); private static final String ANY_VEHICLE_SOUND = "ANY_VEHICLE_SOUND"; private final MediaAnalysisClient mediaAnalysisClient; @@ -49,6 +52,7 @@ public class LLMMediaAnalysisOperateService implements LLMOperateService { String mediaParam = audio ? "audioUrl" : (image ? "imageUrl" : "videoUrl"); List mediaUrls = FlowMediaParamResolver.urls(input, mediaParam); String mediaUrl = FlowMediaParamResolver.lastUrl(input, mediaParam); + List requestMediaUrls = mediaUrls; String profileCode = StringUtils.trimToNull(input.getString("profileCode")); if (StringUtils.isBlank(mediaUrl)) { throw new GlobalException("{}不能为空", audio ? "音频地址" : (image ? "图片地址" : "视频地址")); @@ -108,8 +112,24 @@ public class LLMMediaAnalysisOperateService implements LLMOperateService { if (!Set.of("AUTO", "VISION_MODEL", "TEXT_LOCATE", "REFERENCE_MATCH").contains(analysisMethod)) { throw new GlobalException("图片分析方式无效: {}", analysisMethod); } - options.put("analysisMethod", analysisMethod); String referenceImageUrl = FlowMediaParamResolver.lastUrl(input, "referenceImageUrl"); + // Older image nodes put the scene and reference image in the same + // imageUrl list. Restore that convention before calling the + // analysis service. A single-image node cannot perform template + // matching, so use the original description-based locator. + if ("REFERENCE_MATCH".equals(analysisMethod) && StringUtils.isBlank(referenceImageUrl)) { + if (mediaUrls.size() == 2) { + referenceImageUrl = mediaUrls.get(1); + // The service receives the reference separately; only the + // first URL remains in mediaUrls as the scene image. + requestMediaUrls = List.of(mediaUrls.get(0)); + } else if (mediaUrls.size() == 1) { + log.warn("仅收到一张图片,REFERENCE_MATCH 自动降级为 TEXT_LOCATE: nodeId={}", + message.getNodeId()); + analysisMethod = "TEXT_LOCATE"; + } + } + options.put("analysisMethod", analysisMethod); if (StringUtils.isNotBlank(referenceImageUrl)) { options.put("referenceImageUrl", referenceImageUrl); } @@ -127,7 +147,7 @@ public class LLMMediaAnalysisOperateService implements LLMOperateService { request.put("profileCode", profileCode); request.put("mediaUrl", mediaUrl); if (image) { - request.put("mediaUrls", mediaUrls); + request.put("mediaUrls", requestMediaUrls); } request.put("options", options); request.put("context", context); @@ -231,6 +251,7 @@ public class LLMMediaAnalysisOperateService implements LLMOperateService { String matchSelector = StringUtils.trimToNull(source.getString("matchSelector")); Integer matchIndex = source.getInteger("matchIndex"); String matchSort = StringUtils.trimToNull(source.getString("matchSort")); + Boolean fullResRefine = source.getBoolean("fullResRefine"); validateRange("图片宽度", maxWidth, 640, 1600); validateRange("图片数量", maxImages, 1, 12); validateRange("最大输出长度", maxOutputTokens, 32, 4096); @@ -255,6 +276,7 @@ public class LLMMediaAnalysisOperateService implements LLMOperateService { if (matchSelector != null) tuning.put("matchSelector", matchSelector.toUpperCase()); if (matchIndex != null) tuning.put("matchIndex", matchIndex); if (matchSort != null) tuning.put("matchSort", matchSort.toUpperCase()); + if (fullResRefine != null) tuning.put("fullResRefine", fullResRefine); return tuning; } diff --git a/cmvr-iot-test/src/main/java/com/cmvr/test/service/FlowActionExecutorService.java b/cmvr-iot-test/src/main/java/com/cmvr/test/service/FlowActionExecutorService.java index 69cbe1e..3884fd4 100644 --- a/cmvr-iot-test/src/main/java/com/cmvr/test/service/FlowActionExecutorService.java +++ b/cmvr-iot-test/src/main/java/com/cmvr/test/service/FlowActionExecutorService.java @@ -28,6 +28,7 @@ import com.cmvr.test.flow.runtime.operator.edge.EdgeManualInspectionOperateServi import com.cmvr.test.flow.runtime.operator.edge.EdgeDeviceCommandOperateService; import com.cmvr.test.flow.runtime.operator.edge.EdgeArmOperateService; import com.cmvr.test.flow.runtime.operator.edge.EdgeOperateService; +import com.cmvr.test.flow.runtime.operator.edge.VisionLocateOperateService; import com.cmvr.test.flow.runtime.operator.llm.InspectionMeterRecognizeOperateService; import com.cmvr.test.flow.runtime.operator.llm.LLMOperateService; import com.cmvr.test.model.vo.FlowActionRequestVO; @@ -52,6 +53,7 @@ public class FlowActionExecutorService { private final EdgeArmOperateService edgeArmOperateService; private final EdgeManualInspectionOperateService edgeManualInspectionOperateService; private final EdgeDeviceCommandOperateService edgeDeviceCommandOperateService; + private final VisionLocateOperateService visionLocateOperateService; private final InspectionMeterRecognizeOperateService inspectionMeterRecognizeOperateService; private final List edgeOperateServices; private final List llmOperateServices; @@ -150,6 +152,14 @@ public class FlowActionExecutorService { String robotId = payload.getString("robotId"); String deviceId = payload.getString("deviceId"); + // Keep the legacy single-node fallback complete for actions introduced + // after the generic EDGE operator list was added. + if (ActionEnum.VISION_LOCATE_TARGET.equals(action)) { + TaskNodeExecuteMessage message = buildSingleNodeMessage(action, payload); + message.setRobotId(robotId); + return resultToString(visionLocateOperateService.execute(message)); + } + // 巡检报警监听组件只需要机器人 ID 和事件类型,不需要设备 ID。 if (ActionEnum.INSPECTION_ALERT_LISTEN_START.equals(action) || ActionEnum.INSPECTION_ALERT_LISTEN_STOP.equals(action)) { diff --git a/cmvr-iot-test/src/main/java/com/cmvr/test/vision/domain/VisionCalibrationProfile.java b/cmvr-iot-test/src/main/java/com/cmvr/test/vision/domain/VisionCalibrationProfile.java new file mode 100644 index 0000000..f4a2cb4 --- /dev/null +++ b/cmvr-iot-test/src/main/java/com/cmvr/test/vision/domain/VisionCalibrationProfile.java @@ -0,0 +1,37 @@ +package com.cmvr.test.vision.domain; + +import com.baomidou.mybatisplus.annotation.IdType; +import com.baomidou.mybatisplus.annotation.TableId; +import com.baomidou.mybatisplus.annotation.TableName; +import com.cmvr.common.core.domain.BaseEntity; +import lombok.Data; +import lombok.EqualsAndHashCode; + +import java.math.BigDecimal; +import java.time.LocalDateTime; + +@Data +@EqualsAndHashCode(callSuper = true) +@TableName("te_vision_calibration_profile") +public class VisionCalibrationProfile extends BaseEntity { + @TableId(type = IdType.ASSIGN_UUID) + private String id; + private String profileType; + private String profileKey; + private String profileName; + private Integer versionNo; + private String robotId; + private String armDeviceId; + private String cameraDeviceId; + private String toolCode; + private String status; + private Boolean forcedEnabled; + private String rawMatrixJson; + private String correctionJson; + private String effectiveMatrixJson; + private BigDecimal meanErrorMm; + private BigDecimal maxErrorMm; + private Integer sampleCount; + private String metadataJson; + private LocalDateTime verifiedTime; +} diff --git a/cmvr-iot-test/src/main/java/com/cmvr/test/vision/domain/VisionCalibrationRecord.java b/cmvr-iot-test/src/main/java/com/cmvr/test/vision/domain/VisionCalibrationRecord.java new file mode 100644 index 0000000..e89c585 --- /dev/null +++ b/cmvr-iot-test/src/main/java/com/cmvr/test/vision/domain/VisionCalibrationRecord.java @@ -0,0 +1,32 @@ +package com.cmvr.test.vision.domain; + +import com.baomidou.mybatisplus.annotation.IdType; +import com.baomidou.mybatisplus.annotation.TableId; +import com.baomidou.mybatisplus.annotation.TableName; +import com.cmvr.common.core.domain.BaseEntity; +import lombok.Data; +import lombok.EqualsAndHashCode; + +import java.time.LocalDateTime; + +@Data +@EqualsAndHashCode(callSuper = true) +@TableName("te_vision_calibration_record") +public class VisionCalibrationRecord extends BaseEntity { + @TableId(type = IdType.ASSIGN_UUID) + private String id; + private String profileId; + private String sessionType; + private String robotId; + private String armDeviceId; + private String cameraDeviceId; + private String toolCode; + private String status; + private Integer progressStep; + private Integer totalSteps; + private String samplesJson; + private String resultJson; + private String errorMessage; + private LocalDateTime startedTime; + private LocalDateTime finishedTime; +} diff --git a/cmvr-iot-test/src/main/java/com/cmvr/test/vision/mapper/VisionCalibrationProfileMapper.java b/cmvr-iot-test/src/main/java/com/cmvr/test/vision/mapper/VisionCalibrationProfileMapper.java new file mode 100644 index 0000000..5bd36b3 --- /dev/null +++ b/cmvr-iot-test/src/main/java/com/cmvr/test/vision/mapper/VisionCalibrationProfileMapper.java @@ -0,0 +1,7 @@ +package com.cmvr.test.vision.mapper; + +import com.baomidou.mybatisplus.core.mapper.BaseMapper; +import com.cmvr.test.vision.domain.VisionCalibrationProfile; + +public interface VisionCalibrationProfileMapper extends BaseMapper { +} diff --git a/cmvr-iot-test/src/main/java/com/cmvr/test/vision/mapper/VisionCalibrationRecordMapper.java b/cmvr-iot-test/src/main/java/com/cmvr/test/vision/mapper/VisionCalibrationRecordMapper.java new file mode 100644 index 0000000..c988692 --- /dev/null +++ b/cmvr-iot-test/src/main/java/com/cmvr/test/vision/mapper/VisionCalibrationRecordMapper.java @@ -0,0 +1,7 @@ +package com.cmvr.test.vision.mapper; + +import com.baomidou.mybatisplus.core.mapper.BaseMapper; +import com.cmvr.test.vision.domain.VisionCalibrationRecord; + +public interface VisionCalibrationRecordMapper extends BaseMapper { +} diff --git a/cmvr-iot-test/src/main/java/com/cmvr/test/vision/marker/MarkerObservation.java b/cmvr-iot-test/src/main/java/com/cmvr/test/vision/marker/MarkerObservation.java new file mode 100644 index 0000000..0e08638 --- /dev/null +++ b/cmvr-iot-test/src/main/java/com/cmvr/test/vision/marker/MarkerObservation.java @@ -0,0 +1,8 @@ +package com.cmvr.test.vision.marker; + +import java.util.List; + +public record MarkerObservation(String imageUrl, double[] centerCamera, double[] normalCamera, + double[] xAxisCamera, List pixelCorners, + int width, int height, double planeRmseMm) { +} diff --git a/cmvr-iot-test/src/main/java/com/cmvr/test/vision/marker/SquareMarkerDetector.java b/cmvr-iot-test/src/main/java/com/cmvr/test/vision/marker/SquareMarkerDetector.java new file mode 100644 index 0000000..eaaa89a --- /dev/null +++ b/cmvr-iot-test/src/main/java/com/cmvr/test/vision/marker/SquareMarkerDetector.java @@ -0,0 +1,287 @@ +package com.cmvr.test.vision.marker; + +import cn.hutool.http.HttpRequest; +import cn.hutool.http.HttpResponse; +import cmvr.api.CameraCommand; +import com.cmvr.common.exception.GlobalException; +import com.cmvr.edge.client.model.camera.EdgeCameraRgbdSnapshot; +import nu.pattern.OpenCV; +import org.opencv.core.Core; +import org.opencv.core.CvType; +import org.opencv.core.Mat; +import org.opencv.core.MatOfByte; +import org.opencv.core.MatOfDouble; +import org.opencv.core.MatOfPoint; +import org.opencv.core.MatOfPoint2f; +import org.opencv.core.Point; +import org.opencv.core.Rect; +import org.opencv.core.Scalar; +import org.opencv.imgcodecs.Imgcodecs; +import org.opencv.imgproc.Imgproc; +import org.springframework.stereotype.Component; + +import java.nio.ByteBuffer; +import java.nio.ByteOrder; +import java.util.ArrayList; +import java.util.Arrays; +import java.util.Comparator; +import java.util.List; + +@Component +public class SquareMarkerDetector { + static { + OpenCV.loadLocally(); + } + + public MarkerObservation detect(EdgeCameraRgbdSnapshot snapshot) { + Mat image = load(snapshot.getColorImageUrl()); + try { + Point[] corners = findMarker(image); + Plane plane = fitDepthPlane(snapshot, corners); + Point centerPixel = average(corners); + double[] center = intersectPixel(snapshot, centerPixel, plane); + double[] topLeft = intersectPixel(snapshot, corners[0], plane); + double[] topRight = intersectPixel(snapshot, corners[1], plane); + double[] xAxis = normalize(subtract(topRight, topLeft)); + return new MarkerObservation(snapshot.getColorImageUrl(), center, plane.normal, xAxis, + Arrays.stream(corners).map(point -> new double[]{point.x, point.y}).toList(), + snapshot.getWidth(), snapshot.getHeight(), plane.rmse * 1000.0); + } finally { + image.release(); + } + } + + private Mat load(String imageUrl) { + try (HttpResponse response = HttpRequest.get(imageUrl).timeout(10_000).execute()) { + if (!response.isOk()) throw new GlobalException("下载标定图片失败(" + response.getStatus() + ")"); + MatOfByte bytes = new MatOfByte(response.bodyBytes()); + try { + Mat image = Imgcodecs.imdecode(bytes, Imgcodecs.IMREAD_COLOR); + if (image.empty()) throw new GlobalException("标定图片无法解码"); + return image; + } finally { + bytes.release(); + } + } catch (GlobalException exception) { + throw exception; + } catch (RuntimeException exception) { + throw new GlobalException("读取标定图片失败: " + exception.getMessage()); + } + } + + private Point[] findMarker(Mat image) { + Mat gray = new Mat(); + Mat binary = new Mat(); + Mat hierarchy = new Mat(); + List contours = new ArrayList<>(); + try { + Imgproc.cvtColor(image, gray, Imgproc.COLOR_BGR2GRAY); + Imgproc.GaussianBlur(gray, gray, new org.opencv.core.Size(3, 3), 0); + Imgproc.adaptiveThreshold(gray, binary, 255, Imgproc.ADAPTIVE_THRESH_GAUSSIAN_C, + Imgproc.THRESH_BINARY, 21, 3); + Imgproc.findContours(binary, contours, hierarchy, Imgproc.RETR_TREE, Imgproc.CHAIN_APPROX_SIMPLE); + double imageArea = image.width() * (double) image.height(); + Candidate best = null; + for (int index = 0; index < contours.size(); index++) { + MatOfPoint contour = contours.get(index); + double area = Math.abs(Imgproc.contourArea(contour)); + if (area < 55 || area > imageArea * 0.65) continue; + MatOfPoint2f curve = new MatOfPoint2f(contour.toArray()); + MatOfPoint2f approximation = new MatOfPoint2f(); + try { + double perimeter = Imgproc.arcLength(curve, true); + Imgproc.approxPolyDP(curve, approximation, perimeter * 0.02, true); + Point[] points = approximation.toArray(); + if (points.length != 4) continue; + MatOfPoint polygon = new MatOfPoint(points); + boolean convex = Imgproc.isContourConvex(polygon); + polygon.release(); + if (!convex || !reasonableShape(points)) continue; + Point[] ordered = order(points); + double score = markerScore(gray, ordered) + nestedDepth(hierarchy, index) * 20 + Math.log(area); + if (score > 25 && (best == null || score > best.score)) best = new Candidate(ordered, score); + } finally { + curve.release(); + approximation.release(); + } + } + if (best == null) throw new GlobalException("未识别到唯一方形标定码,请调整距离、角度或光线"); + return best.corners; + } finally { + gray.release(); binary.release(); hierarchy.release(); contours.forEach(Mat::release); + } + } + + private int nestedDepth(Mat hierarchy, int index) { + if (hierarchy.empty()) return 0; + int depth = 0; + int child = (int) hierarchy.get(0, index)[2]; + while (child >= 0 && depth < 5) { + depth++; + child = (int) hierarchy.get(0, child)[2]; + } + return depth; + } + + private double markerScore(Mat gray, Point[] corners) { + Mat source = new MatOfPoint2f(corners); + Mat destination = new MatOfPoint2f(new Point(0, 0), new Point(159, 0), new Point(159, 159), new Point(0, 159)); + Mat transform = Imgproc.getPerspectiveTransform(source, destination); + Mat warped = new Mat(160, 160, CvType.CV_8UC1); + try { + Imgproc.warpPerspective(gray, warped, transform, warped.size(), Imgproc.INTER_LINEAR); + Mat inner = warped.submat(new Rect(20, 20, 120, 120)); + Mat center = warped.submat(new Rect(35, 35, 90, 90)); + try { + Scalar mean = Core.mean(inner); + Scalar centerMean = Core.mean(center); + MatOfDouble meanMat = new MatOfDouble(); + MatOfDouble std = new MatOfDouble(); + Core.meanStdDev(center, meanMat, std); + double deviation = std.get(0, 0)[0]; + meanMat.release(); std.release(); + return deviation + Math.abs(mean.val[0] - centerMean.val[0]) * 0.2; + } finally { + inner.release(); center.release(); + } + } finally { + source.release(); destination.release(); transform.release(); warped.release(); + } + } + + private Plane fitDepthPlane(EdgeCameraRgbdSnapshot snapshot, Point[] corners) { + List points = new ArrayList<>(); + for (int row = 1; row <= 11; row++) { + double v = row / 12.0; + for (int column = 1; column <= 11; column++) { + double u = column / 12.0; + Point pixel = bilinear(corners, u, v); + double depth = medianDepth(snapshot, (int) Math.round(pixel.x), (int) Math.round(pixel.y), 2); + if (Double.isFinite(depth)) points.add(backProject(snapshot, pixel.x, pixel.y, depth)); + } + } + if (points.size() < 30) throw new GlobalException("标定码区域有效深度点不足"); + if (points.size() < 30) { + Point center = average(corners); + double centerDepth = medianDepth(snapshot, (int) Math.round(center.x), (int) Math.round(center.y), 8); + if (Double.isFinite(centerDepth)) { + points.clear(); + points.add(backProject(snapshot, center.x, center.y, centerDepth)); + double[] normal = {0, 0, -1}; + return new Plane(normal, centerDepth, 0.0); + } + } + double medianZ = points.stream().mapToDouble(point -> point[2]).sorted().skip(points.size() / 2).findFirst().orElseThrow(); + points.removeIf(point -> Math.abs(point[2] - medianZ) > 0.025); + double[][] design = new double[3][3]; + double[] target = new double[3]; + for (double[] point : points) { + double[] row = {point[0], point[1], 1}; + for (int i = 0; i < 3; i++) { + target[i] += row[i] * point[2]; + for (int j = 0; j < 3; j++) design[i][j] += row[i] * row[j]; + } + } + double[] solution = solve3(design, target); + double[] normal = normalize(new double[]{solution[0], solution[1], -1}); + if (dot(normal, new double[]{0, 0, 1}) > 0) normal = scale(normal, -1); + double d = solution[2] / Math.sqrt(solution[0] * solution[0] + solution[1] * solution[1] + 1); + double squared = 0; + for (double[] point : points) { + double error = dot(normal, point) + d; + squared += error * error; + } + return new Plane(normal, d, Math.sqrt(squared / points.size())); + } + + private double[] intersectPixel(EdgeCameraRgbdSnapshot snapshot, Point pixel, Plane plane) { + double[] ray = {(pixel.x - snapshot.getCx()) / snapshot.getFx(), + (pixel.y - snapshot.getCy()) / snapshot.getFy(), 1}; + double scale = -plane.d / dot(plane.normal, ray); + return scale(ray, scale); + } + + private double medianDepth(EdgeCameraRgbdSnapshot snapshot, int x, int y, int radius) { + List values = new ArrayList<>(); + for (int py = Math.max(0, y - radius); py <= Math.min(snapshot.getHeight() - 1, y + radius); py++) + for (int px = Math.max(0, x - radius); px <= Math.min(snapshot.getWidth() - 1, x + radius); px++) { + double raw = readDepth(snapshot, py * snapshot.getWidth() + px); + if (Double.isFinite(raw) && raw > 0) values.add(raw * snapshot.getDepthUnitScale()); + } + if (values.isEmpty()) return Double.NaN; + values.sort(Double::compareTo); + return values.get(values.size() / 2); + } + + private double readDepth(EdgeCameraRgbdSnapshot snapshot, int index) { + byte[] bytes = snapshot.getDepthData(); + CameraCommand.FrameData.FrameType type = snapshot.getDepthType(); + if (type == CameraCommand.FrameData.FrameType.U8C1) return Byte.toUnsignedInt(bytes[index]); + ByteBuffer buffer = ByteBuffer.wrap(bytes).order(ByteOrder.LITTLE_ENDIAN); + if (type == CameraCommand.FrameData.FrameType.F32C1) return buffer.getFloat(index * Float.BYTES); + if (type == CameraCommand.FrameData.FrameType.U16C1 || type == CameraCommand.FrameData.FrameType.F16C1) + return Short.toUnsignedInt(buffer.getShort(index * Short.BYTES)); + return Double.NaN; + } + + private double[] backProject(EdgeCameraRgbdSnapshot snapshot, double x, double y, double depth) { + return new double[]{(x - snapshot.getCx()) * depth / snapshot.getFx(), + (y - snapshot.getCy()) * depth / snapshot.getFy(), depth}; + } + + private boolean reasonableShape(Point[] points) { + Point[] ordered = order(points); + double[] lengths = new double[4]; + for (int index = 0; index < 4; index++) lengths[index] = distance(ordered[index], ordered[(index + 1) % 4]); + double min = Arrays.stream(lengths).min().orElse(0), max = Arrays.stream(lengths).max().orElse(1); + return min > 8 && max / min < 2.3; + } + + private Point[] order(Point[] points) { + Point[] ordered = new Point[4]; + ordered[0] = Arrays.stream(points).min(Comparator.comparingDouble(point -> point.x + point.y)).orElseThrow(); + ordered[2] = Arrays.stream(points).max(Comparator.comparingDouble(point -> point.x + point.y)).orElseThrow(); + ordered[1] = Arrays.stream(points).max(Comparator.comparingDouble(point -> point.x - point.y)).orElseThrow(); + ordered[3] = Arrays.stream(points).min(Comparator.comparingDouble(point -> point.x - point.y)).orElseThrow(); + return ordered; + } + + private Point bilinear(Point[] corners, double u, double v) { + return new Point((1-u)*(1-v)*corners[0].x + u*(1-v)*corners[1].x + u*v*corners[2].x + (1-u)*v*corners[3].x, + (1-u)*(1-v)*corners[0].y + u*(1-v)*corners[1].y + u*v*corners[2].y + (1-u)*v*corners[3].y); + } + + private Point average(Point[] points) { + return new Point(Arrays.stream(points).mapToDouble(point -> point.x).average().orElse(0), + Arrays.stream(points).mapToDouble(point -> point.y).average().orElse(0)); + } + + private double[] solve3(double[][] matrix, double[] right) { + double[][] a = new double[3][4]; + for (int row = 0; row < 3; row++) { System.arraycopy(matrix[row], 0, a[row], 0, 3); a[row][3] = right[row]; } + for (int column = 0; column < 3; column++) { + int pivot = column; + for (int row = column + 1; row < 3; row++) if (Math.abs(a[row][column]) > Math.abs(a[pivot][column])) pivot = row; + double[] temp = a[column]; a[column] = a[pivot]; a[pivot] = temp; + if (Math.abs(a[column][column]) < 1e-12) throw new GlobalException("标定码平面拟合失败"); + for (int row = column + 1; row < 3; row++) { + double factor = a[row][column] / a[column][column]; + for (int index = column; index < 4; index++) a[row][index] -= factor * a[column][index]; + } + } + double[] result = new double[3]; + for (int row = 2; row >= 0; row--) { + double value = a[row][3]; for (int column = row + 1; column < 3; column++) value -= a[row][column] * result[column]; + result[row] = value / a[row][row]; + } + return result; + } + + private double distance(Point left, Point right) { return Math.hypot(left.x - right.x, left.y - right.y); } + private double[] subtract(double[] left, double[] right) { return new double[]{left[0]-right[0], left[1]-right[1], left[2]-right[2]}; } + private double dot(double[] left, double[] right) { return left[0]*right[0] + left[1]*right[1] + left[2]*right[2]; } + private double[] scale(double[] value, double factor) { return new double[]{value[0]*factor, value[1]*factor, value[2]*factor}; } + private double[] normalize(double[] value) { double length = Math.sqrt(dot(value, value)); if (length < 1e-10) throw new GlobalException("标定方向无效"); return scale(value, 1/length); } + private record Candidate(Point[] corners, double score) { } + private record Plane(double[] normal, double d, double rmse) { } +} diff --git a/cmvr-iot-test/src/main/java/com/cmvr/test/vision/math/CalibrationResult.java b/cmvr-iot-test/src/main/java/com/cmvr/test/vision/math/CalibrationResult.java new file mode 100644 index 0000000..72dd67e --- /dev/null +++ b/cmvr-iot-test/src/main/java/com/cmvr/test/vision/math/CalibrationResult.java @@ -0,0 +1,5 @@ +package com.cmvr.test.vision.math; + +public record CalibrationResult(double[][] matrix, double meanErrorMm, double maxErrorMm, + int sampleCount, double[] fixedPoint) { +} diff --git a/cmvr-iot-test/src/main/java/com/cmvr/test/vision/math/CalibrationSample.java b/cmvr-iot-test/src/main/java/com/cmvr/test/vision/math/CalibrationSample.java new file mode 100644 index 0000000..5aa8e70 --- /dev/null +++ b/cmvr-iot-test/src/main/java/com/cmvr/test/vision/math/CalibrationSample.java @@ -0,0 +1,4 @@ +package com.cmvr.test.vision.math; + +public record CalibrationSample(double[][] baseToFlange, double[] cameraPoint) { +} diff --git a/cmvr-iot-test/src/main/java/com/cmvr/test/vision/math/VisionCalibrationMath.java b/cmvr-iot-test/src/main/java/com/cmvr/test/vision/math/VisionCalibrationMath.java new file mode 100644 index 0000000..213169f --- /dev/null +++ b/cmvr-iot-test/src/main/java/com/cmvr/test/vision/math/VisionCalibrationMath.java @@ -0,0 +1,355 @@ +package com.cmvr.test.vision.math; + +import cmvr.api.ArmCommand; +import com.alibaba.fastjson2.JSON; +import com.alibaba.fastjson2.JSONArray; +import com.cmvr.common.exception.GlobalException; +import com.cmvr.test.vision.model.VisionCalibrationRequests; + +import java.util.ArrayList; +import java.util.Arrays; +import java.util.List; + +public final class VisionCalibrationMath { + private VisionCalibrationMath() { + } + + public static CalibrationResult solveHandEye(List samples, double[][] initial) { + if (samples == null || samples.size() < 8) { + throw new GlobalException("手眼标定至少需要8组有效样本"); + } + double[] parameters = new double[9]; + double[][] seed = initial == null ? identity() : initial; + double[] rotation = rotationVector(seed); + System.arraycopy(rotation, 0, parameters, 0, 3); + parameters[3] = seed[0][3]; + parameters[4] = seed[1][3]; + parameters[5] = seed[2][3]; + double[] firstPrediction = transform(multiply(samples.get(0).baseToFlange(), seed), + samples.get(0).cameraPoint()); + System.arraycopy(firstPrediction, 0, parameters, 6, 3); + + double lambda = 1e-5; + double currentCost = cost(samples, parameters); + for (int iteration = 0; iteration < 120; iteration++) { + double[] residual = residuals(samples, parameters); + double[][] jacobian = numericJacobian(samples, parameters, residual); + double[][] normal = new double[parameters.length][parameters.length]; + double[] rhs = new double[parameters.length]; + for (int row = 0; row < residual.length; row++) { + for (int column = 0; column < parameters.length; column++) { + rhs[column] -= jacobian[row][column] * residual[row]; + for (int other = 0; other < parameters.length; other++) { + normal[column][other] += jacobian[row][column] * jacobian[row][other]; + } + } + } + for (int index = 0; index < parameters.length; index++) normal[index][index] += lambda; + double[] delta = solve(normal, rhs); + double[] candidate = parameters.clone(); + for (int index = 0; index < candidate.length; index++) candidate[index] += delta[index]; + double candidateCost = cost(samples, candidate); + if (candidateCost < currentCost) { + parameters = candidate; + if (norm(delta) < 1e-10 || Math.abs(currentCost - candidateCost) < 1e-14) break; + currentCost = candidateCost; + lambda = Math.max(1e-10, lambda * 0.35); + } else { + lambda = Math.min(1e6, lambda * 8.0); + } + } + double[][] matrix = fromRotationVector(Arrays.copyOf(parameters, 3)); + matrix[0][3] = parameters[3]; + matrix[1][3] = parameters[4]; + matrix[2][3] = parameters[5]; + return result(matrix, samples, Arrays.copyOfRange(parameters, 6, 9)); + } + + public static CalibrationResult solveToolPivot(List baseToFlangeSamples) { + if (baseToFlangeSamples == null || baseToFlangeSamples.size() < 5) { + throw new GlobalException("工具TCP标定至少需要5个不同姿态"); + } + int rows = baseToFlangeSamples.size() * 3; + double[][] design = new double[rows][6]; + double[] target = new double[rows]; + for (int sampleIndex = 0; sampleIndex < baseToFlangeSamples.size(); sampleIndex++) { + double[][] pose = baseToFlangeSamples.get(sampleIndex); + for (int axis = 0; axis < 3; axis++) { + int row = sampleIndex * 3 + axis; + System.arraycopy(pose[axis], 0, design[row], 0, 3); + design[row][3 + axis] = -1.0; + target[row] = -pose[axis][3]; + } + } + double[] solution = leastSquares(design, target); + double[] tcp = Arrays.copyOf(solution, 3); + double[] fixed = Arrays.copyOfRange(solution, 3, 6); + double[][] matrix = identity(); + matrix[0][3] = tcp[0]; + matrix[1][3] = tcp[1]; + matrix[2][3] = tcp[2]; + List errors = new ArrayList<>(); + for (double[][] pose : baseToFlangeSamples) { + errors.add(distance(transform(pose, tcp), fixed) * 1000.0); + } + return new CalibrationResult(matrix, mean(errors), errors.stream().mapToDouble(Double::doubleValue).max().orElse(0), + baseToFlangeSamples.size(), fixed); + } + + public static double[][] applyToolDirection(double[][] pivotMatrix, double[][] baseToFlange, + double[] planeNormalBase, double[] planeXBase) { + double[][] flangeToBaseRotation = transposeRotation(baseToFlange); + double[] z = normalize(rotationTransform(flangeToBaseRotation, scale(normalize(planeNormalBase), -1))); + double[] xSeed = normalize(rotationTransform(flangeToBaseRotation, normalize(planeXBase))); + double[] x = normalize(subtract(xSeed, scale(z, dot(xSeed, z)))); + if (norm(x) < 1e-8) x = perpendicular(z); + double[] y = normalize(cross(z, x)); + x = normalize(cross(y, z)); + double[][] result = copy(pivotMatrix); + setColumn(result, 0, x); + setColumn(result, 1, y); + setColumn(result, 2, z); + return result; + } + + public static double[][] applyCorrection(double[][] raw, VisionCalibrationRequests.Correction correction) { + if (correction == null) return copy(raw); + double[][] delta = fromEuler(Math.toRadians(value(correction.getRxDeg())), + Math.toRadians(value(correction.getRyDeg())), Math.toRadians(value(correction.getRzDeg()))); + delta[0][3] = value(correction.getXMm()) / 1000.0; + delta[1][3] = value(correction.getYMm()) / 1000.0; + delta[2][3] = value(correction.getZMm()) / 1000.0; + return multiply(raw, delta); + } + + public static double[][] fromProto(ArmCommand.TransformMatrix4x4 matrix) { + return new double[][]{ + {matrix.getM00(), matrix.getM01(), matrix.getM02(), matrix.getM03()}, + {matrix.getM10(), matrix.getM11(), matrix.getM12(), matrix.getM13()}, + {matrix.getM20(), matrix.getM21(), matrix.getM22(), matrix.getM23()}, + {matrix.getM30(), matrix.getM31(), matrix.getM32(), matrix.getM33()} + }; + } + + /** Converts the arm getPose response (metres/radians) to a homogeneous pose matrix. */ + public static double[][] fromCartesianPose(ArmCommand.CartesianPose pose) { + if (pose == null) throw new GlobalException("机械臂位姿为空"); + double[][] matrix = fromEuler(pose.getRx(), pose.getRy(), pose.getRz()); + matrix[0][3] = pose.getX(); + matrix[1][3] = pose.getY(); + matrix[2][3] = pose.getZ(); + return matrix; + } + + public static String toJson(double[][] matrix) { + JSONArray values = new JSONArray(); + for (double[] row : matrix) for (double value : row) values.add(value); + return values.toJSONString(); + } + + public static double[][] parseMatrix(String json) { + try { + JSONArray values = JSON.parseArray(json); + if (values == null || values.size() != 16) throw new IllegalArgumentException(); + double[][] matrix = new double[4][4]; + for (int index = 0; index < 16; index++) matrix[index / 4][index % 4] = values.getDoubleValue(index); + return matrix; + } catch (RuntimeException exception) { + throw new GlobalException("标定矩阵格式无效"); + } + } + + public static double[][] multiply(double[][] left, double[][] right) { + double[][] result = new double[4][4]; + for (int row = 0; row < 4; row++) for (int column = 0; column < 4; column++) + for (int index = 0; index < 4; index++) result[row][column] += left[row][index] * right[index][column]; + return result; + } + + public static double[][] rigidInverse(double[][] matrix) { + double[][] result = identity(); + for (int row = 0; row < 3; row++) for (int column = 0; column < 3; column++) result[row][column] = matrix[column][row]; + for (int row = 0; row < 3; row++) result[row][3] = -(result[row][0] * matrix[0][3] + + result[row][1] * matrix[1][3] + result[row][2] * matrix[2][3]); + return result; + } + + public static double[] transform(double[][] matrix, double[] point) { + return new double[]{matrix[0][0] * point[0] + matrix[0][1] * point[1] + matrix[0][2] * point[2] + matrix[0][3], + matrix[1][0] * point[0] + matrix[1][1] * point[1] + matrix[1][2] * point[2] + matrix[1][3], + matrix[2][0] * point[0] + matrix[2][1] * point[1] + matrix[2][2] * point[2] + matrix[2][3]}; + } + + public static double[] pose(double[][] matrix) { + double ry = Math.asin(clamp(-matrix[2][0], -1, 1)); + double rx = Math.abs(Math.cos(ry)) > 1e-8 ? Math.atan2(matrix[2][1], matrix[2][2]) + : Math.atan2(-matrix[1][2], matrix[1][1]); + double rz = Math.abs(Math.cos(ry)) > 1e-8 ? Math.atan2(matrix[1][0], matrix[0][0]) : 0; + return new double[]{matrix[0][3], matrix[1][3], matrix[2][3], rx, ry, rz}; + } + + public static double[][] fromEuler(double rx, double ry, double rz) { + double cx = Math.cos(rx), sx = Math.sin(rx), cy = Math.cos(ry), sy = Math.sin(ry), cz = Math.cos(rz), sz = Math.sin(rz); + double[][] result = identity(); + result[0][0] = cz * cy; result[0][1] = cz * sy * sx - sz * cx; result[0][2] = cz * sy * cx + sz * sx; + result[1][0] = sz * cy; result[1][1] = sz * sy * sx + cz * cx; result[1][2] = sz * sy * cx - cz * sx; + result[2][0] = -sy; result[2][1] = cy * sx; result[2][2] = cy * cx; + return result; + } + + public static double[][] identity() { + double[][] result = new double[4][4]; + for (int index = 0; index < 4; index++) result[index][index] = 1; + return result; + } + + private static CalibrationResult result(double[][] matrix, List samples, double[] fixed) { + List errors = new ArrayList<>(); + for (CalibrationSample sample : samples) { + double[] prediction = transform(multiply(sample.baseToFlange(), matrix), sample.cameraPoint()); + errors.add(distance(prediction, fixed) * 1000.0); + } + return new CalibrationResult(matrix, mean(errors), errors.stream().mapToDouble(Double::doubleValue).max().orElse(0), + samples.size(), fixed); + } + + private static double[] residuals(List samples, double[] parameters) { + double[][] rotation = fromRotationVector(Arrays.copyOf(parameters, 3)); + rotation[0][3] = parameters[3]; rotation[1][3] = parameters[4]; rotation[2][3] = parameters[5]; + double[] fixed = Arrays.copyOfRange(parameters, 6, 9); + double[] result = new double[samples.size() * 3]; + for (int index = 0; index < samples.size(); index++) { + CalibrationSample sample = samples.get(index); + double[] predicted = transform(multiply(sample.baseToFlange(), rotation), sample.cameraPoint()); + for (int axis = 0; axis < 3; axis++) result[index * 3 + axis] = predicted[axis] - fixed[axis]; + } + return result; + } + + private static double[][] numericJacobian(List samples, double[] parameters, double[] residual) { + double[][] result = new double[residual.length][parameters.length]; + for (int column = 0; column < parameters.length; column++) { + double[] shifted = parameters.clone(); + double epsilon = column < 3 ? 1e-6 : 1e-5; + shifted[column] += epsilon; + double[] shiftedResidual = residuals(samples, shifted); + for (int row = 0; row < residual.length; row++) result[row][column] = (shiftedResidual[row] - residual[row]) / epsilon; + } + return result; + } + + private static double cost(List samples, double[] parameters) { + double sum = 0; + for (double value : residuals(samples, parameters)) sum += value * value; + return sum; + } + + private static double[][] fromRotationVector(double[] vector) { + double angle = norm(vector); + if (angle < 1e-12) return identity(); + double x = vector[0] / angle, y = vector[1] / angle, z = vector[2] / angle; + double c = Math.cos(angle), s = Math.sin(angle), t = 1 - c; + double[][] result = identity(); + result[0][0] = t*x*x+c; result[0][1] = t*x*y-s*z; result[0][2] = t*x*z+s*y; + result[1][0] = t*x*y+s*z; result[1][1] = t*y*y+c; result[1][2] = t*y*z-s*x; + result[2][0] = t*x*z-s*y; result[2][1] = t*y*z+s*x; result[2][2] = t*z*z+c; + return result; + } + + private static double[] rotationVector(double[][] matrix) { + double angle = Math.acos(clamp((matrix[0][0] + matrix[1][1] + matrix[2][2] - 1) / 2, -1, 1)); + if (angle < 1e-10) return new double[3]; + double scale = angle / (2 * Math.sin(angle)); + return new double[]{(matrix[2][1] - matrix[1][2]) * scale, + (matrix[0][2] - matrix[2][0]) * scale, (matrix[1][0] - matrix[0][1]) * scale}; + } + + private static double[] leastSquares(double[][] design, double[] target) { + int columns = design[0].length; + double[][] normal = new double[columns][columns]; + double[] rhs = new double[columns]; + for (int row = 0; row < design.length; row++) for (int column = 0; column < columns; column++) { + rhs[column] += design[row][column] * target[row]; + for (int other = 0; other < columns; other++) normal[column][other] += design[row][column] * design[row][other]; + } + for (int index = 0; index < columns; index++) normal[index][index] += 1e-12; + return solve(normal, rhs); + } + + private static double[] solve(double[][] source, double[] sourceRight) { + int size = sourceRight.length; + double[][] matrix = new double[size][size]; + for (int row = 0; row < size; row++) matrix[row] = source[row].clone(); + double[] right = sourceRight.clone(); + for (int column = 0; column < size; column++) { + int pivot = column; + for (int row = column + 1; row < size; row++) if (Math.abs(matrix[row][column]) > Math.abs(matrix[pivot][column])) pivot = row; + if (Math.abs(matrix[pivot][column]) < 1e-14) throw new GlobalException("标定姿态变化不足,无法求解"); + double[] temp = matrix[column]; matrix[column] = matrix[pivot]; matrix[pivot] = temp; + double tempValue = right[column]; right[column] = right[pivot]; right[pivot] = tempValue; + for (int row = column + 1; row < size; row++) { + double factor = matrix[row][column] / matrix[column][column]; + for (int index = column; index < size; index++) matrix[row][index] -= factor * matrix[column][index]; + right[row] -= factor * right[column]; + } + } + double[] result = new double[size]; + for (int row = size - 1; row >= 0; row--) { + double value = right[row]; + for (int column = row + 1; column < size; column++) value -= matrix[row][column] * result[column]; + result[row] = value / matrix[row][row]; + } + return result; + } + + private static double[][] transposeRotation(double[][] matrix) { + double[][] result = identity(); + for (int row = 0; row < 3; row++) for (int column = 0; column < 3; column++) result[row][column] = matrix[column][row]; + return result; + } + + private static double[] rotationTransform(double[][] rotation, double[] value) { + return new double[]{dot(Arrays.copyOf(rotation[0], 3), value), dot(Arrays.copyOf(rotation[1], 3), value), dot(Arrays.copyOf(rotation[2], 3), value)}; + } + + private static void setColumn(double[][] matrix, int column, double[] value) { + for (int row = 0; row < 3; row++) matrix[row][column] = value[row]; + } + + private static double[][] copy(double[][] matrix) { + double[][] result = new double[matrix.length][]; + for (int index = 0; index < matrix.length; index++) result[index] = matrix[index].clone(); + return result; + } + + private static double[] perpendicular(double[] vector) { + double[] seed = Math.abs(vector[0]) < 0.8 ? new double[]{1, 0, 0} : new double[]{0, 1, 0}; + return normalize(cross(seed, vector)); + } + + private static double[] cross(double[] left, double[] right) { + return new double[]{left[1]*right[2]-left[2]*right[1], left[2]*right[0]-left[0]*right[2], left[0]*right[1]-left[1]*right[0]}; + } + + private static double dot(double[] left, double[] right) { + double result = 0; for (int index = 0; index < left.length; index++) result += left[index] * right[index]; return result; + } + + private static double[] subtract(double[] left, double[] right) { + return new double[]{left[0]-right[0], left[1]-right[1], left[2]-right[2]}; + } + + private static double[] scale(double[] value, double factor) { + return new double[]{value[0]*factor, value[1]*factor, value[2]*factor}; + } + + private static double[] normalize(double[] value) { + double length = norm(value); if (length < 1e-12) throw new GlobalException("无法确定标定方向"); return scale(value, 1 / length); + } + + private static double norm(double[] value) { double result = 0; for (double item : value) result += item * item; return Math.sqrt(result); } + private static double distance(double[] left, double[] right) { return norm(subtract(left, right)); } + private static double mean(List values) { return values.stream().mapToDouble(Double::doubleValue).average().orElse(0); } + private static double value(Double value) { return value == null ? 0 : value; } + private static double clamp(double value, double min, double max) { return Math.max(min, Math.min(max, value)); } +} diff --git a/cmvr-iot-test/src/main/java/com/cmvr/test/vision/model/VisionCalibrationRequests.java b/cmvr-iot-test/src/main/java/com/cmvr/test/vision/model/VisionCalibrationRequests.java new file mode 100644 index 0000000..6bd5d16 --- /dev/null +++ b/cmvr-iot-test/src/main/java/com/cmvr/test/vision/model/VisionCalibrationRequests.java @@ -0,0 +1,46 @@ +package com.cmvr.test.vision.model; + +import lombok.Data; + +public final class VisionCalibrationRequests { + private VisionCalibrationRequests() { + } + + @Data + public static class HandEyeStart { + private String profileName; + private String robotId; + private String armDeviceId; + private String cameraDeviceId; + private Double translationRangeMm = 10.0; + private Double rotationRangeDeg = 5.0; + private Double markerSizeMm = 20.0; + private Double velocity = 0.01; + private Double acceleration = 0.03; + private Integer settleTimeMs = 500; + } + + @Data + public static class ToolStart { + private String profileName; + private String toolCode; + private String robotId; + private String armDeviceId; + private String cameraDeviceId; + } + + @Data + public static class Correction { + private Double xMm = 0.0; + private Double yMm = 0.0; + private Double zMm = 0.0; + private Double rxDeg = 0.0; + private Double ryDeg = 0.0; + private Double rzDeg = 0.0; + } + + @Data + public static class Activate { + private Boolean forced = false; + } +} diff --git a/cmvr-iot-test/src/main/java/com/cmvr/test/vision/model/VisionCalibrationSessionView.java b/cmvr-iot-test/src/main/java/com/cmvr/test/vision/model/VisionCalibrationSessionView.java new file mode 100644 index 0000000..3d2cddb --- /dev/null +++ b/cmvr-iot-test/src/main/java/com/cmvr/test/vision/model/VisionCalibrationSessionView.java @@ -0,0 +1,26 @@ +package com.cmvr.test.vision.model; + +import lombok.Builder; +import lombok.Data; + +import java.util.List; + +@Data +@Builder +public class VisionCalibrationSessionView { + private String sessionId; + private String sessionType; + private String status; + private Integer currentStep; + private Integer totalSteps; + private Integer sampleCount; + private String message; + private String imageUrl; + private String profileId; + private Double meanErrorMm; + private Double maxErrorMm; + private Double directionErrorDeg; + private List markerCorners; + private Integer imageWidth; + private Integer imageHeight; +} diff --git a/cmvr-iot-test/src/main/java/com/cmvr/test/vision/service/VisionCalibrationProfileService.java b/cmvr-iot-test/src/main/java/com/cmvr/test/vision/service/VisionCalibrationProfileService.java new file mode 100644 index 0000000..950686d --- /dev/null +++ b/cmvr-iot-test/src/main/java/com/cmvr/test/vision/service/VisionCalibrationProfileService.java @@ -0,0 +1,190 @@ +package com.cmvr.test.vision.service; + +import cn.hutool.core.util.StrUtil; +import cn.hutool.crypto.digest.DigestUtil; +import com.alibaba.fastjson2.JSON; +import com.alibaba.fastjson2.JSONObject; +import com.baomidou.mybatisplus.core.conditions.query.LambdaQueryWrapper; +import com.baomidou.mybatisplus.core.conditions.update.LambdaUpdateWrapper; +import com.baomidou.mybatisplus.spring.service.impl.ServiceImpl; +import com.cmvr.common.exception.GlobalException; +import com.cmvr.test.vision.domain.VisionCalibrationProfile; +import com.cmvr.test.vision.mapper.VisionCalibrationProfileMapper; +import com.cmvr.test.vision.math.CalibrationResult; +import com.cmvr.test.vision.math.VisionCalibrationMath; +import com.cmvr.test.vision.model.VisionCalibrationRequests; +import org.springframework.stereotype.Service; +import org.springframework.transaction.annotation.Transactional; + +import java.math.BigDecimal; +import java.math.RoundingMode; +import java.time.LocalDateTime; +import java.util.List; + +@Service +public class VisionCalibrationProfileService + extends ServiceImpl { + + public static final String HAND_EYE = "HAND_EYE"; + public static final String TOOL_TCP = "TOOL_TCP"; + public static final String ACTIVE = "ACTIVE"; + public static final String DRAFT = "DRAFT"; + public static final String INVALID = "INVALID"; + + public List list(String profileType, String robotId, String keyword) { + LambdaQueryWrapper query = new LambdaQueryWrapper() + .eq(StrUtil.isNotBlank(profileType), VisionCalibrationProfile::getProfileType, profileType) + .eq(StrUtil.isNotBlank(robotId), VisionCalibrationProfile::getRobotId, robotId) + .and(StrUtil.isNotBlank(keyword), wrapper -> wrapper + .like(VisionCalibrationProfile::getProfileName, keyword) + .or().like(VisionCalibrationProfile::getToolCode, keyword)) + .orderByDesc(VisionCalibrationProfile::getCreateTime); + return baseMapper.selectList(query); + } + + public VisionCalibrationProfile require(String id) { + VisionCalibrationProfile profile = baseMapper.selectById(id); + if (profile == null) throw new GlobalException("标定方案不存在"); + return profile; + } + + public VisionCalibrationProfile activeHandEye(String robotId, String armDeviceId, String cameraDeviceId) { + return active(profileKey(HAND_EYE, robotId, armDeviceId, cameraDeviceId)); + } + + public VisionCalibrationProfile activeTool(String robotId, String armDeviceId, String toolCode) { + if (StrUtil.isBlank(toolCode)) throw new GlobalException("请选择末端工具"); + return active(profileKey(TOOL_TCP, robotId, armDeviceId, toolCode)); + } + + private VisionCalibrationProfile active(String key) { + VisionCalibrationProfile profile = baseMapper.selectOne(new LambdaQueryWrapper() + .eq(VisionCalibrationProfile::getProfileKey, key) + .eq(VisionCalibrationProfile::getStatus, ACTIVE) + .orderByDesc(VisionCalibrationProfile::getVersionNo) + .last("limit 1")); + if (profile == null) throw new GlobalException("当前设备没有已启用的标定方案"); + return profile; + } + + @Transactional(rollbackFor = Exception.class) + public VisionCalibrationProfile createHandEye(VisionCalibrationRequests.HandEyeStart request, + CalibrationResult result, JSONObject metadata, + String operator) { + String key = profileKey(HAND_EYE, request.getRobotId(), request.getArmDeviceId(), request.getCameraDeviceId()); + return create(key, HAND_EYE, request.getProfileName(), request.getRobotId(), request.getArmDeviceId(), + request.getCameraDeviceId(), null, result, metadata, operator); + } + + @Transactional(rollbackFor = Exception.class) + public VisionCalibrationProfile createTool(VisionCalibrationRequests.ToolStart request, + CalibrationResult result, double[][] finalMatrix, + JSONObject metadata, String operator) { + String key = profileKey(TOOL_TCP, request.getRobotId(), request.getArmDeviceId(), request.getToolCode()); + CalibrationResult complete = new CalibrationResult(finalMatrix, result.meanErrorMm(), result.maxErrorMm(), + result.sampleCount(), result.fixedPoint()); + return create(key, TOOL_TCP, request.getProfileName(), request.getRobotId(), request.getArmDeviceId(), + request.getCameraDeviceId(), request.getToolCode(), complete, metadata, operator); + } + + private VisionCalibrationProfile create(String key, String type, String name, String robotId, + String armDeviceId, String cameraDeviceId, String toolCode, + CalibrationResult result, JSONObject metadata, String operator) { + VisionCalibrationProfile latest = baseMapper.selectOne(new LambdaQueryWrapper() + .eq(VisionCalibrationProfile::getProfileKey, key) + .orderByDesc(VisionCalibrationProfile::getVersionNo).last("limit 1")); + VisionCalibrationProfile profile = new VisionCalibrationProfile(); + profile.setProfileType(type); + profile.setProfileKey(key); + profile.setProfileName(StrUtil.blankToDefault(name, HAND_EYE.equals(type) ? "手眼标定" : toolCode)); + profile.setVersionNo(latest == null ? 1 : latest.getVersionNo() + 1); + profile.setRobotId(robotId); + profile.setArmDeviceId(armDeviceId); + profile.setCameraDeviceId(cameraDeviceId); + profile.setToolCode(toolCode); + profile.setRawMatrixJson(VisionCalibrationMath.toJson(result.matrix())); + profile.setCorrectionJson(JSON.toJSONString(new VisionCalibrationRequests.Correction())); + profile.setEffectiveMatrixJson(profile.getRawMatrixJson()); + profile.setMeanErrorMm(decimal(result.meanErrorMm())); + profile.setMaxErrorMm(decimal(result.maxErrorMm())); + profile.setSampleCount(result.sampleCount()); + profile.setMetadataJson(metadata == null ? null : metadata.toJSONString()); + profile.setForcedEnabled(false); + profile.setStatus(result.maxErrorMm() <= 1.0 ? ACTIVE : DRAFT); + profile.setVerifiedTime(LocalDateTime.now()); + profile.setCreateBy(operator); + if (ACTIVE.equals(profile.getStatus())) deactivateOthers(key, null, operator); + baseMapper.insert(profile); + return profile; + } + + @Transactional(rollbackFor = Exception.class) + public VisionCalibrationProfile updateCorrection(String id, VisionCalibrationRequests.Correction correction, + String operator) { + VisionCalibrationProfile profile = require(id); + double[][] effective = VisionCalibrationMath.applyCorrection( + VisionCalibrationMath.parseMatrix(profile.getRawMatrixJson()), correction); + profile.setCorrectionJson(JSON.toJSONString(correction)); + profile.setEffectiveMatrixJson(VisionCalibrationMath.toJson(effective)); + profile.setStatus(DRAFT); + profile.setForcedEnabled(false); + profile.setVerifiedTime(null); + profile.setUpdateBy(operator); + baseMapper.updateById(profile); + return profile; + } + + @Transactional(rollbackFor = Exception.class) + public VisionCalibrationProfile activate(String id, boolean forced, String operator) { + VisionCalibrationProfile profile = require(id); + if (profile.getVerifiedTime() == null) throw new GlobalException("人工修正后必须先完成快速验证"); + if (!forced && profile.getMaxErrorMm() != null && profile.getMaxErrorMm().doubleValue() > 1.0) { + throw new GlobalException("验证误差超过1毫米,请人工确认后强制启用"); + } + deactivateOthers(profile.getProfileKey(), profile.getId(), operator); + profile.setStatus(ACTIVE); + profile.setForcedEnabled(forced); + profile.setUpdateBy(operator); + baseMapper.updateById(profile); + return profile; + } + + public VisionCalibrationProfile markVerified(String id, double meanErrorMm, double maxErrorMm, + int sampleCount, String operator) { + VisionCalibrationProfile profile = require(id); + profile.setMeanErrorMm(decimal(meanErrorMm)); + profile.setMaxErrorMm(decimal(maxErrorMm)); + profile.setSampleCount(sampleCount); + profile.setVerifiedTime(LocalDateTime.now()); + profile.setUpdateBy(operator); + baseMapper.updateById(profile); + return profile; + } + + @Transactional(rollbackFor = Exception.class) + public void invalidate(String id, String operator) { + VisionCalibrationProfile profile = require(id); + profile.setStatus(INVALID); + profile.setUpdateBy(operator); + baseMapper.updateById(profile); + } + + private void deactivateOthers(String key, String exceptId, String operator) { + LambdaUpdateWrapper update = new LambdaUpdateWrapper() + .eq(VisionCalibrationProfile::getProfileKey, key) + .eq(VisionCalibrationProfile::getStatus, ACTIVE) + .set(VisionCalibrationProfile::getStatus, DRAFT) + .set(VisionCalibrationProfile::getUpdateBy, operator) + .set(VisionCalibrationProfile::getUpdateTime, LocalDateTime.now()); + if (StrUtil.isNotBlank(exceptId)) update.ne(VisionCalibrationProfile::getId, exceptId); + baseMapper.update(null, update); + } + + private String profileKey(String type, String... parts) { + return DigestUtil.sha256Hex(type + "|" + String.join("|", parts)); + } + + private BigDecimal decimal(double value) { + return BigDecimal.valueOf(value).setScale(4, RoundingMode.HALF_UP); + } +} diff --git a/cmvr-iot-test/src/main/java/com/cmvr/test/vision/service/VisionCalibrationRecordService.java b/cmvr-iot-test/src/main/java/com/cmvr/test/vision/service/VisionCalibrationRecordService.java new file mode 100644 index 0000000..1955c76 --- /dev/null +++ b/cmvr-iot-test/src/main/java/com/cmvr/test/vision/service/VisionCalibrationRecordService.java @@ -0,0 +1,11 @@ +package com.cmvr.test.vision.service; + +import com.baomidou.mybatisplus.spring.service.impl.ServiceImpl; +import com.cmvr.test.vision.domain.VisionCalibrationRecord; +import com.cmvr.test.vision.mapper.VisionCalibrationRecordMapper; +import org.springframework.stereotype.Service; + +@Service +public class VisionCalibrationRecordService + extends ServiceImpl { +} diff --git a/cmvr-iot-test/src/main/java/com/cmvr/test/vision/service/VisionCalibrationSessionService.java b/cmvr-iot-test/src/main/java/com/cmvr/test/vision/service/VisionCalibrationSessionService.java new file mode 100644 index 0000000..5c60114 --- /dev/null +++ b/cmvr-iot-test/src/main/java/com/cmvr/test/vision/service/VisionCalibrationSessionService.java @@ -0,0 +1,563 @@ +package com.cmvr.test.vision.service; + +import cn.hutool.core.util.StrUtil; +import com.alibaba.fastjson2.JSON; +import com.alibaba.fastjson2.JSONArray; +import com.alibaba.fastjson2.JSONObject; +import com.cmvr.common.exception.GlobalException; +import com.cmvr.edge.client.model.EdgeCommonVO; +import com.cmvr.edge.client.model.camera.EdgeCameraRgbdSnapshot; +import com.cmvr.edge.client.service.EdgeArmService; +import com.cmvr.edge.client.service.EdgeCameraService; +import com.cmvr.test.vision.domain.VisionCalibrationProfile; +import com.cmvr.test.vision.domain.VisionCalibrationRecord; +import com.cmvr.test.vision.marker.MarkerObservation; +import com.cmvr.test.vision.marker.SquareMarkerDetector; +import com.cmvr.test.vision.math.CalibrationResult; +import com.cmvr.test.vision.math.CalibrationSample; +import com.cmvr.test.vision.math.VisionCalibrationMath; +import com.cmvr.test.vision.model.VisionCalibrationRequests; +import com.cmvr.test.vision.model.VisionCalibrationSessionView; +import jakarta.annotation.PreDestroy; +import lombok.RequiredArgsConstructor; +import lombok.extern.slf4j.Slf4j; +import org.springframework.stereotype.Service; + +import java.time.LocalDateTime; +import java.util.ArrayList; +import java.util.List; +import java.util.Map; +import java.util.UUID; +import java.util.concurrent.ConcurrentHashMap; +import java.util.concurrent.ExecutorService; +import java.util.concurrent.Executors; +import java.util.concurrent.ScheduledExecutorService; +import java.util.concurrent.TimeUnit; +import java.util.concurrent.atomic.AtomicBoolean; + +@Service +@RequiredArgsConstructor +@Slf4j +public class VisionCalibrationSessionService { + private static final long HEARTBEAT_TIMEOUT_MS = 6_000; + private static final int HAND_EYE_SAMPLE_COUNT = 12; + private final EdgeArmService armService; + private final EdgeCameraService cameraService; + private final SquareMarkerDetector markerDetector; + private final VisionCalibrationProfileService profileService; + private final VisionCalibrationRecordService recordService; + private final Map sessions = new ConcurrentHashMap<>(); + private final ExecutorService worker = Executors.newCachedThreadPool(runnable -> { + Thread thread = new Thread(runnable, "vision-calibration-worker"); + thread.setDaemon(true); + return thread; + }); + private final ScheduledExecutorService watchdog = Executors.newSingleThreadScheduledExecutor(runnable -> { + Thread thread = new Thread(runnable, "vision-calibration-watchdog"); + thread.setDaemon(true); + return thread; + }); + + public VisionCalibrationSessionView startHandEye(VisionCalibrationRequests.HandEyeStart request, String operator) { + validateHandEye(request); + Session session = create("HAND_EYE", request.getRobotId(), request.getArmDeviceId(), + request.getCameraDeviceId(), null, operator, HAND_EYE_SAMPLE_COUNT); + session.handEyeRequest = request; + sessions.put(session.id, session); + worker.submit(() -> runHandEye(session)); + return view(session); + } + + public VisionCalibrationSessionView startTool(VisionCalibrationRequests.ToolStart request, String operator) { + if (StrUtil.hasBlank(request.getToolCode(), request.getRobotId(), request.getArmDeviceId(), request.getCameraDeviceId())) { + throw new GlobalException("工具名称、机器人、机械臂和相机不能为空"); + } + VisionCalibrationProfile handEye = profileService.activeHandEye( + request.getRobotId(), request.getArmDeviceId(), request.getCameraDeviceId()); + Session session = create("TOOL_TCP", request.getRobotId(), request.getArmDeviceId(), + request.getCameraDeviceId(), request.getToolCode(), operator, 6); + session.toolRequest = request; + session.handEyeProfile = handEye; + try { + session.message = "正在识别工具方向标定码"; + double[][] flange = currentFlange(session); + MarkerObservation marker = captureMarker(session); + double[][] baseToCamera = VisionCalibrationMath.multiply(flange, + VisionCalibrationMath.parseMatrix(handEye.getEffectiveMatrixJson())); + session.planeNormalBase = rotate(baseToCamera, marker.normalCamera()); + session.planeXBase = rotate(baseToCamera, marker.xAxisCamera()); + session.imageUrl = marker.imageUrl(); + session.markerCorners = marker.pixelCorners(); + session.imageWidth = marker.width(); session.imageHeight = marker.height(); + session.message = "标定平面已记录,请将工具中心从不同角度对准页面标示的规范角点"; + updateRecord(session); + } catch (RuntimeException exception) { + fail(session, exception); + } finally { + sessions.put(session.id, session); + } + return view(session); + } + + public VisionCalibrationSessionView startVerification(String profileId, String operator) { + VisionCalibrationProfile profile = profileService.require(profileId); + if (VisionCalibrationProfileService.HAND_EYE.equals(profile.getProfileType())) { + Session session = create("HAND_EYE_VERIFY", profile.getRobotId(), profile.getArmDeviceId(), + profile.getCameraDeviceId(), null, operator, 4); + session.profileToVerify = profile; + VisionCalibrationRequests.HandEyeStart request = new VisionCalibrationRequests.HandEyeStart(); + request.setRobotId(profile.getRobotId()); request.setArmDeviceId(profile.getArmDeviceId()); + request.setCameraDeviceId(profile.getCameraDeviceId()); request.setTranslationRangeMm(15.0); + request.setRotationRangeDeg(6.0); request.setVelocity(0.025); request.setAcceleration(0.06); + request.setSettleTimeMs(500); + session.handEyeRequest = request; + sessions.put(session.id, session); + worker.submit(() -> runHandEyeVerification(session)); + return view(session); + } + Session session = create("TOOL_VERIFY", profile.getRobotId(), profile.getArmDeviceId(), + profile.getCameraDeviceId(), profile.getToolCode(), operator, 4); + session.profileToVerify = profile; + session.handEyeProfile = profileService.activeHandEye( + profile.getRobotId(), profile.getArmDeviceId(), profile.getCameraDeviceId()); + try { + captureToolPlane(session); + session.message = "请将工具中心从3个不同角度对准规范角点并依次记录"; + } finally { + sessions.put(session.id, session); + } + return view(session); + } + + public VisionCalibrationSessionView recordToolPivot(String sessionId) { + Session session = requireRunningTool(sessionId); + double[][] pose = currentFlange(session); + if (session.flangeSamples.stream().anyMatch(previous -> rotationDistanceDeg(previous, pose) < 4.0)) { + throw new GlobalException("本次末端角度与已有样本过于接近,请明显改变姿态后再记录"); + } + session.flangeSamples.add(pose); + session.currentStep = session.flangeSamples.size(); + int required = "TOOL_VERIFY".equals(session.type) ? 3 : 5; + session.message = session.flangeSamples.size() < required + ? "已记录姿态 " + session.flangeSamples.size() + "/" + required + ",请改变末端角度并继续对准同一角点" + : "定点姿态已足够,请将工具垂直贴合标定码平面并记录安全方向"; + updateRecord(session); + return view(session); + } + + public VisionCalibrationSessionView recordToolDirectionAndFinish(String sessionId) { + Session session = requireRunningTool(sessionId); + int required = "TOOL_VERIFY".equals(session.type) ? 3 : 5; + if (session.flangeSamples.size() < required) throw new GlobalException("请先记录至少" + required + "个定点旋转姿态"); + double[][] directionPose = currentFlange(session); + if ("TOOL_VERIFY".equals(session.type)) { + CalibrationResult verification = verifyTool(session, directionPose); + profileService.markVerified(session.profileToVerify.getId(), verification.meanErrorMm(), + verification.maxErrorMm(), session.flangeSamples.size(), session.operator); + complete(session, session.profileToVerify, verification); + session.message = session.directionErrorDeg <= 1.0 && verification.maxErrorMm() <= 1.0 + ? "工具TCP验证通过,可以启用" : "验证完成但误差超限,可继续微调或人工强制启用"; + return view(session); + } + CalibrationResult pivot = VisionCalibrationMath.solveToolPivot(session.flangeSamples); + double[][] matrix = VisionCalibrationMath.applyToolDirection( + pivot.matrix(), directionPose, session.planeNormalBase, session.planeXBase); + JSONObject metadata = new JSONObject() + .fluentPut("handEyeProfileId", session.handEyeProfile.getId()) + .fluentPut("handEyeVersion", session.handEyeProfile.getVersionNo()) + .fluentPut("markerImageUrl", session.imageUrl) + .fluentPut("safeDirectionPose", matrixJson(directionPose)) + .fluentPut("planeNormalBase", session.planeNormalBase) + .fluentPut("planeXBase", session.planeXBase); + VisionCalibrationProfile profile = profileService.createTool( + session.toolRequest, pivot, matrix, metadata, session.operator); + complete(session, profile, pivot); + return view(session); + } + + private void captureToolPlane(Session session) { + try { + double[][] flange = currentFlange(session); + MarkerObservation marker = captureMarker(session); + double[][] baseToCamera = VisionCalibrationMath.multiply(flange, + VisionCalibrationMath.parseMatrix(session.handEyeProfile.getEffectiveMatrixJson())); + session.planeNormalBase = rotate(baseToCamera, marker.normalCamera()); + session.planeXBase = rotate(baseToCamera, marker.xAxisCamera()); + session.imageUrl = marker.imageUrl(); session.markerCorners = marker.pixelCorners(); + session.imageWidth = marker.width(); session.imageHeight = marker.height(); + updateRecord(session); + } catch (RuntimeException exception) { + fail(session, exception); + throw exception; + } + } + + private CalibrationResult verifyTool(Session session, double[][] directionPose) { + double[][] toolMatrix = VisionCalibrationMath.parseMatrix(session.profileToVerify.getEffectiveMatrixJson()); + double[] tcp = {toolMatrix[0][3], toolMatrix[1][3], toolMatrix[2][3]}; + List contactPoints = session.flangeSamples.stream() + .map(pose -> VisionCalibrationMath.transform(pose, tcp)).toList(); + double[] center = new double[3]; + contactPoints.forEach(point -> { for (int axis = 0; axis < 3; axis++) center[axis] += point[axis] / contactPoints.size(); }); + List errors = contactPoints.stream().map(point -> distance(point, center) * 1000.0).toList(); + double[] toolZFlange = {toolMatrix[0][2], toolMatrix[1][2], toolMatrix[2][2]}; + double[] toolZBase = normalize(rotate(directionPose, toolZFlange)); + double[] expected = normalize(scale(session.planeNormalBase, -1)); + session.directionErrorDeg = Math.toDegrees(Math.acos(Math.max(-1, Math.min(1, dot(toolZBase, expected))))); + return new CalibrationResult(toolMatrix, errors.stream().mapToDouble(Double::doubleValue).average().orElse(0), + errors.stream().mapToDouble(Double::doubleValue).max().orElse(0), contactPoints.size(), center); + } + + public VisionCalibrationSessionView heartbeat(String sessionId) { + Session session = require(sessionId); + session.lastHeartbeat = System.currentTimeMillis(); + return view(session); + } + + public VisionCalibrationSessionView status(String sessionId) { + return view(require(sessionId)); + } + + public VisionCalibrationSessionView stop(String sessionId) { + Session session = require(sessionId); + cancel(session, "用户停止标定", true); + return view(session); + } + + private void runHandEye(Session session) { + double[][] initialPose = null; + try { + session.message = "正在记录初始位姿并识别标定码"; + initialPose = currentFlange(session); + session.initialPose = initialPose; + List targets = calibrationTargets(initialPose, session.handEyeRequest); + for (int index = 0; index < targets.size(); index++) { + checkRunning(session); + if (index > 0) { + session.message = "正在移动到标定姿态 " + (index + 1) + "/" + targets.size(); + move(session, targets.get(index), session.handEyeRequest.getVelocity(), + session.handEyeRequest.getAcceleration()); + sleep(session.handEyeRequest.getSettleTimeMs()); + } + checkRunning(session); + double[][] flange = currentFlange(session); + MarkerObservation marker = captureMarker(session); + session.handEyeSamples.add(new CalibrationSample(flange, marker.centerCamera())); + session.imageUrl = marker.imageUrl(); + session.markerCorners = marker.pixelCorners(); + session.imageWidth = marker.width(); session.imageHeight = marker.height(); + session.currentStep = session.handEyeSamples.size(); + session.message = "已采集 " + session.currentStep + "/" + targets.size() + + ",平面拟合误差 " + String.format("%.2f", marker.planeRmseMm()) + " mm"; + updateRecord(session); + } + CalibrationResult solved = solveWithValidation(session.handEyeSamples); + JSONObject metadata = new JSONObject() + .fluentPut("markerSizeMm", session.handEyeRequest.getMarkerSizeMm()) + .fluentPut("translationRangeMm", session.handEyeRequest.getTranslationRangeMm()) + .fluentPut("rotationRangeDeg", session.handEyeRequest.getRotationRangeDeg()) + .fluentPut("fixedMarkerPoint", solved.fixedPoint()) + .fluentPut("validationSamples", 3); + session.message = "标定完成,正在返回初始位姿"; + move(session, initialPose, session.handEyeRequest.getVelocity(), session.handEyeRequest.getAcceleration()); + VisionCalibrationProfile profile = profileService.createHandEye( + session.handEyeRequest, solved, metadata, session.operator); + complete(session, profile, solved); + } catch (CancelledException ignored) { + // The watchdog or stop endpoint already recorded the terminal state. + } catch (RuntimeException exception) { + fail(session, exception); + } + } + + private void runHandEyeVerification(Session session) { + try { + double[][] initial = currentFlange(session); + session.initialPose = initial; + List targets = calibrationTargets(initial, session.handEyeRequest).subList(0, 4); + double[][] cameraTransform = VisionCalibrationMath.parseMatrix(session.profileToVerify.getEffectiveMatrixJson()); + List markerPoints = new ArrayList<>(); + for (int index = 0; index < targets.size(); index++) { + checkRunning(session); + if (index > 0) { + session.message = "正在执行验证姿态 " + (index + 1) + "/4"; + move(session, targets.get(index), session.handEyeRequest.getVelocity(), session.handEyeRequest.getAcceleration()); + sleep(session.handEyeRequest.getSettleTimeMs()); + } + double[][] flange = currentFlange(session); + MarkerObservation marker = captureMarker(session); + markerPoints.add(VisionCalibrationMath.transform( + VisionCalibrationMath.multiply(flange, cameraTransform), marker.centerCamera())); + session.currentStep = index + 1; session.imageUrl = marker.imageUrl(); + session.markerCorners = marker.pixelCorners(); updateRecord(session); + session.imageWidth = marker.width(); session.imageHeight = marker.height(); + } + double[] center = new double[3]; + markerPoints.forEach(point -> { for (int axis = 0; axis < 3; axis++) center[axis] += point[axis] / markerPoints.size(); }); + List errors = markerPoints.stream().map(point -> distance(point, center) * 1000.0).toList(); + CalibrationResult result = new CalibrationResult(cameraTransform, + errors.stream().mapToDouble(Double::doubleValue).average().orElse(0), + errors.stream().mapToDouble(Double::doubleValue).max().orElse(0), markerPoints.size(), center); + move(session, initial, session.handEyeRequest.getVelocity(), session.handEyeRequest.getAcceleration()); + profileService.markVerified(session.profileToVerify.getId(), result.meanErrorMm(), result.maxErrorMm(), + result.sampleCount(), session.operator); + complete(session, session.profileToVerify, result); + session.message = result.maxErrorMm() <= 1.0 ? "快速验证通过,可以启用" : "验证完成但误差超过1毫米"; + } catch (CancelledException ignored) { + } catch (RuntimeException exception) { + fail(session, exception); + } + } + + private CalibrationResult solveWithValidation(List samples) { + List training = samples.subList(0, samples.size() - 3); + List validation = samples.subList(samples.size() - 3, samples.size()); + CalibrationResult solved = VisionCalibrationMath.solveHandEye(training, null); + List errors = new ArrayList<>(); + for (CalibrationSample sample : validation) { + double[] predicted = VisionCalibrationMath.transform( + VisionCalibrationMath.multiply(sample.baseToFlange(), solved.matrix()), sample.cameraPoint()); + errors.add(distance(predicted, solved.fixedPoint()) * 1000.0); + } + double mean = errors.stream().mapToDouble(Double::doubleValue).average().orElse(0); + double max = errors.stream().mapToDouble(Double::doubleValue).max().orElse(0); + return new CalibrationResult(solved.matrix(), mean, max, samples.size(), solved.fixedPoint()); + } + + private List calibrationTargets(double[][] initial, VisionCalibrationRequests.HandEyeStart request) { + double t = clamp(request.getTranslationRangeMm(), 5, 20) / 1000.0; + double r = Math.toRadians(clamp(request.getRotationRangeDeg(), 3, 10)); + double[][] deltas = { + {0,0,0,0,0,0}, {t,0,0,r,0,0}, {-t,0,0,-r,0,0}, + {0,t,0,0,r,0}, {0,-t,0,0,-r,0}, {0,0,t,0,0,r}, + {0,0,-t,0,0,-r}, {t,t,0,r,-r,0}, {-t,t,0,-r,-r,0}, + {t,-t,0,r,r,0}, {-t,-t,0,-r,r,0}, {0,t,t,r,0,r} + }; + List result = new ArrayList<>(); + for (double[] delta : deltas) { + double[][] transform = VisionCalibrationMath.fromEuler(delta[3], delta[4], delta[5]); + transform[0][3] = delta[0]; transform[1][3] = delta[1]; transform[2][3] = delta[2]; + result.add(VisionCalibrationMath.multiply(initial, transform)); + } + return result; + } + + private MarkerObservation captureMarker(Session session) { + EdgeCameraRgbdSnapshot snapshot = cameraService.captureRgbd(target(session.robotId, session.cameraDeviceId)); + return markerDetector.detect(snapshot); + } + + private double[][] currentFlange(Session session) { + return VisionCalibrationMath.fromCartesianPose(armService.getPose( + target(session.robotId, session.armDeviceId), null, null).getPose()); + } + + private void move(Session session, double[][] target, Double velocity, Double acceleration) { + checkRunning(session); + double[] pose = VisionCalibrationMath.pose(target); + // Aubo consumes ZYX Euler angles. Unwrap each target angle around the + // current angle so crossing +/-PI never turns a small move into a full turn. + double[] current = VisionCalibrationMath.pose(currentFlange(session)); + for (int index = 3; index < 6; index++) { + pose[index] = unwrapAngle(pose[index], current[index]); + } + armService.moveL(target(session.robotId, session.armDeviceId), pose[0], pose[1], pose[2], + pose[3], pose[4], pose[5], "BASE", clamp(velocity, 0.003, 0.03), + clamp(acceleration, 0.01, 0.08), 0.0); + } + + private double unwrapAngle(double target, double reference) { + double twoPi = Math.PI * 2.0; + while (target - reference > Math.PI) target -= twoPi; + while (target - reference < -Math.PI) target += twoPi; + return target; + } + + private Session create(String type, String robotId, String armDeviceId, String cameraDeviceId, + String toolCode, String operator, int totalSteps) { + Session session = new Session(); + session.id = UUID.randomUUID().toString().replace("-", ""); + session.type = type; session.robotId = robotId; session.armDeviceId = armDeviceId; + session.cameraDeviceId = cameraDeviceId; session.toolCode = toolCode; session.operator = operator; + session.totalSteps = totalSteps; session.lastHeartbeat = System.currentTimeMillis(); + VisionCalibrationRecord record = new VisionCalibrationRecord(); + record.setId(session.id); record.setSessionType(type); record.setRobotId(robotId); + record.setArmDeviceId(armDeviceId); record.setCameraDeviceId(cameraDeviceId); record.setToolCode(toolCode); + record.setStatus("RUNNING"); record.setProgressStep(0); record.setTotalSteps(totalSteps); + record.setStartedTime(LocalDateTime.now()); record.setCreateBy(operator); + recordService.save(record); + return session; + } + + private void complete(Session session, VisionCalibrationProfile profile, CalibrationResult result) { + session.running.set(false); + session.status = "COMPLETED"; session.profileId = profile.getId(); + session.meanErrorMm = result.meanErrorMm(); session.maxErrorMm = result.maxErrorMm(); + session.message = result.maxErrorMm() <= 1.0 ? "标定和验证通过,方案已自动启用" : "标定完成但误差超过1毫米,请人工微调或强制启用"; + VisionCalibrationRecord record = recordService.getById(session.id); + record.setProfileId(profile.getId()); record.setStatus("COMPLETED"); + record.setFinishedTime(LocalDateTime.now()); record.setProgressStep(session.currentStep); + record.setSamplesJson(samplesJson(session)); + record.setResultJson(new JSONObject().fluentPut("profileId", profile.getId()) + .fluentPut("meanErrorMm", result.meanErrorMm()).fluentPut("maxErrorMm", result.maxErrorMm()) + .fluentPut("matrix", JSON.parseArray(profile.getEffectiveMatrixJson())).toJSONString()); + recordService.updateById(record); + } + + private void fail(Session session, RuntimeException exception) { + if (!session.running.compareAndSet(true, false)) return; + session.status = "FAILED"; session.message = exception.getMessage(); + log.error("视觉标定会话失败: {}", session.id, exception); + if (session.type.startsWith("HAND_EYE")) { + try { armService.stopMotion(target(session.robotId, session.armDeviceId)); } + catch (RuntimeException stopException) { log.error("标定失败后停止机械臂失败: {}", session.id, stopException); } + } + VisionCalibrationRecord record = recordService.getById(session.id); + record.setStatus("FAILED"); record.setErrorMessage(StrUtil.maxLength(exception.getMessage(), 1000)); + record.setFinishedTime(LocalDateTime.now()); record.setProgressStep(session.currentStep); + record.setSamplesJson(samplesJson(session)); recordService.updateById(record); + } + + private void cancel(Session session, String reason, boolean stopArm) { + if (!session.running.compareAndSet(true, false)) return; + session.status = "CANCELLED"; session.message = reason; + if (stopArm) { + try { armService.stopMotion(target(session.robotId, session.armDeviceId)); } + catch (RuntimeException exception) { log.error("停止标定机械臂失败: {}", session.id, exception); } + } + VisionCalibrationRecord record = recordService.getById(session.id); + record.setStatus("CANCELLED"); record.setErrorMessage(reason); record.setFinishedTime(LocalDateTime.now()); + record.setProgressStep(session.currentStep); record.setSamplesJson(samplesJson(session)); recordService.updateById(record); + } + + private void updateRecord(Session session) { + VisionCalibrationRecord record = recordService.getById(session.id); + record.setProgressStep(session.currentStep); record.setSamplesJson(samplesJson(session)); + recordService.updateById(record); + } + + private String samplesJson(Session session) { + JSONArray values = new JSONArray(); + session.handEyeSamples.forEach(sample -> values.add(new JSONObject() + .fluentPut("baseToFlange", matrixJson(sample.baseToFlange())) + .fluentPut("cameraPoint", sample.cameraPoint()))); + session.flangeSamples.forEach(sample -> values.add(new JSONObject().fluentPut("baseToFlange", matrixJson(sample)))); + return values.toJSONString(); + } + + private JSONArray matrixJson(double[][] matrix) { return JSON.parseArray(VisionCalibrationMath.toJson(matrix)); } + + private Session require(String id) { + Session session = sessions.get(id); + if (session == null) throw new GlobalException("标定会话不存在或已过期"); + return session; + } + + private Session requireRunning(String id, String type) { + Session session = require(id); + if (!type.equals(session.type)) throw new GlobalException("标定会话类型不匹配"); + checkRunning(session); + return session; + } + + private Session requireRunningTool(String id) { + Session session = require(id); + if (!"TOOL_TCP".equals(session.type) && !"TOOL_VERIFY".equals(session.type)) + throw new GlobalException("标定会话类型不匹配"); + checkRunning(session); + return session; + } + + private void checkRunning(Session session) { + if (!session.running.get()) throw new CancelledException(); + } + + private void validateHandEye(VisionCalibrationRequests.HandEyeStart request) { + if (StrUtil.hasBlank(request.getRobotId(), request.getArmDeviceId(), request.getCameraDeviceId())) + throw new GlobalException("机器人、机械臂和相机不能为空"); + request.setTranslationRangeMm(clamp(request.getTranslationRangeMm(), 5, 20)); + request.setRotationRangeDeg(clamp(request.getRotationRangeDeg(), 3, 10)); + request.setVelocity(clamp(request.getVelocity(), 0.003, 0.03)); + request.setAcceleration(clamp(request.getAcceleration(), 0.01, 0.08)); + request.setSettleTimeMs(Math.max(200, Math.min(3000, request.getSettleTimeMs() == null ? 500 : request.getSettleTimeMs()))); + } + + private EdgeCommonVO target(String robotId, String deviceId) { + EdgeCommonVO value = new EdgeCommonVO(); value.setRobotId(robotId); value.setDeviceId(deviceId); return value; + } + + private VisionCalibrationSessionView view(Session session) { + return VisionCalibrationSessionView.builder().sessionId(session.id).sessionType(session.type) + .status(session.status).currentStep(session.currentStep).totalSteps(session.totalSteps) + .sampleCount(session.handEyeSamples.size() + session.flangeSamples.size()) + .message(session.message).imageUrl(session.imageUrl).profileId(session.profileId) + .meanErrorMm(session.meanErrorMm).maxErrorMm(session.maxErrorMm) + .directionErrorDeg(session.directionErrorDeg) + .markerCorners(session.markerCorners).imageWidth(session.imageWidth).imageHeight(session.imageHeight).build(); + } + + private double[] rotate(double[][] transform, double[] value) { + return new double[]{transform[0][0]*value[0]+transform[0][1]*value[1]+transform[0][2]*value[2], + transform[1][0]*value[0]+transform[1][1]*value[1]+transform[1][2]*value[2], + transform[2][0]*value[0]+transform[2][1]*value[1]+transform[2][2]*value[2]}; + } + + private double rotationDistanceDeg(double[][] left, double[][] right) { + double trace = 0; + for (int row = 0; row < 3; row++) for (int column = 0; column < 3; column++) trace += left[column][row] * right[column][row]; + return Math.toDegrees(Math.acos(Math.max(-1, Math.min(1, (trace - 1) / 2)))); + } + + private double distance(double[] left, double[] right) { + double sum = 0; for (int index = 0; index < 3; index++) sum += Math.pow(left[index] - right[index], 2); return Math.sqrt(sum); + } + private double[] scale(double[] value, double factor) { return new double[]{value[0]*factor, value[1]*factor, value[2]*factor}; } + private double dot(double[] left, double[] right) { return left[0]*right[0]+left[1]*right[1]+left[2]*right[2]; } + private double[] normalize(double[] value) { double size = Math.sqrt(dot(value, value)); if (size < 1e-10) throw new GlobalException("工具方向无效"); return scale(value, 1/size); } + + private void sleep(Integer milliseconds) { + try { Thread.sleep(milliseconds == null ? 500 : milliseconds); } + catch (InterruptedException exception) { Thread.currentThread().interrupt(); throw new CancelledException(); } + } + + private double clamp(Double value, double min, double max) { return Math.max(min, Math.min(max, value == null ? min : value)); } + + private void inspectHeartbeats() { + long now = System.currentTimeMillis(); + sessions.values().stream().filter(session -> "RUNNING".equals(session.status)) + .filter(session -> now - session.lastHeartbeat > HEARTBEAT_TIMEOUT_MS) + .forEach(session -> cancel(session, "页面连接中断,标定已自动停止", true)); + } + + { + watchdog.scheduleAtFixedRate(this::inspectHeartbeats, 2, 2, TimeUnit.SECONDS); + } + + @PreDestroy + public void close() { + sessions.values().forEach(session -> cancel(session, "服务停止,标定已取消", true)); + worker.shutdownNow(); watchdog.shutdownNow(); + } + + private static final class Session { + private String id, type, status = "RUNNING", message = "标定会话已创建", imageUrl, profileId; + private String robotId, armDeviceId, cameraDeviceId, toolCode, operator; + private int currentStep, totalSteps; + private long lastHeartbeat; + private Double meanErrorMm, maxErrorMm, directionErrorDeg; + private List markerCorners = List.of(); + private Integer imageWidth, imageHeight; + private final AtomicBoolean running = new AtomicBoolean(true); + private final List handEyeSamples = new ArrayList<>(); + private final List flangeSamples = new ArrayList<>(); + private VisionCalibrationRequests.HandEyeStart handEyeRequest; + private VisionCalibrationRequests.ToolStart toolRequest; + private VisionCalibrationProfile handEyeProfile; + private VisionCalibrationProfile profileToVerify; + private double[][] initialPose; + private double[] planeNormalBase, planeXBase; + } + + private static final class CancelledException extends RuntimeException { + } +} diff --git a/pom.xml b/pom.xml index 45cb90f..8038de0 100644 --- a/pom.xml +++ b/pom.xml @@ -42,6 +42,7 @@ 1.6.14 4.2.16.Final 15.4 + 4.9.0-0 @@ -217,6 +218,12 @@ ${cmvr-iot.version} + + org.openpnp + opencv + ${opencv.version} + + com.cmvr cmvr-iot-device diff --git a/sql/resource_center_menu.sql b/sql/resource_center_menu.sql index d04a3f2..6a623e3 100644 --- a/sql/resource_center_menu.sql +++ b/sql/resource_center_menu.sql @@ -7,7 +7,8 @@ VALUES (2201, '机器人', 2200, 1, 'robot', 'resource/robot/index', '', 'ResourceRobot', 1, 0, 'C', '0', '0', 'resource:robot:list', 'monitor', 'admin', NOW(), '', NULL, '平台机器人身份与运行状态'), (2202, '设备', 2200, 2, 'device', 'resource/device/index', '', 'ResourceDevice', 1, 0, 'C', '0', '0', 'resource:device:list', 'list', 'admin', NOW(), '', NULL, '机器人设备发现与用途配置'), (2203, '地图', 2200, 3, 'map', 'resource/map/index', '', 'ResourceMap', 1, 0, 'C', '0', '0', 'resource:map:list', 'map', 'admin', NOW(), '', NULL, '仅有AGV能力的机器人需要绑定地图'), - (2204, '边缘终端', 2200, 4, 'terminal', 'resource/terminal/index', '', 'ResourceTerminal', 1, 0, 'C', '0', '0', 'resource:terminal:list', 'server', 'admin', NOW(), '', NULL, 'QUIC边缘终端运行状态') + (2204, '边缘终端', 2200, 4, 'terminal', 'resource/terminal/index', '', 'ResourceTerminal', 1, 0, 'C', '0', '0', 'resource:terminal:list', 'server', 'admin', NOW(), '', NULL, 'QUIC边缘终端运行状态'), + (2205, '视觉标定管理', 2200, 5, 'vision-calibration', 'resource/visionCalibration/index', '', 'VisionCalibration', 1, 0, 'C', '0', '0', '', 'aim', 'admin', NOW(), '', NULL, '机器人手眼标定与工具TCP管理') ON DUPLICATE KEY UPDATE menu_name = VALUES(menu_name), parent_id = VALUES(parent_id), order_num = VALUES(order_num), path = VALUES(path), component = VALUES(component), route_name = VALUES(route_name), visible = VALUES(visible), status = VALUES(status), perms = VALUES(perms), icon = VALUES(icon), update_by = 'admin', update_time = NOW(), remark = VALUES(remark); @@ -59,8 +60,11 @@ JOIN ( UNION ALL SELECT 2204, 2241 UNION ALL SELECT 2204, 2242 UNION ALL SELECT 2204, 2243 ) f ON f.parent_id = rm.menu_id; -DELETE FROM sys_role_menu WHERE menu_id IN (2205, 2251, 2252, 2253); -DELETE FROM sys_menu WHERE menu_id IN (2251, 2252, 2253, 2205); +DELETE FROM sys_role_menu WHERE menu_id IN (2251, 2252, 2253); +DELETE FROM sys_menu WHERE menu_id IN (2251, 2252, 2253); + +INSERT IGNORE INTO sys_role_menu(role_id, menu_id) +SELECT role_id, 2205 FROM sys_role; -- Remove old device CRUD and inspection resource menus after permissions have been migrated. DELETE FROM sys_role_menu diff --git a/sql/vision_calibration.sql b/sql/vision_calibration.sql new file mode 100644 index 0000000..3d3c707 --- /dev/null +++ b/sql/vision_calibration.sql @@ -0,0 +1,73 @@ +-- Visual hand-eye and tool TCP calibration profiles. Apply once before using the management page. +CREATE TABLE IF NOT EXISTS `te_vision_calibration_profile` ( + `id` varchar(32) NOT NULL, + `profile_type` varchar(20) NOT NULL COMMENT 'HAND_EYE or TOOL_TCP', + `profile_key` varchar(100) NOT NULL COMMENT 'Stable logical key used by workflows', + `profile_name` varchar(100) NOT NULL, + `version_no` int NOT NULL DEFAULT 1, + `robot_id` varchar(128) NOT NULL, + `arm_device_id` varchar(128) NOT NULL, + `camera_device_id` varchar(128) DEFAULT NULL, + `tool_code` varchar(100) DEFAULT NULL, + `status` varchar(20) NOT NULL DEFAULT 'DRAFT' COMMENT 'DRAFT, ACTIVE, INVALID', + `forced_enabled` tinyint(1) NOT NULL DEFAULT 0, + `raw_matrix_json` text NOT NULL, + `correction_json` varchar(500) DEFAULT NULL COMMENT 'x/y/z mm and rx/ry/rz degree correction', + `effective_matrix_json` text NOT NULL, + `mean_error_mm` decimal(10,4) DEFAULT NULL, + `max_error_mm` decimal(10,4) DEFAULT NULL, + `sample_count` int NOT NULL DEFAULT 0, + `metadata_json` text DEFAULT NULL, + `verified_time` datetime DEFAULT NULL, + `create_by` varchar(64) DEFAULT NULL, + `create_time` datetime DEFAULT NULL, + `update_by` varchar(64) DEFAULT NULL, + `update_time` datetime DEFAULT NULL, + `remark` varchar(500) DEFAULT NULL, + PRIMARY KEY (`id`), + UNIQUE KEY `uk_vision_profile_version` (`profile_key`, `version_no`), + KEY `idx_vision_profile_devices` (`profile_type`, `robot_id`, `arm_device_id`, `camera_device_id`), + KEY `idx_vision_profile_tool` (`profile_type`, `robot_id`, `arm_device_id`, `tool_code`), + KEY `idx_vision_profile_active` (`profile_key`, `status`) +) ENGINE=InnoDB DEFAULT CHARSET=utf8mb4 COMMENT='Visual calibration profile versions'; + +CREATE TABLE IF NOT EXISTS `te_vision_calibration_record` ( + `id` varchar(32) NOT NULL, + `profile_id` varchar(32) DEFAULT NULL, + `session_type` varchar(30) NOT NULL COMMENT 'HAND_EYE, HAND_EYE_VERIFY, TOOL_TCP', + `robot_id` varchar(128) NOT NULL, + `arm_device_id` varchar(128) NOT NULL, + `camera_device_id` varchar(128) DEFAULT NULL, + `tool_code` varchar(100) DEFAULT NULL, + `status` varchar(20) NOT NULL COMMENT 'RUNNING, COMPLETED, FAILED, CANCELLED', + `progress_step` int NOT NULL DEFAULT 0, + `total_steps` int NOT NULL DEFAULT 0, + `samples_json` longtext DEFAULT NULL, + `result_json` longtext DEFAULT NULL, + `error_message` varchar(1000) DEFAULT NULL, + `started_time` datetime DEFAULT NULL, + `finished_time` datetime DEFAULT NULL, + `create_by` varchar(64) DEFAULT NULL, + `create_time` datetime DEFAULT NULL, + `update_by` varchar(64) DEFAULT NULL, + `update_time` datetime DEFAULT NULL, + `remark` varchar(500) DEFAULT NULL, + PRIMARY KEY (`id`), + KEY `idx_vision_record_profile` (`profile_id`), + KEY `idx_vision_record_devices` (`robot_id`, `arm_device_id`, `camera_device_id`), + KEY `idx_vision_record_status` (`status`, `started_time`) +) ENGINE=InnoDB DEFAULT CHARSET=utf8mb4 COMMENT='Visual calibration execution and verification records'; + +INSERT INTO sys_menu(menu_id, menu_name, parent_id, order_num, path, component, query, route_name, + is_frame, is_cache, menu_type, visible, status, perms, icon, + create_by, create_time, update_by, update_time, remark) +VALUES + (2205, '视觉标定管理', 2200, 5, 'vision-calibration', 'resource/visionCalibration/index', '', 'VisionCalibration', + 1, 0, 'C', '0', '0', '', 'aim', 'admin', NOW(), '', NULL, '机器人手眼标定与工具TCP管理') +ON DUPLICATE KEY UPDATE menu_name=VALUES(menu_name), parent_id=VALUES(parent_id), order_num=VALUES(order_num), + path=VALUES(path), component=VALUES(component), route_name=VALUES(route_name), visible=VALUES(visible), + status=VALUES(status), perms=VALUES(perms), update_by='admin', update_time=NOW(), remark=VALUES(remark); + +-- Calibration management is available to every application role by product requirement. +INSERT IGNORE INTO sys_role_menu(role_id, menu_id) +SELECT role_id, 2205 FROM sys_role;