feat(vision): 添加视觉定位和标定功能支持

- 新增 LLM_CHAT 和 VISION_LOCATE_TARGET 动作枚举类型
- 实现 RGBD 相机快照捕获功能,包括深度数据处理
- 集成 OpenCV 库用于视觉标定和标记检测
- 添加视觉定位操作服务和数学计算模块
- 在应用菜单中新增视觉标定管理界面
- 优化 TTS 服务调用和流式处理机制
- 添加闲聊功能支持和默认系统提示配置
- 更新应用配置文件以支持新的视觉功能模块
This commit is contained in:
lixiaolong 2026-09-08 14:26:11 +08:00
parent 699bde3bc6
commit 6731c1b19b
35 changed files with 2929 additions and 23 deletions

1
.gitignore vendored
View File

@ -49,3 +49,4 @@ nbdist/
# Local verification tests # 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/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/flow/runtime/operator/edge/EdgeArmOperateServiceTest.java
/cmvr-iot-test/src/test/java/com/cmvr/test/vision/math/VisionCalibrationMathTest.java

View File

@ -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.subflow.FlowSubFlowDefinitionService;
import com.cmvr.test.flow.runtime.operator.edge.ti.TiTouchOperateService; import com.cmvr.test.flow.runtime.operator.edge.ti.TiTouchOperateService;
import com.cmvr.test.model.vo.FlowActionRequestVO; 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.TeDetectItemDeployFlowVO;
import com.cmvr.test.model.vo.TeFlowPublishVO; import com.cmvr.test.model.vo.TeFlowPublishVO;
import com.cmvr.test.model.vo.TeFlowVersionActionVO; 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.RestController;
import org.springframework.web.bind.annotation.PutMapping; import org.springframework.web.bind.annotation.PutMapping;
import org.springframework.beans.factory.annotation.Qualifier; import org.springframework.beans.factory.annotation.Qualifier;
import org.springframework.beans.factory.annotation.Value;
import org.springframework.scheduling.concurrent.ThreadPoolTaskExecutor; import org.springframework.scheduling.concurrent.ThreadPoolTaskExecutor;
import jakarta.annotation.PreDestroy; import jakarta.annotation.PreDestroy;
@ -104,6 +106,18 @@ public class TeFlowController extends BaseController {
*/ */
private volatile Future<JSONObject> currentAiAgentFuture; private volatile Future<JSONObject> 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<String> currentChatFuture;
@Value("${CMVR_CHAT_TTS_STOP_URL:http://192.168.0.148:8080/tts/stop}")
private String chatTtsStopUrl;
@ApiOperation("流程发布") @ApiOperation("流程发布")
@PostMapping("/publish") @PostMapping("/publish")
public AjaxResult publish(@Valid @RequestBody TeFlowPublishVO request) { public AjaxResult publish(@Valid @RequestBody TeFlowPublishVO request) {
@ -367,6 +381,56 @@ public class TeFlowController extends BaseController {
return AjaxResult.ok("ok"); 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<String> 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") @GetMapping("/testrun")
@ApiOperation("测试接口") @ApiOperation("测试接口")
public AjaxResult testrun(@RequestParam("imageUrl") String imageUrl, public AjaxResult testrun(@RequestParam("imageUrl") String imageUrl,
@ -390,6 +454,27 @@ public class TeFlowController extends BaseController {
} }
} }
private void stopCurrentChatFuture(boolean stopTts) {
Future<String> 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 停止接口。 * 调用本地 TTS 停止接口。
*/ */
@ -409,6 +494,8 @@ public class TeFlowController extends BaseController {
@PreDestroy @PreDestroy
public void destroy() { public void destroy() {
stopCurrentAiAgentFuture(true); stopCurrentAiAgentFuture(true);
stopCurrentChatFuture(true);
aiAgentExecutor.shutdownNow(); aiAgentExecutor.shutdownNow();
chatExecutor.shutdownNow();
} }
} }

View File

@ -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));
}
}

