46 #define _WIN32_WINNT 0x0501 47 #pragma warning(disable: 4996) 48 #pragma warning(disable: 4267) 58 #include <sick_scan/RadarScan.h> 60 #include "sick_scan/rosconsole_simu.hpp" 62 #define _USE_MATH_DEFINES 73 if (1 == sscanf(str.c_str(),
"%x", &val))
79 ROS_WARN(
"Problems parsing %s\n", str.c_str());
88 if (1 == sscanf(str.c_str(),
"%x", &val))
94 ROS_WARN(
"Problems parsing %s\n", str.c_str());
104 val = (float)(value * scale + offset);
112 ptr = (
unsigned char *)(&tmpVal);
113 int strLen = str.length();
119 for (
int i = 0; i < 4; i++)
121 std::string dummyStr =
"";
122 dummyStr += str[i * 2];
123 dummyStr += str[i * 2 + 1];
125 unsigned char ch = (0xFF & val);
153 sick_scan::RadarScan *msgPtr,
154 std::vector<SickScanRadarObject> &objectList,
155 std::vector<SickScanRadarRawTarget> &rawTargetList)
160 bool dumpData =
false;
161 int verboseLevel = 0;
162 tmpParam.
getParam(
"verboseLevel", verboseLevel);
166 int HEADER_FIELDS = 32;
171 std::vector<char *> fields;
172 fields.reserve(datagram_length / 2);
175 std::vector<char> datagram_copy_vec;
176 datagram_copy_vec.resize(datagram_length + 1);
177 char* datagram_copy = &(datagram_copy_vec[0]);
179 if (verboseLevel > 0) {
180 ROS_WARN(
"Verbose LEVEL activated. Only for DEBUG.");
183 if (verboseLevel > 0)
186 char szDumpFileName[255] = { 0 };
187 char szDir[255] = { 0 };
189 strcpy(szDir,
"C:\\temp\\");
191 strcpy(szDir,
"/tmp/");
193 sprintf(szDumpFileName,
"%stmp%06d.bin", szDir, cnt);
195 ftmp = fopen(szDumpFileName,
"wb");
198 fwrite(datagram, datagram_length, 1, ftmp);
203 strncpy(datagram_copy, datagram, datagram_length);
204 datagram_copy[datagram_length] = 0;
208 cur_field = strtok(datagram,
" ");
210 while (cur_field != NULL)
212 fields.push_back(cur_field);
214 cur_field = strtok(NULL,
" ");
219 count = fields.size();
222 if (verboseLevel > 0)
225 char szDumpFileName[255] = { 0 };
226 char szDir[255] = { 0 };
228 strcpy(szDir,
"C:\\temp\\");
230 strcpy(szDir,
"/tmp/");
232 sprintf(szDumpFileName,
"%stmp%06d.txt", szDir, cnt);
233 ROS_WARN(
"Verbose LEVEL activated. Only for DEBUG.");
235 ftmp = fopen(szDumpFileName,
"w");
239 for (i = 0; i < count; i++)
241 fprintf(ftmp,
"%3d: %s\n", i, fields[i]);
249 enum PREHEADER_TOKEN_SEQ {PREHEADER_TOKEN_SSN,
250 PREHEADER_TOKEN_LMDRADARDATA,
251 PREHEADER_TOKEN_UI_VERSION_NO,
252 PREHEADER_TOKEN_UI_IDENT,
253 PREHEADER_TOKEN_UDI_SERIAL_NO,
254 PREHEADER_TOKEN_XB_STATE_0,
255 PREHEADER_TOKEN_XB_STATE_1,
256 PREHEADER_TOKEN_TELEGRAM_COUNT,
257 PREHEADER_TOKEN_CYCLE_COUNT,
258 PREHEADER_TOKEN_SYSTEM_COUNT_SCAN,
259 PREHEADER_TOKEN_SYSTEM_COUNT_TRANSMIT,
260 PREHEADER_TOKEN_XB_INPUTS_0,
261 PREHEADER_TOKEN_XB_INPUTS_1,
262 PREHEADER_TOKEN_XB_OUTPUTS_0,
263 PREHEADER_TOKEN_XB_OUTPUTS_1,
264 PREHEADER_TOKEN_CYCLE_DURATION,
265 PREHEADER_TOKEN_NOISE_LEVEL,
266 PREHEADER_NUM_ENCODER_BLOCKS,
267 PREHADERR_TOKEN_FIX_NUM};
305 for (
int i = 0; i < PREHADERR_TOKEN_FIX_NUM; i++)
309 unsigned long int uliDummy;
310 uliDummy = strtoul(fields[i], NULL, 16);
313 case PREHEADER_TOKEN_UI_VERSION_NO:
314 msgPtr->radarPreHeader.uiVersionNo = (
UINT16)(uliDummy & 0xFFFF);
316 case PREHEADER_TOKEN_UI_IDENT:
317 msgPtr->radarPreHeader.radarPreHeaderDeviceBlock.uiIdent = (
UINT16)(uliDummy & 0xFFFF);
319 case PREHEADER_TOKEN_UDI_SERIAL_NO:
320 msgPtr->radarPreHeader.radarPreHeaderDeviceBlock.udiSerialNo = (
UINT32)(uliDummy & 0xFFFFFFFF);
322 case PREHEADER_TOKEN_XB_STATE_0:
323 for (
int j = 0; j < 3; j++)
326 if (0 != (uliDummy & (1 << j)))
332 case 0: msgPtr->radarPreHeader.radarPreHeaderDeviceBlock.bDeviceError = flag;
break;
333 case 1: msgPtr->radarPreHeader.radarPreHeaderDeviceBlock.bContaminationWarning = flag;
break;
334 case 2: msgPtr->radarPreHeader.radarPreHeaderDeviceBlock.bContaminationError = flag;
break;
335 default:
ROS_WARN(
"Flag parsing for this PREHEADER-Flag not implemented");
339 case PREHEADER_TOKEN_XB_STATE_1:
342 case PREHEADER_TOKEN_TELEGRAM_COUNT:
343 msgPtr->radarPreHeader.radarPreHeaderStatusBlock.uiTelegramCount = (
UINT16)(uliDummy & 0xFFFF);
345 case PREHEADER_TOKEN_CYCLE_COUNT:
346 sscanf(fields[i],
"%hu", &uiValue);
347 msgPtr->radarPreHeader.radarPreHeaderStatusBlock.uiCycleCount = (
UINT16)(uliDummy & 0xFFFF);
349 case PREHEADER_TOKEN_SYSTEM_COUNT_SCAN:
350 msgPtr->radarPreHeader.radarPreHeaderStatusBlock.udiSystemCountScan = (
UINT32)(uliDummy & 0xFFFFFFFF);
352 case PREHEADER_TOKEN_SYSTEM_COUNT_TRANSMIT:
353 msgPtr->radarPreHeader.radarPreHeaderStatusBlock.udiSystemCountTransmit = (
UINT32)(uliDummy & 0xFFFFFFFF);
355 case PREHEADER_TOKEN_XB_INPUTS_0:
356 msgPtr->radarPreHeader.radarPreHeaderStatusBlock.uiInputs = (
UINT8)(uliDummy & 0xFF);;
357 msgPtr->radarPreHeader.radarPreHeaderStatusBlock.uiInputs <<= 8;
359 case PREHEADER_TOKEN_XB_INPUTS_1:
360 msgPtr->radarPreHeader.radarPreHeaderStatusBlock.uiInputs |= (
UINT8)(uliDummy & 0xFF);;
362 case PREHEADER_TOKEN_XB_OUTPUTS_0:
363 msgPtr->radarPreHeader.radarPreHeaderStatusBlock.uiOutputs = (
UINT8)(uliDummy & 0xFF);;
364 msgPtr->radarPreHeader.radarPreHeaderStatusBlock.uiOutputs <<= 8;
366 case PREHEADER_TOKEN_XB_OUTPUTS_1:
367 msgPtr->radarPreHeader.radarPreHeaderStatusBlock.uiOutputs |= (
UINT8)(uliDummy & 0xFF);;
369 case PREHEADER_TOKEN_CYCLE_DURATION:
370 msgPtr->radarPreHeader.radarPreHeaderMeasurementParam1Block.uiCycleDuration = (
UINT16)(uliDummy & 0xFFFF);
372 case PREHEADER_TOKEN_NOISE_LEVEL:
373 msgPtr->radarPreHeader.radarPreHeaderMeasurementParam1Block.uiNoiseLevel = (
UINT16)(uliDummy & 0xFFFF);
375 case PREHEADER_NUM_ENCODER_BLOCKS:
379 if (u16NumberOfBlock > 0)
381 msgPtr->radarPreHeader.radarPreHeaderArrayEncoderBlock.resize(u16NumberOfBlock);
383 for (
int j = 0; j < u16NumberOfBlock; j++)
386 int rowIndex = PREHEADER_NUM_ENCODER_BLOCKS + j * 2 + 1;
387 udiValue = strtoul(fields[rowIndex], NULL, 16);
388 msgPtr->radarPreHeader.radarPreHeaderArrayEncoderBlock[j].udiEncoderPos = udiValue;
389 udiValue = strtoul(fields[rowIndex+1], NULL, 16);
390 iEncoderSpeed = (int)udiValue;
391 msgPtr->radarPreHeader.radarPreHeaderArrayEncoderBlock[j].iEncoderSpeed = iEncoderSpeed;
407 std::vector<std::string> keyWordList;
408 #define DIST1_KEYWORD "DIST1" 409 #define AZMT1_KEYWORD "AZMT1" 410 #define VRAD1_KEYWORD "VRAD1" 411 #define AMPL1_KEYWORD "AMPL1" 412 #define MODE1_KEYWORD "MODE1" 414 #define P3DX1_KEYWORD "P3DX1" 415 #define P3DY1_KEYWORD "P3DY1" 416 #define V3DX1_KEYWORD "V3DX1" 417 #define V3DY1_KEYWORD "V3DY1" 418 #define OBJLEN_KEYWORD "OBLE1" 419 #define OBJID_KEYWORD "OBID1" 421 const int RAWTARGET_LOOP = 0;
422 const int OBJECT_LOOP = 1;
427 for (
int iLoop = 0; iLoop < 2; iLoop++)
448 std::vector<int> keyWordPos;
449 std::vector<float> keyWordScale;
450 std::vector<float> keyWordScaleOffset;
451 keyWordPos.resize(keyWordList.size());
452 keyWordScale.resize(keyWordList.size());
453 keyWordScaleOffset.resize(keyWordList.size());
454 for (
int i = 0; i < keyWordPos.size(); i++)
458 int numKeyWords = keyWordPos.size();
459 for (
int i = 0; i < fields.size(); i++)
461 for (
int j = 0; j < keyWordList.size(); j++)
463 if (strcmp(fields[i], keyWordList[j].c_str()) == 0)
470 bool entriesNumOk =
true;
472 if (keyWordPos[0] == -1)
474 entriesNumOk =
false;
479 entriesNum =
getHexValue(fields[keyWordPos[0] + 3]);
480 for (
int i = 0; i < numKeyWords; i++)
482 if (keyWordPos[i] == -1)
484 entriesNumOk =
false;
485 ROS_WARN(
"Missing keyword %s but first keyword found.\n", keyWordList[i].c_str());
486 entriesNumOk =
false;
490 int entriesNumTmp =
getHexValue(fields[keyWordPos[i] + 3]);
491 if (entriesNumTmp != entriesNum)
493 ROS_WARN(
"Number of items for keyword %s differs from number of items for %s\n.",
494 keyWordList[i].c_str(), keyWordList[0].c_str());
495 entriesNumOk =
false;
501 if (
true == entriesNumOk)
505 for (
int i = 0; i < numKeyWords; i++)
507 int scaleLineIdx = keyWordPos[i] + 1;
508 int scaleOffsetLineIdx = keyWordPos[i] + 2;
509 std::string token = fields[scaleLineIdx];
511 token = fields[scaleOffsetLineIdx];
520 rawTargetList.resize(entriesNum);
525 objectList.resize(entriesNum);
529 for (
int i = 0; i < entriesNum; i++)
540 for (
int j = 0; j < numKeyWords; j++)
542 int dataRowIdx = keyWordPos[j] + 4 + i;
543 std::string token = keyWordList[j];
570 rawTargetList[i].Dist(dist);
571 rawTargetList[i].Ampl(ampl);
572 rawTargetList[i].Azimuth(azimuth);
573 rawTargetList[i].Mode(mode);
574 rawTargetList[i].Vrad(vrad);
587 for (
int j = 0; j < numKeyWords; j++)
589 int dataRowIdx = keyWordPos[j] + 4 + i;
590 std::string token = keyWordList[j];
618 objId = (int)(objIdRaw * keyWordScale[j] + 0.5);
622 objectList[i].ObjId(objId);
623 objectList[i].ObjLength(objLen);
624 objectList[i].P3Dx(px);
625 objectList[i].P3Dy(py);
626 objectList[i].V3Dx(vx);
627 objectList[i].V3Dy(vy);
641 static int callCnt = 0;
646 std::string
header =
"\x2sSN LMDradardata 1 1 112F6E9 0 0 DFB6 B055 6E596002 6E5AE0E5 0 0 0 0 0 0 1 0 0 ";
648 int channel16BitCnt = 4;
650 float rawTargetFactorList[] = { 40.0f, 0.16f, 0.04f, 1.00f, 1.00f };
651 float objectFactorList[] = { 64.0f, 64.0f, 0.1f, 0.1f, 0.75f, 1.0f };
653 std::string dist1_intro =
"DIST1 42200000 00000000";
654 std::string azmt1_intro =
"AZMT1 3E23D70A 00000000";
655 std::string vrad1_intro =
"VRAD1 3D23D70A 00000000";
656 std::string ampl1_intro =
"AMPL1 3F800000 00000000";
658 std::string pdx1_intro =
"P3DX1 42800000 00000000";
659 std::string pdy1_intro =
"P3DY1 42800000 00000000";
660 std::string v3dx_intro =
"V3DX1 3DCCCCCD 00000000";
661 std::string v3dy_intro =
"V3DY1 3DCCCCCD 00000000";
662 std::string oblen_intro =
"OBLE1 3F400000 00000000";
664 int rawTargetChannel16BitCnt = 4;
665 int rawTargetChannel8BitCnt = 1;
666 std::string mode1_intro =
"MODE1 3F800000 00000000";
668 std::string obid_intro =
"OBID1 3F800000 00000000";
672 std::string trailer =
"0 0 0 0 0 0\x3";
674 std::vector<std::string> channel16BitID;
675 std::vector<std::string> channel8BitID;
676 channel16BitID.push_back(dist1_intro);
677 channel16BitID.push_back(azmt1_intro);
678 channel16BitID.push_back(vrad1_intro);
679 channel16BitID.push_back(ampl1_intro);
681 channel16BitID.push_back(pdx1_intro);
682 channel16BitID.push_back(pdy1_intro);
683 channel16BitID.push_back(v3dx_intro);
684 channel16BitID.push_back(v3dy_intro);
685 channel16BitID.push_back(oblen_intro);
689 channel8BitID.push_back(mode1_intro);
690 channel8BitID.push_back(obid_intro);
692 int channel8BitCnt = channel8BitID.size();
693 int objectChannel16BitCnt = 5;
694 channel16BitCnt = channel16BitID.size();
697 std::vector<SickScanRadarRawTarget> rawTargetList;
699 #if 0 // simulate railway crossing 700 for (
float y = -20;
y <= 20.0;
y += 2.0)
703 float azimuth = atan2(
y, x);
704 float dist = sqrt(x*x +
y*
y);
705 float vrad = speed * sin(azimuth);
707 float ampl = 50.0 + y;
708 rawTarget.
Ampl(ampl);
709 rawTarget.
Mode(mode);
710 rawTarget.
Dist(dist);
712 rawTarget.
Vrad(vrad);
713 rawTargetList.push_back(rawTarget);
717 std::vector<SickScanRadarObject> objectList;
720 for (
float x = 20; x <= 100.0; x += 50.0)
724 for (
int iY = -1; iY <= 1; iY += 2)
728 float vehicleWidth = 1.8;
730 float speed = y * 10.0f;
739 vehicle.
P3Dx((xOff + 0.1 * speed * (callCnt % 20)) * 1000.0);
740 vehicle.
P3Dy(y * 1000.0);
741 float objLen = 6.0f + y;
744 vehicle.
ObjId(objId++);
745 objectList.push_back(vehicle);
747 for (
int i = 0; i < 2; i++)
751 xp[i] = vehicle.
P3Dx() * 0.001;
752 yp[i] = vehicle.
P3Dy() * 0.001;
756 yp[i] -= vehicleWidth/2.0;
760 yp[i] += vehicleWidth/2.0;
764 xp[i] -= objLen * 0.5;
768 xp[i] += objLen * 0.5;
771 float azimuth = atan2(yp[i], xp[i]);
772 float dist = sqrt(xp[i]*xp[i] + yp[i]*yp[i]);
773 float vrad = speed * cos(azimuth);
777 rawTarget.
Ampl(ampl);
778 rawTarget.
Mode(mode);
779 rawTarget.
Dist(dist);
781 rawTarget.
Vrad(vrad);
782 rawTargetList.push_back(rawTarget);
791 char szDummy[255] = { 0 };
792 std::string resultStr;
794 sprintf(szDummy,
"%x ", channel16BitCnt);
795 resultStr += szDummy;
796 for (
int i = 0; i < channel16BitCnt; i++)
798 resultStr += channel16BitID[i];
799 int valNum = rawTargetList.size();
800 bool processRawTarget =
true;
801 if (i < rawTargetChannel16BitCnt)
803 valNum = rawTargetList.size();
807 processRawTarget =
false;
808 valNum = objectList.size();
811 sprintf(szDummy,
" %x ", valNum);
812 resultStr += szDummy;
814 for (
int j = 0; j < valNum; j++)
818 case 0: val = 1000.0 * rawTargetList[j].Dist();
break;
819 case 1: val = 1.0 /
deg2rad * rawTargetList[j].Azimuth();
break;
820 case 2: val = rawTargetList[j].Vrad();
break;
821 case 3: val = rawTargetList[j].Ampl();
break;
822 case 4: val = objectList[j].P3Dx();
break;
823 case 5: val = objectList[j].P3Dy();
break;
824 case 6: val = objectList[j].V3Dx();
break;
825 case 7: val = objectList[j].V3Dy();
break;
826 case 8: val = objectList[j].ObjLength();
break;
830 if (processRawTarget)
832 val /= rawTargetFactorList[i];
836 int idx = i - rawTargetChannel16BitCnt;
837 val /= objectFactorList[idx];
848 int16_t shortVal = (int16_t)(val);
850 sprintf(szDummy,
"%08x", shortVal);
851 strcpy(szDummy, szDummy + 4);
852 resultStr += szDummy;
857 sprintf(szDummy,
"%x ", channel8BitCnt);
858 resultStr += szDummy;
859 int valNum = rawTargetList.size();
860 for (
int i = 0; i < channel8BitCnt; i++)
862 resultStr += channel8BitID[i];
864 bool processRawTarget =
true;
865 if (i < rawTargetChannel8BitCnt)
867 valNum = rawTargetList.size();
871 processRawTarget =
false;
872 valNum = objectList.size();
874 sprintf(szDummy,
" %x ", valNum);
875 resultStr += szDummy;
876 for (
int j = 0; j < valNum; j++)
880 case 0: val = rawTargetList[j].Mode();
break;
881 case 1: val = objectList[j].ObjId();
break;
885 if (processRawTarget)
887 offset = rawTargetChannel16BitCnt;
888 val /= rawTargetFactorList[i + offset];
892 offset = objectChannel16BitCnt;
893 int idx = i - rawTargetChannel8BitCnt;
894 val /= objectFactorList[idx + offset];
904 int8_t shortVal = (int16_t)(val);
906 sprintf(szDummy,
"%08x", shortVal);
907 strcpy(szDummy, szDummy + 6);
908 resultStr += szDummy;
913 resultStr += trailer;
915 *actual_length = resultStr.length();
916 strcpy((
char *)receiveBuffer, resultStr.c_str());
920 std::string filePattern)
922 static int callCnt = 0;
924 char szLine[1024] = { 0 };
925 char szDummyWord[1024] = { 0 };
926 int actual_length = 0;
928 char szFileName[1024] = {0};
930 receiveBuffer[0] =
'\x02';
933 for (
int iLoop = 0; iLoop < 2; iLoop++)
935 sprintf(szFileName,filePattern.c_str(), callCnt);
937 fin = fopen(szFileName,
"r");
939 if ( (fin == NULL) && (iLoop == 0))
948 if ((iLoop == 1) && (fin == NULL))
950 ROS_ERROR(
"Can not find simulation file corresponding to pattern %s", filePattern.c_str());
954 while (fgets(szLine, 1024, fin) != NULL)
956 char *ptr = strchr(szLine,
'\n');;
962 ptr = strchr(szLine,
':');
965 if (1 == sscanf(ptr + 2,
"%s", szDummyWord))
969 receiveBuffer[actual_length++] =
' ';
971 strcpy((
char *)receiveBuffer + actual_length, szDummyWord);
972 actual_length += strlen(szDummyWord);
977 receiveBuffer[actual_length] = (char)
'\x03';
979 receiveBuffer[actual_length] = (char)
'\x00';
981 *pActual_length = actual_length;
989 enum enumSimulationMode {EMULATE_OFF, EMULATE_SYN, EMULATE_FROM_FILE_TRAIN, EMULATE_FROM_FILE_CAR};
991 int simulationMode = EMULATE_OFF;
995 simulationMode = EMULATE_SYN;
998 switch(simulationMode)
1004 case EMULATE_FROM_FILE_TRAIN:
simulateAsciiDatagramFromFile(receiveBuffer, &actual_length,
"/mnt/hgfs/development/ros/bags/raw/trainSeq/tmp%06d.txt");
1006 case EMULATE_FROM_FILE_CAR:
simulateAsciiDatagramFromFile(receiveBuffer, &actual_length,
"/mnt/hgfs/development/ros/bags/raw/carSeq/tmp%06d.txt");
1010 printf(
"Simulation Mode unknown\n");
1014 sensor_msgs::PointCloud2 cloud_;
1015 sick_scan::RadarScan radarMsg_;
1017 std::vector<SickScanRadarObject> objectList;
1018 std::vector<SickScanRadarRawTarget> rawTargetList;
1020 if (useBinaryProtocol)
1022 throw std::logic_error(
"Binary protocol currently not supported.");
1026 bool dataToProcess =
false;
1027 char *buffer_pos = (
char *)receiveBuffer;
1028 char *dstart = NULL;
1031 dstart = strchr(buffer_pos, 0x02);
1034 dend = strchr(dstart + 1, 0x03);
1036 if ((dstart != NULL) && (dend != NULL))
1038 dataToProcess =
true;
1039 dlength = dend - dstart;
1046 dataToProcess =
false;
1052 enum RADAR_PROC_LIST {RADAR_PROC_RAW_TARGET, RADAR_PROC_TRACK, RADAR_PROC_NUM};
1056 for (
int iLoop = 0; iLoop < RADAR_PROC_NUM; iLoop++)
1061 std::string channelRawTargetId[] = {
"x",
"y",
"z",
"vrad",
"amplitude" };
1062 std::string channelObjectId[] = {
"x",
"y",
"z",
"vx",
"vy",
"vz",
"objLen",
"objId" };
1063 std::vector<std::string> channelList;
1064 std::string frameId =
"radar";
1067 case RADAR_PROC_RAW_TARGET: numTargets = rawTargetList.size();
1068 for (
int i = 0; i <
sizeof(channelRawTargetId) /
sizeof(channelRawTargetId[0]); i++)
1070 channelList.push_back(channelRawTargetId[i]);
1074 case RADAR_PROC_TRACK: numTargets = objectList.size();
1075 for (
int i = 0; i <
sizeof(channelObjectId) /
sizeof(channelObjectId[0]); i++)
1077 channelList.push_back(channelObjectId[i]);
1082 if (numTargets == 0)
1086 int numChannels = channelList.size();
1088 std::vector<float> valSingle;
1089 valSingle.resize(numChannels);
1090 cloud_.header.stamp = timeStamp;
1091 cloud_.header.frame_id = frameId;
1092 cloud_.header.seq = 0;
1094 cloud_.width = numTargets;
1095 cloud_.is_bigendian =
false;
1096 cloud_.is_dense =
true;
1097 cloud_.point_step = numChannels *
sizeof(float);
1098 cloud_.row_step = cloud_.point_step * cloud_.width;
1099 cloud_.fields.resize(numChannels);
1100 for (
int i = 0; i < numChannels; i++) {
1101 cloud_.fields[i].name = channelList[i];
1102 cloud_.fields[i].offset = i *
sizeof(float);
1103 cloud_.fields[i].count = 1;
1104 cloud_.fields[i].datatype = sensor_msgs::PointField::FLOAT32;
1107 cloud_.data.resize(cloud_.row_step * cloud_.height);
1108 float *valPtr = (
float *)(&(cloud_.data[0]));
1110 for (
int i = 0; i < numTargets; i++)
1117 valSingle[0] = rawTargetList[i].Dist() * cos(angle);
1118 valSingle[1] = rawTargetList[i].Dist() * sin(angle);
1120 valSingle[3] = rawTargetList[i].Vrad();
1121 valSingle[4] = rawTargetList[i].Ampl();
1126 valSingle[0] = objectList[i].P3Dx();
1127 valSingle[1] = objectList[i].P3Dy();
1129 valSingle[3] = objectList[i].V3Dx();
1130 valSingle[4] = objectList[i].V3Dy();
1132 valSingle[6] = objectList[i].ObjLength();
1133 valSingle[7] = objectList[i].ObjId();
1137 for (
int j = 0; j < numChannels; j++)
1139 valPtr[off] = valSingle[j];
1143 #if 0 // just for debugging 1146 case RADAR_PROC_RAW_TARGET:
1149 case RADAR_PROC_TRACK:
1155 printf(
"PUBLISH:\n");
1157 if (iLoop == RADAR_PROC_RAW_TARGET)
1160 radarMsg_.targets = cloud_;
1167 radarMsg_.header.stamp = timeStamp;
1168 radarMsg_.header.frame_id =
"radar";
1169 radarMsg_.header.seq = 0;
1171 radarMsg_.objects.resize(objectList.size());
1172 for (
int i = 0; i < radarMsg_.objects.size(); i++)
1174 float vx = objectList[i].V3Dx();
1175 float vy = objectList[i].V3Dy();
1176 float v = sqrt(vx*vx + vy *vy);
1177 float heading = atan2( objectList[i].V3Dy(), objectList[i].V3Dx());
1179 radarMsg_.objects[i].velocity.twist.linear.x = objectList[i].V3Dx();
1180 radarMsg_.objects[i].velocity.twist.linear.y = objectList[i].V3Dy();
1181 radarMsg_.objects[i].velocity.twist.linear.z = 0.0;
1183 radarMsg_.objects[i].bounding_box_center.position.x = objectList[i].P3Dx();
1184 radarMsg_.objects[i].bounding_box_center.position.y = objectList[i].P3Dy();
1185 radarMsg_.objects[i].bounding_box_center.position.z = 0.0;
1187 float heading2 = heading/2.0;
1191 radarMsg_.objects[i].bounding_box_center.orientation.x = 0.0;
1192 radarMsg_.objects[i].bounding_box_center.orientation.y = 0.0;
1193 radarMsg_.objects[i].bounding_box_center.orientation.z = sin(heading2);
1194 radarMsg_.objects[i].bounding_box_center.orientation.w = cos(heading2);
1197 radarMsg_.objects[i].bounding_box_size.x = objectList[i].ObjLength();
1198 radarMsg_.objects[i].bounding_box_size.y = 1.7;
1199 radarMsg_.objects[i].bounding_box_size.z = 1.7;
1200 for (
int ii = 0; ii < 6; ii++)
1202 int mainDiagOffset = ii * 6 + ii;
1203 radarMsg_.objects[i].object_box_center.covariance[mainDiagOffset] = 1.0;
1204 radarMsg_.objects[i].velocity.covariance[mainDiagOffset] = 1.0;
1206 radarMsg_.objects[i].object_box_center.pose = radarMsg_.objects[i].bounding_box_center;
1207 radarMsg_.objects[i].object_box_size= radarMsg_.objects[i].bounding_box_size;
void publish(const boost::shared_ptr< M > &message) const
int parseAsciiDatagram(char *datagram, size_t datagram_length, sick_scan::RadarScan *msgPtr, std::vector< SickScanRadarObject > &objectList, std::vector< SickScanRadarRawTarget > &rawTargetList)
Parsing Ascii datagram.
ros::Publisher cloud_radar_track_pub_
float convertScaledIntValue(int value, float scale, float offset)
void setEmulation(bool _emul)
TFSIMD_FORCE_INLINE const tfScalar & y() const
SickScanCommon * commonPtr
int getHexValue(std::string str)
TFSIMD_FORCE_INLINE tfScalar angle(const Quaternion &q1, const Quaternion &q2)
ros::Publisher cloud_radar_rawtarget_pub_
void simulateAsciiDatagram(unsigned char *receiveBuffer, int *actual_length)
TFSIMD_FORCE_INLINE const tfScalar & x() const
void simulateAsciiDatagramFromFile(unsigned char *receiveBuffer, int *actual_length, std::string filePattern)
ros::Publisher radarScan_pub_
bool getParam(const std::string &key, std::string &s) const
int parseDatagram(ros::Time timeStamp, unsigned char *receiveBuffer, int actual_length, bool useBinaryProtocol)
float getFloatValue(std::string str)
int16_t getShortValue(std::string str)