修复0xaf 0xb1 相关bug

This commit is contained in:
CaiXiang 2025-04-18 11:14:20 +08:00
parent ece8013fec
commit 17d06ce5f2
8 changed files with 181 additions and 80 deletions

View File

@ -26,6 +26,13 @@ public class ByteUtils {
System.arraycopy(src, start, dest, 0, length);
return dest;
}
public static byte[] copyBytesContainStart(byte[] src, int start, int length) {
byte[] dest = new byte[length];
for (int i = 0; i < length; i++) {
dest[i] = src[i+start];
}
return dest;
}
/**
* float 类型的数值按照小端模式转换为字节数组
* @param value 要转换的 float

View File

@ -1,34 +0,0 @@
package org.opentcs.kc.common.enmuc;
public enum ModbusFC {
//心跳变量这个可以要求电控同事加一个不和业务关联只用于通讯
MB_HOLD_REG((byte) 0x03, (byte) 0x10),
Q_OUT((byte) 0x01, (byte) 0x05),
I_IN((byte) 0x02, null),
IW_IN((byte) 0x04, null),
;
private Byte fread;
private Byte fwrite;
ModbusFC(Byte fread, Byte fwrite){
this.fread = fread;
this.fwrite = fwrite;
}
public Byte getFread() {
return fread;
}
public void setFread(Byte fread) {
this.fread = fread;
}
public Byte getFwrite() {
return fwrite;
}
public void setFwrite(Byte fwrite) {
this.fwrite = fwrite;
}
}

View File

@ -7,8 +7,10 @@ import java.util.List;
import org.opentcs.kc.common.byteutils.ByteUtils;
import org.opentcs.kc.udp.agv.param.AgvEvent;
import org.opentcs.kc.udp.agv.param.AgvEventConstant;
import org.opentcs.kc.udp.agv.param.function.af.QueryRobotStatusRsp;
import org.opentcs.kc.udp.agv.param.function.b1.SubscribeInfo;
import org.opentcs.kc.udp.agv.param.function.b1.SubscribeParam;
import org.opentcs.kc.udp.agv.param.function.b1.SubscribeRsp;
import org.opentcs.kc.udp.agv.param.function.navigation.*;
import org.opentcs.kc.udp.agv.param.function.read.ReadParam;
import org.opentcs.kc.udp.agv.param.function.read.ReadStrValue;
@ -17,6 +19,7 @@ import org.opentcs.kc.udp.agv.param.function.write.WriteParam;
import org.opentcs.kc.udp.agv.param.function.write.WriteStrValue;
import org.opentcs.kc.udp.agv.param.function.write.WriteValueMember;
import org.opentcs.kc.udp.agv.param.function.x14.RobotSetPosition;
import org.opentcs.kc.udp.agv.param.function.x17.QueryRobotRunStatusRsp;
import org.opentcs.kc.udp.agv.param.rsp.RcvEventPackage;
import org.opentcs.kc.udp.io.UDPClient;
import org.opentcs.kc.udp.agv.param.AgvEvent;
@ -24,12 +27,12 @@ import org.opentcs.kc.udp.agv.param.AgvEvent;
/**
*
* AGV启动
* 0xAF(查询机器人状态) 👌
* 0xAF(查询机器人状态) 👌 有response
* 0xB1(下发订阅信令) 👌
* 初始化
* 0x03(切换手自动) 👌
* 0x14(手动定位) 👌
* 0x17(查询机器人运行状态) 👌
* 0x17(查询机器人运行状态) 👌 有response
* 0x1F(确认初始位置) 👌
* 运行
* 0xAE(导航控制导航点控制) 👌
@ -41,23 +44,23 @@ import org.opentcs.kc.udp.agv.param.AgvEvent;
*/
public class KCCommandDemo {
public static void main(String [] args) throws Exception{
// {
// //0xAF(查询机器人状态)
// AgvEvent agvEvent = queryStatus();
// printInfo(agvEvent);
// RcvEventPackage rcv = UDPClient.localAGV.send(agvEvent);
// if(rcv.isOk()){
// QueryRobotStatusRsp queryRobotStatusRsp = new QueryRobotStatusRsp(rcv.getDataBytes());
// System.out.println();
// System.out.println("received transationId : "+ "isok:"+rcv.isOk());
// for (byte b:rcv.getValue()){
// System.out.print(byteToHex(b)+" ");
// }
// }else {
// System.out.println();
// System.out.println("received transationId : "+ "isok:"+rcv.isOk());
// }
// }
{
//0xAF(查询机器人状态)
AgvEvent agvEvent = queryStatus();
printInfo(agvEvent);
RcvEventPackage rcv = UDPClient.localAGV.send(agvEvent);
if(rcv.isOk()){
QueryRobotStatusRsp queryRobotStatusRsp = new QueryRobotStatusRsp(rcv.getDataBytes());
System.out.println();
System.out.println("received transationId : "+ "isok:"+rcv.isOk());
for (byte b:rcv.getValue()){
System.out.print(byteToHex(b)+" ");
}
}else {
System.out.println();
System.out.println("received transationId : "+ "isok:"+rcv.isOk());
}
}
// {
// //0xB0(查询载货状态)
@ -86,12 +89,12 @@ public class KCCommandDemo {
// if(rcv.isOk()){
// System.out.println();
// System.out.println("received transationId : "+ "isok:"+rcv.isOk());
// SubscribeRsp subscribeRsp = new SubscribeRsp(rcv.getDataBytes());
// if(subscribeRsp.isOk()){
// //...
// }else {
// //...
// }
//// SubscribeRsp subscribeRsp = new SubscribeRsp(rcv.getDataBytes());
//// if(subscribeRsp.isOk()){
//// //...
//// }else {
//// //...
//// }
// }else {
// System.out.println();
// System.out.println("received transationId : "+ "isok:"+rcv.isOk());
@ -184,19 +187,19 @@ public class KCCommandDemo {
// }
// }
{
//0x1F(确认初始位置)
AgvEvent agvEvent = confirmInitialPosition();
printInfo(agvEvent);
RcvEventPackage rcv = UDPClient.localAGV.send(agvEvent);
if(rcv.isOk()){
System.out.println("0x1F ok");
}else {
System.out.println();
System.out.println("0x1F fail");
System.out.println("received transationId : "+ "isok:"+rcv.isOk());
}
}
// {
// //0x1F(确认初始位置)
// AgvEvent agvEvent = confirmInitialPosition();
// printInfo(agvEvent);
// RcvEventPackage rcv = UDPClient.localAGV.send(agvEvent);
// if(rcv.isOk()){
// System.out.println("0x1F ok");
// }else {
// System.out.println();
// System.out.println("0x1F fail");
// System.out.println("received transationId : "+ "isok:"+rcv.isOk());
// }
// }
// {
// //0xAE(导航控制导航点控制)

View File

@ -1,5 +1,6 @@
package org.opentcs.kc.udp.agv.param.function.af;
import java.util.ArrayList;
import java.util.List;
import org.opentcs.kc.common.byteutils.ByteUtils;
@ -29,22 +30,27 @@ public class QueryRobotStatusRsp {
this.locationStatusInfo = new LocationStatusInfo(ByteUtils.copyBytes(src,4,32));
this.runningStatusInfo = new RunningStatusInfo(ByteUtils.copyBytes(src,36,20));
Integer pointSize = ByteUtils.toInt(src[60]);
Integer edgeSize = ByteUtils.toInt(src[61]);
Integer pointSize = ByteUtils.toInt(src[64]);
Integer edgeSize = ByteUtils.toInt(src[65]);
Integer taskByteSize = 12+8*(pointSize+edgeSize);
this.taskStatusInfo = new TaskStatusInfo(ByteUtils.copyBytes(src,56,taskByteSize));
this.batteryStatusInfo = new BatteryStatusInfo(ByteUtils.copyBytes(src,56+taskByteSize,20));
if(this.abnormal_size>0){
this.abnormalEventStatusInfoList = new ArrayList<>();
for(int i=0;i<this.abnormal_size;i++){
this.abnormalEventStatusInfoList.add(new AbnormalEventStatusInfo(ByteUtils.copyBytes(src,56+taskByteSize+20+12*i,12)));
int index = 56+taskByteSize+20+12*i;
byte[] bytes = ByteUtils.copyBytesContainStart(src, index, 12);
this.abnormalEventStatusInfoList.add(new AbnormalEventStatusInfo(bytes));
}
}
if(this.action_size>0){
this.actionInfoList = new ArrayList<>();
for(int i=0;i<this.action_size;i++){
this.actionInfoList.add(new ActionInfo(ByteUtils.copyBytes(src,56+taskByteSize+20+12*this.abnormal_size+12*i,12)));
this.actionInfoList.add(new ActionInfo(ByteUtils.copyBytesContainStart(src,56+taskByteSize+20+12*this.abnormal_size+12*i,12)));
}
}
}
}

View File

@ -20,7 +20,7 @@ public class SubscribeRsp {
this.src = src;
this.uuid = ByteUtils.copyOfRange(src,0,64);
this.errCode = src[64];
this.reserved = ByteUtils.copyOfRange(src,65,3);
this.reserved = ByteUtils.copyOfRange(src,65,67);
}
public boolean isOk(){

View File

@ -29,8 +29,8 @@ import org.slf4j.LoggerFactory;
public enum UDPClient {
//如果要配置多个链接 local1 local2 .... 这样排下去好了
localAGV("agv1","192.168.0.211",17804,55678),
//local("127.0.0.1",502,true),
//localAGV("agv1","192.168.0.211",17804,55678),
localAGV("agv1","127.0.0.1",17804,55678),
;
private String name;
private String host;

View File

@ -0,0 +1,45 @@
package org.opentcs.kc.udp.server;
import org.opentcs.kc.udp.agv.param.AgvEvent;
/**
* @Desc: ""
* @Author: caixiang
* @DATE: 2025/4/17 16:07
*/
public class main {
public static void main(String[] args) {
//size 148 ,contentSize = 148-28 = 120
String errSrc = "d4 97 44 9c cb cf 0b 4c 95 51 d8 61 70 f1 e7 94 01 01 07 00 10 af 00 00 64 00 00 00 01 00 00 00 14 90 b0 c0 45 c0 d3 3a ca a2 1d 3b 08 00 00 00 12 00 00 00 02 00 00 00 00 01 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 03 00 00 00 00 00 00 00 01 00 00 00 01 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 0a 01 02 00 00 00 00 00 00 00 00 00";
String okSrc = "d4 97 44 9c cb cf 0b 4c 95 51 d8 61 70 f1 e7 94 01 01 03 00 10 af 00 00 64 00 00 00 01 00 00 00 73 1c 30 3d 66 ba 03 3d d9 51 59 bb 00 00 00 00 0c 00 00 00 00 00 00 00 00 01 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 03 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 0a 01 02 00 00 00 00 00 00 00 00 00 73 1c 30 3d 66 ba 03 3d d9 51 59 bb 00 00 00 00 0c 00 00 00 00 00 00 00 00 01 00 00 00 00 00 00 ";
String[] s = errSrc.split(" ");
System.out.println("size: " + s.length);
printInfo(s);
}
public static void printInfo(String[] msgs) {
for (String b:msgs){
System.out.print("(byte)0x"+b+",");
}
}
public static void printInfos(String[] msgs){
for (String b:msgs){
System.out.print(""+byteToHex(Byte.valueOf(b))+" ");
}
}
public static String byteToHex(byte b) {
// 将byte转换为无符号整数
int unsignedByte = b & 0xFF;
// 使用Integer.toHexString方法转换为十六进制字符串
String hexString = Integer.toHexString(unsignedByte);
// 如果字符串长度为1需要在前面补0
if (hexString.length() == 1) {
return "0" + hexString;
}
return hexString;
}
}

View File

@ -0,0 +1,74 @@
package org.opentcs.kc.udp.server;
/**
* @Desc: "模拟假数据"
* @Author: caixiang
* @DATE: 2025/3/17 14:25
*/
import java.net.DatagramPacket;
import java.net.DatagramSocket;
import java.net.InetAddress;
public class udpserver {
public static void main(String[] args) {
int port = 17804; // 服务器端口
int bufferSize = 1024; // 接收缓冲区大小需与客户端协商
try (DatagramSocket socket = new DatagramSocket(port)) {
System.out.println("UDP Server started. Listening for binary data on port " + port);
// 循环处理客户端请求
while (true) {
// 1. 接收二进制数据
byte[] receiveBuffer = new byte[bufferSize];
DatagramPacket receivePacket = new DatagramPacket(receiveBuffer, receiveBuffer.length);
socket.receive(receivePacket); // 阻塞等待数据
// 解析客户端信息
InetAddress clientAddress = receivePacket.getAddress();
int clientPort = receivePacket.getPort();
int receivedLength = receivePacket.getLength(); // 实际接收到的字节数
// 提取原始字节数据避免填充多余的0
byte[] receivedData = new byte[receivedLength];
System.arraycopy(receiveBuffer, 0, receivedData, 0, receivedLength);
byte type = receivedData[21];
// 打印调试信息
System.out.printf(
"Received %d bytes from %s:%d 命令码%s\n",
receivedLength, clientAddress.getHostAddress(), clientPort, type
);
byte[] src = new byte[0];
if(type == (byte)0xAF){ //0XAF 指令
//错误指令 01 00
src = new byte[]{(byte)0xd4,(byte)0x97,(byte)0x44,(byte)0x9c,(byte)0xcb,(byte)0xcf,(byte)0x0b,(byte)0x4c,(byte)0x95,(byte)0x51,(byte)0xd8,(byte)0x61,(byte)0x70,(byte)0xf1,(byte)0xe7,(byte)0x94,(byte)0x01,(byte)0x01,(byte)0x01,(byte)0x00,(byte)0x10,(byte)0xaf,(byte)0x00,(byte)0x00,(byte)0x64,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x01,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x14,(byte)0x90,(byte)0xb0,(byte)0xc0,(byte)0x45,(byte)0xc0,(byte)0xd3,(byte)0x3a,(byte)0xca,(byte)0xa2,(byte)0x1d,(byte)0x3b,(byte)0x08,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x12,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x02,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x01,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x03,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x01,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x01,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x0a,(byte)0x01,(byte)0x02,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00};
//正确指令
//src = new byte[]{(byte)0xd4,(byte)0x97,(byte)0x44,(byte)0x9c,(byte)0xcb,(byte)0xcf,(byte)0x0b,(byte)0x4c,(byte)0x95,(byte)0x51,(byte)0xd8,(byte)0x61,(byte)0x70,(byte)0xf1,(byte)0xe7,(byte)0x94,(byte)0x01,(byte)0x01,(byte)0x03,(byte)0x00,(byte)0x10,(byte)0xaf,(byte)0x00,(byte)0x00,(byte)0x64,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x01,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x73,(byte)0x1c,(byte)0x30,(byte)0x3d,(byte)0x66,(byte)0xba,(byte)0x03,(byte)0x3d,(byte)0xd9,(byte)0x51,(byte)0x59,(byte)0xbb,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x0c,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x01,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x03,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x0a,(byte)0x01,(byte)0x02,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x73,(byte)0x1c,(byte)0x30,(byte)0x3d,(byte)0x66,(byte)0xba,(byte)0x03,(byte)0x3d,(byte)0xd9,(byte)0x51,(byte)0x59,(byte)0xbb,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x0c,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x01,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00};
}else if(type == (byte)0x17){ //0X17 指令
src = new byte[]{(byte)0xd4,(byte)0x97,(byte)0x44,(byte)0x9c,(byte)0xcb,(byte)0xcf,(byte)0x0b,(byte)0x4c,(byte)0x95,(byte)0x51,(byte)0xd8,(byte)0x61,(byte)0x70,(byte)0xf1,(byte)0xe7,(byte)0x94,(byte)0x01,(byte)0x01,(byte)0x01,(byte)0x00,(byte)0x10,(byte)0x17,(byte)0x00,(byte)0x00,(byte)0xc0,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0xe1,(byte)0xb6,(byte)0x11,(byte)0xbb,(byte)0x66,(byte)0x2a,(byte)0xac,(byte)0xbf,(byte)0x67,(byte)0x67,(byte)0x8d,(byte)0xb5,(byte)0x4f,(byte)0x65,(byte)0x1b,(byte)0x40,(byte)0x60,(byte)0x33,(byte)0x1c,(byte)0x39,(byte)0x82,(byte)0xbb,(byte)0xd0,(byte)0xbf,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x80,(byte)0x3f,(byte)0x10,(byte)0x72,(byte)0x3f,(byte)0x00,(byte)0x00,(byte)0xd8,(byte)0xe5,(byte)0x2b,(byte)0x4c,(byte)0xa7,(byte)0x40,(byte)0x00,(byte)0x20,(byte)0x99,(byte)0x3a,(byte)0x91,(byte)0x3b,(byte)0xf3,(byte)0x40,(byte)0x01,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x34,(byte)0x38,(byte)0x65,(byte)0x32,(byte)0x30,(byte)0x33,(byte)0x34,(byte)0x63,(byte)0x2d,(byte)0x64,(byte)0x34,(byte)0x61,(byte)0x31,(byte)0x2d,(byte)0x34,(byte)0x37,(byte)0x37,(byte)0x32,(byte)0x2d,(byte)0x39,(byte)0x31,(byte)0x32,(byte)0x37,(byte)0x2d,(byte)0x31,(byte)0x38,(byte)0x31,(byte)0x63,(byte)0x35,(byte)0x66,(byte)0x61,(byte)0x62,(byte)0x34,(byte)0x33,(byte)0x63,(byte)0x65,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x29,(byte)0xe9,(byte)0x35,(byte)0x3f,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00};
}
// 2. 原样返回数据ECHO模式
DatagramPacket sendPacket = new DatagramPacket(
src,
src.length,
clientAddress,
clientPort
);
socket.send(sendPacket);
}
} catch (Exception e) {
e.printStackTrace();
}
}
}