View File

@ -140,9 +140,9 @@ media-analysis:
read-timeout-ms: 600000 read-timeout-ms: 600000
# Isolate the resource-center development instance from the currently running robot gateway. # Isolate the resource-center development instance from the currently running robot gateway.
cmvr: #cmvr:
resource: # resource:
robot-state: # robot-state:
enabled: false # enabled: true
quic: # quic:
enabled: false # enabled: true

View File

@ -60,7 +60,7 @@ spring:
# 国际化资源文件路径 # 国际化资源文件路径
basename: i18n/messages basename: i18n/messages
profiles: profiles:
active: aima active: test
# 文件上传 # 文件上传
servlet: servlet:
multipart: multipart:

View File

@ -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;
}

View File

@ -3,6 +3,7 @@ package com.cmvr.edge.client.service;
import cmvr.api.CameraCommand; import cmvr.api.CameraCommand;
import com.cmvr.edge.client.model.EdgeCommonVO; import com.cmvr.edge.client.model.EdgeCommonVO;
import com.cmvr.edge.client.model.camera.EdgeCameraPtzVO; import com.cmvr.edge.client.model.camera.EdgeCameraPtzVO;
import com.cmvr.edge.client.model.camera.EdgeCameraRgbdSnapshot;
import io.grpc.stub.StreamObserver; import io.grpc.stub.StreamObserver;
import java.io.File; import java.io.File;
@ -64,6 +65,8 @@ public interface EdgeCameraService {
*/ */
public String getRGBDImages(EdgeCommonVO edgeCommonVO); public String getRGBDImages(EdgeCommonVO edgeCommonVO);
EdgeCameraRgbdSnapshot captureRgbd(EdgeCommonVO edgeCommonVO);
String controlPtz(EdgeCameraPtzVO ptzVO); String controlPtz(EdgeCameraPtzVO ptzVO);
StreamObserver<CameraCommand.GetRGBImageStreamCommand.Request> getRGBImageStream(EdgeCommonVO edgeCommonVO); StreamObserver<CameraCommand.GetRGBImageStreamCommand.Request> getRGBImageStream(EdgeCommonVO edgeCommonVO);

View File

@ -13,6 +13,7 @@ import com.cmvr.edge.client.manage.GrpcServiceManager;
import com.cmvr.edge.client.manage.GrpcStreamManager; import com.cmvr.edge.client.manage.GrpcStreamManager;
import com.cmvr.edge.client.model.EdgeCommonVO; import com.cmvr.edge.client.model.EdgeCommonVO;
import com.cmvr.edge.client.model.camera.EdgeCameraPtzVO; 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.EdgeCameraService;
import com.cmvr.edge.client.service.EdgeStreamService; import com.cmvr.edge.client.service.EdgeStreamService;
import com.cmvr.edge.client.utils.EdgeCommonUtil; import com.cmvr.edge.client.utils.EdgeCommonUtil;
@ -323,6 +324,55 @@ public class EdgeCameraServiceImpl implements EdgeCameraService, EdgeStreamServi
// return JSON.toJSONString(result); // 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 @Override
public String controlPtz(EdgeCameraPtzVO ptzVO) { public String controlPtz(EdgeCameraPtzVO ptzVO) {
CameraCommand.ControlPtzCommand.Command command; CameraCommand.ControlPtzCommand.Command command;

View File

@ -1,18 +1,19 @@
package com.cmvr.llm.service.impl; package com.cmvr.llm.service.impl;
import com.alibaba.fastjson2.JSONObject; import com.alibaba.fastjson2.JSONObject;
import com.cmvr.common.utils.http.CallAPIUtil; import cn.hutool.core.util.StrUtil;
import com.cmvr.llm.service.LLMAiAgentPlatformService; 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.service.LLMAiTtsService;
import com.cmvr.llm.util.LlmChatService; import com.cmvr.common.exception.GlobalException;
import lombok.RequiredArgsConstructor; import lombok.RequiredArgsConstructor;
import lombok.extern.slf4j.Slf4j;
import org.springframework.stereotype.Service; import org.springframework.stereotype.Service;
import java.util.HashMap;
import java.util.Map;
@Service @Service
@RequiredArgsConstructor @RequiredArgsConstructor
@Slf4j
public class LLMAiTtsServiceImpl implements LLMAiTtsService { public class LLMAiTtsServiceImpl implements LLMAiTtsService {
@Override @Override
public JSONObject play(String url, String text, String voice, String speed, String volume) { 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); // throw new RuntimeException(e);
// } // }
// 通过post调用tts接口 // 通过post调用tts接口
String result = CallAPIUtil.doPostJson(url, new HashMap<>(), new JSONObject().fluentPut("text", text).fluentPut("voice", voice).fluentPut("speed", speed).fluentPut("volume", volume)); if (StrUtil.isBlank(url)) {
throw new GlobalException("TTS播放地址不能为空");
return new JSONObject().fluentPut("result", result); }
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());
}
} }
} }

