LdmrsSectorChangeApp.cpp
Go to the documentation of this file.
1 //
2 // LdmrsSectorChangeApp.cpp
3 //
4 // Demonstrates setting and checking the FlexibleResolution feature of the
5 // LDMRS. This feature is not available in all firmware versions.
6 //
7 // The App starts a thread that, after some time, sets the ResolutionMap of the scanner,
8 // activates it and later deactivates it.
9 //
10 // All changes to the sensor configuration are non-permanent.
11 //
12 
13 #include "LdmrsSectorChangeApp.hpp"
14 #include "../tools/errorhandler.hpp" // for printInfoMessage()
15 #include "../tools/toolbox.hpp" // for toString()
16 #include "../tools/MathToolbox.hpp"
17 #include "../datatypes/Scan.hpp"
18 #include "../datatypes/Object.hpp"
19 #include "../datatypes/Msg.hpp"
20 #include "../datatypes/Measurement.hpp"
21 #include "../datatypes/Fields.hpp"
22 #include "../datatypes/EvalCases.hpp"
23 #include "../datatypes/EvalCaseResults.hpp"
24 #include "../devices/LD_MRS.hpp"
25 
26 namespace application
27 {
28 
29 //
30 // Constructor
31 //
33  m_manager(manager)
34 {
35  m_beVerbose = true;
36 
37  printInfoMessage("LdmrsSectorChangeApp constructor done.", m_beVerbose);
38 
39  // Start the thread that changes the configuration.
40  if (m_changeThread.isRunning() == false)
41  {
42  m_changeThread.run(this);
43  }
44 }
45 
46 
47 // Destructor
48 // Clean up all dynamic data structures
50 {
51  printInfoMessage("LdmrsSectorChangeApp says Goodbye!", m_beVerbose);
52 }
53 
54 //
55 // Convert the current Sector or ResolutionMap (which is a vector of sectors) to a readable string.
56 //
57 std::string sectorToString(const ScannerInfo::ResolutionMap& sector)
58 {
59  UINT32 i = 0;
60  std::ostringstream oss;
61  oss << std::endl;
62  for(ScannerInfo::ResolutionMap::const_iterator s = sector.begin(); s != sector.end(); ++s)
63  {
64 
65  oss << "Sector (" << i << "): start=" << doubleToString(s->first*rad2deg, 3) << " res=" << doubleToString(s->second*rad2deg,3) << std::endl;
66  i++;
67  }
68 
69  return oss.str();
70 }
71 
73 {
74  Scan::const_iterator ret = iter;
75  for (++iter /*start with the next point*/ ; iter != scan.end(); ++iter)
76  {
77  if (iter->getLayer() == ret->getLayer())
78  {
79  ret = iter;
80  break;
81  }
82  }
83  return ret;
84 
85 }
86 
88 {
89  Scan::const_iterator ret = iter;
90  ++iter;
91  if (iter != scan.end())
92  {
93  ret = iter;
94  }
95  return ret;
96 
97 }
98 
100 {
101  if (scan.size() < 10)
102  {
103  // do not process on scans with too few scan points
104  return;
105  }
106 
107  // We need the scan in scanner coordinate system (sorted)
108  if ((scan.getFlags() & Scan::FlagVehicleCoordinates) != 0)
109  {
110  // in vehicle coordinate
111  printWarning("LdmrsSectorChangeApp::checkResolution: Scan is in vehicle coordinates and thus cannot be processed!");
112  return;
113  }
114 
115  // iterate through all scan points and make a diff of the angles
117  float angleDiff; // compute differences
118 
119  ScannerInfo::ResolutionMap sector; // first = start angle, second = resolution
120 
121  for (p = scan.begin(); p != scan.end(); ++p)
122  {
123  Scan::const_iterator p2 = getNextPoint(p, scan);
125  float interlaced = 1.;
126  if (fuzzyCompare(p2->getHAngle(), p3->getHAngle()))
127  {
128  // we are close to the border -> only 2 scan layers left
129  interlaced = 0.5;
130 
131  }
132  angleDiff = std::fabs(p2->getHAngle() - p->getHAngle()) * interlaced;
133 
134  if (angleDiff > 0)
135  {
136  if (sector.size() == 0)
137  {
138  sector.push_back(std::make_pair(p->getHAngle(), angleDiff));
139  }
140  else
141  {
142  // we detected a new resolution
143  if (fuzzyCompare(float(sector.back().second), angleDiff) == false)
144  {
145  sector.push_back(std::make_pair(p->getHAngle(),angleDiff));
146  }
147  }
148  }
149  }
150 
151  if (sector != m_lastMeasuredSector)
152  {
153  std::ostringstream oss;
154  oss << "Sector:" << sectorToString(sector);
155  printInfoMessage(oss.str(), m_beVerbose);
156  m_lastMeasuredSector = sector;
157  }
158 }
159 
160 //
161 // Receiver for new data from the manager.
162 //
164 {
165  // we are just interested in scan data
166  switch (data.getDatatype())
167  {
168  case Datatype_Scan:
169  {
170  Scan* scan = dynamic_cast<Scan*>(&data);
171 
172  if (scan)
173  {
174  checkResolution(*scan);
175  }
176  }
177  break;
178  default:
179  break;
180  }
181 }
182 
191 INT16 radian2Int (float radAngle)
192 {
193  float rad2ticks = float(devices::LuxBase::ANGULAR_TICKS_PER_ROTATION) / (2.f * PI); // conversion factor from [rad] to [ticks]
194  radAngle = normalizeRadians(radAngle); // shift [pi, 2*pi) to [-pi, 0)
195  INT16 angleInt = round_to_int<INT16> (radAngle * rad2ticks);
196  return angleInt;
197 }
198 
200 {
201  // read error variable
202  UINT32 code;
203 
205  if (scanner)
206  {
207  if(!scanner->getParameter(devices::ParaDetailedError, &code))
208  {
209  printWarning("NOT ABLE to to read detailed error code");
210  return false;
211  }
212  switch(code)
213  {
215  printInfoMessage("Detailed Error code: FlexResFreqInvalid", m_beVerbose);
216  break;
218  printInfoMessage("Detailed Error code: FlexResNumSectorsInvalid", m_beVerbose);
219  break;
221  printInfoMessage("Detailed Error code: FlexResNumShotsInvalid", m_beVerbose);
222  break;
224  printInfoMessage("Detailed Error code: FlexResResolutionInvalid", m_beVerbose);
225  break;
227  printInfoMessage("Detailed Error code: FlexResScannerNotIdle", m_beVerbose);
228  break;
230  printInfoMessage("Detailed Error code: FlexResSectorsOverlapping", m_beVerbose);
231  break;
233  printInfoMessage("Detailed Error code: FlexResSizeOneEighthSectorInvalid", m_beVerbose);
234  break;
235  default:
236  break;
237  }
238 
239  if (errCode != NULL)
240  {
241  *errCode = code;
242  }
243  }
244  else
245  {
246  return false;
247  }
248 
249  return true;
250 }
251 
253 {
254  if(configuredRM.size() == 0 || configuredRM.size() > 8)
255  {
256  printWarning("Invalid resolution map size.");
257  return false;
258  }
259 
261  if (scanner)
262  {
263  if (!scanner->setParameter(devices::ParaNumSectors, UINT32(configuredRM.size() ) ) )
264  {
265  printInfoMessage("LdmrsSectorChangeApp::changeFlexResConfiguration(): set NumSectors not successful", m_beVerbose);
267  return false;
268  }
269 
270  for (UINT32 i = 0; i < configuredRM.size(); ++i)
271  {
272  // set start angle
273  if (!scanner->setParameter((devices::MrsParameterId)(0x4001 + i), radian2Int(configuredRM[i].first) ) )
274  {
275  printInfoMessage("LdmrsSectorChangeApp::changeFlexResConfiguration(): set start angle not successful: " + doubleToString(configuredRM[i].first, 3) , m_beVerbose);
277  return false;
278  }
279  // set resolution
280  if (!scanner->setParameter((devices::MrsParameterId)(0x4009 + i), radian2Int(configuredRM[i].second) ) )
281  {
282  printInfoMessage("LdmrsSectorChangeApp::changeFlexResConfiguration(): set resolution not successful: " + doubleToString(configuredRM[i].first, 3) , m_beVerbose);
284  return false;
285  }
286  }
287  }
288 
289  return true;
290 }
291 
292 //
293 // Set the AngularResolutionType to either Focused, Constant or FlexRes.
294 //
296 {
298  if (scanner)
299  {
301  {
302 // printInfoMessage("LdmrsSectorChangeApp::changeThreadFunction(): set NumSectors to 9 not successful", m_beVerbose);
303  }
304  else
305  {
306 // traceNote(VERSION) << "NOT ABLE to change ResType to: type=" << type << std::endl;
308  return false;
309  }
310  }
311 
312  return true;
313 }
314 
315 //
316 // Thread that does the actual changing of parameters.
317 //
318 void LdmrsSectorChangeApp::changeThreadFunction(bool& endThread, UINT16& waitTimeMs)
319 {
320  printInfoMessage("LdmrsSectorChangeApp::changeThreadFunction(): started", m_beVerbose);
321 
323 
324  if (ldmrs)
325  {
326  ldmrs->setScanAngles(50.0 * deg2rad, -60.0 * deg2rad);
327 
328  UINT32 value = 0;
329  if (!ldmrs->getParameter(devices::ParaNumSectors, &value))
330  {
331  // could not read parameter
332  }
333  else
334  {
335  printInfoMessage("LdmrsSectorChangeApp::changeThreadFunction(): read parameter NumSectors=" + toString(value), m_beVerbose);
336  }
337 
338  if (!ldmrs->setParameter(devices::ParaNumSectors, 9))
339  {
340  // could not set parameter
341  printInfoMessage("LdmrsSectorChangeApp::changeThreadFunction(): set NumSectors to 9 not successful", m_beVerbose);
342 
343  // read detailed error
344  UINT32 code;
345  readDetailedErrorCode(&code);
346 
348  {
349  printWarning("Received wrong detailed error code.");
350  }
351  }
352 
353  // sleep some time to receive some scans first
354  UINT32 sleepTimeMs = 10000;
355  usleep(sleepTimeMs * 1000);
356 
357  ScannerInfo::ResolutionMap configuredRM;
358  configuredRM.push_back(std::make_pair(50.f * deg2rad, 1.f * deg2rad));
359  configuredRM.push_back(std::make_pair(35.f * deg2rad, .5f * deg2rad));
360  configuredRM.push_back(std::make_pair(30.f * deg2rad, .25f * deg2rad));
361  configuredRM.push_back(std::make_pair(20.f * deg2rad, .125f * deg2rad));
362  configuredRM.push_back(std::make_pair(0.f * deg2rad, .25f * deg2rad));
363  configuredRM.push_back(std::make_pair(-20.f * deg2rad, .5f * deg2rad));
364  configuredRM.push_back(std::make_pair(-31.f * deg2rad, 1.f * deg2rad));
365  configuredRM.push_back(std::make_pair(-40.f * deg2rad, .5f * deg2rad));
366 
367  changeFlexResConfiguration(configuredRM);
368 
370 
371  usleep(sleepTimeMs * 1000);
372 
373  // change back
375  }
376 
377  endThread = true;
378  waitTimeMs = 0;
379  printInfoMessage("LdmrsSectorChangeApp::changeThreadFunction(): All done, leaving.", m_beVerbose);
380 }
381 
382 } // namespace application
Scan::const_iterator getNextPoint(Scan::const_iterator iter, const Scan &scan)
std::string toString(const PositionWGS84::PositionWGS84SourceType &type)
UINT32 getFlags() const
Definition: Scan.hpp:168
const_iterator end() const
Definition: Scan.hpp:227
size_type size() const
Definition: Scan.hpp:110
#define rad2deg
uint16_t UINT16
Scan::const_iterator getNextPointInSameLayer(Scan::const_iterator iter, const Scan &scan)
std::string doubleToString(double val, std::string::size_type digits_before_decimal_point, std::string::size_type digits_after_decimal_point)
Definition: toolbox.cpp:307
devices::BasicDevice * getFirstDeviceByType(Sourcetype type)
Definition: manager.cpp:412
#define printInfoMessage(a, b)
bool setScanAngles(double startAngle, double endAngle)
Definition: LD_MRS.cpp:335
uint32_t UINT32
ScannerInfo::ResolutionMap m_lastMeasuredSector
INT16 radian2Int(float radAngle)
#define deg2rad
MrsParameterId
Definition: LuxBase.hpp:51
std::vector< std::pair< double, double > > ResolutionMap
Definition: ScannerInfo.hpp:39
SickThread< LdmrsSectorChangeApp,&LdmrsSectorChangeApp::changeThreadFunction > m_changeThread
bool readDetailedErrorCode(UINT32 *code=NULL)
AngularResolutionType
Definition: LuxBase.hpp:118
bool getParameter(MrsParameterId id, UINT32 *value)
Definition: LD_MRS.cpp:588
bool setParameter(MrsParameterId id, UINT32 value)
Definition: LD_MRS.cpp:593
const_iterator begin() const
Definition: Scan.hpp:221
bool changeFlexResConfiguration(const ScannerInfo::ResolutionMap &configuredRM)
bool changeAngularResolutionType(devices::AngularResolutionType type)
bool fuzzyCompare(double a, double b)
Tests if two double values are nearly equal.
Definition: MathToolbox.hpp:28
double normalizeRadians(double radians)
Definition: MathToolbox.cpp:31
PointList::const_iterator const_iterator
Definition: Scan.hpp:43
void changeThreadFunction(bool &endThread, UINT16 &waitTimeMs)
static const UINT32 ANGULAR_TICKS_PER_ROTATION
Definition: LuxBase.hpp:307
int16_t INT16
Bit 11: Scanpoint coordinate system; 0 = scanner coordinates, 1 = vehicle / reference coordinates...
Definition: Scan.hpp:51
void printWarning(std::string message)
#define PI
std::string sectorToString(const ScannerInfo::ResolutionMap &sector)


libsick_ldmrs
Author(s): SICK AG , Martin Günther , Jochen Sprickerhof
autogenerated on Sat Jun 8 2019 17:57:33