SSIZE_T parse( BYTE *data, SIZE_T len, Descriptors &descriptors ) {
SSIZE_T offset = 0;
WORD compatibilityLen = RW(data,offset);
if (!compatibilityLen) {
// Ignore if not present
return 2;
}
WORD descriptorCount = RW(data,offset);
printf( "[dsmcc::compatiblity] Compatibility descriptor: dataLen=%ld, descLen=%d, count=%d\n",
len, compatibilityLen, descriptorCount );
// Check if len is ok
if (len < compatibilityLen) {
printf( "[dsmcc::compatiblity] No data available to parse Compatibility descriptor: descLen=%ld, available=%d\n",
len, compatibilityLen );
return compatibilityLen+2;
}
// Parse descriptors
for (WORD desc=0; desc<descriptorCount; desc++) {
Descriptor desc;
desc.type = RB(data,offset);
//BYTE dLen = RB(data,offset);
offset += 1;
desc.specifier = RDW(data,offset);
desc.model = RW(data,offset);
desc.version = RW(data,offset);
// Parse sub descriptors
BYTE subCount = RB(data,offset);
for (BYTE sub=0; sub<subCount; sub++) {
BYTE subType = RB(data,offset);
BYTE subLen = RB(data,offset);
// AdditionalInformation
offset += subLen;
printf( "[dsmcc::compatibility] Warning, subdescriptor not parsed: count=%d, type=%x, len=%x\n", subCount, subType, subLen );
}
descriptors.push_back( desc );
}
return compatibilityLen+2;
}
SSIZE_T Object::objectKey( const BYTE *data, SIZE_T len, ObjectKeyType &key ) {
SSIZE_T off=0;
// Object key len
BYTE objectKeyLen = RB(data,off);
if (objectKeyLen > 4) {
throw std::bad_cast();
}
assert(len >= SIZE_T(1+objectKeyLen));
// Object key
key = RDW( data, off );
//printf( "[dsmcc::Object] Parsing object key: len=%08lx, key=%08lx\n", len, key );
return off;
}
SSIZE_T parse( BYTE *data, SIZE_T /*len*/, Modules &modules, bool skipVersion/*=false*/ ) {
SSIZE_T offset = 0;
WORD numberOfModules = RW(data,offset);
for (int mod=0; mod<numberOfModules; mod++) {
Type module;
module.id = RW(data,offset);
module.size = RDW(data,offset);
module.version = (skipVersion) ? 0 : RB(data,offset);
BYTE moduleInfoLen = RB(data,offset);
if (moduleInfoLen) {
module.info.copy( (char *)(data+offset), moduleInfoLen );
offset += moduleInfoLen;
}
modules.push_back( module );
}
return offset;
}
Object *Object::parseObject( ResourceManager *resMgr, Module *module, SSIZE_T &off ) {
Object *obj;
ObjectLocation loc;
BYTE buf[BIOP_MAX_HEADER];
SSIZE_T offset = 0;
SIZE_T len = module->read( off, buf, BIOP_MAX_HEADER );
if (len < BIOP_MAX_HEADER) {
printf( "[dsmcc::Object] Warning: Not enough buffer: len=%ld, offset=%ld\n", len, offset );
return NULL;
}
DWORD magic = RDW(buf,offset);
if (magic != BIOP_MAGIC) {
printf( "[dsmcc::Object] Warning: Bad magic number: magic=%x, found=%lx\n", BIOP_MAGIC, magic );
return NULL;
}
WORD biopVersion = RW(buf,offset);
if (biopVersion != BIOP_VER) {
printf( "[dsmcc::Object] Warning: Invalid version\n" );
return NULL;
}
BYTE byteOrder = RB(buf,offset);
if (byteOrder) {
printf( "[dsmcc::Object] Warning: Invalid byte order\n" );
return NULL;
}
BYTE msgType = RB(buf,offset);
if (msgType) {
printf( "[dsmcc::Object] Warning: Invalid type\n" );
return NULL;
}
// Check BIOP message size
DWORD msgSize = RDW(buf,offset);
if (module->size()-off-offset < msgSize) {
printf( "[dsmcc::Object] Warning: Not enough bytes for BIOP message: msgSize=%ld, rest%ld\n", msgSize, module->size()-off-offset );
return NULL;
}
// 12 bytes parsed = BIOP_MIN_HEADER
offset += objectKey( buf+offset, len-offset, loc.keyID );
DWORD objectKindLen = RDW(buf,offset);
if (objectKindLen != 4) {
printf( "[dsmcc::Object] Warning: Invalid object kind\n" );
return NULL;
}
// Chapter 11: The downloadID field of the DownloadDataBlock messages shall have the same value
// as the carouselID field of the U-U Object Carousel
loc.carouselID = module->downloadID();
loc.moduleID = module->id();
DWORD objectKind = RDW(buf,offset);
switch (objectKind) {
case BIOP_OBJECT_FILE:
obj = new File( loc );
break;
case BIOP_OBJECT_DIR:
case BIOP_OBJECT_GW:
obj = new Directory( loc );
break;
// case BIOP_OBJECT_STR:
// break;
case BIOP_OBJECT_STE:
obj = new StreamEvent( loc );
break;
default:
obj = NULL;
printf( "[dsmcc::Object] Warning: BIOP object kind %lx not processed\n", objectKind );
}
WORD objectInfoLen = RW(buf,offset);
// Update offset (27 bytes parsed = BIOP_MAX_HEADER)
off += offset;
if (obj && !obj->parse( resMgr, module, off, objectInfoLen )) {
delete obj;
obj = NULL;
}
return obj;
}
void parseDescriptors( const util::Buffer &info, desc::MapOfDescriptors &descriptors ) {
SIZE_T len = info.length();
BYTE *data = (BYTE *)info.buffer();
SSIZE_T offset = 0;
while (offset < len) {
BYTE dTag = RB(data,offset);
BYTE dLen = RB(data,offset);
SSIZE_T parsed = offset;
printf( "[dsmcc::module] Parse tag descriptor: tag=%x, len=%d\n", dTag, dLen );
switch (dTag) {
case MODULE_DESC_TYPE: // Type descriptor
{
std::string type( (char *)(data+parsed), dLen );
descriptors[dTag] = type;
parsed += dLen;
break;
}
case MODULE_DESC_NAME: // Name descriptor
{
std::string name( (char *)(data+parsed), dLen );
descriptors[dTag] = name;
parsed += dLen;
break;
}
case MODULE_DESC_INFO: // Info descriptor
{
module::InfoDescriptor desc;
desc.language = std::string( (char *)(data+parsed), 3 );
desc.text = std::string( (char *)(data+parsed+3), dLen-3 );
descriptors[dTag] = desc;
parsed += dLen;
break;
}
case MODULE_DESC_LINK: // Module link descriptor
{
module::LinkDescriptor desc;
desc.position = RB(data,parsed);
desc.moduleID = RW(data,parsed);
descriptors[dTag] = desc;
break;
}
case MODULE_DESC_CRC32: // CRC32 descriptor
{
DWORD crc = RDW(data,parsed);
descriptors[dTag] = crc;
break;
}
case MODULE_DESC_LOCATION: // Location descriptor
{
BYTE location = RB(data,parsed);
descriptors[dTag] = location;
break;
}
case MODULE_DESC_EST_DOWNLOAD: // Estimated download time descriptor
{
DWORD est = RDW(data,parsed);
descriptors[dTag] = est;
break;
}
case MODULE_DESC_COMPRESSION: // Compression type descriptor
case MODULE_DESC_COMPRESSED:
{
module::CompressionTypeDescriptor desc;
desc.type = RB(data,parsed);
desc.originalSize = RDW(data,parsed);
descriptors[dTag] = desc;
break;
}
};
offset += dLen;
if (offset != parsed) {
printf( "[dsmcc::module] Descriptor not parsed or parsed was incomplete: dTag=%x, len=%d, parsed=%ld\n",
dTag, dLen, parsed );
}
}
//printf( "[dsmcc::module] Descriptors parsed: %d\n", descriptors.size() );
}
// Initialization process
void EkfNode::init() {
/**************************************************************************
* Initialize firstRun, time, and frame names
**************************************************************************/
// Set first run to true for encoders. Once a message is received, this will
// be set to false.
firstRunEnc_ = true;
firstRunSys_ = true;
// Set Times
ros::Time currTime = ros::Time::now();
lastEncTime_ = currTime;
lastSysTime_ = currTime;
// Use the ROS parameter server to initilize parameters
if(!private_nh_.getParam("base_frame", base_frame_))
base_frame_ = "base_link";
if(!private_nh_.getParam("odom_frame", base_frame_))
odom_frame_ = "odom";
if(!private_nh_.getParam("map_frame", map_frame_))
map_frame_ = "map";
/**************************************************************************
* Initialize state for ekf_odom_ and ekf_map_
**************************************************************************/
// Create temp array to initialize state
double state_temp[] = {0, 0, 0, 0, 0, 0};
// And a std::vector, which will be used to initialize an Eigen Matrix
std::vector<double> state (state_temp, state_temp + sizeof(state_temp) / sizeof(double));
// Check the parameter server and initialize state
if(!private_nh_.getParam("state", state)) {
ROS_WARN_STREAM("No state found. Using default.");
}
// Check to see if the size is not equal to 6
if (state.size() != 6) {
ROS_WARN_STREAM("state isn't 6 elements long!");
}
// And initialize the Matrix
typedef Matrix<double, 6, 1> Vector6d;
Vector6d stateMat(state.data());
std::cout << "state_map = " << std::endl;
std::cout << stateMat << std::endl;
ekf_map_.initState(stateMat);
// Odom is always initialized at all zeros.
stateMat << 0, 0, 0, 0, 0, 0;
ekf_odom_.initState(stateMat);
std::cout << "state_odom = " << std::endl;
std::cout << stateMat << std::endl;
/**************************************************************************
* Initialize covariance for ekf_odom_ and ekf_map_
**************************************************************************/
// Create temp array to initialize covariance
double cov_temp[] = {0.01, 0, 0, 0, 0, 0,
0, 0.01, 0, 0, 0, 0,
0, 0, 0.01, 0, 0, 0,
0, 0, 0, 0.01, 0, 0,
0, 0, 0, 0, 0.01, 0,
0, 0, 0, 0, 0, 0.01};
// And a std::vector, which will be used to initialize an Eigen Matrix
std::vector<double> cov (cov_temp, cov_temp + sizeof(cov_temp) / sizeof(double));
// Check the parameter server and initialize cov
if(!private_nh_.getParam("covariance", cov)) {
ROS_WARN_STREAM("No covariance found. Using default.");
}
// Check to see if the size is not equal to 36
if (cov.size() != 36) {
ROS_WARN_STREAM("cov isn't 36 elements long!");
}
// And initialize the Matrix
typedef Matrix<double, 6, 6, RowMajor> Matrix66;
Matrix66 covMat(cov.data());
std::cout << "covariance = " << std::endl;
std::cout << covMat << std::endl;
ekf_map_.initCov(covMat);
// Initialize odom covariance the same as the map covariance (this isn't
// correct. But, since it is all an estimate anyway, it should be fine.
ekf_odom_.initCov(covMat);
/**************************************************************************
* Initialize Q for ekf_odom_ and ekf_map_
**************************************************************************/
// Create temp array to initialize Q
double Q_temp[] = {0.01, 0, 0, 0, 0, 0,
0, 0.01, 0, 0, 0, 0,
0, 0, 0.01, 0, 0, 0,
0, 0, 0, 0.01, 0, 0,
0, 0, 0, 0, 0.01, 0,
0, 0, 0, 0, 0, 0.01};
// And a std::vector, which will be used to initialize an Eigen Matrix
std::vector<double> Q (Q_temp, Q_temp + sizeof(Q_temp) / sizeof(double));
// Check the parameter server and initialize Q
if(!private_nh_.getParam("Q", Q)) {
ROS_WARN_STREAM("No Q found. Using default.");
}
// Check to see if the size is not equal to 36
if (Q.size() != 36) {
ROS_WARN_STREAM("Q isn't 36 elements long!");
}
// And initialize the Matrix
Matrix66 QMat(Q.data());
std::cout << "Q = " << std::endl;
std::cout << QMat << std::endl;
ekf_map_.initSystem(QMat);
ekf_odom_.initSystem(QMat);
/**************************************************************************
* Initialize Decawave for ekf_map_ (not used in ekf_odom_)
**************************************************************************/
// Create temp array to initialize R for DecaWave
double RDW_temp[] = {0.01, 0, 0, 0,
0, 0.01, 0, 0,
0, 0, 0.01, 0,
0, 0, 0, 0.01};
// And a std::vector, which will be used to initialize an Eigen Matrix
std::vector<double> RDW (RDW_temp, RDW_temp + sizeof(RDW_temp) / sizeof(double));
// Check the parameter server and initialize RDW
if(!private_nh_.getParam("DW_R", RDW)) {
ROS_WARN_STREAM("No DW_R found. Using default.");
}
// Check to see if the size is not equal to 16
if (RDW.size() != 16) {
ROS_WARN_STREAM("DW_R isn't 16 elements long!");
}
// And initialize the Matrix
typedef Matrix<double, 4, 4, RowMajor> Matrix44;
Matrix44 RDWMat(RDW.data());
std::cout << "RDecaWave = " << std::endl;
std::cout << RDWMat << std::endl;
// Create temp array to initialize beacon locations for DecaWave
double DWBL_temp[] = {0, 0,
5, 0,
5, 15,
0, 15};
// And a std::vector, which will be used to initialize an Eigen Matrix
std::vector<double> DWBL (DWBL_temp, DWBL_temp + sizeof(DWBL_temp) / sizeof(double));
// Check the parameter server and initialize DWBL
if(!private_nh_.getParam("DW_Beacon_Loc", DWBL)) {
ROS_WARN_STREAM("No DW_Beacon_Loc found. Using default.");
}
// Check to see if the size is not equal to 8
if (DWBL.size() != 8) {
ROS_WARN_STREAM("DW_Beacon_Loc isn't 8 elements long!");
}
// And initialize the Matrix
typedef Matrix<double, 4, 2, RowMajor> Matrix42;
Matrix42 DWBLMat(DWBL.data());
std::cout << "DW_Beacon_Loc = " << std::endl;
std::cout << DWBLMat << std::endl;
MatrixXd DecaWaveBeaconLoc(4,2);
MatrixXd DecaWaveOffset(2,1);
double DW_offset_x;
double DW_offset_y;
if(!private_nh_.getParam("DW_offset_x", DW_offset_x))
DW_offset_x = 0.0;
if(!private_nh_.getParam("DW_offset_y", DW_offset_y))
DW_offset_y = 0.0;
DecaWaveOffset << DW_offset_x, DW_offset_y;
std::cout << "DecaWaveOffset = " << std::endl;
std::cout << DecaWaveOffset << std::endl;
ekf_map_.initDecaWave(RDWMat, DWBLMat, DecaWaveOffset);
// Decawave is not used in odom, so no need to initialize
/**************************************************************************
* Initialize Encoders for ekf_odom_ and ekf_map_
**************************************************************************/
// Create temp array to initialize R for DecaWave
double REnc_temp[] = {0.01, 0,
0, 0.01};
// And a std::vector, which will be used to initialize an Eigen Matrix
std::vector<double> REnc (REnc_temp, REnc_temp + sizeof(REnc_temp) / sizeof(double));
// Check the parameter server and initialize RDW
if(!private_nh_.getParam("Enc_R", REnc)) {
ROS_WARN_STREAM("No Enc_R found. Using default.");
}
// Check to see if the size is not equal to 4
if (REnc.size() != 4) {
ROS_WARN_STREAM("Enc_R isn't 4 elements long!");
}
// And initialize the Matrix
typedef Matrix<double, 2, 2, RowMajor> Matrix22;
Matrix22 REncMat(REnc.data());
std::cout << "R_Enc = " << std::endl;
std::cout << REncMat << std::endl;
double b, tpmRight, tpmLeft;
if(!private_nh_.getParam("track_width", b))
b = 1;
if(!private_nh_.getParam("tpm_right", tpmRight))
tpmRight = 1;
if(!private_nh_.getParam("tpm_left", tpmLeft))
tpmLeft = 1;
ekf_map_.initEnc(REncMat, b, tpmRight, tpmLeft);
ekf_odom_.initEnc(REncMat, b, tpmRight, tpmLeft);
std::cout << "track_width = " << b << std::endl;
std::cout << "tpm_right = " << tpmRight << std::endl;
std::cout << "tpm_left = " << tpmLeft << std::endl;
/**************************************************************************
* Initialize IMU for ekf_odom_ and ekf_map_
**************************************************************************/
double RIMU;
if(!private_nh_.getParam("R_IMU", RIMU))
RIMU = 0.01;
ekf_map_.initIMU(RIMU);
ekf_odom_.initIMU(RIMU);
std::cout << "R_IMU = " << RIMU << std::endl;
// Publish the initialized state and exit initialization.
publishState(currTime);
ROS_INFO_STREAM("Finished with initialization.");
}