View File

@ -273,6 +273,8 @@ public class LlmChatService {
// 流正常结束 // 流正常结束
fullContent.append(localContent); fullContent.append(localContent);
finalMessageIdRef.set(finalMessageId); finalMessageIdRef.set(finalMessageId);
// Some agents close SSE without an explicit end event.
streamFinished.set(true);
} catch (IOException e) { } catch (IOException e) {
errorRef.set(e); errorRef.set(e);
@ -292,6 +294,7 @@ public class LlmChatService {
if (!playTts) return; if (!playTts) return;
sentenceExecutor.submit(() -> { sentenceExecutor.submit(() -> {
try {
while (true) { while (true) {
String buffer = receiveBuffer.toString(); String buffer = receiveBuffer.toString();
// 寻找最后一个句子结束符 // 寻找最后一个句子结束符
@ -332,6 +335,10 @@ public class LlmChatService {
break; 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) { private void yourAsyncMethod(String sentence, JSONObject ttsConfig) {
try { try {
log.info("正在处理句子: " + sentence); 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) { } catch (Exception e) {
throw new RuntimeException(e); throw new RuntimeException(e);
} }

View File

@ -52,6 +52,11 @@
<version>${nashorn.version}</version> <version>${nashorn.version}</version>
</dependency> </dependency>
<dependency>
<groupId>org.openpnp</groupId>
<artifactId>opencv</artifactId>
</dependency>
<dependency> <dependency>
<groupId>junit</groupId> <groupId>junit</groupId>
<artifactId>junit</artifactId> <artifactId>junit</artifactId>

View File

@ -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() {
}
}

View File

@ -92,6 +92,8 @@ public enum ActionEnum {
AUDIO_EVENT_CLASSIFY("LLM", "AUDIO_EVENT_CLASSIFY", "声音类型检测"), AUDIO_EVENT_CLASSIFY("LLM", "AUDIO_EVENT_CLASSIFY", "声音类型检测"),
VIDEO_ANALYZE("LLM", "VIDEO_ANALYZE", "视频智能分析"), VIDEO_ANALYZE("LLM", "VIDEO_ANALYZE", "视频智能分析"),
IMAGE_ANALYZE("LLM", "IMAGE_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", "路径搜索"), TI_PATH_SEARCH("EDGE", "TI_PATH_SEARCH", "路径搜索"),

View File

@ -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));
}
}

View File

@ -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<Double> 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<double[]> 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<Double> 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) {
}
}

View File

@ -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;
}
}

View File

@ -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.message.TaskNodeExecuteResult;
import com.cmvr.test.flow.runtime.operator.FlowMediaParamResolver; import com.cmvr.test.flow.runtime.operator.FlowMediaParamResolver;
import org.apache.commons.lang3.StringUtils; import org.apache.commons.lang3.StringUtils;
import org.slf4j.Logger;
import org.slf4j.LoggerFactory;
import org.springframework.beans.factory.annotation.Autowired; import org.springframework.beans.factory.annotation.Autowired;
import org.springframework.stereotype.Service; import org.springframework.stereotype.Service;
@ -18,6 +20,7 @@ import java.util.UUID;
@Service @Service
public class LLMMediaAnalysisOperateService implements LLMOperateService { public class LLMMediaAnalysisOperateService implements LLMOperateService {
private static final Logger log = LoggerFactory.getLogger(LLMMediaAnalysisOperateService.class);
private static final Set<String> VIDEO_ANALYSIS_MODES = Set.of("AUTO", "FAST", "ACCURATE"); private static final Set<String> VIDEO_ANALYSIS_MODES = Set.of("AUTO", "FAST", "ACCURATE");
private static final String ANY_VEHICLE_SOUND = "ANY_VEHICLE_SOUND"; private static final String ANY_VEHICLE_SOUND = "ANY_VEHICLE_SOUND";
private final MediaAnalysisClient mediaAnalysisClient; private final MediaAnalysisClient mediaAnalysisClient;
@ -49,6 +52,7 @@ public class LLMMediaAnalysisOperateService implements LLMOperateService {
String mediaParam = audio ? "audioUrl" : (image ? "imageUrl" : "videoUrl"); String mediaParam = audio ? "audioUrl" : (image ? "imageUrl" : "videoUrl");
List<String> mediaUrls = FlowMediaParamResolver.urls(input, mediaParam); List<String> mediaUrls = FlowMediaParamResolver.urls(input, mediaParam);
String mediaUrl = FlowMediaParamResolver.lastUrl(input, mediaParam); String mediaUrl = FlowMediaParamResolver.lastUrl(input, mediaParam);
List<String> requestMediaUrls = mediaUrls;
String profileCode = StringUtils.trimToNull(input.getString("profileCode")); String profileCode = StringUtils.trimToNull(input.getString("profileCode"));
if (StringUtils.isBlank(mediaUrl)) { if (StringUtils.isBlank(mediaUrl)) {
throw new GlobalException("{}不能为空", audio ? "音频地址" : (image ? "图片地址" : "视频地址")); 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)) { if (!Set.of("AUTO", "VISION_MODEL", "TEXT_LOCATE", "REFERENCE_MATCH").contains(analysisMethod)) {
throw new GlobalException("图片分析方式无效: {}", analysisMethod); throw new GlobalException("图片分析方式无效: {}", analysisMethod);
} }
options.put("analysisMethod", analysisMethod);
String referenceImageUrl = FlowMediaParamResolver.lastUrl(input, "referenceImageUrl"); 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)) { if (StringUtils.isNotBlank(referenceImageUrl)) {
options.put("referenceImageUrl", referenceImageUrl); options.put("referenceImageUrl", referenceImageUrl);
} }
@ -127,7 +147,7 @@ public class LLMMediaAnalysisOperateService implements LLMOperateService {
request.put("profileCode", profileCode); request.put("profileCode", profileCode);
request.put("mediaUrl", mediaUrl); request.put("mediaUrl", mediaUrl);
if (image) { if (image) {
request.put("mediaUrls", mediaUrls); request.put("mediaUrls", requestMediaUrls);
} }
request.put("options", options); request.put("options", options);
request.put("context", context); request.put("context", context);
@ -231,6 +251,7 @@ public class LLMMediaAnalysisOperateService implements LLMOperateService {
String matchSelector = StringUtils.trimToNull(source.getString("matchSelector")); String matchSelector = StringUtils.trimToNull(source.getString("matchSelector"));
Integer matchIndex = source.getInteger("matchIndex"); Integer matchIndex = source.getInteger("matchIndex");
String matchSort = StringUtils.trimToNull(source.getString("matchSort")); String matchSort = StringUtils.trimToNull(source.getString("matchSort"));
Boolean fullResRefine = source.getBoolean("fullResRefine");
validateRange("图片宽度", maxWidth, 640, 1600); validateRange("图片宽度", maxWidth, 640, 1600);
validateRange("图片数量", maxImages, 1, 12); validateRange("图片数量", maxImages, 1, 12);
validateRange("最大输出长度", maxOutputTokens, 32, 4096); validateRange("最大输出长度", maxOutputTokens, 32, 4096);
@ -255,6 +276,7 @@ public class LLMMediaAnalysisOperateService implements LLMOperateService {
if (matchSelector != null) tuning.put("matchSelector", matchSelector.toUpperCase()); if (matchSelector != null) tuning.put("matchSelector", matchSelector.toUpperCase());
if (matchIndex != null) tuning.put("matchIndex", matchIndex); if (matchIndex != null) tuning.put("matchIndex", matchIndex);
if (matchSort != null) tuning.put("matchSort", matchSort.toUpperCase()); if (matchSort != null) tuning.put("matchSort", matchSort.toUpperCase());
if (fullResRefine != null) tuning.put("fullResRefine", fullResRefine);
return tuning; return tuning;
} }

View File

@ -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.EdgeDeviceCommandOperateService;
import com.cmvr.test.flow.runtime.operator.edge.EdgeArmOperateService; 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.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.InspectionMeterRecognizeOperateService;
import com.cmvr.test.flow.runtime.operator.llm.LLMOperateService; import com.cmvr.test.flow.runtime.operator.llm.LLMOperateService;
import com.cmvr.test.model.vo.FlowActionRequestVO; import com.cmvr.test.model.vo.FlowActionRequestVO;
@ -52,6 +53,7 @@ public class FlowActionExecutorService {
private final EdgeArmOperateService edgeArmOperateService; private final EdgeArmOperateService edgeArmOperateService;
private final EdgeManualInspectionOperateService edgeManualInspectionOperateService; private final EdgeManualInspectionOperateService edgeManualInspectionOperateService;
private final EdgeDeviceCommandOperateService edgeDeviceCommandOperateService; private final EdgeDeviceCommandOperateService edgeDeviceCommandOperateService;
private final VisionLocateOperateService visionLocateOperateService;
private final InspectionMeterRecognizeOperateService inspectionMeterRecognizeOperateService; private final InspectionMeterRecognizeOperateService inspectionMeterRecognizeOperateService;
private final List<EdgeOperateService> edgeOperateServices; private final List<EdgeOperateService> edgeOperateServices;
private final List<LLMOperateService> llmOperateServices; private final List<LLMOperateService> llmOperateServices;
@ -150,6 +152,14 @@ public class FlowActionExecutorService {
String robotId = payload.getString("robotId"); String robotId = payload.getString("robotId");
String deviceId = payload.getString("deviceId"); 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。 // 巡检报警监听组件只需要机器人 ID 和事件类型,不需要设备 ID。
if (ActionEnum.INSPECTION_ALERT_LISTEN_START.equals(action) if (ActionEnum.INSPECTION_ALERT_LISTEN_START.equals(action)
|| ActionEnum.INSPECTION_ALERT_LISTEN_STOP.equals(action)) { || ActionEnum.INSPECTION_ALERT_LISTEN_STOP.equals(action)) {

View File

@ -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;
}

View File

@ -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;
}

View File

@ -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<VisionCalibrationProfile> {
}

View File

@ -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<VisionCalibrationRecord> {
}

View File

@ -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<double[]> pixelCorners,
int width, int height, double planeRmseMm) {
}

View File

@ -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<MatOfPoint> 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<double[]> 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<Double> 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) { }
}

View File

@ -0,0 +1,5 @@
package com.cmvr.test.vision.math;
public record CalibrationResult(double[][] matrix, double meanErrorMm, double maxErrorMm,
int sampleCount, double[] fixedPoint) {
}

View File

@ -0,0 +1,4 @@
package com.cmvr.test.vision.math;
public record CalibrationSample(double[][] baseToFlange, double[] cameraPoint) {
}

View File

@ -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<CalibrationSample> 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<double[][]> 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<Double> 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<CalibrationSample> samples, double[] fixed) {
List<Double> 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<CalibrationSample> 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<CalibrationSample> 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<CalibrationSample> 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<Double> 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)); }
}

