786 lines
27 KiB
C++
786 lines
27 KiB
C++
// KcConfigDlg.cpp : 实现文件
|
||
//
|
||
|
||
#include "stdafx.h"
|
||
#include "KcProtocol.h"
|
||
#include "KcStruct.h"
|
||
#include "KcCtrl.h"
|
||
#include "json.h"
|
||
|
||
|
||
|
||
CKcProtocol::CKcProtocol()
|
||
{
|
||
|
||
}
|
||
|
||
CKcProtocol::~CKcProtocol()
|
||
{
|
||
}
|
||
|
||
|
||
void CKcProtocol::BuildRequestMessage(unsigned char cCommand, unsigned char acContent[], int &nContentLength, Json::Value param)
|
||
{
|
||
|
||
int nMessageLen = 0;
|
||
static unsigned short nSerial = 1;
|
||
|
||
|
||
//拼接协议授权码
|
||
memcpy(acContent + nMessageLen, theApp.m_acAuthCoce, 16);
|
||
nMessageLen += 16;
|
||
|
||
//构建请求报文头
|
||
ST_KC_MESSAGE_HEADER stMessageHeader = {0};
|
||
stMessageHeader.version = 0x01;
|
||
stMessageHeader.type = 0x00;
|
||
stMessageHeader.serial_number = nSerial++;
|
||
stMessageHeader.server_code = 0x10;
|
||
stMessageHeader.command_code = cCommand;
|
||
stMessageHeader.run_code = 0x00;
|
||
stMessageHeader.reserve1 = 0x00;
|
||
stMessageHeader.reserve2[0] = 0x00;
|
||
stMessageHeader.reserve2[1] = 0x00;
|
||
|
||
switch (cCommand)
|
||
{
|
||
//发送空报文
|
||
case 0xAF:
|
||
case 0x1F:
|
||
case 0x17:
|
||
case 0xB0:
|
||
stMessageHeader.content_length = 0;
|
||
memcpy(acContent + nMessageLen, &stMessageHeader, sizeof(ST_KC_MESSAGE_HEADER));
|
||
nMessageLen += sizeof(ST_KC_MESSAGE_HEADER);
|
||
nContentLength = nMessageLen;
|
||
break;
|
||
case 0xAE:
|
||
{
|
||
//构造AE消息体
|
||
unsigned char acMsgBody[1024] = { 0 };
|
||
int nMsgBodyLength = 0;
|
||
Build0xAEMessage(param, acMsgBody, nMsgBodyLength);
|
||
stMessageHeader.content_length = nMsgBodyLength; //消息头中写入消息体的长度
|
||
|
||
//拼接消息头
|
||
memcpy(acContent + nMessageLen, &stMessageHeader, sizeof(ST_KC_MESSAGE_HEADER));
|
||
nMessageLen += sizeof(ST_KC_MESSAGE_HEADER);
|
||
nContentLength = nMessageLen;
|
||
|
||
//拼接消息体
|
||
memcpy(acContent + nMessageLen, acMsgBody, nMsgBodyLength);
|
||
nMessageLen += nMsgBodyLength;
|
||
nContentLength = nMessageLen;
|
||
|
||
break;
|
||
}
|
||
case 0x02:
|
||
{
|
||
//构造AE消息体
|
||
unsigned char acMsgBody[1024] = { 0 };
|
||
int nMsgBodyLength = 0;
|
||
Build0x02Message(param, acMsgBody, nMsgBodyLength);
|
||
stMessageHeader.content_length = nMsgBodyLength; //消息头中写入消息体的长度
|
||
|
||
memcpy(acContent + nMessageLen, &stMessageHeader, sizeof(ST_KC_MESSAGE_HEADER));//拼接消息头
|
||
nMessageLen += sizeof(ST_KC_MESSAGE_HEADER);
|
||
nContentLength = nMessageLen;
|
||
|
||
//拼接消息体
|
||
memcpy(acContent + nMessageLen, acMsgBody, nMsgBodyLength);
|
||
nMessageLen += nMsgBodyLength;
|
||
nContentLength = nMessageLen;
|
||
break;
|
||
}
|
||
|
||
case 0x14:
|
||
{
|
||
//构造AE消息体
|
||
unsigned char acMsgBody[1024] = { 0 };
|
||
int nMsgBodyLength = 0;
|
||
Build0x14Message(param, acMsgBody, nMsgBodyLength);
|
||
stMessageHeader.content_length = nMsgBodyLength; //消息头中写入消息体的长度
|
||
|
||
memcpy(acContent + nMessageLen, &stMessageHeader, sizeof(ST_KC_MESSAGE_HEADER));//拼接消息头
|
||
nMessageLen += sizeof(ST_KC_MESSAGE_HEADER);
|
||
nContentLength = nMessageLen;
|
||
|
||
//拼接消息体
|
||
memcpy(acContent + nMessageLen, acMsgBody, nMsgBodyLength);
|
||
nMessageLen += nMsgBodyLength;
|
||
nContentLength = nMessageLen;
|
||
break;
|
||
}
|
||
case 0xB2:
|
||
{
|
||
//构造AE消息体
|
||
unsigned char acMsgBody[1024] = { 0 };
|
||
int nMsgBodyLength = 0;
|
||
Build0xB2Message(param, acMsgBody, nMsgBodyLength);
|
||
stMessageHeader.content_length = nMsgBodyLength; //消息头中写入消息体的长度
|
||
|
||
memcpy(acContent + nMessageLen, &stMessageHeader, sizeof(ST_KC_MESSAGE_HEADER));//拼接消息头
|
||
nMessageLen += sizeof(ST_KC_MESSAGE_HEADER);
|
||
nContentLength = nMessageLen;
|
||
|
||
//拼接消息体
|
||
memcpy(acContent + nMessageLen, acMsgBody, nMsgBodyLength);
|
||
nMessageLen += nMsgBodyLength;
|
||
nContentLength = nMessageLen;
|
||
break;
|
||
}
|
||
case 0x03:
|
||
{
|
||
//构造AE消息体
|
||
unsigned char acMsgBody[1024] = { 0 };
|
||
int nMsgBodyLength = 0;
|
||
Build0x03Message(param, acMsgBody, nMsgBodyLength);
|
||
stMessageHeader.content_length = nMsgBodyLength; //消息头中写入消息体的长度
|
||
|
||
memcpy(acContent + nMessageLen, &stMessageHeader, sizeof(ST_KC_MESSAGE_HEADER));//拼接消息头
|
||
nMessageLen += sizeof(ST_KC_MESSAGE_HEADER);
|
||
nContentLength = nMessageLen;
|
||
|
||
//拼接消息体
|
||
memcpy(acContent + nMessageLen, acMsgBody, nMsgBodyLength);
|
||
nMessageLen += nMsgBodyLength;
|
||
nContentLength = nMessageLen;
|
||
break;
|
||
}
|
||
|
||
}
|
||
}
|
||
|
||
|
||
void CKcProtocol::Build0xAEMessage(Json::Value param, unsigned char acMsgBody[], int &nMsgLength)
|
||
{
|
||
static UINT32 pointSerialNumber = 0;
|
||
static UINT32 pathSerialNumber = 0;
|
||
|
||
ST_PATH_JOINT stPathJoint = { 0 };
|
||
stPathJoint.order_id = param["order_id"].asInt();
|
||
stPathJoint.task_key = param["task_key"].asInt();
|
||
stPathJoint.point_size = (uint8_t)(param["point_info"].size());
|
||
stPathJoint.path_size = (uint8_t)(param["path_info"].size());
|
||
stPathJoint.guide_type = 0;// 默认导航方式为路径拼接，根据 opentcs 规划的路径进行下发
|
||
|
||
|
||
int nOffset = 0;
|
||
int nMsgLen = sizeof(ST_PATH_JOINT) - sizeof(ST_GUIDE_POINT*) - sizeof(ST_PATH*);
|
||
memcpy(acMsgBody, &stPathJoint, nMsgLen);
|
||
nOffset += nMsgLen;
|
||
|
||
theApp.m_nReqPointId = -1;
|
||
theApp.m_nReqPointType = -1;
|
||
|
||
//构造点信息
|
||
for (int i = 0; i < stPathJoint.point_size; i++)
|
||
{
|
||
ST_GUIDE_0_POINT stPointInfo = { 0 };
|
||
stPointInfo.serial_number = ++pointSerialNumber;
|
||
//stPointInfo.serial_number = param["point_info"][i]["pointSerialNumber"].asUInt();
|
||
stPointInfo.point_id = param["point_info"][i]["point_id"].asUInt();
|
||
|
||
//全局记录请求点信息
|
||
int actionType = param["point_info"][i]["point_action"].asUInt();
|
||
if (actionType > 0)
|
||
{
|
||
theApp.m_nReqPointType = actionType;
|
||
theApp.m_nReqPointId = param["point_info"][i]["point_id"].asUInt();
|
||
}
|
||
|
||
float fAngle = (float)(param["point_info"][i]["point_angle"].asDouble());
|
||
|
||
if (fAngle > 300) //不指定角度
|
||
{
|
||
stPointInfo.angle = 0;
|
||
stPointInfo.is_assign_angle = 0;
|
||
}
|
||
else //指定角度
|
||
{
|
||
stPointInfo.angle = fAngle;
|
||
stPointInfo.is_assign_angle = 1;
|
||
}
|
||
stPointInfo.action_size = 0;
|
||
|
||
nMsgLen = sizeof(ST_GUIDE_0_POINT) - sizeof(ST_ACTION*);
|
||
memcpy(acMsgBody + nOffset, &stPointInfo, nMsgLen);
|
||
nOffset += nMsgLen;
|
||
|
||
for (int j = 0; j < stPointInfo.action_size; j++)
|
||
{
|
||
ST_ACTION stActionInfo = { 0 };
|
||
stActionInfo.action_type = 0;//动作类型
|
||
stActionInfo.action_run_type = 0;//执行动作并行方式
|
||
stActionInfo.action_id = 0;//动作ID
|
||
stActionInfo.param_size = 0;//参数长度
|
||
|
||
|
||
nMsgLen = sizeof(ST_ACTION) - sizeof(unsigned char *);
|
||
memcpy(acMsgBody + nOffset, &stActionInfo, nMsgLen);
|
||
nOffset += nMsgLen;
|
||
|
||
//继续拷贝action内容
|
||
//memcpy(acMsgBody + nOffset, , stActionInfo.param_size); //参数内容
|
||
nOffset += stActionInfo.param_size;
|
||
}
|
||
}
|
||
|
||
|
||
//构造段信息
|
||
for (int i = 0; i < stPathJoint.path_size; i++)
|
||
{
|
||
ST_PATH stPathInfo = { 0 };
|
||
//stPathInfo.serial_number = param["path_info"][i]["pathSerialNumber"].asUInt();
|
||
stPathInfo.serial_number = ++pathSerialNumber;
|
||
stPathInfo.segment_id = param["path_info"][i]["path_id"].asUInt();
|
||
|
||
float fFixedAngle = (float)(param["path_info"][i]["fixed_angle"].asDouble());
|
||
if (fFixedAngle > 300) //不指定角度
|
||
{
|
||
stPathInfo.fixed_angle = 0;
|
||
stPathInfo.is_fixed_angle = 0;
|
||
}
|
||
else //指定角度
|
||
{
|
||
stPathInfo.fixed_angle = fFixedAngle;
|
||
stPathInfo.is_fixed_angle = 1;
|
||
}
|
||
|
||
|
||
stPathInfo.driver_pose = (uint8_t)(param["path_info"][i]["driver_pose"].asUInt());
|
||
stPathInfo.max_speed = (float)(param["path_info"][i]["max_speed"].asDouble());
|
||
stPathInfo.max_angular_speed = (float)(param["path_info"][i]["max_angle_speed"].asDouble());
|
||
|
||
stPathInfo.action_size = 0;
|
||
|
||
nMsgLen = sizeof(ST_PATH) - sizeof(ST_ACTION*);
|
||
memcpy(acMsgBody + nOffset, &stPathInfo, nMsgLen);
|
||
nOffset += nMsgLen;
|
||
|
||
for (int j = 0; j < stPathInfo.action_size; j++)
|
||
{
|
||
ST_ACTION stActionInfo = { 0 };
|
||
stActionInfo.action_type = 0;//动作类型
|
||
stActionInfo.action_run_type = 0;//执行动作并行方式
|
||
stActionInfo.action_id = 0;//动作ID
|
||
stActionInfo.param_size = 0;//参数长度
|
||
|
||
nMsgLen = sizeof(ST_ACTION) - sizeof(unsigned char *);
|
||
memcpy(acMsgBody + nOffset, &stActionInfo, nMsgLen);
|
||
nOffset += nMsgLen;
|
||
//memcpy(acMsgBody + nOffset, , stActionInfo.param_size); //参数内容
|
||
nOffset += stActionInfo.param_size;
|
||
}
|
||
}
|
||
nMsgLength = nOffset;
|
||
}
|
||
|
||
|
||
void CKcProtocol::Build0x02Message(Json::Value param, unsigned char acMsgBody[], int &nMsgLength)
|
||
{
|
||
int nVarCount = param["varCount"].asInt();
|
||
static uint32_t nSerialId = 0;
|
||
|
||
READ_MULTIVARIABLE_REQ stReadMutiVariable = { 0 };
|
||
stReadMutiVariable.variable_count = (uint8_t)nVarCount;
|
||
stReadMutiVariable.value_id = ++nSerialId;
|
||
|
||
int nOffset = 0;
|
||
int nMsgLen = sizeof(READ_MULTIVARIABLE_REQ) - sizeof(ST_READ_STR_VALUE *);
|
||
memcpy(acMsgBody, &stReadMutiVariable, nMsgLen);
|
||
nOffset += nMsgLen;
|
||
|
||
for (int i = 0; i < nVarCount; i++)
|
||
{
|
||
ST_WRITE_STR_VALUE stWriteValue = { 0 };
|
||
memset(stWriteValue.name, 0, 16);
|
||
memcpy(stWriteValue.name, param["var"][i]["name"].asString().c_str(), param["var"][i]["name"].asString().length());
|
||
stWriteValue.count = param["var"][i]["count"].asInt();
|
||
|
||
nMsgLen = sizeof(WRITE_STR_VALUE) - sizeof(WRITE_VALUE_MEMBER *);
|
||
memcpy(acMsgBody + nOffset, &stWriteValue, nMsgLen);
|
||
nOffset += nMsgLen;
|
||
|
||
Json::Value member = param["var"][i]["member"];
|
||
|
||
|
||
for (int j = 0; j < stWriteValue.count; j++)
|
||
{
|
||
READ_VALUE_MEMBER stReadMember = { 0 };
|
||
//stWriteMember.value = member[j]["value"].asUInt();
|
||
stReadMember.length = (uint16_t)(member[j]["length"].asUInt());
|
||
stReadMember.offset = (uint16_t)(member[j]["offset"].asUInt());
|
||
|
||
nMsgLen = sizeof(READ_VALUE_MEMBER);
|
||
memcpy(acMsgBody + nOffset, &stReadMember, nMsgLen);
|
||
nOffset += nMsgLen;
|
||
}
|
||
}
|
||
nMsgLength = nOffset;
|
||
}
|
||
|
||
void CKcProtocol::Build0x03Message(Json::Value param, unsigned char acMsgBody[], int &nMsgLength)
|
||
{
|
||
int nVarCount = param["var_count"].asInt();
|
||
int nValueId = param["value_id"].asInt();
|
||
|
||
ST_WRITE_MULTIVARIABLE_REQ stWriteMutiVariable = {0};
|
||
stWriteMutiVariable.variable_count = (uint8_t)nVarCount;
|
||
|
||
int nOffset = 0;
|
||
int nMsgLen = sizeof(ST_WRITE_MULTIVARIABLE_REQ) - sizeof(ST_WRITE_STR_VALUE *) ;
|
||
memcpy(acMsgBody, &stWriteMutiVariable, nMsgLen);
|
||
nOffset += nMsgLen;
|
||
|
||
for (int i = 0; i < nVarCount; i++)
|
||
{
|
||
ST_WRITE_STR_VALUE stWriteValue = {0};
|
||
memset(stWriteValue.name, 0, 16);
|
||
memcpy(stWriteValue.name, param["var"][i]["name"].asString().c_str(), param["var"][i]["name"].asString().length());
|
||
stWriteValue.count = param["var"][i]["count"].asInt();
|
||
|
||
nMsgLen = sizeof(WRITE_STR_VALUE) - sizeof(WRITE_VALUE_MEMBER *);
|
||
memcpy(acMsgBody + nOffset, &stWriteValue, nMsgLen);
|
||
nOffset += nMsgLen;
|
||
|
||
Json::Value member = param["var"][i]["member"];
|
||
|
||
|
||
for (int j = 0; j < stWriteValue.count; j++)
|
||
{
|
||
ST_WRITE_VALUE_MEMBER stWriteMember = {0};
|
||
stWriteMember.value = member[j]["value"].asUInt();
|
||
stWriteMember.length = (uint16_t)(member[j]["length"].asUInt());
|
||
stWriteMember.offset = (uint16_t)(member[j]["offset"].asUInt());
|
||
|
||
nMsgLen = sizeof(ST_WRITE_VALUE_MEMBER);
|
||
memcpy(acMsgBody + nOffset, &stWriteMember, nMsgLen);
|
||
nOffset += nMsgLen;
|
||
}
|
||
}
|
||
nMsgLength = nOffset;
|
||
|
||
}
|
||
|
||
|
||
void CKcProtocol::Build0x14Message(Json::Value param, unsigned char acMsgBody[], int &nMsgLength)
|
||
{
|
||
|
||
ST_MANUAL_LOCATION stLocation;
|
||
stLocation.angular = param["angle"].asDouble();
|
||
stLocation.x = param["x"].asDouble();
|
||
stLocation.y = param["y"].asDouble();
|
||
|
||
|
||
nMsgLength = sizeof(ST_MANUAL_LOCATION) ;
|
||
memcpy(acMsgBody, &stLocation, nMsgLength);
|
||
|
||
}
|
||
|
||
void CKcProtocol::Build0xB2Message(Json::Value param, unsigned char acMsgBody[], int &nMsgLength)
|
||
{
|
||
static uint32_t nActionId = 0;
|
||
ST_IMMEDIATE_ACTION stAction = {0};
|
||
stAction.action_type = (uint16_t)(param["actionType"].asUInt()); //动作类型：1=暂停任务；2=继续执行任务；3=撤销任务；
|
||
stAction.action_parallel = (uint8_t)(param["actionParallelManner"].asUInt()); //执行动作并行方式
|
||
stAction.action_id = ++nActionId;
|
||
|
||
|
||
|
||
int nOffset = 0;
|
||
int nMsgLen = sizeof(ST_IMMEDIATE_ACTION) - sizeof(uint8_t *);
|
||
memcpy(acMsgBody, &stAction, nMsgLen);
|
||
nOffset += nMsgLen;
|
||
|
||
//暂停任务
|
||
if (1 == stAction.action_type)
|
||
{
|
||
int nMsgLen = 0;
|
||
for (int i = 0; i < param["paramData"].size(); i++)
|
||
{
|
||
CONTENT_TYPE1 data = {0};
|
||
data.isStopImmediately = param["paramData"][i]["isStopImmediately"].asUInt();//是否立即停止移动
|
||
memcpy(acMsgBody + nOffset, &data, sizeof(CONTENT_TYPE1));
|
||
nOffset += sizeof(CONTENT_TYPE1);
|
||
}
|
||
stAction.param_size = 4;
|
||
}
|
||
//继续执行任务
|
||
else if (2 == stAction.action_type)
|
||
{
|
||
for (int i = 0; i < param["paramData"].size(); i++)
|
||
{
|
||
CONTENT_TYPE2 data = { 0 };
|
||
data.orderId = param["paramData"][i]["orderId"].asUInt(); //订单 ID
|
||
data.taskKey = param["paramData"][i]["taskKey"].asUInt(); //任务 key
|
||
memcpy(acMsgBody + nOffset, &data, sizeof(CONTENT_TYPE2));
|
||
nOffset += sizeof(CONTENT_TYPE2);
|
||
}
|
||
stAction.param_size = 8;
|
||
}
|
||
//撤销任务
|
||
else if (3 == stAction.action_type)
|
||
{
|
||
for (int i = 0; i < param["paramData"].size(); i++)
|
||
{
|
||
CONTENT_TYPE3 data = { 0 };
|
||
data.orderId = param["paramData"][i]["orderId"].asUInt(); //订单 ID
|
||
data.isStopImmediately = param["paramData"][i]["isStopImmediately"].asUInt(); //是否立即停止移动
|
||
memcpy(acMsgBody + nOffset, &data, sizeof(CONTENT_TYPE3));
|
||
nOffset += sizeof(CONTENT_TYPE3);
|
||
}
|
||
stAction.param_size = 8;
|
||
}
|
||
|
||
nMsgLength = nOffset;
|
||
|
||
}
|
||
|
||
|
||
/*
|
||
Json::Value CKcProtocol::Analysis0xAFMessage(unsigned char *pMsg, int nLen)
|
||
{
|
||
ST_AGV_STATUS *pstAgvStatus;
|
||
|
||
pstAgvStatus = (ST_AGV_STATUS *)(pMsg+28);
|
||
|
||
int nOffset = 0;
|
||
Json::Value root;
|
||
|
||
Json::Value location_status_info;
|
||
location_status_info["x"] = pstAgvStatus->stLocationStatusInfo.x;
|
||
location_status_info["y"] = pstAgvStatus->stLocationStatusInfo.y;
|
||
location_status_info["angle"] = pstAgvStatus->stLocationStatusInfo.angle;
|
||
location_status_info["point_id"] = pstAgvStatus->stLocationStatusInfo.point_id;
|
||
location_status_info["segment_id"] = pstAgvStatus->stLocationStatusInfo.segment_id;
|
||
location_status_info["point_seq"] = pstAgvStatus->stLocationStatusInfo.point_seq;
|
||
location_status_info["confidence"] = pstAgvStatus->stLocationStatusInfo.confidence;
|
||
root["LOCATION_STATUS_INFO"] = location_status_info;
|
||
nOffset += sizeof(ST_AGV_STATUS);
|
||
|
||
|
||
|
||
|
||
Json::Value running_status_info;
|
||
running_status_info["line_speed"] = pstAgvStatus->stRunningStatusInfo.line_speed;
|
||
running_status_info["acceleration"] = pstAgvStatus->stRunningStatusInfo.acceleration;
|
||
running_status_info["angular_speed"] = pstAgvStatus->stRunningStatusInfo.angular_speed;
|
||
running_status_info["mode"] = pstAgvStatus->stRunningStatusInfo.mode;
|
||
running_status_info["status"] = pstAgvStatus->stRunningStatusInfo.status;
|
||
running_status_info["capability"] = pstAgvStatus->stRunningStatusInfo.capability;
|
||
root["RUNNING_STATUS_INFO"] = running_status_info;
|
||
nOffset += sizeof(ST_RUNNING_STATUS_INFO);
|
||
|
||
char *pPoint1 =(char *) &(pstAgvStatus->stRunningStatusInfo);
|
||
|
||
|
||
Json::Value task_status_info;
|
||
task_status_info["order_id"] = pstAgvStatus->stTaskStatusInfo.order_id;
|
||
task_status_info["task_key"] = pstAgvStatus->stTaskStatusInfo.task_key;
|
||
|
||
int nTaskStatusInfoLen = sizeof(TASK_STATUS_INFO) - 2 * sizeof(ST_POINT_STATE_SEQUENCE *)
|
||
+ pstAgvStatus->stTaskStatusInfo.point_size * sizeof(ST_POINT_STATE_SEQUENCE)
|
||
+ pstAgvStatus->stTaskStatusInfo.path_size * sizeof(ST_PATH_STATE_SEQUENCE);
|
||
|
||
Json::Value point_array = Json::arrayValue;;
|
||
ST_POINT_STATE_SEQUENCE * pPointStateSequence = (ST_POINT_STATE_SEQUENCE *)((unsigned char *) &(pstAgvStatus->stRunningStatusInfo) + sizeof(TASK_STATUS_INFO)
|
||
- sizeof(ST_POINT_STATE_SEQUENCE *) - sizeof(ST_PATH_STATE_SEQUENCE *));
|
||
for (int i = 0; i < pstAgvStatus->stTaskStatusInfo.point_size; i++)
|
||
{
|
||
Json::Value point_item;
|
||
point_item["sequence"] = (pPointStateSequence+i)->sequence;
|
||
point_item["point_id"] = (pPointStateSequence+i)->point_id;
|
||
point_array.append(point_item);
|
||
}
|
||
|
||
Json::Value path_array = Json::arrayValue;
|
||
ST_PATH_STATE_SEQUENCE * pPathStateSequence = (ST_PATH_STATE_SEQUENCE *)((unsigned char *) pPointStateSequence + pstAgvStatus->stTaskStatusInfo.point_size * sizeof(ST_POINT_STATE_SEQUENCE));
|
||
|
||
for (int i = 0; i < pstAgvStatus->stTaskStatusInfo.path_size; i++)
|
||
{
|
||
Json::Value abnormal_item;
|
||
abnormal_item["sequence"] = (pPathStateSequence + i)->sequence;
|
||
abnormal_item["path_id"] = (pPathStateSequence + i)->path_id;
|
||
path_array.append(abnormal_item);
|
||
}
|
||
task_status_info["POINT_STATE_SEQUENCE"] = point_array;
|
||
task_status_info["PATH_STATE_SEQUENCE"] = path_array;
|
||
root["TASK_STATUS_INFO"] = task_status_info;
|
||
|
||
|
||
char *pPoint2 = (char *) &(pstAgvStatus->stTaskStatusInfo);
|
||
|
||
|
||
ST_BATTERY_STATUS_INFO *pBatteryStatusInfo;
|
||
Json::Value battery_status_info;
|
||
pBatteryStatusInfo = (ST_BATTERY_STATUS_INFO *)((unsigned char*) &(pstAgvStatus->stTaskStatusInfo) + nTaskStatusInfoLen);
|
||
battery_status_info["power"] = pBatteryStatusInfo->power;
|
||
battery_status_info["voltage"] = pBatteryStatusInfo->voltage;
|
||
battery_status_info["current"] = pBatteryStatusInfo->current;
|
||
battery_status_info["status"] = pBatteryStatusInfo->status;
|
||
root["BATTERY_STATUS_INFO"] = battery_status_info;
|
||
|
||
|
||
|
||
ABNORMAL_EVENT_STATUS_INFO *pAbnormalEventStatusInfo;
|
||
pAbnormalEventStatusInfo = (ABNORMAL_EVENT_STATUS_INFO *)((unsigned char *)pBatteryStatusInfo + sizeof(ST_BATTERY_STATUS_INFO));
|
||
Json::Value abnormal_array;
|
||
for (int i = 0; i < pstAgvStatus->abnormal_size; i++)
|
||
{
|
||
Json::Value abnormal_item;
|
||
abnormal_item["event_code"] = (pAbnormalEventStatusInfo + i)->event_code;
|
||
abnormal_item["event_level"] = (pAbnormalEventStatusInfo + i)->event_level;
|
||
abnormal_array.append(abnormal_item);
|
||
}
|
||
root["ABNORMAL_EVENT_STATUS_INFO"] = abnormal_array;
|
||
|
||
|
||
ST_ACTION_STATUS_INFO *pActionStatusInfo;
|
||
pActionStatusInfo = (ST_ACTION_STATUS_INFO *)((unsigned char *)pAbnormalEventStatusInfo + pstAgvStatus->abnormal_size*sizeof(ABNORMAL_EVENT_STATUS_INFO));
|
||
Json::Value action_array = Json::arrayValue;
|
||
for (int i = 0; i < pstAgvStatus->action_size; i++)
|
||
{
|
||
Json::Value action_item;
|
||
action_item["action_id"] = (pActionStatusInfo + i)->action_id;
|
||
action_item["action_status"] = (pActionStatusInfo + i)->action_status;
|
||
action_array.append(action_item);
|
||
}
|
||
root["ACTION_STATUS_INFO"] = action_array;
|
||
|
||
|
||
REPORT_INFO *pReportInfo;
|
||
pReportInfo = (REPORT_INFO *)((unsigned char *)pActionStatusInfo + pstAgvStatus->action_size * sizeof(ST_ACTION_STATUS_INFO));
|
||
Json::Value info_array = Json::arrayValue;;
|
||
for (int i = 0; i < pstAgvStatus->info_size; i++)
|
||
{
|
||
Json::Value info_item;
|
||
info_item["info_type"] = (pReportInfo + i)->info_type;
|
||
info_item["info_size"] = (pReportInfo + i)->info_size;
|
||
info_array.append(info_item);
|
||
}
|
||
root["REPORT_INFO"] = info_array;
|
||
|
||
return root;
|
||
|
||
}
|
||
*/
|
||
Json::Value CKcProtocol::Analysis0xAFMessage(unsigned char *pMsg, int nLen)
|
||
{
|
||
ST_AGV_STATUS *pstAgvStatus;
|
||
|
||
pstAgvStatus = (ST_AGV_STATUS *)(pMsg + 28);
|
||
|
||
int nOffset = 0;
|
||
Json::Value root;
|
||
|
||
root["x"] = pstAgvStatus->stLocationStatusInfo.x; //X
|
||
root["y"] = pstAgvStatus->stLocationStatusInfo.y; //Y
|
||
root["angle"] = pstAgvStatus->stLocationStatusInfo.angle; //角度
|
||
root["point"] = pstAgvStatus->stLocationStatusInfo.point_id; //最终经过点
|
||
nOffset += sizeof(ST_AGV_STATUS);
|
||
|
||
|
||
ChangeGuideType(pstAgvStatus->stLocationStatusInfo.point_id);
|
||
|
||
|
||
root["agv_model"] = pstAgvStatus->stRunningStatusInfo.mode;
|
||
root["agv_status"] = pstAgvStatus->stRunningStatusInfo.status;
|
||
nOffset += sizeof(ST_RUNNING_STATUS_INFO);
|
||
|
||
char *pPoint1 = (char *) &(pstAgvStatus->stRunningStatusInfo);
|
||
|
||
|
||
Json::Value task_status_info;
|
||
task_status_info["order_id"] = pstAgvStatus->stTaskStatusInfo.order_id;
|
||
task_status_info["task_key"] = pstAgvStatus->stTaskStatusInfo.task_key;
|
||
|
||
int nTaskStatusInfoLen = sizeof(TASK_STATUS_INFO) - 2 * sizeof(ST_POINT_STATE_SEQUENCE *)
|
||
+ pstAgvStatus->stTaskStatusInfo.point_size * sizeof(ST_POINT_STATE_SEQUENCE)
|
||
+ pstAgvStatus->stTaskStatusInfo.path_size * sizeof(ST_PATH_STATE_SEQUENCE);
|
||
|
||
//Json::Value point_array = Json::arrayValue;;
|
||
ST_POINT_STATE_SEQUENCE * pPointStateSequence = (ST_POINT_STATE_SEQUENCE *)((unsigned char *) &(pstAgvStatus->stRunningStatusInfo) + sizeof(TASK_STATUS_INFO)
|
||
- sizeof(ST_POINT_STATE_SEQUENCE *) - sizeof(ST_PATH_STATE_SEQUENCE *));
|
||
/*for (int i = 0; i < pstAgvStatus->stTaskStatusInfo.point_size; i++)
|
||
{
|
||
Json::Value point_item;
|
||
point_item["sequence"] = (pPointStateSequence + i)->sequence;
|
||
point_item["point_id"] = (pPointStateSequence + i)->point_id;
|
||
point_array.append(point_item);
|
||
}
|
||
|
||
Json::Value path_array = Json::arrayValue;*/
|
||
ST_PATH_STATE_SEQUENCE * pPathStateSequence = (ST_PATH_STATE_SEQUENCE *)((unsigned char *)pPointStateSequence + pstAgvStatus->stTaskStatusInfo.point_size * sizeof(ST_POINT_STATE_SEQUENCE));
|
||
|
||
/*for (int i = 0; i < pstAgvStatus->stTaskStatusInfo.path_size; i++)
|
||
{
|
||
Json::Value path_item;
|
||
path_item["sequence"] = (pPathStateSequence + i)->sequence;
|
||
path_item["path_id"] = (pPathStateSequence + i)->path_id;
|
||
path_array.append(path_item);
|
||
}
|
||
task_status_info["POINT_STATE_SEQUENCE"] = point_array;
|
||
task_status_info["PATH_STATE_SEQUENCE"] = path_array;
|
||
root["TASK_STATUS_INFO"] = task_status_info;*/
|
||
|
||
|
||
char *pPoint2 = (char *) &(pstAgvStatus->stTaskStatusInfo);
|
||
|
||
|
||
ST_BATTERY_STATUS_INFO *pBatteryStatusInfo;
|
||
pBatteryStatusInfo = (ST_BATTERY_STATUS_INFO *)((unsigned char*) &(pstAgvStatus->stTaskStatusInfo) + nTaskStatusInfoLen);
|
||
root["power"] = pBatteryStatusInfo->power; //电池电量
|
||
root["charge_status"] = pBatteryStatusInfo->status; //充电情况：放电 = 0，充电 = 1, 充满 = 2
|
||
|
||
root["cargo_status"] = 1; //默认载货
|
||
|
||
//异常信息
|
||
ABNORMAL_EVENT_STATUS_INFO *pAbnormalEventStatusInfo;
|
||
pAbnormalEventStatusInfo = (ABNORMAL_EVENT_STATUS_INFO *)((unsigned char *)pBatteryStatusInfo + sizeof(ST_BATTERY_STATUS_INFO));
|
||
Json::Value abnormal_array;
|
||
for (int i = 0; i < pstAgvStatus->abnormal_size; i++)
|
||
{
|
||
Json::Value abnormal_item;
|
||
abnormal_item["event_code"] = (pAbnormalEventStatusInfo + i)->event_code;
|
||
abnormal_item["event_level"] = (pAbnormalEventStatusInfo + i)->event_level;
|
||
abnormal_array.append(abnormal_item);
|
||
}
|
||
//root["error"] = abnormal_array;
|
||
root["error"] = "错误信息";
|
||
|
||
/*
|
||
ST_ACTION_STATUS_INFO *pActionStatusInfo;
|
||
pActionStatusInfo = (ST_ACTION_STATUS_INFO *)((unsigned char *)pAbnormalEventStatusInfo + pstAgvStatus->abnormal_size * sizeof(ABNORMAL_EVENT_STATUS_INFO));
|
||
Json::Value action_array = Json::arrayValue;
|
||
for (int i = 0; i < pstAgvStatus->action_size; i++)
|
||
{
|
||
Json::Value action_item;
|
||
action_item["action_id"] = (pActionStatusInfo + i)->action_id;
|
||
action_item["action_status"] = (pActionStatusInfo + i)->action_status;
|
||
action_array.append(action_item);
|
||
}
|
||
root["ACTION_STATUS_INFO"] = action_array;
|
||
|
||
|
||
REPORT_INFO *pReportInfo;
|
||
pReportInfo = (REPORT_INFO *)((unsigned char *)pActionStatusInfo + pstAgvStatus->action_size * sizeof(ST_ACTION_STATUS_INFO));
|
||
Json::Value info_array = Json::arrayValue;;
|
||
for (int i = 0; i < pstAgvStatus->info_size; i++)
|
||
{
|
||
Json::Value info_item;
|
||
info_item["info_type"] = (pReportInfo + i)->info_type;
|
||
info_item["info_size"] = (pReportInfo + i)->info_size;
|
||
info_array.append(info_item);
|
||
}
|
||
root["REPORT_INFO"] = info_array;
|
||
*/
|
||
return root;
|
||
|
||
}
|
||
|
||
Json::Value CKcProtocol::Analysis0x17Message(unsigned char *pMsg, int nLen)
|
||
{
|
||
AGV_RUN_STATUS *pstAgvRunStatus;
|
||
|
||
pstAgvRunStatus = (AGV_RUN_STATUS *)(pMsg + 28);
|
||
|
||
|
||
Json::Value root;
|
||
root["temperature"] = pstAgvRunStatus->temperature;
|
||
root["x"] = pstAgvRunStatus->x;
|
||
root["y"] = pstAgvRunStatus->y;
|
||
|
||
root["angular"] = pstAgvRunStatus->angular;
|
||
root["power"] = pstAgvRunStatus->power;
|
||
root["is_block"] = pstAgvRunStatus->is_block;
|
||
root["is_charge"] = pstAgvRunStatus->is_charge;
|
||
root["mode"] = pstAgvRunStatus->mode;
|
||
root["map_statue"] = pstAgvRunStatus->map_statue;
|
||
|
||
root["point_id"] = pstAgvRunStatus->point_id;
|
||
root["travel_speed"] = pstAgvRunStatus->travel_speed;
|
||
root["turning_speed"] = pstAgvRunStatus->turning_speed;
|
||
root["voltage"] = pstAgvRunStatus->voltage;
|
||
root["current"] = pstAgvRunStatus->current;
|
||
|
||
root["task_status"] = pstAgvRunStatus->task_status;
|
||
root["location_type"] = pstAgvRunStatus->location_type;
|
||
root["map_version"] = pstAgvRunStatus->map_version;
|
||
root["total_mileage"] = pstAgvRunStatus->total_mileage;
|
||
root["this_runtime"] = pstAgvRunStatus->this_runtime;
|
||
root["total_runtime"] = pstAgvRunStatus->total_runtime;
|
||
root["location_status"] = pstAgvRunStatus->location_status;
|
||
|
||
root["is_traffic_control"] = pstAgvRunStatus->is_traffic_control;
|
||
root["map_count"] = pstAgvRunStatus->map_count;
|
||
root["map_name"] = pstAgvRunStatus->map_name;
|
||
root["confidence"] = pstAgvRunStatus->confidence;
|
||
|
||
return root;
|
||
|
||
}
|
||
|
||
|
||
Json::Value CKcProtocol::Analysis0x02Message(unsigned char *pMsg, int nLen)
|
||
{
|
||
Json::Value root;
|
||
|
||
|
||
ST_READ_MULTIVARIABLE_RESP *pstReadVariableResp;
|
||
|
||
|
||
pstReadVariableResp = (ST_READ_MULTIVARIABLE_RESP *)(pMsg + 28);
|
||
|
||
return root;
|
||
}
|
||
|
||
|
||
Json::Value CKcProtocol::Analysis0xB0Message(unsigned char *pMsg, int nLen)
|
||
{
|
||
Json::Value root;
|
||
|
||
unsigned char *pData = (unsigned char *)(pMsg + 28);
|
||
|
||
root["load_size"] = (uint32_t)(*pData);
|
||
|
||
return root;
|
||
}
|
||
|
||
|
||
|
||
void CKcProtocol::ChangeGuideType(int nPositionId)
|
||
{
|
||
//经过请求点
|
||
if (theApp.m_nReqPointId == nPositionId)
|
||
{
|
||
//取请求
|
||
if (theApp.m_nReqPointType == 1)
|
||
{
|
||
//无货，切换巷道二维码导航
|
||
if (theApp.m_nCargoStatus == 0)
|
||
{
|
||
theApp.ChangeGuideType(QRCODE_TYPE);
|
||
}
|
||
else
|
||
{
|
||
//已取到货，切换激光导航
|
||
theApp.ChangeGuideType(SLAM_TYPE);
|
||
}
|
||
}
|
||
//放请求
|
||
else if (theApp.m_nReqPointType == 2 && theApp.m_nCargoStatus == 1)
|
||
{
|
||
//无货（已放完），切换到激光导航
|
||
if (theApp.m_nCargoStatus == 0)
|
||
{
|
||
theApp.ChangeGuideType(SLAM_TYPE);
|
||
}
|
||
else
|
||
{
|
||
//有货，切换巷道二维码导航
|
||
theApp.ChangeGuideType(QRCODE_TYPE);
|
||
}
|
||
}
|
||
}
|
||
} |