feat(agv): 添加自由导航功能支持
- 新增 robotGoTargetWithFreeGo 方法支持坐标导航 - 实现自由导航到指定坐标位置的功能 - 添加机器人实时位置查询接口 - 提供基于坐标的导航控制能力 - 支持双轮差速底盘的自由导航操作
This commit is contained in:
parent
6d39c4296e
commit
db17d50ef6
@ -172,5 +172,38 @@ public class InspectionRobotController extends BaseController
|
||||
return success(inspectionRobotService.syncRobotStatus(robotId));
|
||||
}
|
||||
|
||||
/**
|
||||
* 获取机器人实时位置
|
||||
*/
|
||||
@ApiOperation("获取机器人实时位置")
|
||||
@PreAuthorize("@ss.hasPermi('inspection:robot:query')")
|
||||
@GetMapping("/{robotId}/location")
|
||||
public AjaxResult getRobotRealtimeLocation(@PathVariable String robotId)
|
||||
{
|
||||
AgvCommand.AgvRobotLocation location = inspectionRobotService.getRobotRealtimeLocation(robotId);
|
||||
return success(location);
|
||||
}
|
||||
|
||||
/**
|
||||
* 导航到指定位置（坐标）
|
||||
*/
|
||||
@ApiOperation("导航到指定位置")
|
||||
@PreAuthorize("@ss.hasPermi('inspection:robot:edit')")
|
||||
@Log(title = "导航到指定位置", businessType = BusinessType.OTHER)
|
||||
@PostMapping("/{robotId}/navigate-to-position")
|
||||
public AjaxResult navigateToPosition(
|
||||
@PathVariable @ApiParam("机器人ID") String robotId,
|
||||
@RequestParam @ApiParam("X坐标（米）") Double x,
|
||||
@RequestParam @ApiParam("Y坐标（米）") Double y,
|
||||
@RequestParam(required = false) @ApiParam("朝向角（弧度）") Double theta,
|
||||
@RequestParam(required = false) @ApiParam("任务ID") String taskId
|
||||
)
|
||||
{
|
||||
AgvCommand.RobotGoTargetResData result = inspectionRobotService.navigateToPosition(
|
||||
robotId, x, y, theta, taskId
|
||||
);
|
||||
return success(result);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
@ -79,6 +79,20 @@ public interface EdgeAgvService {
|
||||
AgvCommand.RobotGoTargetResData robotGoTarget(EdgeCommonVO edgeCommonVO, String sourceId, String targetId,
|
||||
String taskId, String operation, Double jackHeight);
|
||||
|
||||
/**
|
||||
* 自由导航到指定坐标位置（命令码3051，使用freego参数）
|
||||
* 直接导航到指定的X、Y坐标和角度，不依赖站点
|
||||
* 注意：此命令会取消当前正在执行的任务，仅支持双轮差速底盘
|
||||
*
|
||||
* @param edgeCommonVO 边缘通用参数
|
||||
* @param freeGoPoint 自由导航目标点（包含x、y、theta）
|
||||
* @param taskId 任务ID（可选，建议提供）
|
||||
* @return 导航任务下发结果
|
||||
*/
|
||||
AgvCommand.RobotGoTargetResData robotGoTargetWithFreeGo(EdgeCommonVO edgeCommonVO,
|
||||
AgvCommand.FreeGoPoint freeGoPoint,
|
||||
String taskId);
|
||||
|
||||
/**
|
||||
* 暂停当前导航任务（命令码3001）
|
||||
* 暂停AGV当前正在执行的导航任务，AGV将减速停止
|
||||
|
||||
@ -156,6 +156,46 @@ public class EdgeAgvServiceImpl implements EdgeAgvService {
|
||||
return result;
|
||||
}
|
||||
|
||||
@Override
|
||||
public AgvCommand.RobotGoTargetResData robotGoTargetWithFreeGo(EdgeCommonVO edgeCommonVO,
|
||||
AgvCommand.FreeGoPoint freeGoPoint,
|
||||
String taskId) {
|
||||
if (freeGoPoint == null) {
|
||||
throw new GlobalException("自由导航目标点不能为空");
|
||||
}
|
||||
|
||||
AgvServiceGrpc.AgvServiceBlockingStub stub = grpcServiceManager.getGrpcClient(
|
||||
edgeCommonVO.getTerminalId(), AgvServiceGrpc.AgvServiceBlockingStub.class);
|
||||
|
||||
// 构建请求数据，使用freego参数
|
||||
AgvCommand.RobotGoTargetReqData.Builder dataBuilder = AgvCommand.RobotGoTargetReqData.newBuilder()
|
||||
.setSourceId("SELF_POSITION") // 从当前位置开始
|
||||
.setId("SELF_POSITION") // 不使用站点ID
|
||||
.setFreego(freeGoPoint); // 设置自由导航点
|
||||
|
||||
// 设置可选参数
|
||||
if (StrUtil.isNotBlank(taskId)) {
|
||||
dataBuilder.setTaskId(taskId);
|
||||
}
|
||||
|
||||
|
||||
|
||||
AgvCommand.RobotGoTargetCommand.Request request = AgvCommand.RobotGoTargetCommand.Request.newBuilder()
|
||||
.setHeader(EdgeCommonUtil.buildRequest(edgeCommonVO.getDeviceId()))
|
||||
.setData(dataBuilder.build())
|
||||
.build();
|
||||
|
||||
AgvCommand.RobotGoTargetResData result = executeGrpcCall(() -> stub.robotGoTarget(request)).getData();
|
||||
|
||||
if (result.getRetCode() != 0) {
|
||||
throw new GlobalException("自由导航任务下发失败: " + result.getErrMsg());
|
||||
}
|
||||
|
||||
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
@Override
|
||||
public AgvCommand.RobotTaskPauseCommand.Feedback.Result robotTaskPause(EdgeCommonVO edgeCommonVO) {
|
||||
AgvServiceGrpc.AgvServiceBlockingStub stub = grpcServiceManager.getGrpcClient(
|
||||
|
||||
@ -98,4 +98,25 @@ public interface IInspectionRobotService extends IService<InspectionRobot>
|
||||
*/
|
||||
InspectionRobot syncRobotStatus(String robotId);
|
||||
|
||||
/**
|
||||
* 获取机器人实时位置
|
||||
*
|
||||
* @param robotId 机器人ID
|
||||
* @return 机器人位置信息（包含X、Y坐标、角度、当前站点等）
|
||||
*/
|
||||
AgvCommand.AgvRobotLocation getRobotRealtimeLocation(String robotId);
|
||||
|
||||
/**
|
||||
* 导航到指定位置（坐标）
|
||||
*
|
||||
* @param robotId 机器人ID
|
||||
* @param x X坐标（米）
|
||||
* @param y Y坐标（米）
|
||||
* @param theta 朝向角（弧度）
|
||||
* @param taskId 任务ID（可选）
|
||||
* @return 导航任务下发结果
|
||||
*/
|
||||
AgvCommand.RobotGoTargetResData navigateToPosition(String robotId, Double x, Double y,
|
||||
Double theta, String taskId);
|
||||
|
||||
}
|
||||
@ -332,4 +332,63 @@ public class InspectionRobotServiceImpl extends ServiceImpl<InspectionRobotMappe
|
||||
return robot;
|
||||
}
|
||||
|
||||
/**
|
||||
* 获取机器人实时位置
|
||||
*
|
||||
* @param robotId 机器人ID
|
||||
* @return 机器人位置信息（包含X、Y坐标、角度、当前站点等）
|
||||
*/
|
||||
@Override
|
||||
public AgvCommand.AgvRobotLocation getRobotRealtimeLocation(String robotId)
|
||||
{
|
||||
InspectionRobot robot = inspectionRobotMapper.selectInspectionRobotById(robotId);
|
||||
if (robot == null) {
|
||||
throw new GlobalException("机器人不存在");
|
||||
}
|
||||
|
||||
EdgeCommonVO edgeCommonVO = new EdgeCommonVO();
|
||||
edgeCommonVO.setTerminalId(robot.getTerminalId());
|
||||
edgeCommonVO.setDeviceId(robot.getRobotCode());
|
||||
|
||||
return edgeAgvService.getRobotLocation(edgeCommonVO);
|
||||
}
|
||||
|
||||
/**
|
||||
* 导航到指定位置（坐标）
|
||||
*
|
||||
* @param robotId 机器人ID
|
||||
* @param x X坐标（米）
|
||||
* @param y Y坐标（米）
|
||||
* @param theta 朝向角（弧度）
|
||||
* @param taskId 任务ID（可选）
|
||||
* @return 导航任务下发结果
|
||||
*/
|
||||
@Override
|
||||
public AgvCommand.RobotGoTargetResData navigateToPosition(String robotId, Double x, Double y,
|
||||
Double theta, String taskId)
|
||||
{
|
||||
InspectionRobot robot = inspectionRobotMapper.selectInspectionRobotById(robotId);
|
||||
if (robot == null) {
|
||||
throw new GlobalException("机器人不存在");
|
||||
}
|
||||
|
||||
if (x == null || y == null) {
|
||||
throw new GlobalException("X和Y坐标不能为空");
|
||||
}
|
||||
|
||||
// 构建自由导航点
|
||||
AgvCommand.FreeGoPoint freeGoPoint = AgvCommand.FreeGoPoint.newBuilder()
|
||||
.setX(x)
|
||||
.setY(y)
|
||||
.setTheta(theta != null ? theta : 0.0)
|
||||
.build();
|
||||
|
||||
EdgeCommonVO edgeCommonVO = new EdgeCommonVO();
|
||||
edgeCommonVO.setTerminalId(robot.getTerminalId());
|
||||
edgeCommonVO.setDeviceId(robot.getRobotCode());
|
||||
|
||||
// 调用边缘端接口，使用freego参数进行自由导航
|
||||
return edgeAgvService.robotGoTargetWithFreeGo(edgeCommonVO, freeGoPoint, taskId);
|
||||
}
|
||||
|
||||
}
|
||||
Loading…
Reference in New Issue
Block a user