View File

@ -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;
}
}

View File

@ -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<double[]> markerCorners;
private Integer imageWidth;
private Integer imageHeight;
}

View File

@ -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<VisionCalibrationProfileMapper, VisionCalibrationProfile> {
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<VisionCalibrationProfile> list(String profileType, String robotId, String keyword) {
LambdaQueryWrapper<VisionCalibrationProfile> query = new LambdaQueryWrapper<VisionCalibrationProfile>()
.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<VisionCalibrationProfile>()
.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<VisionCalibrationProfile>()
.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<VisionCalibrationProfile> update = new LambdaUpdateWrapper<VisionCalibrationProfile>()
.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);
}
}

View File

@ -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<VisionCalibrationRecordMapper, VisionCalibrationRecord> {
}

View File

@ -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<String, Session> 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<double[]> 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<Double> 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<double[][]> 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<double[][]> targets = calibrationTargets(initial, session.handEyeRequest).subList(0, 4);
double[][] cameraTransform = VisionCalibrationMath.parseMatrix(session.profileToVerify.getEffectiveMatrixJson());
List<double[]> 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<Double> 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<CalibrationSample> samples) {
List<CalibrationSample> training = samples.subList(0, samples.size() - 3);
List<CalibrationSample> validation = samples.subList(samples.size() - 3, samples.size());
CalibrationResult solved = VisionCalibrationMath.solveHandEye(training, null);
List<Double> 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<double[][]> 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<double[][]> 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<double[]> markerCorners = List.of();
private Integer imageWidth, imageHeight;
private final AtomicBoolean running = new AtomicBoolean(true);
private final List<CalibrationSample> handEyeSamples = new ArrayList<>();
private final List<double[][]> 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 {
}
}

