sick_generic_field_mon.cpp
Go to the documentation of this file.
1 
45 /*
46 #ifdef _MSC_VER
47 #define _WIN32_WINNT 0x0501
48 #pragma warning(disable: 4996)
49 #pragma warning(disable: 4267)
50 #endif
51 
52 #ifndef _MSC_VER
53 
54 
55 #endif
56 
57 #include <sick_scan/sick_scan_common_nw.h>
58 #include <sick_scan/RadarScan.h> // generated by msg-generator
59 
60 #ifndef _MSC_VER
61 
62 #include <dynamic_reconfigure/server.h>
63 #include <sick_scan/SickScanConfig.h>
64 
65 #endif
66 
67 #include "sick_scan/sick_generic_parser.h"
68 #include "sick_scan/sick_scan_common_nw.h"
69 
70 #include <sick_scan/sick_scan_common_tcp.h>
71 #include <sick_scan/sick_generic_parser.h>
72 #include <sick_scan/sick_generic_field_mon.h>
73 #ifdef _MSC_VER
74 #include "sick_scan/rosconsole_simu.hpp"
75 #endif
76 #define _USE_MATH_DEFINES
77 
78 #include <math.h>
79 #include "string"
80 #include <stdio.h>
81 #include <stdlib.h>
82 */
83 #include <sick_scan/sick_scan_common.h>
84 #include <sick_scan/sick_generic_field_mon.h>
85 
86 namespace sick_scan
87 {
88 
89  /*
90  ** @brief Converts a point of a segmented field to carthesian coordinates
91  ** @param[in] range range in meter
92  ** @param[in] angle_rad angle in radians
93  ** @param[out] x x in meter in ros coordinate system
94  ** @param[out] y y in meter in ros coordinate system
95  */
96  void SickScanMonFieldConverter::segmentedFieldPointToCarthesian(float range, float angle_rad, float& x, float& y)
97  {
98  y = -range * std::cos(angle_rad); // y_ros = -x_sick = -range * std::cos(angle_rad)
99  x = range * std::sin(angle_rad); // x_ros = +y_sick = +range * std::sin(angle_rad)
100  }
101 
102  /*
103  ** @brief Converts a rectangular field to carthesian coordinates, i.e. to 4 corner points carthesian (ros) coordinates
104  ** @param[in] distRefPointMeter range of ref point (rect center) in meter
105  ** @param[in] angleRefPointRad angle of ref point (rect center) in radians
106  ** @param[in] rotAngleRad rotation of rectangle in radians
107  ** @param[in] rectWidthMeter width of rectangle in meter
108  ** @param[in] rectLengthMeter width of rectangle in meter
109  ** @param[out] points_x x of corner points in meter in ros coordinate system
110  ** @param[out] points_y y of corner points in meter in ros coordinate system
111  */
112  void SickScanMonFieldConverter::rectangularFieldToCarthesian(float distRefPointMeter, float angleRefPointRad, float rotAngleRad, float rectWidthMeter, float rectLengthMeter, float points_x[4], float points_y[4])
113  {
114  // unrotated rectangle at base point, x_ros = +y_sick, y_ros = -x_sick
115  // base point is the lower left corner of the rectangle
116  points_x[0] = 0;
117  points_y[0] = 0;
118  points_x[1] = 0;
119  points_y[1] = -rectWidthMeter;
120  points_x[2] = rectLengthMeter;
121  points_y[2] = -rectWidthMeter;
122  points_x[3] = rectLengthMeter;
123  points_y[3] = 0;
124  // rotate by rotAngleRad
125  float sin_angle = sinf(rotAngleRad);
126  float cos_angle = cosf(rotAngleRad);
127  for(int n = 0; n < 4; n++)
128  {
129  float x = points_x[n], y = points_y[n];
130  points_x[n] = x * cos_angle - y * sin_angle;
131  points_y[n] = x * sin_angle + y * cos_angle;
132  }
133  // move to refPoint
134  float refPointX = 0, refPointY = 0;
135  segmentedFieldPointToCarthesian(distRefPointMeter, angleRefPointRad, refPointX, refPointY);
136  for(int n = 0; n < 4; n++)
137  {
138  points_x[n] += refPointX;
139  points_y[n] += refPointY;
140  }
141  }
142 
143  /*
144  ** @brief Converts a dynamic field to carthesian coordinates. The dynamic field is just converted into 2 rectangular fields,
145  ** each rectangular field described by to 4 corner points carthesian (ros) coordinates.
146  ** The first rectangular field has size rectWidthMeter x maxLengthMeter, the second rectangular field has size rectWidthMeter x rectLengthMeter.
147  ** The rectangular fields are returned by 4 corner points (points_x[n], points_y[n]) with 0 <= n < 4 for the first (big) rectangle and 4 <= n < 8 for the second (smaller) rectangle.
148  ** @param[in] distRefPointMeter range of ref point (rect center) in meter
149  ** @param[in] angleRefPointRad angle of ref point (rect center) in radians
150  ** @param[in] rotAngleRad rotation of rectangle in radians
151  ** @param[in] rectWidthMeter width of rectangle in meter
152  ** @param[in] rectLengthMeter width of rectangle in meter at v = 0
153  ** @param[in] maxSpeedMeterPerSec max speed (unused)
154  ** @param[in] maxLengthMeter width of rectangle in meter at v = max. speed
155  ** @param[out] points_x x of corner points in meter in ros coordinate system
156  ** @param[out] points_y y of corner points in meter in ros coordinate system
157  */
158  void SickScanMonFieldConverter::dynamicFieldPointToCarthesian(float distRefPointMeter, float angleRefPointRad, float rotAngleRad, float rectWidthMeter, float rectLengthMeter, float maxSpeedMeterPerSec, float maxLengthMeter, float points_x[8], float points_y[8])
159  {
160  // set 2 rectangular fields: 1. rectangular field with rectWidthMeter x maxLengthMeter, 2.nd rectangular field with rectWidthMeter x rectLengthMeter
161  float field1_points_x[4], field1_points_y[4], field2_points_x[4], field2_points_y[4];
162  rectangularFieldToCarthesian(distRefPointMeter, angleRefPointRad, rotAngleRad, rectWidthMeter, maxLengthMeter, field1_points_x, field1_points_y);
163  rectangularFieldToCarthesian(distRefPointMeter, angleRefPointRad, rotAngleRad, rectWidthMeter, rectLengthMeter, field2_points_x, field2_points_y);
164  for(int n = 0; n < 4; n++)
165  {
166  points_x[n + 0] = field1_points_x[n];
167  points_y[n + 0] = field1_points_y[n];
168  points_x[n + 4] = field2_points_x[n];
169  points_y[n + 4] = field2_points_y[n];
170  }
171  }
172 
173  /* Null, because instance will be initialized on demand. */
174  SickScanFieldMonSingleton *SickScanFieldMonSingleton::instance = 0;
175 
176  SickScanFieldMonSingleton *SickScanFieldMonSingleton::getInstance()
177  {
178  if (instance == 0)
179  {
180  instance = new SickScanFieldMonSingleton();
181  }
182 
183  return instance;
184  }
185 
186  SickScanFieldMonSingleton::SickScanFieldMonSingleton()
187  {
188  this->monFields.resize(48);
189  // just for debugging, but very helpful for the start
190  this->active_mon_fieldset = 0;
191  }
192 
198  int SickScanFieldMonSingleton::parseAsciiLIDinputstateMsg(unsigned char* datagram, int datagram_length)
199  {
200  ROS_ERROR("SickScanFieldMonSingleton::parseAsciiLIDinputstateMsg not implemented.");
201  int exitCode=ExitSuccess;
202  return (exitCode);
203  }
204 
210  int SickScanFieldMonSingleton::parseBinaryLIDinputstateMsg(unsigned char* datagramm, int datagram_length)
211  {
212  int exitCode=ExitSuccess;
213  if(datagram_length > 36)
214  {
215  int fieldset = 0;
216  for(int offset = 35; offset >= 32; offset--) // datagramm[32]=INT1, datagramm[33]=INT2, datagramm[34]=INT3, datagramm[35]=INT4
217  {
218  fieldset = (fieldset << 1);
219  fieldset |= ((datagramm[offset] != 0) ? 1 : 0);
220  }
221  setActiveFieldset(fieldset);
222  }
223  else
224  {
225  exitCode = ExitError;
226  }
227  return exitCode;
228  }
229 
235  int SickScanFieldMonSingleton::parseAsciiDatagram(std::vector<unsigned char> datagramm)
236  {
237  ROS_ERROR("SickScanFieldMonSingleton::parseAsciiDatagram not implemented.");
238  int exitCode=ExitSuccess;
239  return (exitCode);
240  }
241 
242  int SickScanFieldMonSingleton::parseBinaryDatagram(std::vector<unsigned char> datagram)
243  {
244  int exitCode = ExitSuccess;
245  int fieldNumberFromCMD=0;
246  std::string sDatagramm( datagram.begin()+8, datagram.end() );
247  sscanf(sDatagramm.c_str(), "sRA field%d", &fieldNumberFromCMD);
248  float distScaleFactor;
249  float distScaleFactorOffset;
250  uint32_t angScaleFactor;
251  int32_t angScaleFactorOffset;
252  uint8_t fieldType;
253  uint8_t fieldNumber;
254  uint16_t segmentedFieldConfigured = 0;
255  uint16_t rectangularFieldConfigured = 0;
256  uint16_t dynamicFieldConfigured = 0;
257  uint32_t dataPtrOffset = 0;
258 
259  unsigned char *dataPtr= &(datagram[0]);
260  memcpy(&distScaleFactor, dataPtr + 21, 4);
261  memcpy(&distScaleFactorOffset, dataPtr + 25, 4);
262  memcpy(&angScaleFactor, dataPtr + 29, 4);
263  memcpy(&angScaleFactorOffset, dataPtr + 33, 4);
264  memcpy(&fieldType, dataPtr + 37, 1);
265  memcpy(&fieldNumber, dataPtr + 38, 1);
266  memcpy(&segmentedFieldConfigured, dataPtr + 39, 2);
267  swap_endian((unsigned char *) &distScaleFactor, 4);
268  swap_endian((unsigned char *) &distScaleFactorOffset, 4);
269  swap_endian((unsigned char *) &angScaleFactor, 4);
270  swap_endian((unsigned char *) &angScaleFactorOffset, 4);
271  swap_endian((unsigned char *) &fieldType, 1);
272  swap_endian((unsigned char *) &fieldNumber, 1);
273  swap_endian((unsigned char *) &segmentedFieldConfigured, 2);
274  monFields[fieldNumberFromCMD].fieldType() = (SickScanMonFieldType)(fieldType);
275  dataPtrOffset = 41;
276  if(segmentedFieldConfigured==1) // configuration for segmented fields
277  {
278  uint16_t numOfFieldPoints;
279  uint16_t angIDX;
280  uint16_t startDist,stopDist;
281  memcpy(&numOfFieldPoints, dataPtr + 41, 2);
282  swap_endian((unsigned char *) &numOfFieldPoints, 2);
283  monFields[fieldNumberFromCMD].pushFieldPointCarthesian(0, 0);
284  for(uint16_t point=0;point<numOfFieldPoints;point++)
285  {
286  memcpy(&angIDX, dataPtr + 43+point*6, 2);
287  memcpy(&startDist, dataPtr + 45+point*6, 2);
288  memcpy(&stopDist, dataPtr + 47+point*6, 2);
289  swap_endian((unsigned char *) &angIDX, 2);
290  swap_endian((unsigned char *) &startDist, 2);
291  swap_endian((unsigned char *) &stopDist, 2);
292  float angRad=(angIDX*angScaleFactor/1e4+angScaleFactorOffset/1e4)*deg2rad;
293  float distMeter=(stopDist*distScaleFactor+distScaleFactorOffset)/1000.0f;
294  float point_x = 0, point_y = 0;
295  SickScanMonFieldConverter::segmentedFieldPointToCarthesian(distMeter, angRad, point_x, point_y);
296  monFields[fieldNumberFromCMD].pushFieldPointCarthesian(point_x, point_y);
297  }
298  dataPtrOffset = 43 + numOfFieldPoints * 6;
299  }
300  memcpy(&rectangularFieldConfigured, dataPtr + dataPtrOffset, 2);
301  swap_endian((unsigned char *) &rectangularFieldConfigured, 2);
302  dataPtrOffset += 2;
303  if(rectangularFieldConfigured==1) // configuration for rectangular fields
304  {
305  int32_t angleRefPoint;
306  uint16_t distRefPoint;
307  int32_t rotAngle;
308  uint32_t rectWidth, rectLength;
309  memcpy(&angleRefPoint, dataPtr + dataPtrOffset + 0, 4);
310  memcpy(&distRefPoint, dataPtr + dataPtrOffset + 4, 2);
311  memcpy(&rotAngle, dataPtr + dataPtrOffset + 6, 4);
312  memcpy(&rectWidth, dataPtr + dataPtrOffset + 10, 4);
313  memcpy(&rectLength, dataPtr + dataPtrOffset + 14, 4);
314  dataPtrOffset += 18;
315  swap_endian((unsigned char *) &angleRefPoint, 4);
316  swap_endian((unsigned char *) &distRefPoint, 2);
317  swap_endian((unsigned char *) &rotAngle, 4);
318  swap_endian((unsigned char *) &rectWidth, 4);
319  swap_endian((unsigned char *) &rectLength, 4);
320  float angleRefPointRad=(angleRefPoint/1e4+angScaleFactorOffset/1e4)*deg2rad;
321  float distRefPointMeter=(distRefPoint*distScaleFactor+distScaleFactorOffset)/1000.0f;
322  float rotAngleRad=(rotAngle/1e4)*deg2rad;
323  float rectWidthMeter=(rectWidth)/1000.0f;
324  float rectLengthMeter=(rectLength)/1000.0f;
325  float points_x[4] = {0}, points_y[4] = {0};
326  SickScanMonFieldConverter::rectangularFieldToCarthesian(distRefPointMeter, angleRefPointRad, rotAngleRad, rectWidthMeter, rectLengthMeter, points_x, points_y);
327  for(int n = 0; n < 4; n++)
328  monFields[fieldNumberFromCMD].pushFieldPointCarthesian(points_x[n], points_y[n]);
329  }
330  dataPtrOffset += 2; // 2 byte radial field configured, always 0, currently not supported (not used in LMS5xx and TiM7xx)
331  memcpy(&dynamicFieldConfigured, dataPtr + dataPtrOffset, 2);
332  swap_endian((unsigned char *) &dynamicFieldConfigured, 2);
333  dataPtrOffset += 2;
334  if(dynamicFieldConfigured==1) // configuration for dynamic fields
335  {
336  int32_t angleRefPoint;
337  uint16_t distRefPoint;
338  int32_t rotAngle;
339  uint32_t rectWidth, rectLength;
340  int16_t maxSpeed;
341  uint32_t maxLength;
342  memcpy(&angleRefPoint, dataPtr + dataPtrOffset + 0, 4);
343  memcpy(&distRefPoint, dataPtr + dataPtrOffset + 4, 2);
344  memcpy(&rotAngle, dataPtr + dataPtrOffset + 6, 4);
345  memcpy(&rectWidth, dataPtr + dataPtrOffset + 10, 4);
346  memcpy(&rectLength, dataPtr + dataPtrOffset + 14, 4);
347  memcpy(&maxSpeed, dataPtr + dataPtrOffset + 18, 2);
348  memcpy(&maxLength, dataPtr + dataPtrOffset + 20, 4);
349  dataPtrOffset += 24;
350  swap_endian((unsigned char *) &angleRefPoint, 4);
351  swap_endian((unsigned char *) &distRefPoint, 2);
352  swap_endian((unsigned char *) &rotAngle, 4);
353  swap_endian((unsigned char *) &rectWidth, 4);
354  swap_endian((unsigned char *) &rectLength, 4);
355  swap_endian((unsigned char *) &maxSpeed, 2);
356  swap_endian((unsigned char *) &maxLength, 4);
357  float angleRefPointRad=(angleRefPoint/1e4+angScaleFactorOffset/1e4)*deg2rad;
358  float distRefPointMeter=(distRefPoint*distScaleFactor+distScaleFactorOffset)/1000.0f;
359  float rotAngleRad=(rotAngle/1e4)*deg2rad;
360  float rectWidthMeter=(rectWidth)/1000.0f;
361  float rectLengthMeter=(rectLength)/1000.0f;
362  float maxSpeedMeterPerSec=(maxSpeed)/1000.0f;
363  float maxLengthMeter=(maxLength)/1000.0f;
364  float points_x[8] = {0}, points_y[8] = {0};
365  SickScanMonFieldConverter::dynamicFieldPointToCarthesian(distRefPointMeter, angleRefPointRad, rotAngleRad, rectWidthMeter, rectLengthMeter, maxSpeedMeterPerSec, maxLengthMeter, points_x, points_y);
366  for(int n = 0; n < 8; n++)
367  monFields[fieldNumberFromCMD].pushFieldPointCarthesian(points_x[n], points_y[n]);
368  }
369 
370  return (exitCode);
371  }
372 
373 }
f
f
sick_scan::ExitSuccess
Definition: abstract_parser.h:46
sick_scan::ExitError
Definition: abstract_parser.h:47
swap_endian
void swap_endian(unsigned char *ptr, int numBytes)
Universal swapping function.
Definition: sick_scan_common.cpp:102
y
TFSIMD_FORCE_INLINE const tfScalar & y() const
deg2rad
#define deg2rad
Definition: BasicDatatypes.hpp:37
sick_scan::SickScanFieldMonSingleton
Definition: sick_generic_field_mon.h:151
sick_scan::SickScanFieldMonSingleton::getInstance
static SickScanFieldMonSingleton * getInstance()
Definition: sick_generic_field_mon.cpp:176
sick_scan::SickScanMonFieldConverter::rectangularFieldToCarthesian
static void rectangularFieldToCarthesian(float distRefPointMeter, float angleRefPointRad, float rotAngleRad, float rectWidthMeter, float rectLengthMeter, float points_x[4], float points_y[4])
Definition: sick_generic_field_mon.cpp:112
sick_scan::SickScanMonFieldConverter::dynamicFieldPointToCarthesian
static void dynamicFieldPointToCarthesian(float distRefPointMeter, float angleRefPointRad, float rotAngleRad, float rectWidthMeter, float rectLengthMeter, float maxSpeedMeterPerSec, float maxLengthMeter, float points_x[8], float points_y[8])
Definition: sick_generic_field_mon.cpp:158
sick_scan::SickScanMonFieldConverter::segmentedFieldPointToCarthesian
static void segmentedFieldPointToCarthesian(float range, float angle_rad, float &x, float &y)
Definition: sick_generic_field_mon.cpp:96
x
TFSIMD_FORCE_INLINE const tfScalar & x() const
sick_scan::SickScanFieldMonSingleton::parseAsciiDatagram
int parseAsciiDatagram(std::vector< unsigned char > datagramm)
Parsing Ascii datagram.
Definition: sick_generic_field_mon.cpp:235
sick_generic_field_mon.h
sick_scan::SickScanFieldMonSingleton::parseAsciiLIDinputstateMsg
int parseAsciiLIDinputstateMsg(unsigned char *datagram, int datagram_length)
Parse binary LIDinputstate message and set active field set.
Definition: sick_generic_field_mon.cpp:198
sick_scan_common.h
sick_scan
Definition: abstract_parser.cpp:50
sick_scan::SickScanMonFieldType
SickScanMonFieldType
Definition: sick_generic_field_mon.h:71
sick_scan::SickScanFieldMonSingleton::parseBinaryLIDinputstateMsg
int parseBinaryLIDinputstateMsg(unsigned char *datagram, int datagram_length)
Parse binary LIDinputstate message and set active field set.
Definition: sick_generic_field_mon.cpp:210
sick_scan::SickScanFieldMonSingleton::SickScanFieldMonSingleton
SickScanFieldMonSingleton()
Definition: sick_generic_field_mon.cpp:186
sick_scan::SickScanFieldMonSingleton::parseBinaryDatagram
int parseBinaryDatagram(std::vector< unsigned char > datagramm)
Definition: sick_generic_field_mon.cpp:242
ROS_ERROR
#define ROS_ERROR(...)
sick_scan::SickScanFieldMonSingleton::instance
static SickScanFieldMonSingleton * instance
Definition: sick_generic_field_mon.h:155

sick_scan
Author(s): Michael Lehning , Jochen Sprickerhof , Martin Günther
autogenerated on Wed May 5 2021 03:05:48