00001
00045 #ifdef _MSC_VER
00046 #define _WIN32_WINNT 0x0501
00047 #pragma warning(disable: 4996)
00048 #pragma warning(disable: 4267)
00049 #endif
00050
00051 #ifndef _MSC_VER
00052
00053
00054 #endif
00055 #include <sick_scan/sick_scan_common_tcp.h>
00056 #include <sick_scan/sick_generic_parser.h>
00057 #include <sick_scan/sick_generic_radar.h>
00058 #include <sick_scan/RadarScan.h>
00059 #ifdef _MSC_VER
00060 #include "sick_scan/rosconsole_simu.hpp"
00061 #endif
00062 #define _USE_MATH_DEFINES
00063 #include <math.h>
00064 #include "string"
00065 #include <stdio.h>
00066 #include <stdlib.h>
00067
00068 namespace sick_scan
00069 {
00070 int16_t getShortValue(std::string str)
00071 {
00072 int val = 0;
00073 if (1 == sscanf(str.c_str(), "%x", &val))
00074 {
00075
00076 }
00077 else
00078 {
00079 ROS_WARN("Problems parsing %s\n", str.c_str());
00080 }
00081 return(val);
00082
00083 }
00084
00085 int getHexValue(std::string str)
00086 {
00087 int val = 0;
00088 if (1 == sscanf(str.c_str(), "%x", &val))
00089 {
00090
00091 }
00092 else
00093 {
00094 ROS_WARN("Problems parsing %s\n", str.c_str());
00095 }
00096 return(val);
00097
00098 }
00099
00100
00101 float convertScaledIntValue(int value, float scale, float offset)
00102 {
00103 float val = 0;
00104 val = (float)(value * scale + offset);
00105 return(val);
00106 }
00107
00108 float getFloatValue(std::string str)
00109 {
00110 float tmpVal = 0.0;
00111 unsigned char *ptr;
00112 ptr = (unsigned char *)(&tmpVal);
00113 int strLen = str.length();
00114 if (strLen < 8)
00115 {
00116 }
00117 else
00118 {
00119 for (int i = 0; i < 4; i++)
00120 {
00121 std::string dummyStr = "";
00122 dummyStr += str[i * 2];
00123 dummyStr += str[i * 2 + 1];
00124 int val = getHexValue(dummyStr);
00125 unsigned char ch = (0xFF & val);
00126 ptr[3 - i] = ch;
00127 }
00128 }
00129 return(tmpVal);
00130 }
00131
00132 void SickScanRadar::setEmulation(bool _emul)
00133 {
00134 emul = _emul;
00135 }
00136
00137 bool SickScanRadar::getEmulation(void)
00138 {
00139 return(emul);
00140 }
00141
00152 int SickScanRadar::parseAsciiDatagram(char* datagram, size_t datagram_length,
00153 sick_scan::RadarScan *msgPtr,
00154 std::vector<SickScanRadarObject> &objectList,
00155 std::vector<SickScanRadarRawTarget> &rawTargetList)
00156
00157 {
00158 int exitCode = ExitSuccess;
00159 ros::NodeHandle tmpParam("~");
00160 bool dumpData = false;
00161 int verboseLevel = 0;
00162 tmpParam.getParam("verboseLevel", verboseLevel);
00163
00164
00165
00166 int HEADER_FIELDS = 32;
00167 char* cur_field;
00168 size_t count;
00169
00170
00171 std::vector<char *> fields;
00172 fields.reserve(datagram_length / 2);
00173
00174
00175 std::vector<char> datagram_copy_vec;
00176 datagram_copy_vec.resize(datagram_length + 1);
00177 char* datagram_copy = &(datagram_copy_vec[0]);
00178
00179 if (verboseLevel > 0) {
00180 ROS_WARN("Verbose LEVEL activated. Only for DEBUG.");
00181 }
00182
00183 if (verboseLevel > 0)
00184 {
00185 static int cnt = 0;
00186 char szDumpFileName[255] = { 0 };
00187 char szDir[255] = { 0 };
00188 #ifdef _MSC_VER
00189 strcpy(szDir, "C:\\temp\\");
00190 #else
00191 strcpy(szDir, "/tmp/");
00192 #endif
00193 sprintf(szDumpFileName, "%stmp%06d.bin", szDir, cnt);
00194 FILE *ftmp;
00195 ftmp = fopen(szDumpFileName, "wb");
00196 if (ftmp != NULL)
00197 {
00198 fwrite(datagram, datagram_length, 1, ftmp);
00199 fclose(ftmp);
00200 }
00201 }
00202
00203 strncpy(datagram_copy, datagram, datagram_length);
00204 datagram_copy[datagram_length] = 0;
00205
00206
00207 count = 0;
00208 cur_field = strtok(datagram, " ");
00209
00210 while (cur_field != NULL)
00211 {
00212 fields.push_back(cur_field);
00213
00214 cur_field = strtok(NULL, " ");
00215 }
00216
00217
00218
00219 count = fields.size();
00220
00221
00222 if (verboseLevel > 0)
00223 {
00224 static int cnt = 0;
00225 char szDumpFileName[255] = { 0 };
00226 char szDir[255] = { 0 };
00227 #ifdef _MSC_VER
00228 strcpy(szDir, "C:\\temp\\");
00229 #else
00230 strcpy(szDir, "/tmp/");
00231 #endif
00232 sprintf(szDumpFileName, "%stmp%06d.txt", szDir, cnt);
00233 ROS_WARN("Verbose LEVEL activated. Only for DEBUG.");
00234 FILE *ftmp;
00235 ftmp = fopen(szDumpFileName, "w");
00236 if (ftmp != NULL)
00237 {
00238 int i;
00239 for (i = 0; i < count; i++)
00240 {
00241 fprintf(ftmp, "%3d: %s\n", i, fields[i]);
00242 }
00243 fclose(ftmp);
00244 }
00245 cnt++;
00246 }
00247
00248
00249 enum PREHEADER_TOKEN_SEQ {PREHEADER_TOKEN_SSN,
00250 PREHEADER_TOKEN_LMDRADARDATA,
00251 PREHEADER_TOKEN_UI_VERSION_NO,
00252 PREHEADER_TOKEN_UI_IDENT,
00253 PREHEADER_TOKEN_UDI_SERIAL_NO,
00254 PREHEADER_TOKEN_XB_STATE_0,
00255 PREHEADER_TOKEN_XB_STATE_1,
00256 PREHEADER_TOKEN_TELEGRAM_COUNT,
00257 PREHEADER_TOKEN_CYCLE_COUNT,
00258 PREHEADER_TOKEN_SYSTEM_COUNT_SCAN,
00259 PREHEADER_TOKEN_SYSTEM_COUNT_TRANSMIT,
00260 PREHEADER_TOKEN_XB_INPUTS_0,
00261 PREHEADER_TOKEN_XB_INPUTS_1,
00262 PREHEADER_TOKEN_XB_OUTPUTS_0,
00263 PREHEADER_TOKEN_XB_OUTPUTS_1,
00264 PREHEADER_TOKEN_CYCLE_DURATION,
00265 PREHEADER_TOKEN_NOISE_LEVEL,
00266 PREHEADER_NUM_ENCODER_BLOCKS,
00267 PREHADERR_TOKEN_FIX_NUM};
00268
00269
00270
00271
00272
00273
00274
00275
00276
00277
00278
00279
00280
00281
00282
00283
00284
00285
00286
00287
00288
00289
00290
00291
00292
00293
00294
00295
00296
00297
00298
00299
00300
00301
00302
00303
00304
00305 for (int i = 0; i < PREHADERR_TOKEN_FIX_NUM; i++)
00306 {
00307 UINT16 uiValue = 0x00;
00308 UINT32 udiValue = 0x00;
00309 unsigned long int uliDummy;
00310 uliDummy = strtoul(fields[i], NULL, 16);
00311 switch(i)
00312 {
00313 case PREHEADER_TOKEN_UI_VERSION_NO:
00314 msgPtr->radarPreHeader.uiVersionNo = (UINT16)(uliDummy & 0xFFFF);
00315 break;
00316 case PREHEADER_TOKEN_UI_IDENT:
00317 msgPtr->radarPreHeader.radarPreHeaderDeviceBlock.uiIdent = (UINT16)(uliDummy & 0xFFFF);
00318 break;
00319 case PREHEADER_TOKEN_UDI_SERIAL_NO:
00320 msgPtr->radarPreHeader.radarPreHeaderDeviceBlock.udiSerialNo = (UINT32)(uliDummy & 0xFFFFFFFF);
00321 break;
00322 case PREHEADER_TOKEN_XB_STATE_0:
00323 for (int j = 0; j < 3; j++)
00324 {
00325 bool flag = false;
00326 if (0 != (uliDummy & (1 << j)))
00327 {
00328 flag = true;
00329 }
00330 switch (j)
00331 {
00332 case 0: msgPtr->radarPreHeader.radarPreHeaderDeviceBlock.bDeviceError = flag; break;
00333 case 1: msgPtr->radarPreHeader.radarPreHeaderDeviceBlock.bContaminationWarning = flag; break;
00334 case 2: msgPtr->radarPreHeader.radarPreHeaderDeviceBlock.bContaminationError = flag; break;
00335 default: ROS_WARN("Flag parsing for this PREHEADER-Flag not implemented");
00336 }
00337 }
00338 break;
00339 case PREHEADER_TOKEN_XB_STATE_1:
00340
00341 break;
00342 case PREHEADER_TOKEN_TELEGRAM_COUNT:
00343 msgPtr->radarPreHeader.radarPreHeaderStatusBlock.uiTelegramCount = (UINT16)(uliDummy & 0xFFFF);
00344 break;
00345 case PREHEADER_TOKEN_CYCLE_COUNT:
00346 sscanf(fields[i],"%hu", &uiValue);
00347 msgPtr->radarPreHeader.radarPreHeaderStatusBlock.uiCycleCount = (UINT16)(uliDummy & 0xFFFF);
00348 break;
00349 case PREHEADER_TOKEN_SYSTEM_COUNT_SCAN:
00350 msgPtr->radarPreHeader.radarPreHeaderStatusBlock.udiSystemCountScan = (UINT32)(uliDummy & 0xFFFFFFFF);
00351 break;
00352 case PREHEADER_TOKEN_SYSTEM_COUNT_TRANSMIT:
00353 msgPtr->radarPreHeader.radarPreHeaderStatusBlock.udiSystemCountTransmit = (UINT32)(uliDummy & 0xFFFFFFFF);
00354 break;
00355 case PREHEADER_TOKEN_XB_INPUTS_0:
00356 msgPtr->radarPreHeader.radarPreHeaderStatusBlock.uiInputs = (UINT8)(uliDummy & 0xFF);;
00357 msgPtr->radarPreHeader.radarPreHeaderStatusBlock.uiInputs <<= 8;
00358 break;
00359 case PREHEADER_TOKEN_XB_INPUTS_1:
00360 msgPtr->radarPreHeader.radarPreHeaderStatusBlock.uiInputs |= (UINT8)(uliDummy & 0xFF);;
00361 break;
00362 case PREHEADER_TOKEN_XB_OUTPUTS_0:
00363 msgPtr->radarPreHeader.radarPreHeaderStatusBlock.uiOutputs = (UINT8)(uliDummy & 0xFF);;
00364 msgPtr->radarPreHeader.radarPreHeaderStatusBlock.uiOutputs <<= 8;
00365 break;
00366 case PREHEADER_TOKEN_XB_OUTPUTS_1:
00367 msgPtr->radarPreHeader.radarPreHeaderStatusBlock.uiOutputs |= (UINT8)(uliDummy & 0xFF);;
00368 break;
00369 case PREHEADER_TOKEN_CYCLE_DURATION:
00370 msgPtr->radarPreHeader.radarPreHeaderMeasurementParam1Block.uiCycleDuration = (UINT16)(uliDummy & 0xFFFF);
00371 break;
00372 case PREHEADER_TOKEN_NOISE_LEVEL:
00373 msgPtr->radarPreHeader.radarPreHeaderMeasurementParam1Block.uiNoiseLevel = (UINT16)(uliDummy & 0xFFFF);
00374 break;
00375 case PREHEADER_NUM_ENCODER_BLOCKS:
00376 {
00377 UINT16 u16NumberOfBlock = (UINT16)(uliDummy & 0xFFFF);
00378
00379 if (u16NumberOfBlock > 0)
00380 {
00381 msgPtr->radarPreHeader.radarPreHeaderArrayEncoderBlock.resize(u16NumberOfBlock);
00382
00383 for (int j = 0; j < u16NumberOfBlock; j++)
00384 {
00385 INT16 iEncoderSpeed;
00386 int rowIndex = PREHEADER_NUM_ENCODER_BLOCKS + j * 2 + 1;
00387 udiValue = strtoul(fields[rowIndex], NULL, 16);
00388 msgPtr->radarPreHeader.radarPreHeaderArrayEncoderBlock[j].udiEncoderPos = udiValue;
00389 udiValue = strtoul(fields[rowIndex+1], NULL, 16);
00390 iEncoderSpeed = (int)udiValue;
00391 msgPtr->radarPreHeader.radarPreHeaderArrayEncoderBlock[j].iEncoderSpeed = iEncoderSpeed;
00392
00393 }
00394 }
00395 }
00396 break;
00397 }
00398 }
00399
00400
00401
00402
00403
00404
00405
00406
00407 std::vector<std::string> keyWordList;
00408 #define DIST1_KEYWORD "DIST1"
00409 #define AZMT1_KEYWORD "AZMT1"
00410 #define VRAD1_KEYWORD "VRAD1"
00411 #define AMPL1_KEYWORD "AMPL1"
00412 #define MODE1_KEYWORD "MODE1"
00413
00414 #define P3DX1_KEYWORD "P3DX1"
00415 #define P3DY1_KEYWORD "P3DY1"
00416 #define V3DX1_KEYWORD "V3DX1"
00417 #define V3DY1_KEYWORD "V3DY1"
00418 #define OBJLEN_KEYWORD "OBLE1"
00419 #define OBJID_KEYWORD "OBID1"
00420
00421 const int RAWTARGET_LOOP = 0;
00422 const int OBJECT_LOOP = 1;
00423
00424
00425
00426
00427 for (int iLoop = 0; iLoop < 2; iLoop++)
00428 {
00429 keyWordList.clear();
00430 switch (iLoop)
00431 {
00432 case RAWTARGET_LOOP:
00433 keyWordList.push_back(DIST1_KEYWORD);
00434 keyWordList.push_back(AZMT1_KEYWORD);
00435 keyWordList.push_back(VRAD1_KEYWORD);
00436 keyWordList.push_back(AMPL1_KEYWORD);
00437 keyWordList.push_back(MODE1_KEYWORD);
00438 break;
00439 case OBJECT_LOOP:
00440 keyWordList.push_back(P3DX1_KEYWORD);
00441 keyWordList.push_back(P3DY1_KEYWORD);
00442 keyWordList.push_back(V3DX1_KEYWORD);
00443 keyWordList.push_back(V3DY1_KEYWORD);
00444 keyWordList.push_back(OBJLEN_KEYWORD);
00445 keyWordList.push_back(OBJID_KEYWORD);
00446 break;
00447 }
00448 std::vector<int> keyWordPos;
00449 std::vector<float> keyWordScale;
00450 std::vector<float> keyWordScaleOffset;
00451 keyWordPos.resize(keyWordList.size());
00452 keyWordScale.resize(keyWordList.size());
00453 keyWordScaleOffset.resize(keyWordList.size());
00454 for (int i = 0; i < keyWordPos.size(); i++)
00455 {
00456 keyWordPos[i] = -1;
00457 }
00458 int numKeyWords = keyWordPos.size();
00459 for (int i = 0; i < fields.size(); i++)
00460 {
00461 for (int j = 0; j < keyWordList.size(); j++)
00462 {
00463 if (strcmp(fields[i], keyWordList[j].c_str()) == 0)
00464 {
00465 keyWordPos[j] = i;
00466 }
00467 }
00468 }
00469
00470 bool entriesNumOk = true;
00471 int entriesNum = 0;
00472 if (keyWordPos[0] == -1)
00473 {
00474 entriesNumOk = false;
00475 }
00476 else
00477 {
00478
00479 entriesNum = getHexValue(fields[keyWordPos[0] + 3]);
00480 for (int i = 0; i < numKeyWords; i++)
00481 {
00482 if (keyWordPos[i] == -1)
00483 {
00484 entriesNumOk = false;
00485 ROS_WARN("Missing keyword %s but first keyword found.\n", keyWordList[i].c_str());
00486 entriesNumOk = false;
00487 }
00488 else
00489 {
00490 int entriesNumTmp = getHexValue(fields[keyWordPos[i] + 3]);
00491 if (entriesNumTmp != entriesNum)
00492 {
00493 ROS_WARN("Number of items for keyword %s differs from number of items for %s\n.",
00494 keyWordList[i].c_str(), keyWordList[0].c_str());
00495 entriesNumOk = false;
00496 }
00497 }
00498 }
00499 }
00500
00501 if (true == entriesNumOk)
00502 {
00503
00504
00505 for (int i = 0; i < numKeyWords; i++)
00506 {
00507 int scaleLineIdx = keyWordPos[i] + 1;
00508 int scaleOffsetLineIdx = keyWordPos[i] + 2;
00509 std::string token = fields[scaleLineIdx];
00510 keyWordScale[i] = getFloatValue(token);
00511 token = fields[scaleOffsetLineIdx];
00512 keyWordScaleOffset[i] = getFloatValue(token);
00513
00514 }
00515
00516 switch (iLoop)
00517 {
00518 case RAWTARGET_LOOP:
00519 {
00520 rawTargetList.resize(entriesNum);
00521 break;
00522 }
00523 case OBJECT_LOOP:
00524 {
00525 objectList.resize(entriesNum);
00526 break;
00527 }
00528 }
00529 for (int i = 0; i < entriesNum; i++)
00530 {
00531 switch (iLoop)
00532 {
00533 case RAWTARGET_LOOP:
00534 {
00535 float dist = 0.0;
00536 float azimuth = 0.0;
00537 float ampl = 0.0;
00538 float vrad = 0.0;
00539 int mode = 0;
00540 for (int j = 0; j < numKeyWords; j++)
00541 {
00542 int dataRowIdx = keyWordPos[j] + 4 + i;
00543 std::string token = keyWordList[j];
00544 if (token.compare(DIST1_KEYWORD) == 0)
00545 {
00546 int distRaw = getHexValue(fields[dataRowIdx]);
00547 dist = convertScaledIntValue(distRaw, keyWordScale[j], keyWordScaleOffset[j]) * 0.001;
00548 }
00549 if (token.compare(AZMT1_KEYWORD) == 0)
00550 {
00551 int azimuthRaw = getShortValue(fields[dataRowIdx]);
00552 azimuth = (float)convertScaledIntValue(azimuthRaw, keyWordScale[j], keyWordScaleOffset[j]);
00553 }
00554 if (token.compare(VRAD1_KEYWORD) == 0)
00555 {
00556 int vradRaw = getShortValue(fields[dataRowIdx]);
00557 vrad = (float)convertScaledIntValue(vradRaw, keyWordScale[j], keyWordScaleOffset[j]);
00558 }
00559 if (token.compare(MODE1_KEYWORD) == 0)
00560 {
00561 int modeRaw = getHexValue(fields[dataRowIdx]);
00562 mode = (int)(convertScaledIntValue(modeRaw, keyWordScale[j], keyWordScaleOffset[j]) + 0.5);
00563 }
00564 if (token.compare(AMPL1_KEYWORD) == 0)
00565 {
00566 int amplRaw = getShortValue(fields[dataRowIdx]);
00567 ampl = (int)(convertScaledIntValue(amplRaw, keyWordScale[j], keyWordScaleOffset[j]) + 0.5);
00568 }
00569 }
00570 rawTargetList[i].Dist(dist);
00571 rawTargetList[i].Ampl(ampl);
00572 rawTargetList[i].Azimuth(azimuth);
00573 rawTargetList[i].Mode(mode);
00574 rawTargetList[i].Vrad(vrad);
00575
00576 }
00577 break;
00578 case OBJECT_LOOP:
00579 {
00580 float px = 0.0;
00581 float py = 0.0;
00582 float vx = 0.0;
00583 float vy = 0.0;
00584 float objLen = 0.0;
00585 int objId = 0;
00586
00587 for (int j = 0; j < numKeyWords; j++)
00588 {
00589 int dataRowIdx = keyWordPos[j] + 4 + i;
00590 std::string token = keyWordList[j];
00591 int intVal = getShortValue(fields[dataRowIdx]);
00592 float val = convertScaledIntValue(intVal, keyWordScale[j], keyWordScaleOffset[j]);
00593
00594 if (token.compare(P3DX1_KEYWORD) == 0)
00595 {
00596 px = val * 0.001f;
00597 }
00598 if (token.compare(P3DY1_KEYWORD) == 0)
00599 {
00600 py = val * 0.001f;
00601 }
00602 if (token.compare(V3DX1_KEYWORD) == 0)
00603 {
00604 vx = val;
00605 }
00606 if (token.compare(V3DY1_KEYWORD) == 0)
00607 {
00608 vy = val;
00609
00610 }
00611 if (token.compare(OBJLEN_KEYWORD) == 0)
00612 {
00613 objLen = val;
00614 }
00615 if (token.compare(OBJID_KEYWORD) == 0)
00616 {
00617 int objIdRaw = getHexValue(fields[dataRowIdx]);
00618 objId = (int)(objIdRaw * keyWordScale[j] + 0.5);
00619 }
00620 }
00621
00622 objectList[i].ObjId(objId);
00623 objectList[i].ObjLength(objLen);
00624 objectList[i].P3Dx(px);
00625 objectList[i].P3Dy(py);
00626 objectList[i].V3Dx(vx);
00627 objectList[i].V3Dy(vy);
00628 }
00629 break;
00630 }
00631 }
00632 }
00633
00634 }
00635
00636 return(exitCode);
00637 }
00638
00639 void SickScanRadar::simulateAsciiDatagram(unsigned char * receiveBuffer, int* actual_length)
00640 {
00641 static int callCnt = 0;
00642
00643 callCnt++;
00644
00645
00646 std::string header = "\x2sSN LMDradardata 1 1 112F6E9 0 0 DFB6 B055 6E596002 6E5AE0E5 0 0 0 0 0 0 1 0 0 ";
00647
00648 int channel16BitCnt = 4;
00649
00650 float rawTargetFactorList[] = { 40.0f, 0.16f, 0.04f, 1.00f, 1.00f };
00651 float objectFactorList[] = { 64.0f, 64.0f, 0.1f, 0.1f, 0.75f, 1.0f };
00652
00653 std::string dist1_intro = "DIST1 42200000 00000000";
00654 std::string azmt1_intro = "AZMT1 3E23D70A 00000000";
00655 std::string vrad1_intro = "VRAD1 3D23D70A 00000000";
00656 std::string ampl1_intro = "AMPL1 3F800000 00000000";
00657
00658 std::string pdx1_intro = "P3DX1 42800000 00000000";
00659 std::string pdy1_intro = "P3DY1 42800000 00000000";
00660 std::string v3dx_intro = "V3DX1 3DCCCCCD 00000000";
00661 std::string v3dy_intro = "V3DY1 3DCCCCCD 00000000";
00662 std::string oblen_intro = "OBLE1 3F400000 00000000";
00663
00664 int rawTargetChannel16BitCnt = 4;
00665 int rawTargetChannel8BitCnt = 1;
00666 std::string mode1_intro = "MODE1 3F800000 00000000";
00667
00668 std::string obid_intro = "OBID1 3F800000 00000000";
00669
00670
00671
00672 std::string trailer = "0 0 0 0 0 0\x3";
00673
00674 std::vector<std::string> channel16BitID;
00675 std::vector<std::string> channel8BitID;
00676 channel16BitID.push_back(dist1_intro);
00677 channel16BitID.push_back(azmt1_intro);
00678 channel16BitID.push_back(vrad1_intro);
00679 channel16BitID.push_back(ampl1_intro);
00680
00681 channel16BitID.push_back(pdx1_intro);
00682 channel16BitID.push_back(pdy1_intro);
00683 channel16BitID.push_back(v3dx_intro);
00684 channel16BitID.push_back(v3dy_intro);
00685 channel16BitID.push_back(oblen_intro);
00686
00687
00688
00689 channel8BitID.push_back(mode1_intro);
00690 channel8BitID.push_back(obid_intro);
00691
00692 int channel8BitCnt = channel8BitID.size();
00693 int objectChannel16BitCnt = 5;
00694 channel16BitCnt = channel16BitID.size();
00695 float x = 20.0;
00696 float speed = 50.0;
00697 std::vector<SickScanRadarRawTarget> rawTargetList;
00698
00699 #if 0 // simulate railway crossing
00700 for (float y = -20; y <= 20.0; y += 2.0)
00701 {
00702 SickScanRadarRawTarget rawTarget;
00703 float azimuth = atan2(y, x);
00704 float dist = sqrt(x*x + y*y);
00705 float vrad = speed * sin(azimuth);
00706 float mode = 0;
00707 float ampl = 50.0 + y;
00708 rawTarget.Ampl(ampl);
00709 rawTarget.Mode(mode);
00710 rawTarget.Dist(dist);
00711 rawTarget.Azimuth(azimuth);
00712 rawTarget.Vrad(vrad);
00713 rawTargetList.push_back(rawTarget);
00714 }
00715 #endif
00716
00717 std::vector<SickScanRadarObject> objectList;
00718
00719 int objId = 0;
00720 for (float x = 20; x <= 100.0; x += 50.0)
00721 {
00722 SickScanRadarObject vehicle;
00723 float y = 0.0;
00724 for (int iY = -1; iY <= 1; iY += 2)
00725 {
00726 float xp[2] = {0};
00727 float yp[2] = {0};
00728 float vehicleWidth = 1.8;
00729 y = iY * 2.0;
00730 float speed = y * 10.0f;
00731 vehicle.V3Dx(speed);
00732 vehicle.V3Dy(0.1f);
00733
00734 float xOff = 20.0;
00735 if (speed < 0.0)
00736 {
00737 xOff = 100.0;
00738 }
00739 vehicle.P3Dx((xOff + 0.1 * speed * (callCnt % 20)) * 1000.0);
00740 vehicle.P3Dy(y * 1000.0);
00741 float objLen = 6.0f + y;
00742 vehicle.ObjLength(objLen);
00743
00744 vehicle.ObjId(objId++);
00745 objectList.push_back(vehicle);
00746
00747 for (int i = 0; i < 2; i++)
00748 {
00749 SickScanRadarRawTarget rawTarget;
00750
00751 xp[i] = vehicle.P3Dx() * 0.001;
00752 yp[i] = vehicle.P3Dy() * 0.001;
00753
00754 if (i == 0)
00755 {
00756 yp[i] -= vehicleWidth/2.0;
00757 }
00758 else
00759 {
00760 yp[i] += vehicleWidth/2.0;
00761 }
00762 if (speed < 0.0)
00763 {
00764 xp[i] -= objLen * 0.5;
00765 }
00766 else
00767 {
00768 xp[i] += objLen * 0.5;
00769 }
00770
00771 float azimuth = atan2(yp[i], xp[i]);
00772 float dist = sqrt(xp[i]*xp[i] + yp[i]*yp[i]);
00773 float vrad = speed * cos(azimuth);
00774 float mode = 0;
00775 float ampl = 50.0;
00776
00777 rawTarget.Ampl(ampl);
00778 rawTarget.Mode(mode);
00779 rawTarget.Dist(dist);
00780 rawTarget.Azimuth(azimuth);
00781 rawTarget.Vrad(vrad);
00782 rawTargetList.push_back(rawTarget);
00783
00784 }
00785
00786 }
00787
00788
00789 }
00790
00791 char szDummy[255] = { 0 };
00792 std::string resultStr;
00793 resultStr += header;
00794 sprintf(szDummy, "%x ", channel16BitCnt);
00795 resultStr += szDummy;
00796 for (int i = 0; i < channel16BitCnt; i++)
00797 {
00798 resultStr += channel16BitID[i];
00799 int valNum = rawTargetList.size();
00800 bool processRawTarget = true;
00801 if (i < rawTargetChannel16BitCnt)
00802 {
00803 valNum = rawTargetList.size();
00804 }
00805 else
00806 {
00807 processRawTarget = false;
00808 valNum = objectList.size();
00809 }
00810
00811 sprintf(szDummy, " %x ", valNum);
00812 resultStr += szDummy;
00813 float val = 0.0;
00814 for (int j = 0; j < valNum; j++)
00815 {
00816 switch (i)
00817 {
00818 case 0: val = 1000.0 * rawTargetList[j].Dist(); break;
00819 case 1: val = 1.0 / deg2rad * rawTargetList[j].Azimuth(); break;
00820 case 2: val = rawTargetList[j].Vrad(); break;
00821 case 3: val = rawTargetList[j].Ampl(); break;
00822 case 4: val = objectList[j].P3Dx(); break;
00823 case 5: val = objectList[j].P3Dy(); break;
00824 case 6: val = objectList[j].V3Dx(); break;
00825 case 7: val = objectList[j].V3Dy(); break;
00826 case 8: val = objectList[j].ObjLength(); break;
00827 }
00828
00829
00830 if (processRawTarget)
00831 {
00832 val /= rawTargetFactorList[i];
00833 }
00834 else
00835 {
00836 int idx = i - rawTargetChannel16BitCnt;
00837 val /= objectFactorList[idx];
00838 }
00839
00840 if (val > 0.0)
00841 {
00842 val += 0.5;
00843 }
00844 else
00845 {
00846 val -= 0.5;
00847 }
00848 int16_t shortVal = (int16_t)(val);
00849
00850 sprintf(szDummy, "%08x", shortVal);
00851 strcpy(szDummy, szDummy + 4);
00852 resultStr += szDummy;
00853 resultStr += " ";
00854 }
00855 }
00856
00857 sprintf(szDummy, "%x ", channel8BitCnt);
00858 resultStr += szDummy;
00859 int valNum = rawTargetList.size();
00860 for (int i = 0; i < channel8BitCnt; i++)
00861 {
00862 resultStr += channel8BitID[i];
00863 float val = 0.0;
00864 bool processRawTarget = true;
00865 if (i < rawTargetChannel8BitCnt)
00866 {
00867 valNum = rawTargetList.size();
00868 }
00869 else
00870 {
00871 processRawTarget = false;
00872 valNum = objectList.size();
00873 }
00874 sprintf(szDummy, " %x ", valNum);
00875 resultStr += szDummy;
00876 for (int j = 0; j < valNum; j++)
00877 {
00878 switch (i)
00879 {
00880 case 0: val = rawTargetList[j].Mode(); break;
00881 case 1: val = objectList[j].ObjId(); break;
00882 }
00883
00884 int offset = 0;
00885 if (processRawTarget)
00886 {
00887 offset = rawTargetChannel16BitCnt;
00888 val /= rawTargetFactorList[i + offset];
00889 }
00890 else
00891 {
00892 offset = objectChannel16BitCnt;
00893 int idx = i - rawTargetChannel8BitCnt;
00894 val /= objectFactorList[idx + offset];
00895 }
00896 if (val > 0.0)
00897 {
00898 val += 0.5;
00899 }
00900 else
00901 {
00902 val -= 0.5;
00903 }
00904 int8_t shortVal = (int16_t)(val);
00905
00906 sprintf(szDummy, "%08x", shortVal);
00907 strcpy(szDummy, szDummy + 6);
00908 resultStr += szDummy;
00909 resultStr += " ";
00910 }
00911 }
00912
00913 resultStr += trailer;
00914
00915 *actual_length = resultStr.length();
00916 strcpy((char *)receiveBuffer, resultStr.c_str());
00917 }
00918
00919 void SickScanRadar::simulateAsciiDatagramFromFile(unsigned char *receiveBuffer, int *pActual_length,
00920 std::string filePattern)
00921 {
00922 static int callCnt = 0;
00923 FILE *fin;
00924 char szLine[1024] = { 0 };
00925 char szDummyWord[1024] = { 0 };
00926 int actual_length = 0;
00927 int cnt = 0;
00928 char szFileName[1024] = {0};
00929
00930 receiveBuffer[0] = '\x02';
00931 actual_length++;
00932
00933 for (int iLoop = 0; iLoop < 2; iLoop++)
00934 {
00935 sprintf(szFileName,filePattern.c_str(), callCnt);
00936 callCnt++;
00937 fin = fopen(szFileName, "r");
00938
00939 if ( (fin == NULL) && (iLoop == 0))
00940 {
00941 callCnt = 0;
00942 }
00943 else
00944 {
00945 break;
00946 }
00947
00948 if ((iLoop == 1) && (fin == NULL))
00949 {
00950 ROS_ERROR("Can not find simulation file corresponding to pattern %s", filePattern.c_str());
00951 }
00952 }
00953
00954 while (fgets(szLine, 1024, fin) != NULL)
00955 {
00956 char *ptr = strchr(szLine, '\n');;
00957 if (ptr != NULL)
00958 {
00959 *ptr = '\0';
00960 }
00961
00962 ptr = strchr(szLine, ':');
00963 if (ptr != NULL)
00964 {
00965 if (1 == sscanf(ptr + 2, "%s", szDummyWord))
00966 {
00967 if (cnt > 0)
00968 {
00969 receiveBuffer[actual_length++] = ' ';
00970 }
00971 strcpy((char *)receiveBuffer + actual_length, szDummyWord);
00972 actual_length += strlen(szDummyWord);
00973 }
00974 cnt++;
00975 }
00976 }
00977 receiveBuffer[actual_length] = (char)'\x03';
00978 actual_length++;
00979 receiveBuffer[actual_length] = (char)'\x00';
00980 actual_length++;
00981 *pActual_length = actual_length;
00982 fclose(fin);
00983 }
00984
00985 int SickScanRadar::parseDatagram(ros::Time timeStamp, unsigned char *receiveBuffer, int actual_length, bool useBinaryProtocol)
00986 {
00987 int exitCode = ExitSuccess;
00988
00989 enum enumSimulationMode {EMULATE_OFF, EMULATE_SYN, EMULATE_FROM_FILE_TRAIN, EMULATE_FROM_FILE_CAR};
00990
00991 int simulationMode = EMULATE_OFF;
00992
00993 if (this->getEmulation())
00994 {
00995 simulationMode = EMULATE_SYN;
00996
00997 }
00998 switch(simulationMode)
00999 {
01000 case EMULATE_OFF:
01001 break;
01002 case EMULATE_SYN: simulateAsciiDatagram(receiveBuffer, &actual_length);
01003 break;
01004 case EMULATE_FROM_FILE_TRAIN: simulateAsciiDatagramFromFile(receiveBuffer, &actual_length, "/mnt/hgfs/development/ros/bags/raw/trainSeq/tmp%06d.txt");
01005 break;
01006 case EMULATE_FROM_FILE_CAR: simulateAsciiDatagramFromFile(receiveBuffer, &actual_length, "/mnt/hgfs/development/ros/bags/raw/carSeq/tmp%06d.txt");
01007 break;
01008
01009 default:
01010 printf("Simulation Mode unknown\n");
01011
01012 }
01013
01014 sensor_msgs::PointCloud2 cloud_;
01015 sick_scan::RadarScan radarMsg_;
01016
01017 std::vector<SickScanRadarObject> objectList;
01018 std::vector<SickScanRadarRawTarget> rawTargetList;
01019
01020 if (useBinaryProtocol)
01021 {
01022 throw std::logic_error("Binary protocol currently not supported.");
01023 }
01024 else
01025 {
01026 bool dataToProcess = false;
01027 char *buffer_pos = (char *)receiveBuffer;
01028 char *dstart = NULL;
01029 char *dend = NULL;
01030 int dlength = 0;
01031 dstart = strchr(buffer_pos, 0x02);
01032 if (dstart != NULL)
01033 {
01034 dend = strchr(dstart + 1, 0x03);
01035 }
01036 if ((dstart != NULL) && (dend != NULL))
01037 {
01038 dataToProcess = true;
01039 dlength = dend - dstart;
01040 *dend = '\0';
01041 dstart++;
01042 parseAsciiDatagram(dstart, dlength, &radarMsg_, objectList, rawTargetList);
01043 }
01044 else
01045 {
01046 dataToProcess = false;
01047 }
01048
01049
01050
01051
01052 enum RADAR_PROC_LIST {RADAR_PROC_RAW_TARGET, RADAR_PROC_TRACK, RADAR_PROC_NUM};
01053
01054
01055
01056 for (int iLoop = 0; iLoop < RADAR_PROC_NUM; iLoop++)
01057 {
01058 int numTargets = 0;
01059 if (dataToProcess)
01060 {
01061 std::string channelRawTargetId[] = { "x", "y", "z", "vrad","amplitude" };
01062 std::string channelObjectId[] = { "x", "y", "z", "vx","vy","vz","objLen","objId" };
01063 std::vector<std::string> channelList;
01064 std::string frameId = "radar";
01065 switch (iLoop)
01066 {
01067 case RADAR_PROC_RAW_TARGET: numTargets = rawTargetList.size();
01068 for (int i = 0; i < sizeof(channelRawTargetId) / sizeof(channelRawTargetId[0]); i++)
01069 {
01070 channelList.push_back(channelRawTargetId[i]);
01071 }
01072 frameId = "radar";
01073 break;
01074 case RADAR_PROC_TRACK: numTargets = objectList.size();
01075 for (int i = 0; i < sizeof(channelObjectId) / sizeof(channelObjectId[0]); i++)
01076 {
01077 channelList.push_back(channelObjectId[i]);
01078 }
01079 frameId = "radar";
01080 break;
01081 }
01082 if (numTargets == 0)
01083 {
01084 continue;
01085 }
01086 int numChannels = channelList.size();
01087
01088 std::vector<float> valSingle;
01089 valSingle.resize(numChannels);
01090 cloud_.header.stamp = timeStamp;
01091 cloud_.header.frame_id = frameId;
01092 cloud_.header.seq = 0;
01093 cloud_.height = 1;
01094 cloud_.width = numTargets;
01095 cloud_.is_bigendian = false;
01096 cloud_.is_dense = true;
01097 cloud_.point_step = numChannels * sizeof(float);
01098 cloud_.row_step = cloud_.point_step * cloud_.width;
01099 cloud_.fields.resize(numChannels);
01100 for (int i = 0; i < numChannels; i++) {
01101 cloud_.fields[i].name = channelList[i];
01102 cloud_.fields[i].offset = i * sizeof(float);
01103 cloud_.fields[i].count = 1;
01104 cloud_.fields[i].datatype = sensor_msgs::PointField::FLOAT32;
01105 }
01106
01107 cloud_.data.resize(cloud_.row_step * cloud_.height);
01108 float *valPtr = (float *)(&(cloud_.data[0]));
01109 int off = 0;
01110 for (int i = 0; i < numTargets; i++)
01111 {
01112 switch (iLoop)
01113 {
01114 case 0:
01115 {
01116 float angle = deg2rad * rawTargetList[i].Azimuth();
01117 valSingle[0] = rawTargetList[i].Dist() * cos(angle);
01118 valSingle[1] = rawTargetList[i].Dist() * sin(angle);
01119 valSingle[2] = 0.0;
01120 valSingle[3] = rawTargetList[i].Vrad();
01121 valSingle[4] = rawTargetList[i].Ampl();
01122 }
01123 break;
01124
01125 case 1:
01126 valSingle[0] = objectList[i].P3Dx();
01127 valSingle[1] = objectList[i].P3Dy();
01128 valSingle[2] = 0.0;
01129 valSingle[3] = objectList[i].V3Dx();
01130 valSingle[4] = objectList[i].V3Dy();
01131 valSingle[5] = 0.0;
01132 valSingle[6] = objectList[i].ObjLength();
01133 valSingle[7] = objectList[i].ObjId();
01134 break;
01135 }
01136
01137 for (int j = 0; j < numChannels; j++)
01138 {
01139 valPtr[off] = valSingle[j];
01140 off++;
01141 }
01142 #ifndef _MSC_VER
01143 #if 0 // just for debugging
01144 switch (iLoop)
01145 {
01146 case RADAR_PROC_RAW_TARGET:
01147 this->commonPtr->cloud_radar_rawtarget_pub_.publish(cloud_);
01148 break;
01149 case RADAR_PROC_TRACK:
01150 this->commonPtr->cloud_radar_track_pub_.publish(cloud_);
01151 break;
01152 }
01153 #endif
01154 #else
01155 printf("PUBLISH:\n");
01156 #endif
01157 if (iLoop == RADAR_PROC_RAW_TARGET)
01158 {
01159
01160 radarMsg_.targets = cloud_;
01161 }
01162 }
01163 }
01164 }
01165
01166
01167 radarMsg_.header.stamp = timeStamp;
01168 radarMsg_.header.frame_id = "radar";
01169 radarMsg_.header.seq = 0;
01170
01171 radarMsg_.objects.resize(objectList.size());
01172 for (int i = 0; i < radarMsg_.objects.size(); i++)
01173 {
01174 float vx = objectList[i].V3Dx();
01175 float vy = objectList[i].V3Dy();
01176 float v = sqrt(vx*vx + vy *vy);
01177 float heading = atan2( objectList[i].V3Dy(), objectList[i].V3Dx());
01178
01179 radarMsg_.objects[i].velocity.twist.linear.x = objectList[i].V3Dx();
01180 radarMsg_.objects[i].velocity.twist.linear.y = objectList[i].V3Dy();
01181 radarMsg_.objects[i].velocity.twist.linear.z = 0.0;
01182
01183 radarMsg_.objects[i].bounding_box_center.position.x = objectList[i].P3Dx();
01184 radarMsg_.objects[i].bounding_box_center.position.y = objectList[i].P3Dy();
01185 radarMsg_.objects[i].bounding_box_center.position.z = 0.0;
01186
01187 float heading2 = heading/2.0;
01188
01190
01191 radarMsg_.objects[i].bounding_box_center.orientation.x = 0.0;
01192 radarMsg_.objects[i].bounding_box_center.orientation.y = 0.0;
01193 radarMsg_.objects[i].bounding_box_center.orientation.z = sin(heading2);
01194 radarMsg_.objects[i].bounding_box_center.orientation.w = cos(heading2);
01195
01196
01197 radarMsg_.objects[i].bounding_box_size.x = objectList[i].ObjLength();
01198 radarMsg_.objects[i].bounding_box_size.y = 1.7;
01199 radarMsg_.objects[i].bounding_box_size.z = 1.7;
01200 for (int ii = 0; ii < 6; ii++)
01201 {
01202 int mainDiagOffset = ii * 6 + ii;
01203 radarMsg_.objects[i].object_box_center.covariance[mainDiagOffset] = 1.0;
01204 radarMsg_.objects[i].velocity.covariance[mainDiagOffset] = 1.0;
01205 }
01206 radarMsg_.objects[i].object_box_center.pose = radarMsg_.objects[i].bounding_box_center;
01207 radarMsg_.objects[i].object_box_size= radarMsg_.objects[i].bounding_box_size;
01208
01209
01210
01211
01212
01213 }
01214
01215 this->commonPtr->radarScan_pub_.publish(radarMsg_);
01216 }
01217 return(exitCode);
01218 }
01219
01220 }