sick_scan_test.cpp
Go to the documentation of this file.
1 /*
2 * Copyright (C) 2018, Ing.-Buero Dr. Michael Lehning, Hildesheim
3 * Copyright (C) 2018, SICK AG, Waldkirch
4 * All rights reserved.
5 *
6 * Redistribution and use in source and binary forms, with or without
7 * modification, are permitted provided that the following conditions are met:
8 *
9 * * Redistributions of source code must retain the above copyright
10 * notice, this list of conditions and the following disclaimer.
11 * * Redistributions in binary form must reproduce the above copyright
12 * notice, this list of conditions and the following disclaimer in the
13 * documentation and/or other materials provided with the distribution.
14 * * Neither the name of Osnabr�ck University nor the names of its
15 * contributors may be used to endorse or promote products derived from
16 * this software without specific prior written permission.
17 * * Neither the name of SICK AG nor the names of its
18 * contributors may be used to endorse or promote products derived from
19 * this software without specific prior written permission
20 * * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its
21 * contributors may be used to endorse or promote products derived from
22 * this software without specific prior written permission
23 *
24 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
25 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
26 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
27 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
28 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
29 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
30 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
31 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
32 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
33 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34 * POSSIBILITY OF SUCH DAMAGE.
35 *
36 * Last modified: 27th Mar 2018
37 *
38 * Authors:
39 * Michael Lehning <michael.lehning@lehning.de>
40 *
41 */
42 
43 #define WIN32_LEAN_AND_MEAN
44 #include "boost/filesystem.hpp"
45 #include <boost/regex.hpp>
46 #include <boost/algorithm/string.hpp>
47 #include <algorithm> // for std::min
48 
49 
50 #include <stdio.h>
51 #include <stdlib.h>
52 #include <string.h>
53 
54 #include <iostream>
55 #ifndef _MSC_VER
56 #include <sys/wait.h>
57 #endif
58 #include <unistd.h>
59 #include <cstdio>
60 #include <cstdlib>
61 
62 
64 
65 #include <ros/console.h>
66 
67 #ifdef _MSC_VER
68 #include "sick_scan/rosconsole_simu.hpp"
69 #endif
70 
71 #include "tinystr.h"
72 #include "tinyxml.h"
73 
74 #define MAX_NAME_LEN (1024)
75 
76 // 001.000.000 Initial Version
77 #define SICK_SCAN_TEST_MAJOR_VER "001"
78 #define SICK_SCAN_TEST_MINOR_VER "000"
79 #define SICK_SCAN_TEST_PATCH_LEVEL "000"
80 
81 
82 
84 {
85 public:
86  paramEntryAscii(std::string _nameVal, std::string _typeVal, std::string _valueVal)
87  {
88  nameVal = _nameVal;
89  typeVal = _typeVal;
90  valueVal = _valueVal;
91  setCheckStatus(999,"untested");
92  minMaxGiven = false;
93  };
94 
95  void setPointerToXmlNode(TiXmlElement *paramEntryPtr)
96  {
97  this->nodePtr = paramEntryPtr;
98  }
99 
101  {
102  return( this->nodePtr);
103  }
104  void setValues(std::string _nameVal, std::string _typeVal, std::string _valueVal)
105  {
106  nameVal = _nameVal;
107  typeVal = _typeVal;
108  valueVal = _valueVal;
109  };
110 
111 
113  {
114  return(minMaxGiven);
115  }
116 
117  void setMinMaxValues(std::string _valueMinVal, std::string _valueMaxVal)
118  {
119 
120  valueMinVal = _valueMinVal;
121  valueMaxVal = _valueMaxVal;
122  minMaxGiven = true;
123 
124  };
125 
126  std::string getName()
127  {
128  return(nameVal);
129  }
130 
131  std::string getType()
132  {
133  return(typeVal);
134  }
135 
136  std::string getValue()
137  {
138  return(valueVal);
139  }
140 
141  std::string getMinValue()
142  {
143  return(valueMinVal);
144  }
145 
146  std::string getMaxValue()
147  {
148  return(valueMaxVal);
149  }
150 
151  void setCheckStatus(int errCode, std::string errMsg)
152  {
153  errorCode = errCode;
154  errorMsg = errMsg;
155  };
156 
158  {
159  return(errorCode);
160  }
161 
162  std::string getErrorMsg()
163  {
164  return(errorMsg);
165  }
166 
167 private:
168  std::string nameVal;
169  std::string typeVal;
170  std::string valueVal;
171  std::string valueMinVal;
172  std::string valueMaxVal;
175  std::string errorMsg;
177 };
178 
179 
180 
181 
182 
183 void sudokill(pid_t tokill)
184 {
185 #ifndef _MSC_VER
186  kill(tokill, SIGTERM);
187 #endif
188  sleep(5);
189 }
190 
191 std::vector<paramEntryAscii> getParamList(TiXmlNode *paramList)
192 {
193  std::vector<paramEntryAscii> tmpList;
194 
195 
196  TiXmlElement *paramEntry = (TiXmlElement *)paramList->FirstChild("param"); // first child
197  while (paramEntry)
198  {
199  std::string nameVal = "";
200  std::string typeVal = "";
201  std::string valueVal = "";
202  std::string minValueVal = "";
203  std::string maxValueVal = "";
204 
205  bool minValFnd = false;
206  bool maxValFnd = false;
207  // is this a param-node?
208  // if this is valid than process attributes
209  const char *val = paramEntry->Value();
210  bool searchAttributes = true;
211  if (strcmp(val,"param") == 0)
212  {
213  // expected value
214  }
215  else
216  {
217  searchAttributes = false;
218  }
219  if (paramEntry->Type() == TiXmlNode::TINYXML_ELEMENT)
220  {
221  // expected value
222  }
223  else
224  {
225  searchAttributes = false;
226  }
227  if (searchAttributes)
228  {
229  for (TiXmlAttribute* node = paramEntry->FirstAttribute(); ; node = node->Next())
230  {
231  const char *tag = node->Name();
232  const char *val = node->Value();
233 
234  if (strcmp(tag, "name") == 0)
235  {
236  nameVal = val;
237  }
238  if (strcmp(tag, "type") == 0)
239  {
240  typeVal = val;
241  }
242  if (strcmp(tag, "value") == 0)
243  {
244  valueVal = val;
245  }
246  if (strcmp(tag, "valueMin") == 0)
247  {
248  minValFnd = true;
249  minValueVal = val;
250 
251  }
252  if (strcmp(tag, "valueMax") == 0)
253  {
254  maxValFnd = true;
255  maxValueVal = val;
256  }
257  if (node == paramEntry->LastAttribute())
258  {
259 
260  break;
261  }
262  }
263 
264  paramEntryAscii tmpEntry(nameVal, typeVal, valueVal);
265  if (maxValFnd && minValFnd)
266  {
267  tmpEntry.setMinMaxValues(minValueVal, maxValueVal);
268  }
269 
270  tmpEntry.setPointerToXmlNode(paramEntry);
271  tmpList.push_back(tmpEntry);
272  }
273  paramEntry = (TiXmlElement *)paramEntry->NextSibling(); // go to next sibling
274  }
275 
276  return(tmpList);
277 }
278 
279 
280 
281 void replaceParamList(TiXmlNode * node, std::vector<paramEntryAscii> paramOrgList)
282 {
283  bool fndParam = false;
284  do
285  {
286  fndParam = false;
287  TiXmlElement *paramEntry = (TiXmlElement *)node->FirstChild("param");
288  if (paramEntry != NULL)
289  {
290  fndParam = true;
291  node->RemoveChild(paramEntry);
292  }
293  } while (fndParam == true);
294 
295  for (int i = 0; i < paramOrgList.size(); i++)
296  {
297  paramEntryAscii tmp = paramOrgList[i];
298  TiXmlElement *paramTmp = new TiXmlElement("param");
299  paramTmp->SetAttribute("name", tmp.getName().c_str());
300  paramTmp->SetAttribute("type", tmp.getType().c_str());
301  paramTmp->SetAttribute("value", tmp.getValue().c_str());
302  node->LinkEndChild(paramTmp);
303  }
304 }
305 
306 pid_t pid;
307 
308 pid_t launchRosFile(std::string cmdLine)
309 {
310  std::string cmd = cmdLine;
311  typedef std::vector<std::string > split_vector_type;
312 
313  split_vector_type splitVec; // #2: Search for tokens
314  boost::split(splitVec, cmdLine, boost::is_any_of(" "), boost::token_compress_on); // SplitVec == { "hello abc","ABC","aBc goodbye" }
315 
316 
317 
318  pid_t pid = fork(); // create child process
319  int status;
320 
321  switch (pid)
322  {
323  case -1: // error
324  perror("fork");
325  exit(1);
326 
327  case 0: // child process
328  execl("/opt/ros/kinetic/bin/roslaunch", splitVec[0].c_str(), splitVec[1].c_str(), splitVec[2].c_str(), NULL); // run the command
329  perror("execl"); // execl doesn't return unless there is a problem
330  exit(1);
331 
332  default: // parent process, pid now contains the child pid
333  /*
334  while (-1 == waitpid(pid, &status, 0)); // wait for child to complete
335  if (!WIFEXITED(status) || WEXITSTATUS(status) != 0)
336  {
337  // handle error
338  std::cerr << "process " << cmd << " (pid=" << pid << ") failed" << std::endl;
339  }
340  */
341  break;
342 
343  }
344 
345  return(pid);
346 }
347 
348 
349 int createTestLaunchFile(std::string launchFileFullName, std::vector<paramEntryAscii> entryList, std::string& testLaunchFile)
350 {
351  printf("Try loading launchfile :%s",launchFileFullName.c_str());
352  TiXmlDocument doc;
353  doc.LoadFile(launchFileFullName.c_str());
354 
355  if (doc.Error() == true)
356  {
357  ROS_ERROR("ERROR parsing launch file %s\nRow: %d\nCol: %d", doc.ErrorDesc(), doc.ErrorRow(), doc.ErrorCol());
358  return(-1);
359  }
360  TiXmlNode *node = doc.FirstChild("launch");
361  if (node != NULL)
362  {
363  node = node->FirstChild("node");
364  std::vector<paramEntryAscii> paramOrgList = getParamList(node);
365  for (int i = 0; i < entryList.size(); i++)
366  {
367  bool replaceEntry = false;
368  for (int j = 0; j < paramOrgList.size(); j++)
369  {
370  if (entryList[i].getName().compare(paramOrgList[j].getName()) == 0)
371  {
372  paramOrgList[j].setValues(entryList[i].getName(), entryList[i].getType(), entryList[i].getValue());
373  replaceEntry = true;
374  }
375  }
376  if (replaceEntry == false) // add new one to list
377  {
378  paramEntryAscii tmp(entryList[i].getName(), entryList[i].getType(), entryList[i].getValue());
379  paramOrgList.push_back(tmp);
380  }
381  }
382  // delete all parameters and replace with new one
383  replaceParamList(node, paramOrgList);
384  }
385 
386  // append source extension if any
387  size_t pos = launchFileFullName.rfind('.');
388  std::string resultFileName = "";
389  testLaunchFile = "";
390  if (pos != std::string::npos)
391  {
392  resultFileName = launchFileFullName.substr(0, pos);
393  resultFileName += "_test.launch";
394  doc.SaveFile(resultFileName.c_str());
395  testLaunchFile = resultFileName;
396  }
397  return(0);
398 }
399 
400 int cloud_width = 0; // hacky
401 int cloud_height = 0;
402 int callbackCnt = 0;
403 double intensityStdDev = 0.0;
404 double rangeStdDev = 0.0;
405 
406 
407 //
409 
410 
411 void cloud_callback(const sensor_msgs::PointCloud2ConstPtr& cloud_msg)
412 {
413  static int cnt = 0;
414  callbackCnt++;
415  cloud_width = cloud_msg->width;
416  cloud_height = cloud_msg->height;
417  int intensity_idx = -1;
418  int cartesianIdxArr[3];
419  for (int i = 0; i < 3; i++)
420  {
421  cartesianIdxArr[i] = -1;
422  }
423 
424 
425  for (int i = 0; i < cloud_msg->fields.size(); i++)
426  {
427  std::string fieldName = cloud_msg->fields[i].name;
428  if (fieldName.compare("intensity") == 0)
429  {
430  intensity_idx = i;
431  }
432  if (fieldName.compare("x") == 0)
433  {
434  cartesianIdxArr[0] = i;
435  }
436  if (fieldName.compare("y") == 0)
437  {
438  cartesianIdxArr[1] = i;
439  }
440  if (fieldName.compare("z") == 0)
441  {
442  cartesianIdxArr[2] = i;
443  }
444 
445  }
446  if (intensity_idx != -1)
447  {
448  float intensitySum = 0.0;
449  float intensity2Sum = 0.0;
450  int intensityNum = cloud_width * cloud_height;
451  for (int i = 0; i < cloud_height; i++)
452  for (int j = 0; j < cloud_width; j++)
453  {
454  int idx = i * cloud_width + j;
455  idx *= cloud_msg->point_step; // calculate byte address offset
456  int relOff = cloud_msg->fields[intensity_idx].offset;
457  float *intensityPtr = (float *)(&(cloud_msg->data[idx+relOff]));
458  float intensity = *intensityPtr;
459  intensitySum += intensity;
460  intensity2Sum += intensity * intensity;
461  }
462  if (intensityNum > 1)
463  {
464  intensityStdDev = (intensity2Sum - 1.0 / intensityNum * (intensitySum * intensitySum));
465  {
466  intensityStdDev /= (intensityNum - 1);
468  }
469  }
470  }
471 
472  if (cartesianIdxArr[0] != -1 &&cartesianIdxArr[1] != -1 &&cartesianIdxArr[2] != -1)
473  {
474  float rangeSum = 0.0;
475  float range2Sum = 0.0;
476  int rangeNum = cloud_width * cloud_height;
477  for (int i = 0; i < cloud_height; i++)
478  for (int j = 0; j < cloud_width; j++)
479  {
480  int idx = i * cloud_width + j;
481  idx *= cloud_msg->point_step; // calculate byte address offset
482  int relOffX = cloud_msg->fields[cartesianIdxArr[0]].offset;
483  float *XPtr = (float *)(&(cloud_msg->data[idx+relOffX]));
484  float X = *XPtr;
485  int relOffY = cloud_msg->fields[cartesianIdxArr[1]].offset;
486  float *YPtr = (float *)(&(cloud_msg->data[idx+relOffY]));
487  float Y = *YPtr;
488  int relOffZ = cloud_msg->fields[cartesianIdxArr[2]].offset;
489  float *ZPtr = (float *)(&(cloud_msg->data[idx+relOffZ]));
490  float Z = *ZPtr;
491  float range =sqrt(X*X+Y*Y+Z*Z);
492  rangeSum += range;
493  range2Sum += range * range;
494  }
495  if (rangeNum > 1)
496  {
497  rangeStdDev = (range2Sum - 1.0 / rangeNum * (rangeSum * rangeSum));
498  {
499  rangeStdDev /= (rangeNum - 1);
501  }
502  }
503  }
504  ROS_INFO("valid PointCloud2 message received");
505 
506 }
507 
508 float ERR_CONST = -1E3;
509 float scan_angle_min = -1E3;
510 float scan_angle_max = -1E3;
511 float scan_time_increment = -1E3;
512 float scan_angle_increment = -1E3;
513 float range_min = -1E3;
514 float range_max = -1E3;
515 
516 
517 void processLaserScan(const sensor_msgs::LaserScan::ConstPtr& scan)
518 {
519  if (callbackScanCnt == 0)
520  {
521  scan_angle_min = scan->angle_min;
522  scan_angle_max = scan->angle_max;
523  scan_time_increment = scan->time_increment;
524  scan_angle_increment = scan->angle_increment;
525  range_min = scan->range_min;
526  range_max = scan->range_max;
527  }
528  callbackScanCnt++;
529 }
530 
531 
532 int startCallbackTest(int argc, char** argv)
533 {
534 
535  ros::NodeHandle nh;
536  ros::Rate loop_rate(10);
537  ros::Subscriber sub;
538  ros::Subscriber scanSub;
539  ROS_INFO("Cloudtester started."
540  );
541  sub = nh.subscribe("cloud", 1, cloud_callback);
542  scanSub=nh.subscribe<sensor_msgs::LaserScan>("/scan",10,processLaserScan);
543 
544  while (callbackCnt < 10)
545  {
546  ros::spinOnce();
547  }
548  sub.shutdown();
549  ros::shutdown();
550 
552  return(0);
553 
554 }
555 
556 
557 void createPrefixFromExeName(std::string exeName, std::string& prefix)
558 {
559 #ifdef _MSC_VER
560 #else
561 #endif
562  std::string searchPattern = "(.*)cmake\\-build\\-debug.*";
563  boost::regex expr(searchPattern);
564  boost::smatch what;
565 
566  if (boost::regex_search(exeName, what, expr))
567  {
568  std::string prefixPath = what[1]; // started from CLION IDE - build complete path
569  prefix = prefixPath;
570  prefix += "test";
571  }
572  else
573  {
574  prefix = ".";
575  }
576 
577 }
578 
579 
580 void searchForXmlFileInPathList(std::string pathList, std::string& packagePath, std::string xmlFileName)
581 {
582  typedef std::vector<std::string > split_vector_type;
583  packagePath = "???";
584  split_vector_type splitVec; // #2: Search for tokens
585  boost::split(splitVec, pathList, boost::is_any_of(":"), boost::token_compress_on); // SplitVec == { "hello abc","ABC","aBc goodbye" }
586  if (splitVec.size() > 0)
587  {
588  packagePath = splitVec[0];
589  }
590 
591  for (int i = 0; i < splitVec.size(); i++)
592  {
593  FILE *fin;
594  std::string tmpFileName = splitVec[i] + "/" + xmlFileName;
595  fin = fopen(tmpFileName.c_str(),"r");
596  if (fin != NULL)
597  {
598  packagePath = splitVec[i];
599  fclose(fin);
600  }
601  }
602 }
603 
604 bool getPackageRootFolder(std::string exePath, std::string& packageRootFolder)
605 {
606  packageRootFolder = "";
607  // try to retrive package name from exePat
608  typedef std::vector<std::string > split_vector_type;
609  split_vector_type splitVec; // #2: Search for tokens
610  boost::split(splitVec, exePath, boost::is_any_of("/"), boost::token_compress_on); // SplitVec == { "hello abc","ABC","aBc goodbye" }
611  bool bFnd = false;
612  for (int i = splitVec.size() - 1; i >= 0; i--) // last part is exe-Name - so forget this
613  {
614  std::string tmpPath = "";
615  for (int j = 1; j < i; j++)
616  {
617  tmpPath += '/';
618  tmpPath += splitVec[j];
619  }
620 
621  std::string fileTmpPath = tmpPath;
622  fileTmpPath += "/package.xml";
623  if (boost::filesystem::exists(fileTmpPath))
624  {
625  packageRootFolder = tmpPath;
626  bFnd = true;
627  break;
628  }
629  }
630 
631  return(bFnd);
632 
633 }
634 
641 int main(int argc, char **argv)
642 {
643  int result = 0;
644  char sep = boost::filesystem::path::preferred_separator;
645  std::string prefix = ".";
646  std::string packageRootFolder;
647 
648  std::string exeName = argv[0];
649 
650  if (argc == 1)
651  {
652  printf("Usage: %s <XML-Test-Ctrl-File>\n", exeName.c_str());
653  printf("\n");
654  printf("The XML-Test-Ctrl-File controls the parameter checking of various scanner types.");
655  printf("The XML-File sick_scan_test.xml in the test-Directory shows an example of this file type.\n");
656  exit(-1);
657  }
658  else
659  {
660  printf("Given program arguments:\n");
661  for (int i = 0; i < argc; i++)
662  {
663  printf("%d: %s\n", i, argv[i]);
664  }
665  }
666 #if 0
667  createPrefixFromExeName(exeName, prefix);
668 #ifdef _MSC_VER
669  prefix = std::string("..") + sep + std::string("..") + sep + "test";
670 #endif
672 #endif
673 
674  ros::init(argc, argv, "cloudtester");
675 
676  std::string nodeName = ros::this_node::getName();
677 
678  printf("Nodename: %s\n", nodeName.c_str());
679  bool bFnd = getPackageRootFolder(argv[0], packageRootFolder);
680 
681  printf("Package Path: %s\n", packageRootFolder.c_str());
682 
683  char* pPath;
684  pPath = getenv("ROS_PACKAGE_PATH");
685 #ifdef _MSC_VER
686  pPath = "..\\..:\tmp";
687 #endif
688  if (pPath != NULL)
689  {
690  ROS_INFO("Current root path entries for launch file search: %s\n", pPath);
691  }
692  else
693  {
694  ROS_FATAL("Cannot find ROS_PACKAGE_PATH - please run <Workspace>/devel/setup.bash");
695  }
696 
697  std::string packagePath;
698  // try to find xml-file in path list ... and give the result back in the variable packagePath
699  std::string testCtrlXmlFileName = argv[1];
700  searchForXmlFileInPathList(pPath, packagePath, testCtrlXmlFileName);
701 
702 // std::string testCtrlXmlFileName = packagePath + std::string(1, sep) + "sick_scan" + std::string(1, sep) + "test" + std::string(1, sep) + "sick_scan_test.xml";
703 // $ROS_PACKAGE_PATH/sick_scan/test/sick_scan_test.xml
704  boost::replace_all(testCtrlXmlFileName, "$ROS_PACKAGE_PATH", packagePath);
705 
706 
707  testCtrlXmlFileName = packagePath + std::string(1,'/') + testCtrlXmlFileName;
708  TiXmlDocument doc;
709  doc.LoadFile(testCtrlXmlFileName.c_str());
710  if (doc.Error())
711  {
712  ROS_ERROR("Package root folder %s", packageRootFolder.c_str());
713  ROS_ERROR("Cannot load/parse %s", testCtrlXmlFileName.c_str());
714  ROS_ERROR("Details: %s\n", doc.ErrorDesc());
715  exit(-1);
716  }
717 
718  bool launch_only = false;
719  boost::filesystem::path p(testCtrlXmlFileName);
720  boost::filesystem::path parentPath = p.parent_path();
721  std::string pathName = parentPath.string();
722  std::string launchFilePrefix = parentPath.parent_path().string() + std::string(1, sep) + "launch" + std::string(1, sep);
723  TiXmlNode *node = doc.FirstChild("launch");
724  if (node == NULL)
725  {
726  ROS_ERROR("Cannot find tag <launch>\n");
727  exit(-1);
728  }
729 
730  if (node)
731  {
732  TiXmlElement *fileNameEntry;
733  fileNameEntry = (TiXmlElement *)node->FirstChild("filename");
734  if (fileNameEntry == NULL)
735  {
736  ROS_ERROR("Cannot find tag <filename>\n");
737  exit(-1);
738  }
739  TiXmlText *fileNameVal = (TiXmlText *)fileNameEntry->FirstChild();
740  if (fileNameVal != NULL)
741  {
742  ROS_INFO("Processing launch file %s\n", fileNameVal->Value());
743  std::string launchFileFullName = launchFilePrefix + fileNameVal->Value();
744  ROS_INFO("Processing %s\n", launchFileFullName.c_str());
745  TiXmlNode *paramListNode = (TiXmlNode *)node->FirstChild("paramList");
746  if (paramListNode != NULL)
747  {
748  std::vector<paramEntryAscii> paramList = getParamList(paramListNode);
749  for (int i = 0; i < paramList.size(); i++)
750  {
751  if (paramList[i].getName().compare("launch_only") == 0)
752  {
753  if (paramList[i].getValue().compare("true") == 0)
754  {
755  printf("launch_only set to true. Just modifying launch file and launch (without testing).");
756  launch_only = true;
757  }
758  }
759 
760 
761  }
762 
763  std::string testLaunchFile;
764  createTestLaunchFile(launchFileFullName, paramList, testLaunchFile);
765  std::string commandLine;
766  boost::filesystem::path p(testLaunchFile);
767  std::string testOnlyLaunchFileName = p.filename().string();
768  commandLine = "roslaunch sick_scan " + testOnlyLaunchFileName;
769  ROS_INFO("launch ROS test ... [%s]", commandLine.c_str());
770 
771  // int result = std::system(commandLine.c_str());
772 
773  pid_t pidId = launchRosFile(commandLine.c_str());
774 
775  if (launch_only)
776  {
777  printf("Launch file [ %s ] started ...\n", commandLine.c_str());
778  printf("No further testing...\n");
779  exit(0);
780  }
781  startCallbackTest(argc, argv); // get 10 pointcloud messages
782 
784  ROS_INFO("If you receive an error message like \"... is neither a launch file ... \""
785  "please source ROS env. via source ./devel/setup.bash");
786 
787 
788  TiXmlNode *resultListNode = (TiXmlNode *)node->FirstChild("resultList");
789  if (resultListNode != NULL)
790  {
791  std::vector<paramEntryAscii> resultList = getParamList(resultListNode);
792 
793  enum KeyWord_Idx {MIN_ANG_KEYWORD_IDX, MAX_ANG_KEYWORD_IDX,
794  SCAN_TIME_INCREMENT_KEYWORD_IDX,
795  SCAN_ANGLE_INCREMENT_KEYWORD_IDX,
796  MIN_RANGE_KEYWORD_IDX,
797  MAX_RANGE_KEYWORD_IDX,
798  KEYWORD_IDX_NUM};
799 
800  std::vector<std::string> keyWordList;
801  keyWordList.resize(KEYWORD_IDX_NUM);
802 
803  keyWordList[MIN_ANG_KEYWORD_IDX] = "min_ang";
804  keyWordList[MAX_ANG_KEYWORD_IDX] = "max_ang";
805  keyWordList[MIN_RANGE_KEYWORD_IDX] = "range_min";
806  keyWordList[MAX_RANGE_KEYWORD_IDX] = "range_max";
807  keyWordList[SCAN_ANGLE_INCREMENT_KEYWORD_IDX] = "angle_increment";
808  keyWordList[SCAN_TIME_INCREMENT_KEYWORD_IDX] = "time_increment";
809 
810  for (int i = 0; i < resultList.size(); i++)
811  {
812 
813  for (int ki = 0; ki < keyWordList.size(); ki++)
814  {
815 
816  float measurementVal = .0F;
817  std::string keyWordTag = keyWordList[ki];
818 
819  switch(ki)
820  {
821  case MIN_ANG_KEYWORD_IDX: measurementVal = scan_angle_min; break;
822  case MAX_ANG_KEYWORD_IDX: measurementVal = scan_angle_max; break;
823  case SCAN_ANGLE_INCREMENT_KEYWORD_IDX: measurementVal = scan_angle_increment; break;
824  case SCAN_TIME_INCREMENT_KEYWORD_IDX: measurementVal = scan_time_increment; break;
825  case MIN_RANGE_KEYWORD_IDX: measurementVal = range_min; break;
826  case MAX_RANGE_KEYWORD_IDX: measurementVal = range_max; break;
827  default: printf("Did not find a correspondence for index %d\n", ki); break;
828  }
829  char errMsg[255] = {0};
830  if (keyWordTag.compare("min_ang") == 0)
831  {
832  measurementVal = scan_angle_min;
833  }
834  if (resultList[i].getName().compare(keyWordTag) == 0)
835  {
836 
837  enum Expected_Idx {MIN_EXP_IDX, DEF_EXP_IDX, MAX_EXP_IDX, EXP_IDX_NUM};
838  float expectedValues[EXP_IDX_NUM];
839  std::string valString;
840 
841  for (int j = 0; j < EXP_IDX_NUM; j++)
842  {
843  switch(j)
844  {
845  case MIN_EXP_IDX: valString = resultList[i].getMinValue(); break;
846  case DEF_EXP_IDX: valString = resultList[i].getValue(); break;
847  case MAX_EXP_IDX: valString = resultList[i].getMaxValue(); break;
848  default: ROS_ERROR("Check index"); break;
849 
850  }
851 
852  const char *valPtr = valString.c_str();
853  if (valPtr != NULL)
854  {
855  float tmpVal = .0F;
856  sscanf(valPtr, "%f", &tmpVal);
857  expectedValues[j] = tmpVal;
858  }
859  }
860  valString = resultList[i].getMinValue();
861 
862  int errorCode = 0;
863  std::string resultStr = "FAILED";
864  std::string appendResultStr = " <-----";
865  if ( (measurementVal >= expectedValues[MIN_EXP_IDX]) && (measurementVal <= expectedValues[MAX_EXP_IDX]))
866  {
867  appendResultStr = "";
868  resultStr = "PASSED";
869  errorCode = 0;
870  }
871  else
872  {
873  errorCode = 1;
874  }
875  sprintf(errMsg, "CHECK %s! Key: %-20s %14.9f in [%14.9f,%14.9f] %s", resultStr.c_str(), keyWordTag.c_str(), measurementVal,
876  expectedValues[MIN_EXP_IDX], expectedValues[MAX_EXP_IDX], appendResultStr.c_str());
877 
878  printf("%s\n", errMsg);
879  resultList[i].setCheckStatus(errorCode, (errorCode == 0) ? "OK" : errMsg);
880 
881  }
882  }
883 
884 
885  if (resultList[i].getName().compare("shotsPerLayer") == 0)
886  {
887  int expectedWidth = -1;
888  std::string valString = resultList[i].getValue();
889  const char *valPtr = valString.c_str();
890  if (valPtr != NULL)
891  {
892  sscanf(valPtr, "%d", &expectedWidth);
893  }
894 
895 
896  ROS_INFO("Test\n");
897  int errorCode = 0;
898  if (expectedWidth == cloud_width)
899  {
900  printf("CHECK PASSED! WIDTH %d == %d\n", expectedWidth, cloud_width);
901  errorCode = 0;
902  }
903  else
904  {
905  printf("CHECK FAILED! WIDTH %d <> %d\n", expectedWidth, cloud_width);
906  errorCode = 1;
907 
908  }
909  resultList[i].setCheckStatus(errorCode, (errorCode == 0) ? "OK" : "Unexpected number of shots");
910 
911  }
912  if (resultList[i].getName().compare("pointCloud2Height") == 0)
913  {
914  int expectedHeight = -1;
915  std::string valString = resultList[i].getValue();
916  const char *valPtr = valString.c_str();
917  if (valPtr != NULL)
918  {
919  sscanf(valPtr, "%d", &expectedHeight);
920  }
921  ROS_INFO("Test\n");
922  int errorCode = 0;
923  if (expectedHeight == cloud_height)
924  {
925  printf("CHECK PASSED! HEIGHT %d == %d\n", expectedHeight, cloud_height);
926  errorCode = 0;
927  }
928  else
929  {
930  printf("CHECK FAILED! HEIGHT %d <> %d\n", expectedHeight, cloud_height);
931  errorCode = 1;
932  }
933  resultList[i].setCheckStatus(errorCode, (errorCode == 0) ? "OK" : "Unexpected pointCloud2 height");
934  cloud_height = 0;
935  }
936 
937  if (resultList[i].getName().compare("RSSIEnabled") == 0)
938  {
939  int expectedWidth = -1;
940  std::string valString = resultList[i].getValue();
941  bool rssiEnabled = false;
942  if (boost::iequals(valString, "true"))
943  {
944  rssiEnabled = true;
945  } else if (boost::iequals(valString, "false"))
946  {
947  rssiEnabled = false;
948  } else
949  {
950  ROS_WARN("RSSIEnabled parameter wrong. True or False are valid parameters!\n");
951  }
952 
953  ROS_INFO("Test\n");
954  int errorCode = 0;
955  if (rssiEnabled == true)
956  {
957  if (intensityStdDev >= 1e-5)
958  {
959  printf("CHECK PASSED! RSSI Standard deviation %.3e is not 0.\n",intensityStdDev);
960  errorCode = 0;
961  } else
962  {
963  printf("CHECK FAILED! RSSI standard deviation is samller than %.10e even though RSSI is enabled\n",intensityStdDev);
964  errorCode = 1;
965 
966  }
967  }
968 
969  if (rssiEnabled == false)
970  {
971  if (intensityStdDev <= 1e-5)
972  {
973  printf("CHECK FAILED! RSSI standard deviation %.3e is bigger than 1e-5 even though RSSI is disabled\n",intensityStdDev);
974  errorCode = 1;
975  } else
976  {
977  printf("CHECK PASSED! RSSI Standard deviation %.3e is smaller than 1e-5.\n",intensityStdDev);
978  errorCode = 0;
979 
980  }
981  }
982  }
983  if (resultList[i].getName().compare("ranges") == 0)
984  {
985  int expectedWidth = -1;
986  std::string valString = resultList[i].getValue();
987  bool rangeTest=false;
988  if (boost::iequals(valString, "true"))
989  {
990  rangeTest=true;
991  }
992  else
993  {
994  ROS_WARN("ranges parameter wrong. True or False are valid parameters!\n");
995  }
996 
997  ROS_INFO("Test\n");
998  int errorCode = 0;
999  if (rangeTest==true)
1000  {
1001  if (rangeStdDev >= 1e-5)
1002  {
1003  printf("CHECK PASSED! Range Standard deviation %.3e is not 0.\n", rangeStdDev);
1004  errorCode = 0;
1005  }
1006  else
1007  {
1008  printf("CHECK FAILED! Range standard deviation %.3e is smaller than 1e-5!\n", rangeStdDev);
1009  errorCode = 1;
1010 
1011  }
1012  resultList[i].setCheckStatus(errorCode, (errorCode == 0) ? "OK" : "Range standard deviation is 0 !\n");
1013  }
1014 
1015 
1016  }
1017  TiXmlElement *paramSet = resultList[i].getPointerToXmlNode();
1018  paramSet->SetAttribute("errorCode", resultList[i].getErrorCode());
1019  paramSet->SetAttribute("errorMsg", resultList[i].getErrorMsg().c_str());
1020  }
1021 
1022  }
1023 
1024  printf("Killing process %d\n", pidId);
1025  sudokill(pidId);
1026 
1027  std::string testCtrlResultXmlFileName = "";
1028  size_t pos = testCtrlXmlFileName.rfind('.');
1029  if (pos != std::string::npos)
1030  {
1031  boost::filesystem::path tmpFilePath =testCtrlXmlFileName.substr(0,pos);
1032  boost::filesystem::path xmlDir= tmpFilePath.parent_path();
1033  std::string xmlDirName = xmlDir.string();
1034  xmlDirName.append("/results");
1035  if (boost::filesystem::exists(xmlDirName))
1036  {
1037 
1038  }
1039  else
1040  {
1041  boost::filesystem::create_directory(xmlDirName);
1042  }
1043  std::string tmpFilNamewoExtension= tmpFilePath.stem().string();
1044  std::string resultFileName=xmlDir.string();
1045  resultFileName.append("/");
1046  resultFileName.append(tmpFilNamewoExtension);
1047  resultFileName.append(".res");
1048  printf("Save to %s\n", resultFileName.c_str());
1049  doc.SaveFile(resultFileName.c_str());
1050  }
1051 
1052  }
1053  }
1054  else
1055  {
1056  ROS_WARN("Cannot find filename entry");
1057  }
1058 
1059 
1060  }
1061  return result;
1062 
1063 }
1064 
paramEntryAscii::valueMinVal
std::string valueMinVal
Definition: sick_scan_test.cpp:171
TiXmlElement
Definition: tinyxml.h:1073
replaceParamList
void replaceParamList(TiXmlNode *node, std::vector< paramEntryAscii > paramOrgList)
Definition: sick_scan_test.cpp:281
paramEntryAscii::setPointerToXmlNode
void setPointerToXmlNode(TiXmlElement *paramEntryPtr)
Definition: sick_scan_test.cpp:95
TiXmlDocument
Definition: tinyxml.h:1592
paramEntryAscii::isMinMaxGiven
bool isMinMaxGiven()
Definition: sick_scan_test.cpp:112
sick_generic_laser.h
ROS::sleep
void sleep(double seconds)
Definition: ros_wrapper.cpp:110
paramEntryAscii::nodePtr
TiXmlElement * nodePtr
Definition: sick_scan_test.cpp:176
processLaserScan
void processLaserScan(const sensor_msgs::LaserScan::ConstPtr &scan)
Definition: sick_scan_test.cpp:517
paramEntryAscii
Definition: sick_scan_test.cpp:83
ros::init
ROSCPP_DECL void init(const M_string &remappings, const std::string &name, uint32_t options=0)
scan_angle_min
float scan_angle_min
Definition: sick_scan_test.cpp:509
TiXmlElement::LastAttribute
TiXmlAttribute * LastAttribute()
Definition: tinyxml.h:1231
scan_angle_increment
float scan_angle_increment
Definition: sick_scan_test.cpp:512
createTestLaunchFile
int createTestLaunchFile(std::string launchFileFullName, std::vector< paramEntryAscii > entryList, std::string &testLaunchFile)
Definition: sick_scan_test.cpp:349
TiXmlNode::FirstChild
TiXmlNode * FirstChild()
Definition: tinyxml.h:580
TiXmlNode::Type
int Type() const
Definition: tinyxml.h:772
getName
ROSCONSOLE_CONSOLE_IMPL_DECL std::string getName(void *handle)
TiXmlNode::TINYXML_ELEMENT
@ TINYXML_ELEMENT
Definition: tinyxml.h:515
TiXmlNode::NextSibling
const TiXmlNode * NextSibling() const
Navigate to a sibling node.
Definition: tinyxml.h:706
ros::spinOnce
ROSCPP_DECL void spinOnce()
tinyxml.h
ros::shutdown
ROSCPP_DECL void shutdown()
paramEntryAscii::getMinValue
std::string getMinValue()
Definition: sick_scan_test.cpp:141
paramEntryAscii::getValue
std::string getValue()
Definition: sick_scan_test.cpp:136
main
int main(int argc, char **argv)
Startup routine.
Definition: sick_scan_test.cpp:641
ros::Subscriber::shutdown
void shutdown()
TiXmlDocument::Error
bool Error() const
Definition: tinyxml.h:1670
paramEntryAscii::valueMaxVal
std::string valueMaxVal
Definition: sick_scan_test.cpp:172
TiXmlNode::LinkEndChild
TiXmlNode * LinkEndChild(TiXmlNode *addThis)
Definition: tinyxml.cpp:186
ros::console::set_logger_level
ROSCONSOLE_DECL bool set_logger_level(const std::string &name, levels::Level level)
searchForXmlFileInPathList
void searchForXmlFileInPathList(std::string pathList, std::string &packagePath, std::string xmlFileName)
Definition: sick_scan_test.cpp:580
paramEntryAscii::paramEntryAscii
paramEntryAscii(std::string _nameVal, std::string _typeVal, std::string _valueVal)
Definition: sick_scan_test.cpp:86
TiXmlDocument::ErrorRow
int ErrorRow() const
Definition: tinyxml.h:1690
console.h
callbackScanCnt
int callbackScanCnt
Definition: sick_scan_test.cpp:408
ros::console::levels::Debug
Debug
rangeStdDev
double rangeStdDev
Definition: sick_scan_test.cpp:404
tinystr.h
paramEntryAscii::minMaxGiven
bool minMaxGiven
Definition: sick_scan_test.cpp:173
TiXmlDocument::SaveFile
bool SaveFile() const
Save a file using the current document value. Returns true if successful.
Definition: tinyxml.cpp:960
SICK_SCAN_TEST_MAJOR_VER
#define SICK_SCAN_TEST_MAJOR_VER
Definition: sick_scan_test.cpp:77
cloud_callback
void cloud_callback(const sensor_msgs::PointCloud2ConstPtr &cloud_msg)
Definition: sick_scan_test.cpp:411
cloud_height
int cloud_height
Definition: sick_scan_test.cpp:401
TiXmlAttribute
Definition: tinyxml.h:883
scan_time_increment
float scan_time_increment
Definition: sick_scan_test.cpp:511
paramEntryAscii::getName
std::string getName()
Definition: sick_scan_test.cpp:126
callbackCnt
int callbackCnt
Definition: sick_scan_test.cpp:402
TiXmlElement::SetAttribute
void SetAttribute(const char *name, const char *_value)
Definition: tinyxml.cpp:780
paramEntryAscii::getPointerToXmlNode
TiXmlElement * getPointerToXmlNode(void)
Definition: sick_scan_test.cpp:100
range_max
float range_max
Definition: sick_scan_test.cpp:514
paramEntryAscii::errorCode
int errorCode
Definition: sick_scan_test.cpp:174
paramEntryAscii::getErrorCode
int getErrorCode()
Definition: sick_scan_test.cpp:157
ROS_WARN
#define ROS_WARN(...)
paramEntryAscii::setValues
void setValues(std::string _nameVal, std::string _typeVal, std::string _valueVal)
Definition: sick_scan_test.cpp:104
getParamList
std::vector< paramEntryAscii > getParamList(TiXmlNode *paramList)
Definition: sick_scan_test.cpp:191
paramEntryAscii::setCheckStatus
void setCheckStatus(int errCode, std::string errMsg)
Definition: sick_scan_test.cpp:151
TiXmlAttribute::Next
const TiXmlAttribute * Next() const
Get the next sibling attribute in the DOM. Returns null at end.
Definition: tinyxml.cpp:1170
launchRosFile
pid_t launchRosFile(std::string cmdLine)
Definition: sick_scan_test.cpp:308
ros::NodeHandle::subscribe
Subscriber subscribe(const std::string &topic, uint32_t queue_size, const boost::function< void(C)> &callback, const VoidConstPtr &tracked_object=VoidConstPtr(), const TransportHints &transport_hints=TransportHints())
TiXmlNode
Definition: tinyxml.h:472
sudokill
void sudokill(pid_t tokill)
Definition: sick_scan_test.cpp:183
paramEntryAscii::getErrorMsg
std::string getErrorMsg()
Definition: sick_scan_test.cpp:162
ROS_FATAL
#define ROS_FATAL(...)
paramEntryAscii::nameVal
std::string nameVal
Definition: sick_scan_test.cpp:168
TiXmlDocument::ErrorDesc
const char * ErrorDesc() const
Contains a textual (english) description of the error if one occurs.
Definition: tinyxml.h:1674
paramEntryAscii::getType
std::string getType()
Definition: sick_scan_test.cpp:131
getPackageRootFolder
bool getPackageRootFolder(std::string exePath, std::string &packageRootFolder)
Definition: sick_scan_test.cpp:604
ros::this_node::getName
const ROSCPP_DECL std::string & getName()
scan_angle_max
float scan_angle_max
Definition: sick_scan_test.cpp:510
paramEntryAscii::setMinMaxValues
void setMinMaxValues(std::string _valueMinVal, std::string _valueMaxVal)
Definition: sick_scan_test.cpp:117
startCallbackTest
int startCallbackTest(int argc, char **argv)
Definition: sick_scan_test.cpp:532
SICK_SCAN_TEST_MINOR_VER
#define SICK_SCAN_TEST_MINOR_VER
Definition: sick_scan_test.cpp:78
range_min
float range_min
Definition: sick_scan_test.cpp:513
TiXmlNode::Value
const char * Value() const
Definition: tinyxml.h:537
SICK_SCAN_TEST_PATCH_LEVEL
#define SICK_SCAN_TEST_PATCH_LEVEL
Definition: sick_scan_test.cpp:79
ROS_ERROR
#define ROS_ERROR(...)
cloud_width
int cloud_width
Definition: sick_scan_test.cpp:400
TiXmlDocument::LoadFile
bool LoadFile(TiXmlEncoding encoding=TIXML_DEFAULT_ENCODING)
Definition: tinyxml.cpp:954
TiXmlNode::RemoveChild
bool RemoveChild(TiXmlNode *removeThis)
Delete a child of this node.
Definition: tinyxml.cpp:335
ERR_CONST
float ERR_CONST
Definition: sick_scan_test.cpp:508
TiXmlElement::FirstAttribute
TiXmlAttribute * FirstAttribute()
Definition: tinyxml.h:1226
pid
pid_t pid
Definition: sick_scan_test.cpp:306
ros::Rate
paramEntryAscii::valueVal
std::string valueVal
Definition: sick_scan_test.cpp:170
paramEntryAscii::typeVal
std::string typeVal
Definition: sick_scan_test.cpp:169
ROSCONSOLE_DEFAULT_NAME
#define ROSCONSOLE_DEFAULT_NAME
createPrefixFromExeName
void createPrefixFromExeName(std::string exeName, std::string &prefix)
Definition: sick_scan_test.cpp:557
cmd
string cmd
ROS_INFO
#define ROS_INFO(...)
TiXmlText
Definition: tinyxml.h:1371
paramEntryAscii::errorMsg
std::string errorMsg
Definition: sick_scan_test.cpp:175
paramEntryAscii::getMaxValue
std::string getMaxValue()
Definition: sick_scan_test.cpp:146
intensityStdDev
double intensityStdDev
Definition: sick_scan_test.cpp:403
ros::NodeHandle
ros::Subscriber


sick_scan
Author(s): Michael Lehning , Jochen Sprickerhof , Martin Günther
autogenerated on Thu Sep 8 2022 02:30:19