feat(vision): 添加视觉定位和标定功能支持
- 新增 LLM_CHAT 和 VISION_LOCATE_TARGET 动作枚举类型 - 实现 RGBD 相机快照捕获功能,包括深度数据处理 - 集成 OpenCV 库用于视觉标定和标记检测 - 添加视觉定位操作服务和数学计算模块 - 在应用菜单中新增视觉标定管理界面 - 优化 TTS 服务调用和流式处理机制 - 添加闲聊功能支持和默认系统提示配置 - 更新应用配置文件以支持新的视觉功能模块
This commit is contained in:
parent
699bde3bc6
commit
6731c1b19b
1
.gitignore
vendored
1
.gitignore
vendored
@ -49,3 +49,4 @@ nbdist/
|
||||
# Local verification tests
|
||||
/cmvr-iot-test/src/test/java/com/cmvr/test/flow/runtime/operator/edge/EdgeAgvOperateServiceTest.java
|
||||
/cmvr-iot-test/src/test/java/com/cmvr/test/flow/runtime/operator/edge/EdgeArmOperateServiceTest.java
|
||||
/cmvr-iot-test/src/test/java/com/cmvr/test/vision/math/VisionCalibrationMathTest.java
|
||||
|
||||
@ -17,6 +17,7 @@ import com.cmvr.test.flow.runtime.engine.FlowTaskRuntimeService;
|
||||
import com.cmvr.test.flow.runtime.subflow.FlowSubFlowDefinitionService;
|
||||
import com.cmvr.test.flow.runtime.operator.edge.ti.TiTouchOperateService;
|
||||
import com.cmvr.test.model.vo.FlowActionRequestVO;
|
||||
import com.cmvr.test.constant.LLMChatConstants;
|
||||
import com.cmvr.test.model.vo.TeDetectItemDeployFlowVO;
|
||||
import com.cmvr.test.model.vo.TeFlowPublishVO;
|
||||
import com.cmvr.test.model.vo.TeFlowVersionActionVO;
|
||||
@ -41,6 +42,7 @@ import org.springframework.web.bind.annotation.RequestParam;
|
||||
import org.springframework.web.bind.annotation.RestController;
|
||||
import org.springframework.web.bind.annotation.PutMapping;
|
||||
import org.springframework.beans.factory.annotation.Qualifier;
|
||||
import org.springframework.beans.factory.annotation.Value;
|
||||
import org.springframework.scheduling.concurrent.ThreadPoolTaskExecutor;
|
||||
|
||||
import jakarta.annotation.PreDestroy;
|
||||
@ -104,6 +106,18 @@ public class TeFlowController extends BaseController {
|
||||
*/
|
||||
private volatile Future<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("流程发布")
|
||||
@PostMapping("/publish")
|
||||
public AjaxResult publish(@Valid @RequestBody TeFlowPublishVO request) {
|
||||
@ -367,6 +381,56 @@ public class TeFlowController extends BaseController {
|
||||
return AjaxResult.ok("ok");
|
||||
}
|
||||
|
||||
/**
|
||||
* 调用工作流闲聊组件。模型响应由组件流式接收,开启 TTS 后按句即时播放,
|
||||
* HTTP 接口在组件完成后返回完整文本。
|
||||
*/
|
||||
@GetMapping("/llm/chat/query")
|
||||
@ApiOperation("调用闲聊组件")
|
||||
public AjaxResult queryChat(String text,
|
||||
@RequestParam(defaultValue = "true") boolean invokeTts) {
|
||||
if (StringUtils.isBlank(text)) {
|
||||
return AjaxResult.error("text不能为空");
|
||||
}
|
||||
|
||||
stopCurrentChatFuture(true);
|
||||
JSONObject payload = new JSONObject();
|
||||
payload.put("text", text.trim());
|
||||
payload.put("promptMode", "CUSTOM");
|
||||
payload.put("prompt", LLMChatConstants.DEFAULT_SYSTEM_PROMPT);
|
||||
payload.put("invokeTts", invokeTts);
|
||||
|
||||
FlowActionRequestVO request = new FlowActionRequestVO();
|
||||
request.setAction("LLM_CHAT");
|
||||
request.setPayload(payload);
|
||||
|
||||
Future<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")
|
||||
@ApiOperation("测试接口")
|
||||
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 停止接口。
|
||||
*/
|
||||
@ -409,6 +494,8 @@ public class TeFlowController extends BaseController {
|
||||
@PreDestroy
|
||||
public void destroy() {
|
||||
stopCurrentAiAgentFuture(true);
|
||||
stopCurrentChatFuture(true);
|
||||
aiAgentExecutor.shutdownNow();
|
||||
chatExecutor.shutdownNow();
|
||||
}
|
||||
}
|
||||
|
||||
@ -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));
|
||||
}
|
||||
}
|
||||
@ -140,9 +140,9 @@ media-analysis:
|
||||
read-timeout-ms: 600000
|
||||
|
||||
# Isolate the resource-center development instance from the currently running robot gateway.
|
||||
cmvr:
|
||||
resource:
|
||||
robot-state:
|
||||
enabled: false
|
||||
quic:
|
||||
enabled: false
|
||||
#cmvr:
|
||||
# resource:
|
||||
# robot-state:
|
||||
# enabled: true
|
||||
# quic:
|
||||
# enabled: true
|
||||
|
||||
@ -60,7 +60,7 @@ spring:
|
||||
# 国际化资源文件路径
|
||||
basename: i18n/messages
|
||||
profiles:
|
||||
active: aima
|
||||
active: test
|
||||
# 文件上传
|
||||
servlet:
|
||||
multipart:
|
||||
|
||||
@ -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;
|
||||
}
|
||||
@ -3,6 +3,7 @@ package com.cmvr.edge.client.service;
|
||||
import cmvr.api.CameraCommand;
|
||||
import com.cmvr.edge.client.model.EdgeCommonVO;
|
||||
import com.cmvr.edge.client.model.camera.EdgeCameraPtzVO;
|
||||
import com.cmvr.edge.client.model.camera.EdgeCameraRgbdSnapshot;
|
||||
import io.grpc.stub.StreamObserver;
|
||||
|
||||
import java.io.File;
|
||||
@ -64,6 +65,8 @@ public interface EdgeCameraService {
|
||||
*/
|
||||
public String getRGBDImages(EdgeCommonVO edgeCommonVO);
|
||||
|
||||
EdgeCameraRgbdSnapshot captureRgbd(EdgeCommonVO edgeCommonVO);
|
||||
|
||||
String controlPtz(EdgeCameraPtzVO ptzVO);
|
||||
|
||||
StreamObserver<CameraCommand.GetRGBImageStreamCommand.Request> getRGBImageStream(EdgeCommonVO edgeCommonVO);
|
||||
|
||||
@ -13,6 +13,7 @@ import com.cmvr.edge.client.manage.GrpcServiceManager;
|
||||
import com.cmvr.edge.client.manage.GrpcStreamManager;
|
||||
import com.cmvr.edge.client.model.EdgeCommonVO;
|
||||
import com.cmvr.edge.client.model.camera.EdgeCameraPtzVO;
|
||||
import com.cmvr.edge.client.model.camera.EdgeCameraRgbdSnapshot;
|
||||
import com.cmvr.edge.client.service.EdgeCameraService;
|
||||
import com.cmvr.edge.client.service.EdgeStreamService;
|
||||
import com.cmvr.edge.client.utils.EdgeCommonUtil;
|
||||
@ -323,6 +324,55 @@ public class EdgeCameraServiceImpl implements EdgeCameraService, EdgeStreamServi
|
||||
// return JSON.toJSONString(result);
|
||||
}
|
||||
|
||||
@Override
|
||||
public EdgeCameraRgbdSnapshot captureRgbd(EdgeCommonVO edgeCommonVO) {
|
||||
if (!checkStatus(edgeCommonVO)) {
|
||||
start(edgeCommonVO);
|
||||
}
|
||||
CameraServiceGrpc.CameraServiceBlockingStub stub = grpcServiceManager.getGrpcClient(
|
||||
edgeCommonVO.getRobotId(), CameraServiceGrpc.CameraServiceBlockingStub.class);
|
||||
CameraCommand.GetRGBDImagesCommand.Request request = CameraCommand.GetRGBDImagesCommand.Request.newBuilder()
|
||||
.setHeader(EdgeCommonUtil.buildRequest(edgeCommonVO.getDeviceId()))
|
||||
.build();
|
||||
CameraCommand.GetRGBDImagesCommand.Feedback feedback = executeGrpcCall(() -> stub.getRGBDImages(request));
|
||||
requireSuccess(feedback.getHeader(), "获取RGBD图像");
|
||||
if (!feedback.hasColorFrame() || !feedback.hasDepthFrame()) {
|
||||
throw new GlobalException("RGBD响应缺少彩色图或深度图");
|
||||
}
|
||||
CameraCommand.FrameData color = feedback.getColorFrame();
|
||||
CameraCommand.FrameData depth = feedback.getDepthFrame();
|
||||
if (color.getWidth() <= 0 || color.getHeight() <= 0
|
||||
|| depth.getWidth() != color.getWidth() || depth.getHeight() != color.getHeight()) {
|
||||
throw new GlobalException("RGBD图像尺寸不一致");
|
||||
}
|
||||
String colorPath = saveColorImage(color);
|
||||
String colorUrl;
|
||||
try {
|
||||
colorUrl = sysFileService.uploadFile(convertFileToMultipartFile(colorPath), FileType.IMAGE.code());
|
||||
} catch (IOException exception) {
|
||||
throw new GlobalException("彩色图上传失败: " + exception.getMessage());
|
||||
} finally {
|
||||
deleteTempFile(colorPath);
|
||||
}
|
||||
if (StrUtil.isBlank(colorUrl)) {
|
||||
throw new GlobalException("彩色图上传失败: 未返回地址");
|
||||
}
|
||||
return EdgeCameraRgbdSnapshot.builder()
|
||||
.colorImageUrl(colorUrl)
|
||||
.depthData(depth.getData().toByteArray())
|
||||
.width(depth.getWidth())
|
||||
.height(depth.getHeight())
|
||||
.depthType(depth.getType())
|
||||
.depthUnitScale(depth.getType() == CameraCommand.FrameData.FrameType.F32C1 ? 1.0 : 0.001)
|
||||
.fx(feedback.getIntrinsics().getFx())
|
||||
.fy(feedback.getIntrinsics().getFy())
|
||||
.cx(feedback.getIntrinsics().getCx())
|
||||
.cy(feedback.getIntrinsics().getCy())
|
||||
.captureTimestamp(depth.getCaptureUtcNs())
|
||||
.sourceFrameNumber(depth.getSourceFrameNumber())
|
||||
.build();
|
||||
}
|
||||
|
||||
@Override
|
||||
public String controlPtz(EdgeCameraPtzVO ptzVO) {
|
||||
CameraCommand.ControlPtzCommand.Command command;
|
||||
|
||||
@ -1,18 +1,19 @@
|
||||
package com.cmvr.llm.service.impl;
|
||||
|
||||
import com.alibaba.fastjson2.JSONObject;
|
||||
import com.cmvr.common.utils.http.CallAPIUtil;
|
||||
import com.cmvr.llm.service.LLMAiAgentPlatformService;
|
||||
import cn.hutool.core.util.StrUtil;
|
||||
import cn.hutool.http.ContentType;
|
||||
import cn.hutool.http.HttpRequest;
|
||||
import cn.hutool.http.HttpResponse;
|
||||
import com.cmvr.llm.service.LLMAiTtsService;
|
||||
import com.cmvr.llm.util.LlmChatService;
|
||||
import com.cmvr.common.exception.GlobalException;
|
||||
import lombok.RequiredArgsConstructor;
|
||||
import lombok.extern.slf4j.Slf4j;
|
||||
import org.springframework.stereotype.Service;
|
||||
|
||||
import java.util.HashMap;
|
||||
import java.util.Map;
|
||||
|
||||
@Service
|
||||
@RequiredArgsConstructor
|
||||
@Slf4j
|
||||
public class LLMAiTtsServiceImpl implements LLMAiTtsService {
|
||||
@Override
|
||||
public JSONObject play(String url, String text, String voice, String speed, String volume) {
|
||||
@ -33,8 +34,29 @@ public class LLMAiTtsServiceImpl implements LLMAiTtsService {
|
||||
// throw new RuntimeException(e);
|
||||
// }
|
||||
// 通过post调用tts接口
|
||||
String result = CallAPIUtil.doPostJson(url, new HashMap<>(), new JSONObject().fluentPut("text", text).fluentPut("voice", voice).fluentPut("speed", speed).fluentPut("volume", volume));
|
||||
|
||||
if (StrUtil.isBlank(url)) {
|
||||
throw new GlobalException("TTS播放地址不能为空");
|
||||
}
|
||||
JSONObject body = new JSONObject()
|
||||
.fluentPut("text", text)
|
||||
.fluentPut("voice", voice)
|
||||
.fluentPut("speed", speed)
|
||||
.fluentPut("volume", volume);
|
||||
try (HttpResponse response = HttpRequest.post(url)
|
||||
.contentType(ContentType.JSON.toString())
|
||||
.body(body.toJSONString())
|
||||
.timeout(300000)
|
||||
.execute()) {
|
||||
String result = response.body();
|
||||
if (!response.isOk()) {
|
||||
throw new GlobalException("TTS播放调用失败(" + response.getStatus() + "): " + result);
|
||||
}
|
||||
log.debug("TTS播放调用成功,地址: {},文本长度: {}", url, text == null ? 0 : text.length());
|
||||
return new JSONObject().fluentPut("result", result);
|
||||
} catch (GlobalException exception) {
|
||||
throw exception;
|
||||
} catch (Exception exception) {
|
||||
throw new GlobalException("TTS播放服务不可用: " + exception.getMessage());
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -273,6 +273,8 @@ public class LlmChatService {
|
||||
// 流正常结束
|
||||
fullContent.append(localContent);
|
||||
finalMessageIdRef.set(finalMessageId);
|
||||
// Some agents close SSE without an explicit end event.
|
||||
streamFinished.set(true);
|
||||
|
||||
} catch (IOException e) {
|
||||
errorRef.set(e);
|
||||
@ -292,6 +294,7 @@ public class LlmChatService {
|
||||
if (!playTts) return;
|
||||
|
||||
sentenceExecutor.submit(() -> {
|
||||
try {
|
||||
while (true) {
|
||||
String buffer = receiveBuffer.toString();
|
||||
// 寻找最后一个句子结束符
|
||||
@ -332,6 +335,10 @@ public class LlmChatService {
|
||||
break;
|
||||
}
|
||||
}
|
||||
} catch (Throwable exception) {
|
||||
log.error("流式TTS处理失败", exception);
|
||||
errorRef.compareAndSet(null, exception);
|
||||
}
|
||||
});
|
||||
}
|
||||
});
|
||||
@ -362,7 +369,9 @@ public class LlmChatService {
|
||||
private void yourAsyncMethod(String sentence, JSONObject ttsConfig) {
|
||||
try {
|
||||
log.info("正在处理句子: " + sentence);
|
||||
llmAiTtsService.play(ttsConfig.getString("url"), sentence, ttsConfig.getString("voice"), ttsConfig.getString("speed"), ttsConfig.getString("volume"));
|
||||
JSONObject response = llmAiTtsService.play(ttsConfig.getString("url"), sentence,
|
||||
ttsConfig.getString("voice"), ttsConfig.getString("speed"), ttsConfig.getString("volume"));
|
||||
log.info("流式TTS提交成功,文本长度: {},响应: {}", sentence.length(), response);
|
||||
} catch (Exception e) {
|
||||
throw new RuntimeException(e);
|
||||
}
|
||||
|
||||
@ -52,6 +52,11 @@
|
||||
<version>${nashorn.version}</version>
|
||||
</dependency>
|
||||
|
||||
<dependency>
|
||||
<groupId>org.openpnp</groupId>
|
||||
<artifactId>opencv</artifactId>
|
||||
</dependency>
|
||||
|
||||
<dependency>
|
||||
<groupId>junit</groupId>
|
||||
<artifactId>junit</artifactId>
|
||||
|
||||
@ -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() {
|
||||
}
|
||||
}
|
||||
@ -92,6 +92,8 @@ public enum ActionEnum {
|
||||
AUDIO_EVENT_CLASSIFY("LLM", "AUDIO_EVENT_CLASSIFY", "声音类型检测"),
|
||||
VIDEO_ANALYZE("LLM", "VIDEO_ANALYZE", "视频智能分析"),
|
||||
IMAGE_ANALYZE("LLM", "IMAGE_ANALYZE", "通用图片分析"),
|
||||
LLM_CHAT("LLM", "LLM_CHAT", "闲聊"),
|
||||
VISION_LOCATE_TARGET("EDGE", "VISION_LOCATE_TARGET", "视觉定位并计算工具末端位姿"),
|
||||
|
||||
// 触控交互
|
||||
TI_PATH_SEARCH("EDGE", "TI_PATH_SEARCH", "路径搜索"),
|
||||
|
||||
@ -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));
|
||||
}
|
||||
}
|
||||
@ -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) {
|
||||
}
|
||||
}
|
||||
@ -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;
|
||||
}
|
||||
}
|
||||
@ -8,6 +8,8 @@ import com.cmvr.test.flow.runtime.message.TaskNodeExecuteMessage;
|
||||
import com.cmvr.test.flow.runtime.message.TaskNodeExecuteResult;
|
||||
import com.cmvr.test.flow.runtime.operator.FlowMediaParamResolver;
|
||||
import org.apache.commons.lang3.StringUtils;
|
||||
import org.slf4j.Logger;
|
||||
import org.slf4j.LoggerFactory;
|
||||
import org.springframework.beans.factory.annotation.Autowired;
|
||||
import org.springframework.stereotype.Service;
|
||||
|
||||
@ -18,6 +20,7 @@ import java.util.UUID;
|
||||
@Service
|
||||
public class LLMMediaAnalysisOperateService implements LLMOperateService {
|
||||
|
||||
private static final Logger log = LoggerFactory.getLogger(LLMMediaAnalysisOperateService.class);
|
||||
private static final Set<String> VIDEO_ANALYSIS_MODES = Set.of("AUTO", "FAST", "ACCURATE");
|
||||
private static final String ANY_VEHICLE_SOUND = "ANY_VEHICLE_SOUND";
|
||||
private final MediaAnalysisClient mediaAnalysisClient;
|
||||
@ -49,6 +52,7 @@ public class LLMMediaAnalysisOperateService implements LLMOperateService {
|
||||
String mediaParam = audio ? "audioUrl" : (image ? "imageUrl" : "videoUrl");
|
||||
List<String> mediaUrls = FlowMediaParamResolver.urls(input, mediaParam);
|
||||
String mediaUrl = FlowMediaParamResolver.lastUrl(input, mediaParam);
|
||||
List<String> requestMediaUrls = mediaUrls;
|
||||
String profileCode = StringUtils.trimToNull(input.getString("profileCode"));
|
||||
if (StringUtils.isBlank(mediaUrl)) {
|
||||
throw new GlobalException("{}不能为空", audio ? "音频地址" : (image ? "图片地址" : "视频地址"));
|
||||
@ -108,8 +112,24 @@ public class LLMMediaAnalysisOperateService implements LLMOperateService {
|
||||
if (!Set.of("AUTO", "VISION_MODEL", "TEXT_LOCATE", "REFERENCE_MATCH").contains(analysisMethod)) {
|
||||
throw new GlobalException("图片分析方式无效: {}", analysisMethod);
|
||||
}
|
||||
options.put("analysisMethod", analysisMethod);
|
||||
String referenceImageUrl = FlowMediaParamResolver.lastUrl(input, "referenceImageUrl");
|
||||
// Older image nodes put the scene and reference image in the same
|
||||
// imageUrl list. Restore that convention before calling the
|
||||
// analysis service. A single-image node cannot perform template
|
||||
// matching, so use the original description-based locator.
|
||||
if ("REFERENCE_MATCH".equals(analysisMethod) && StringUtils.isBlank(referenceImageUrl)) {
|
||||
if (mediaUrls.size() == 2) {
|
||||
referenceImageUrl = mediaUrls.get(1);
|
||||
// The service receives the reference separately; only the
|
||||
// first URL remains in mediaUrls as the scene image.
|
||||
requestMediaUrls = List.of(mediaUrls.get(0));
|
||||
} else if (mediaUrls.size() == 1) {
|
||||
log.warn("仅收到一张图片,REFERENCE_MATCH 自动降级为 TEXT_LOCATE: nodeId={}",
|
||||
message.getNodeId());
|
||||
analysisMethod = "TEXT_LOCATE";
|
||||
}
|
||||
}
|
||||
options.put("analysisMethod", analysisMethod);
|
||||
if (StringUtils.isNotBlank(referenceImageUrl)) {
|
||||
options.put("referenceImageUrl", referenceImageUrl);
|
||||
}
|
||||
@ -127,7 +147,7 @@ public class LLMMediaAnalysisOperateService implements LLMOperateService {
|
||||
request.put("profileCode", profileCode);
|
||||
request.put("mediaUrl", mediaUrl);
|
||||
if (image) {
|
||||
request.put("mediaUrls", mediaUrls);
|
||||
request.put("mediaUrls", requestMediaUrls);
|
||||
}
|
||||
request.put("options", options);
|
||||
request.put("context", context);
|
||||
@ -231,6 +251,7 @@ public class LLMMediaAnalysisOperateService implements LLMOperateService {
|
||||
String matchSelector = StringUtils.trimToNull(source.getString("matchSelector"));
|
||||
Integer matchIndex = source.getInteger("matchIndex");
|
||||
String matchSort = StringUtils.trimToNull(source.getString("matchSort"));
|
||||
Boolean fullResRefine = source.getBoolean("fullResRefine");
|
||||
validateRange("图片宽度", maxWidth, 640, 1600);
|
||||
validateRange("图片数量", maxImages, 1, 12);
|
||||
validateRange("最大输出长度", maxOutputTokens, 32, 4096);
|
||||
@ -255,6 +276,7 @@ public class LLMMediaAnalysisOperateService implements LLMOperateService {
|
||||
if (matchSelector != null) tuning.put("matchSelector", matchSelector.toUpperCase());
|
||||
if (matchIndex != null) tuning.put("matchIndex", matchIndex);
|
||||
if (matchSort != null) tuning.put("matchSort", matchSort.toUpperCase());
|
||||
if (fullResRefine != null) tuning.put("fullResRefine", fullResRefine);
|
||||
return tuning;
|
||||
}
|
||||
|
||||
|
||||
@ -28,6 +28,7 @@ import com.cmvr.test.flow.runtime.operator.edge.EdgeManualInspectionOperateServi
|
||||
import com.cmvr.test.flow.runtime.operator.edge.EdgeDeviceCommandOperateService;
|
||||
import com.cmvr.test.flow.runtime.operator.edge.EdgeArmOperateService;
|
||||
import com.cmvr.test.flow.runtime.operator.edge.EdgeOperateService;
|
||||
import com.cmvr.test.flow.runtime.operator.edge.VisionLocateOperateService;
|
||||
import com.cmvr.test.flow.runtime.operator.llm.InspectionMeterRecognizeOperateService;
|
||||
import com.cmvr.test.flow.runtime.operator.llm.LLMOperateService;
|
||||
import com.cmvr.test.model.vo.FlowActionRequestVO;
|
||||
@ -52,6 +53,7 @@ public class FlowActionExecutorService {
|
||||
private final EdgeArmOperateService edgeArmOperateService;
|
||||
private final EdgeManualInspectionOperateService edgeManualInspectionOperateService;
|
||||
private final EdgeDeviceCommandOperateService edgeDeviceCommandOperateService;
|
||||
private final VisionLocateOperateService visionLocateOperateService;
|
||||
private final InspectionMeterRecognizeOperateService inspectionMeterRecognizeOperateService;
|
||||
private final List<EdgeOperateService> edgeOperateServices;
|
||||
private final List<LLMOperateService> llmOperateServices;
|
||||
@ -150,6 +152,14 @@ public class FlowActionExecutorService {
|
||||
String robotId = payload.getString("robotId");
|
||||
String deviceId = payload.getString("deviceId");
|
||||
|
||||
// Keep the legacy single-node fallback complete for actions introduced
|
||||
// after the generic EDGE operator list was added.
|
||||
if (ActionEnum.VISION_LOCATE_TARGET.equals(action)) {
|
||||
TaskNodeExecuteMessage message = buildSingleNodeMessage(action, payload);
|
||||
message.setRobotId(robotId);
|
||||
return resultToString(visionLocateOperateService.execute(message));
|
||||
}
|
||||
|
||||
// 巡检报警监听组件只需要机器人 ID 和事件类型,不需要设备 ID。
|
||||
if (ActionEnum.INSPECTION_ALERT_LISTEN_START.equals(action)
|
||||
|| ActionEnum.INSPECTION_ALERT_LISTEN_STOP.equals(action)) {
|
||||
|
||||
@ -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;
|
||||
}
|
||||
@ -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;
|
||||
}
|
||||
@ -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> {
|
||||
}
|
||||
@ -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> {
|
||||
}
|
||||
@ -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) {
|
||||
}
|
||||
@ -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) { }
|
||||
}
|
||||
@ -0,0 +1,5 @@
|
||||
package com.cmvr.test.vision.math;
|
||||
|
||||
public record CalibrationResult(double[][] matrix, double meanErrorMm, double maxErrorMm,
|
||||
int sampleCount, double[] fixedPoint) {
|
||||
}
|
||||
@ -0,0 +1,4 @@
|
||||
package com.cmvr.test.vision.math;
|
||||
|
||||
public record CalibrationSample(double[][] baseToFlange, double[] cameraPoint) {
|
||||
}
|
||||
@ -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)); }
|
||||
}
|
||||
@ -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;
|
||||
}
|
||||
}
|
||||
@ -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;
|
||||
}
|
||||
@ -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);
|
||||
}
|
||||
}
|
||||
@ -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> {
|
||||
}
|
||||
@ -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 {
|
||||
}
|
||||
}
|
||||
7
pom.xml
7
pom.xml
@ -42,6 +42,7 @@
|
||||
<swagger.annotations.version>1.6.14</swagger.annotations.version>
|
||||
<netty.quic.version>4.2.16.Final</netty.quic.version>
|
||||
<nashorn.version>15.4</nashorn.version>
|
||||
<opencv.version>4.9.0-0</opencv.version>
|
||||
</properties>
|
||||
|
||||
<!-- 依赖声明 -->
|
||||
@ -217,6 +218,12 @@
|
||||
<version>${cmvr-iot.version}</version>
|
||||
</dependency>
|
||||
|
||||
<dependency>
|
||||
<groupId>org.openpnp</groupId>
|
||||
<artifactId>opencv</artifactId>
|
||||
<version>${opencv.version}</version>
|
||||
</dependency>
|
||||
|
||||
<dependency>
|
||||
<groupId>com.cmvr</groupId>
|
||||
<artifactId>cmvr-iot-device</artifactId>
|
||||
|
||||
@ -7,7 +7,8 @@ VALUES
|
||||
(2201, '机器人', 2200, 1, 'robot', 'resource/robot/index', '', 'ResourceRobot', 1, 0, 'C', '0', '0', 'resource:robot:list', 'monitor', 'admin', NOW(), '', NULL, '平台机器人身份与运行状态'),
|
||||
(2202, '设备', 2200, 2, 'device', 'resource/device/index', '', 'ResourceDevice', 1, 0, 'C', '0', '0', 'resource:device:list', 'list', 'admin', NOW(), '', NULL, '机器人设备发现与用途配置'),
|
||||
(2203, '地图', 2200, 3, 'map', 'resource/map/index', '', 'ResourceMap', 1, 0, 'C', '0', '0', 'resource:map:list', 'map', 'admin', NOW(), '', NULL, '仅有AGV能力的机器人需要绑定地图'),
|
||||
(2204, '边缘终端', 2200, 4, 'terminal', 'resource/terminal/index', '', 'ResourceTerminal', 1, 0, 'C', '0', '0', 'resource:terminal:list', 'server', 'admin', NOW(), '', NULL, 'QUIC边缘终端运行状态')
|
||||
(2204, '边缘终端', 2200, 4, 'terminal', 'resource/terminal/index', '', 'ResourceTerminal', 1, 0, 'C', '0', '0', 'resource:terminal:list', 'server', 'admin', NOW(), '', NULL, 'QUIC边缘终端运行状态'),
|
||||
(2205, '视觉标定管理', 2200, 5, 'vision-calibration', 'resource/visionCalibration/index', '', 'VisionCalibration', 1, 0, 'C', '0', '0', '', 'aim', 'admin', NOW(), '', NULL, '机器人手眼标定与工具TCP管理')
|
||||
ON DUPLICATE KEY UPDATE menu_name = VALUES(menu_name), parent_id = VALUES(parent_id), order_num = VALUES(order_num),
|
||||
path = VALUES(path), component = VALUES(component), route_name = VALUES(route_name), visible = VALUES(visible), status = VALUES(status),
|
||||
perms = VALUES(perms), icon = VALUES(icon), update_by = 'admin', update_time = NOW(), remark = VALUES(remark);
|
||||
@ -59,8 +60,11 @@ JOIN (
|
||||
UNION ALL SELECT 2204, 2241 UNION ALL SELECT 2204, 2242 UNION ALL SELECT 2204, 2243
|
||||
) f ON f.parent_id = rm.menu_id;
|
||||
|
||||
DELETE FROM sys_role_menu WHERE menu_id IN (2205, 2251, 2252, 2253);
|
||||
DELETE FROM sys_menu WHERE menu_id IN (2251, 2252, 2253, 2205);
|
||||
DELETE FROM sys_role_menu WHERE menu_id IN (2251, 2252, 2253);
|
||||
DELETE FROM sys_menu WHERE menu_id IN (2251, 2252, 2253);
|
||||
|
||||
INSERT IGNORE INTO sys_role_menu(role_id, menu_id)
|
||||
SELECT role_id, 2205 FROM sys_role;
|
||||
|
||||
-- Remove old device CRUD and inspection resource menus after permissions have been migrated.
|
||||
DELETE FROM sys_role_menu
|
||||
|
||||
73
sql/vision_calibration.sql
Normal file
73
sql/vision_calibration.sql
Normal 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;
|
||||
Loading…
Reference in New Issue
Block a user