View File

@ -42,6 +42,7 @@
<swagger.annotations.version>1.6.14</swagger.annotations.version> <swagger.annotations.version>1.6.14</swagger.annotations.version>
<netty.quic.version>4.2.16.Final</netty.quic.version> <netty.quic.version>4.2.16.Final</netty.quic.version>
<nashorn.version>15.4</nashorn.version> <nashorn.version>15.4</nashorn.version>
<opencv.version>4.9.0-0</opencv.version>
</properties> </properties>
<!-- 依赖声明 --> <!-- 依赖声明 -->
@ -217,6 +218,12 @@
<version>${cmvr-iot.version}</version> <version>${cmvr-iot.version}</version>
</dependency> </dependency>
<dependency>
<groupId>org.openpnp</groupId>
<artifactId>opencv</artifactId>
<version>${opencv.version}</version>
</dependency>
<dependency> <dependency>
<groupId>com.cmvr</groupId> <groupId>com.cmvr</groupId>
<artifactId>cmvr-iot-device</artifactId> <artifactId>cmvr-iot-device</artifactId>

View File

@ -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, '平台机器人身份与运行状态'), (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, '机器人设备发现与用途配置'), (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能力的机器人需要绑定地图'), (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), 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), 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); 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 UNION ALL SELECT 2204, 2241 UNION ALL SELECT 2204, 2242 UNION ALL SELECT 2204, 2243
) f ON f.parent_id = rm.menu_id; ) f ON f.parent_id = rm.menu_id;
DELETE FROM sys_role_menu WHERE menu_id IN (2205, 2251, 2252, 2253); DELETE FROM sys_role_menu WHERE menu_id IN (2251, 2252, 2253);
DELETE FROM sys_menu WHERE menu_id IN (2251, 2252, 2253, 2205); 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. -- Remove old device CRUD and inspection resource menus after permissions have been migrated.
DELETE FROM sys_role_menu DELETE FROM sys_role_menu

View File

@ -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;