Ur5State Ur5MessageDecoder::analyzeRawPacket(QByteArray packet)
{
if(headerLength(packet)==560 || headerLength(packet) == 1254)
{
return setState(packet);
}
else if(headerLength(packet)==812)
{
return setRTState(slicePacket(packet,sizeof(int),headerLength(packet)));
}
else
{
return Ur5State(false);
}
}
bool Ur5MessageDecoder::isValidHeader(QByteArray data)
{
// std::set<int> typeLength = {53,251,29,37,64,61}; // Cart. info, Joint data, Robot modus, Robot data1, Robot data2
// std::set<int> types = {4,1,0,2,3,20,7}; // Cart. info, Joint data, Robot modus, Robot data1, Robot data2, Robot Messages, Force data
// return (typeLength.find(headerLength(data)) != typeLength.end()
// && types.find(headerID(data)) !=types.end());
return (headerLength(data)==53 && headerID(data)==4);
}
void DataFrameModel::setDataFrame(SEXP dataframe, SEXP roles, SEXP rowHeader,
SEXP colHeader)
{
R_PreserveObject(dataframe);
R_PreserveObject(roles);
R_PreserveObject(rowHeader);
R_PreserveObject(colHeader);
// need dimension changes up-front
beginChanges(headerLength(rowHeader), headerLength(colHeader));
int oldnr = rowCount(QModelIndex()); // returns -1 if no dataframe
int oldnc = columnCount(QModelIndex());
_dataframe = dataframe;
_roles = roles;
_rowHeader = rowHeader;
_colHeader = colHeader;
// finish change notifications
endChanges(oldnr, oldnc);
}
Ur5State Ur5MessageDecoder::setState(QByteArray data)
{
Ur5State state(false);
for(int i=0;i<data.size();i++)
{
if(isValidHeader(getHeader(data,i)))
{
pushState(slicePacket(data,i,headerLength(getHeader(data,i))),state);
}
}
setTransformationMatrix(state);
state.updated = true;
return state;
}
int TPCI::encode(unique_ptr<NetworkBuffer>& nsdu) {
if (nsdu.get() != nullptr)
throw InvalidArgumentException();
int total = totalLength();
int userlen = 0;
if (tp_buffer != nullptr) {
total += tp_buffer->remaining();
} else
if (tp_len > 0 && tp_data != nullptr) {
total += tp_len;
}
unique_ptr<NetworkBuffer> newBuffer(new NetworkBuffer(total));
nsdu = std::move(newBuffer);
nsdu->putByte(tp_li = headerLength());
nsdu->putByte(tp_code);
return OK;
}
void SIMessageData::addRawDataBlock(const QByteArray& raw_data_with_header)
{
int block_no_offset = 0;
Command cmd = command(raw_data_with_header);
//qfInfo() << "addDataBlock CMD:" << QString::number(cmd, 16);
switch(cmd) {
case SIMessageData::CmdSICard5DetectedExt:
case SIMessageData::CmdSICard6DetectedExt:
case SIMessageData::CmdSICard8AndHigherDetectedExt:
case SIMessageData::CmdSICardRemovedExt:
break;
case SIMessageData::CmdGetSICard5:
case SIMessageData::CmdGetSICard5Ext:
break;
case SIMessageData::CmdGetSICard6:
block_no_offset = 2;
break;
case SIMessageData::CmdGetSICard6Ext:
case SIMessageData::CmdGetSICard8Ext:
block_no_offset = 4;
break;
default:
break;
}
int block_no = 0;
if(block_no_offset > 0) {
if(block_no_offset < raw_data_with_header.length()) {
block_no = (unsigned char)raw_data_with_header[block_no_offset];
}
else {
qfError() << "Buffer is too short - offset:" << block_no_offset << "buff length:" << raw_data_with_header.length();
}
}
int hdr_len = headerLength(command(raw_data_with_header));
if(block_no == 0) {
QByteArray hdr = raw_data_with_header.mid(0, hdr_len);
setHeader(hdr);
}
QByteArray data = raw_data_with_header.mid(hdr_len);
if(!data.isEmpty())
f_blockIndex[block_no] = data;
}
int columnCount (const QModelIndex &/*parent*/) const {
return headerLength(_colHeader);
}
int rowCount(const QModelIndex &/*parent*/) const {
return headerLength(_rowHeader);
}
bool WSHyBiFrameHeader::isHeaderComplete() const {
if (_data.size() < 2)
return false;
return _data.size() >= (size_t)headerLength();
}
