recordGen.cpp
Go to the documentation of this file.
1 
18 #include <ros/ros.h>
19 #include <dynamic_reconfigure/server.h>
20 #include <std_msgs/Float64MultiArray.h>
21 #include <std_msgs/Empty.h>
22 #include <std_msgs/UInt32.h>
23 #include <std_msgs/UInt32MultiArray.h>
24 #include <std_msgs/String.h>
25 
26 #include <visualization_msgs/Marker.h>
27 #include <visualization_msgs/MarkerArray.h>
28 
29 #include <ISM/common_type/Point.hpp>
30 #include <ISM/common_type/Track.hpp>
31 #include <ISM/common_type/Tracks.hpp>
32 #include <ISM/common_type/Object.hpp>
33 #include "ISM/recognizer/VotingSpace.hpp"
34 #include "ISM/recorder/Recorder.hpp"
35 #include <ISM/utility/TableHelper.hpp>
36 #include <ISM/utility/MathHelper.hpp>
37 
38 #include <asr_ism/recordGenConfig.h>
39 
40 #include <asr_ism_visualizations/ObjectModelVisualizerRVIZ.hpp>
41 #include "asr_ism_visualizations/VizHelperRVIZ.hpp"
42 
43 #include <boost/filesystem/path.hpp>
44 #include <boost/property_tree/ptree.hpp>
45 #include <boost/property_tree/xml_parser.hpp>
46 #include <boost/algorithm/string.hpp>
47 
48 //#include <eigen3/Eigen/Geometry.hpp>
49 #include <eigen3/unsupported/Eigen/Splines>
50 #include <Eigen/Geometry>
51 
52 //#include "visStuff.hpp"
53 
54 #include <boost/math/constants/constants.hpp>
55 #include <boost/lexical_cast.hpp>
56 #include <boost/random.hpp>
57 #include <boost/random/variate_generator.hpp>
58 #include <boost/random/uniform_real.hpp>
59 #include <random>
60 #include <iostream>
61 #include <fstream>
62 #include <sstream>
63 
64 #include <math.h>
65 #include <stdint.h>
66 #include <tuple>
67 
68 
69 using visualization_msgs::MarkerArray;
70 using namespace VIZ;
71 
72 double frand(double min, double max) {
73  double f = (double) rand() / RAND_MAX;
74  return min + f * (max - min);
75 }
76 
77 enum class RecGenPointType
78 {
79  Fix = 0,
80  MidSpline = 1,
81  MidSplineAlt = 2,
82  Undefined = 3
83 };
84 
85 namespace H
86 {
87  template<class C, class T>
88  auto contains(const C& v, const T& x)
89  -> decltype(end(v), true)
90  {
91  return end(v) != std::find(begin(v), end(v), x);
92  }
93 }
94 
95 RecGenPointType recGenPointTypeFromString(const std::string& s)
96 {
97  if(s == "Fix")
98  {
99  return RecGenPointType::Fix;
100  } else if(s == "MidSpline")
101  {
102  return RecGenPointType::MidSpline;
103  } else if(s == "MidSplineAlt")
104  {
105  return RecGenPointType::MidSplineAlt;
106  }
107 
108  return RecGenPointType::Undefined;
109 }
110 
111 enum class OrientCalcType
112 {
113  Fix = 0,
114  Random = 1,
115  Linear = 2,
116  Undefined = 3
117 };
118 
119 OrientCalcType orientCalcTypeFromString(const std::string& s)
120 {
121  if(s == "Fix")
122  {
123  return OrientCalcType::Fix;
124  } else if (s == "Random")
125  {
126  return OrientCalcType::Random;
127  } else if (s == "Linear")
128  {
129  return OrientCalcType::Linear;
130  }
131  return OrientCalcType::Undefined;
132 }
133 
134 class RecordGenPoint
135 {
136 public:
137  RecordGenPoint(std::string name, int32_t id, RecGenPointType type, double posX, double posY,
138  double posZ, double orientW, double orientX, double orientY, double orientZ,
139  OrientCalcType orientCalcType, double posErr, double orientDerivation, unsigned int from,
140  unsigned int to, double linearDegX, double linearDegY, double linearDegZ):
141  mTimestep(0),
142  mCurrentApplied(false),
143  mName(name),
144  mId(id),
145  mType(type),
146  mCurrentPosX(posX),
147  mCurrentPosY(posY),
148  mCurrentPosZ(posZ),
149  mCurrentOrientW(orientW),
150  mCurrentOrientX(orientX),
151  mCurrentOrientY(orientY),
152  mCurrentOrientZ(orientZ),
153  mInitOrientW(orientW),
154  mInitOrientX(orientX),
155  mInitOrientY(orientY),
156  mInitOrientZ(orientZ),
157  mOrientCalcType(orientCalcType),
158  mPosErr(posErr),
159  mOrientDer(orientDerivation),
160  mFrom(from),
161  mTo(to),
162  mLinearDegX(linearDegX),
163  mLinearDegY(linearDegY),
164  mLinearDegZ(linearDegZ),
165  mUniform(-1,1),
166  mNormal(0, mOrientDer)
167 
168  {
169  if(mOrientCalcType == OrientCalcType::Undefined)
170  {
171  throw std::runtime_error("Type of OrientCalc is undefined");
172  }
173  if(mType == RecGenPointType::Undefined)
174  {
175  throw std::runtime_error("Type of RecGenPoint is undefined");
176  }
177  mTrack.reset(new ISM::Track(name, boost::lexical_cast<std::string>(id)));
178  std::random_device rd;
179  mRandomGen.seed(rd());
180  mRandomGenOrient.seed(rd());
181  mVariantGen = new boost::variate_generator<boost::mt19937, boost::uniform_real<>>
182  (mRandomGen, mUniform);
183  mVarGenOrient = new boost::variate_generator<boost::mt19937, boost::normal_distribution<>>
184  (mRandomGenOrient, mNormal);
185  }
186  virtual ~RecordGenPoint()
187  {
188  }
189  virtual bool isVisible() = 0;
190  virtual void calcCurrObPos() = 0;
191  virtual void calcCurrObOrient()
192  {
193  ISM::PointPtr currPos;
194  if(mCurrentO && mCurrentO->pose)
195  {
196  currPos = mCurrentO->pose->point;
197  }
198 
199  switch(mOrientCalcType)
200  {
201  case OrientCalcType::Fix:
202  {
203  mCurrentO.reset(new ISM::Object(mName,
204  new ISM::Pose(currPos,
205  new ISM::Quaternion(mCurrentOrientW, mCurrentOrientX,
206  mCurrentOrientY, mCurrentOrientZ)),
207  boost::lexical_cast<std::string>(mId)));
208 
209  break;
210  }
211  case OrientCalcType::Random:
212  {
213  mCurrentOrientW = (*mVariantGen)();
214  mCurrentOrientX = (*mVariantGen)();
215  mCurrentOrientY = (*mVariantGen)();
216  mCurrentOrientZ = (*mVariantGen)();
217 
218  mCurrentO.reset(new ISM::Object(mName,
219  new ISM::Pose(mCurrentO->pose->point,
220  new ISM::Quaternion(mCurrentOrientW, mCurrentOrientX,
221  mCurrentOrientY, mCurrentOrientZ)),
222  boost::lexical_cast<std::string>(mId)));
223  break;
224  }
225  case OrientCalcType::Linear:
226  {
227  if(mTimestep > 0 && mFrom < mTo)
228  {
229  const double pi = boost::math::constants::pi<double>();
230  double totSpan = mTo - mFrom;
231  double currentLinEulX = mLinearDegX * ((double) mTimestep) / totSpan
232  *pi / 180.0;
233  double currentLinEulY = mLinearDegY * ((double) mTimestep) / totSpan
234  *pi / 180.0;
235  double currentLinEulZ = mLinearDegZ * ((double) mTimestep) / totSpan
236  *pi / 180.0;
237 
238  Eigen::Quaterniond linQ = Eigen::AngleAxisd(currentLinEulX, Eigen::Vector3d::UnitX())
239  * Eigen::AngleAxisd(currentLinEulY, Eigen::Vector3d::UnitY())
240  * Eigen::AngleAxisd(currentLinEulZ, Eigen::Vector3d::UnitZ());
241  Eigen::Quaterniond q(mInitOrientW, mInitOrientX, mInitOrientY, mInitOrientZ);
242  Eigen::Quaterniond totalQ = q * linQ;
243 
244  mCurrentOrientW = totalQ.w();
245  mCurrentOrientX = totalQ.x();
246  mCurrentOrientY = totalQ.y();
247  mCurrentOrientZ = totalQ.z();
248  }
249  mCurrentO.reset(new ISM::Object(mName,
250  new ISM::Pose(mCurrentO->pose->point,
251  new ISM::Quaternion(mCurrentOrientW, mCurrentOrientX,
252  mCurrentOrientY, mCurrentOrientZ)),
253  boost::lexical_cast<std::string>(mId)));
254  break;
255  }
256  case OrientCalcType::Undefined:
257  {
258  throw std::runtime_error("OrientCalcType is Undefined");
259  break;
260  }
261  }
262  }
263  virtual void forceInvisible(bool) = 0;
264  virtual void apply()
265  {
266  if(!mCurrentO || !mCurrentO->pose->point ||
267  !mCurrentO->pose->quat)
268  throw "RecordGenPoint: Current Object must always be initialized";
269  if(!mCurrentApplied)
270  {
271  auto point = mCurrentO->pose->point;
272  auto quat = mCurrentO->pose->quat;
273  point->eigen.x() = point->eigen.x() + (*mVariantGen)() * mPosErr;
274  point->eigen.y() = point->eigen.y() + (*mVariantGen)() * mPosErr;
275  point->eigen.z() = point->eigen.z() + (*mVariantGen)() * mPosErr;
276  if(point->eigen.x() < -1.0 || point->eigen.x() > 1.0
277  || point->eigen.y() < -1.0 || point->eigen.y() > 1.0
278  ||point->eigen.z() < -1.0 || point->eigen.z() > 1.0 )
279  {
280  std::cerr << "Warning Point does not lie in range: " << point << std::endl
281  << "type: " << mName << " id: " << mId << std::endl
282  << "at timestep: " << mTimestep << std::endl;
283  std::cin.ignore();
284  }
285  const double pi = boost::math::constants::pi<double>();
286 
287  Eigen::Matrix3d m;
288  m = Eigen::AngleAxisd(pi * (*mVarGenOrient)(), Eigen::Vector3d::UnitX())
289  * Eigen::AngleAxisd(pi * (*mVarGenOrient)(), Eigen::Vector3d::UnitY())
290  * Eigen::AngleAxisd(pi * (*mVarGenOrient)(), Eigen::Vector3d::UnitZ());
291 
292  Eigen::Quaterniond errorQ(m);
293  Eigen::Quaterniond q(quat->eigen.w(), quat->eigen.x(), quat->eigen.y(), quat->eigen.z());
294  Eigen::Quaterniond totalQ = q * errorQ;
295 
296  quat->eigen.w() = totalQ.w();
297  quat->eigen.x() = totalQ.x();
298  quat->eigen.y() = totalQ.y();
299  quat->eigen.z() = totalQ.z();
300  /*
301  quat->setW(quat->w);
302  quat->setX(quat->x);
303  quat->setY(quat->y);
304  quat->setZ(quat->z);
305  */
306 
307  if(isVisible())
308  {
309  //TODO not in current ws
310  //mCurrentO->mTimestep = mTimestep;
311  mTrack->objects.push_back(mCurrentO);
312  }
313  else
314  {
315  mTrack->objects.push_back(ISM::ObjectPtr());
316  std::cout << mName << " is not visible" << std::endl;
317  }
318 
319  mCurrentApplied = true;
320  }
321  }
322  virtual void applyAndGotoNextTimestep()
323  {
324  apply();
325  ++mTimestep;
326  mCurrentApplied = false;
327  calcCurrObPos();
328  calcCurrObOrient();
329  }
330 
331  unsigned int mTimestep;
332  bool mCurrentApplied;
333  std::string mName;
334  int32_t mId;
335  ISM::TrackPtr mTrack;
336  ISM::ObjectPtr mCurrentO;
337  RecGenPointType mType;
338  double mCurrentPosX;
339  double mCurrentPosY;
340  double mCurrentPosZ;
341  double mCurrentOrientW;
342  double mCurrentOrientX;
343  double mCurrentOrientY;
344  double mCurrentOrientZ;
345  double mInitOrientW;
346  double mInitOrientX;
347  double mInitOrientY;
348  double mInitOrientZ;
349  OrientCalcType mOrientCalcType;
350  double mPosErr;
351  double mOrientDer;
352  unsigned int mFrom;
353  unsigned int mTo;
354  double mLinearDegX;
355  double mLinearDegY;
356  double mLinearDegZ;
357  boost::mt19937 mRandomGen;
358  boost::mt19937 mRandomGenOrient;
359  boost::uniform_real<> mUniform;
360  boost::normal_distribution<> mNormal;
361  boost::variate_generator<boost::mt19937, boost::uniform_real<>>* mVariantGen;
362  boost::variate_generator<boost::mt19937, boost::normal_distribution<>>* mVarGenOrient;
363  //ISM::PosePtr mCurrentPose;
364 };
365 
366 class MidSplinePoint : public RecordGenPoint
367 {
368 private:
369  void setCol(Eigen::MatrixXd &m, int col, Eigen::Vector3d &v, double time)
370  {
371  m(0, col) = time;
372  m(1, col) = v.x();
373  m(2, col) = v.y();
374  m(3, col) = v.z();
375  }
376 public:
377  void calcSpline()
378  {
379  Eigen::Vector3d s = Eigen::Vector3d(mXs, mYs , mZs);
380  Eigen::Vector3d e = Eigen::Vector3d(mXe, mYe , mZe);
381  Eigen::Vector3d sToe = e - s;
382  Eigen::Vector3d m = s + sToe/2;
383  Eigen::Vector3d temp = sToe;
384  temp.normalize();
385  temp = temp * 2;
386  Eigen::Vector3d orthSToe = temp.unitOrthogonal();
387  orthSToe.normalize();
388  orthSToe *= (sToe.norm()/2) * tan(mDegree * (boost::math::constants::pi<double>() / 180.0));
389  Eigen::Vector3d p3 = m + orthSToe;
390 
391  //dimension, num points
392  int numPoints = 3;
393  double timeToNextPoint = mTimeSpan / (double) numPoints;
394  Eigen::MatrixXd points(4, numPoints);
395  setCol(points, 0, s, 0);
396  setCol(points, 1, p3, timeToNextPoint);
397  setCol(points, 2, e, mTimeSpan);
398  mSpline = Eigen::SplineFitting<Eigen::Spline<double, 4>>::Interpolate(points, numPoints - 1);
399  }
400  MidSplinePoint(std::string name, int32_t id, double xs, double ys, double zs,
401  double xe, double ye, double ze, double degree, double timeSpan,
402  double orientW, double orientX, double orientY, double orientZ,
403  OrientCalcType orientCalcType, double posErr, double orientErr,
404  unsigned int from, unsigned int to,
405  double linearDegX, double linearDegY, double linearDegZ) :
406  RecordGenPoint(name, id, RecGenPointType::MidSpline, xs, ys, zs, orientW,
407  orientX, orientY, orientZ, orientCalcType, posErr,
408  orientErr, from, to,
409  linearDegX, linearDegY, linearDegZ),
410  mIsInvisible(false),
411  mXs(xs), mYs(ys), mZs(zs), mXe(xe), mYe(ye), mZe(ze), mDegree(degree), mTimeSpan(timeSpan)
412  {
413  calcSpline();
414  calcCurrObPos();
415  calcCurrObOrient();
416  }
417 
418  void setNewParams(double xs, double ys, double zs,
419  double xe, double ye, double ze, double degree, double timeSpan)
420  {
421  mXs = xs;
422  mYs = ys;
423  mZs = zs;
424 
425  mXe = xe;
426  mYe = ye;
427  mZe = ze;
428 
429  mDegree = degree;
430  mTimeSpan = timeSpan;
431  calcSpline();
432  calcCurrObPos();
433  }
434 
435  virtual ~MidSplinePoint()
436  {
437  }
438 
439  virtual bool isVisible()
440  {
441  return !mIsInvisible;
442  }
443 
444  virtual void forceInvisible(bool b)
445  {
446  mIsInvisible = b;
447  }
448 
449  virtual void calcCurrObPos()
450  {
451  ISM::QuaternionPtr currOrient;
452  if(mCurrentO && mCurrentO->pose)
453  {
454  currOrient = mCurrentO->pose->quat;
455  }
456 
457  Eigen::Vector4d values;
458  if(mTimestep > mTimeSpan)
459  values = mSpline(1);
460  else
461  values = mSpline(mTimestep/mTimeSpan);
462 
463  mCurrentPosX = values(1);
464  mCurrentPosY = values(2);
465  mCurrentPosZ = values(3);
466  mCurrentO.reset(new ISM::Object(mName,
467  new ISM::Pose(new ISM::Point(mCurrentPosX, mCurrentPosY, mCurrentPosZ),
468  currOrient), boost::lexical_cast<std::string>(mId)));
469 
470  }
471  bool mIsInvisible;
472  double mXs;
473  double mYs;
474  double mZs;
475  double mXe;
476  double mYe;
477  double mZe;
478  Eigen::Spline<double, 4> mSpline;
479  double mDegree;
480  double mTimeSpan;
481 };
482 
483 class MidSplinePointAlt : public MidSplinePoint
484 {
485 public:
486  MidSplinePointAlt(std::string name, int32_t id, double xs, double ys, double zs,
487  double xe, double ye, double ze, double degree, double timeSpan,
488  double orientW, double orientX, double orientY, double orientZ,
489  OrientCalcType orientCalcType, double posErr, double orientErr,
490  unsigned int from, unsigned int to , double linearDegX, double linearDegY, double linearDegZ) :
491  MidSplinePoint(name, id, xs, ys, zs,
492  xe, ye, ze, degree, timeSpan, orientW, orientX, orientY, orientZ,
493  orientCalcType, posErr, orientErr, from, to, linearDegX, linearDegY, linearDegZ)
494  {
495 
496  }
497  virtual void calcCurrObPos()
498  {
499  ISM::QuaternionPtr currOrient;
500  if(mCurrentO && mCurrentO->pose)
501  {
502  currOrient = mCurrentO->pose->quat;
503  }
504 
505  Eigen::Vector4d values;
506  int timesTimeSpan = mTimestep / mTimeSpan;
507  if((timesTimeSpan % 2) == 0)
508  values = mSpline((mTimestep - (timesTimeSpan * mTimeSpan)) / mTimeSpan);
509  else
510  values = mSpline(1 - (mTimestep - (timesTimeSpan * mTimeSpan)) / mTimeSpan);
511 
512  mCurrentPosX = values(1);
513  mCurrentPosY = values(2);
514  mCurrentPosZ = values(3);
515  mCurrentO.reset(new ISM::Object(mName,
516  new ISM::Pose(new ISM::Point(mCurrentPosX, mCurrentPosY, mCurrentPosZ),
517  currOrient), boost::lexical_cast<std::string>(mId)));
518 
519  }
520 };
521 class FixRecordGenPoint : public RecordGenPoint
522 {
523 public:
524  FixRecordGenPoint(std::string name, int32_t id, double x, double y, double z,
525  double orientW, double orientX, double orientY, double orientZ,
526  OrientCalcType orientCalcType, double posErr, double orientErr,
527  unsigned int from, unsigned int to, double linearDegX, double linearDegY, double linearDegZ):
528  RecordGenPoint(name, id, RecGenPointType::Fix, x, y, z, orientW, orientX, orientY, orientZ,
529  orientCalcType, posErr, orientErr, from, to, linearDegX, linearDegY, linearDegZ),
530  mIsInvisible(false)
531  {
532  calcCurrObPos();
533  calcCurrObOrient();
534 #if 0
535  if(mTimestep > 0)
536  {
537  unsigned int desTs = mTimestep;
538  mTimestep = 0;
539  forceInvisible(true);
540  for(unsigned int i = 0; i < desTs; ++i)
541  {
542  applyAndGotoNextTimestep();
543  }
544  forceInvisible(false);
545  }
546 #endif
547  }
548 
549  virtual ~FixRecordGenPoint()
550  {
551  }
552 
553  void setNewParams(double x, double y, double z)
554  {
555  mCurrentPosX = x;
556  mCurrentPosY = y;
557  mCurrentPosZ = z;
558  calcCurrObPos();
559  }
560 
561  virtual bool isVisible()
562  {
563  return !mIsInvisible;
564  }
565 
566  virtual void forceInvisible(bool b)
567  {
568  mIsInvisible = b;
569  }
570 
571  virtual void calcCurrObPos()
572  {
573  ISM::QuaternionPtr currOrient;
574  if(mCurrentO && mCurrentO->pose)
575  {
576  currOrient = mCurrentO->pose->quat;
577  }
578  mCurrentO.reset(new ISM::Object(mName,
579  new ISM::Pose(new ISM::Point(mCurrentPosX, mCurrentPosY, mCurrentPosZ),
580  currOrient), boost::lexical_cast<std::string>(mId)));
581 
582  }
583 
584  bool mIsInvisible;
585 };
586 
587 using boost::filesystem::path;
588 typedef boost::shared_ptr<RecordGenPoint> RecordGenPointPtr;
589 typedef std::tuple<uint32_t,uint32_t,RecordGenPointPtr> FromToPointTuple;
590 namespace pt = boost::property_tree;
591 
592 struct cmpFromToPointTuple {
593  bool operator()(const FromToPointTuple& a, const FromToPointTuple& b) const {
594  if(std::get<2>(a)->mName == std::get<2>(b)->mName &&
595  std::get<2>(a)->mId == std::get<2>(b)->mId)
596  {
597  return std::get<1>(a) < std::get<0>(b);
598  } else
599  {
600  return (uintptr_t)(std::get<2>(a).get()) < (uintptr_t)(std::get<2>(b).get());
601  }
602  }
603 };
604 
605 class RecordGenerator
606 {
607 public:
608  void parseXml(path xmlFile)
609  {
610  pt::ptree tree;
611 
612  pt::read_xml(xmlFile.string(), tree);
613 
614  for(auto& fstLvlNode : tree)
615  {
616  if(fstLvlNode.first == "object")
617  {
618  std::string name = fstLvlNode.second.get<std::string>("<xmlattr>.name");
619  int32_t id = fstLvlNode.second.get<int32_t>("<xmlattr>.id");
620  ISM::TrackPtr sharedTrack = ISM::TrackPtr(new ISM::Track(name,
621  boost::lexical_cast<std::string>(id)));
622  mTracks.push_back(sharedTrack);
623  for(auto& pointNode : fstLvlNode.second)
624  {
625  if(pointNode.first == "point")
626  {
627  auto& pointAttrs = pointNode.second;
628 
629  RecGenPointType pointType = recGenPointTypeFromString
630  (pointAttrs.get<std::string>("<xmlattr>.type", ""));
631  int from = pointAttrs.get<uint32_t>("from");
632  int to = pointAttrs.get<uint32_t>("to");
633  if(from > to)
634  {
635  std::stringstream ss;
636  ss << "At: " << name << "," << id << " from must be less than to";
637  throw std::runtime_error(ss.str());
638  }
639  double posX = pointAttrs.get<double>("posX", 0.0);
640  double posY = pointAttrs.get<double>("posY", 0.0);
641  double posZ = pointAttrs.get<double>("posZ", 0.0);
642  double orientW = pointAttrs.get<double>("orientW", 0.0);
643  double orientX = pointAttrs.get<double>("orientX", 0.0);
644  double orientY = pointAttrs.get<double>("orientY", 0.0);
645  double orientZ = pointAttrs.get<double>("orientZ", 0.0);
646  double xs = pointAttrs.get<double>("xs", 0.0);
647  double ys = pointAttrs.get<double>("ys", 0.0);
648  double zs = pointAttrs.get<double>("zs", 0.0);
649  double xe = pointAttrs.get<double>("xe", 0.0);
650  double ye = pointAttrs.get<double>("ye", 0.0);
651  double ze = pointAttrs.get<double>("ze", 0.0);
652  double degree = pointAttrs.get<double>("degree", 0.0);
653  double timeSpan = pointAttrs.get<double>("timeSpan", 0.0);
654  double posErr = pointAttrs.get<double>("posErr", 0.0);
655  double orientErr = pointAttrs.get<double>("orientErr", 0.0);
656  double linearDegX = pointAttrs.get<double>("linearDegX", 0.0);
657  double linearDegY = pointAttrs.get<double>("linearDegY", 0.0);
658  double linearDegZ = pointAttrs.get<double>("linearDegZ", 0.0);
659  OrientCalcType oCT = orientCalcTypeFromString(pointAttrs.get<std::string>("orientCalcType", ""));
660  std::vector<unsigned int> timestampVisible;
661  for(auto& pointAttr : pointAttrs)
662  {
663  if(pointAttr.first == "visible")
664  {
665  std::string rangeString = pointAttr.second.get_value("");
666  std::vector<std::string> range;
667  boost::split(range, rangeString, boost::is_any_of("-"));
668  if(range.size() == 2)
669  {
670  for(unsigned int i = boost::lexical_cast<unsigned int>(range[0]);
671  i <= boost::lexical_cast<unsigned int>(range[1]); ++i)
672  {
673  timestampVisible.push_back(i);
674  }
675  } else
676  {
677  timestampVisible.push_back(boost::lexical_cast<unsigned int>(range[0]));
678  }
679  }
680  }
681  RecordGenPointPtr p;
682  switch(pointType)
683  {
684  case RecGenPointType::Fix:
685  {
686  p = RecordGenPointPtr(new FixRecordGenPoint(name, id, posX, posY, posZ, orientW,
687  orientX, orientY, orientZ, oCT, posErr, orientErr, from, to
688  ,linearDegX, linearDegY, linearDegZ));
689  break;
690  }
691  case RecGenPointType::MidSpline:
692  {
693  p = RecordGenPointPtr(new MidSplinePoint(name, id, xs, ys, zs, xe, ye, ze, degree,
694  timeSpan, orientW, orientX, orientY, orientZ, oCT, posErr, orientErr, from, to,
695  linearDegX, linearDegY, linearDegZ));
696  break;
697  }
698  case RecGenPointType::MidSplineAlt:
699  {
700  p = RecordGenPointPtr(new MidSplinePointAlt(name, id, xs, ys, zs, xe, ye, ze, degree,
701  timeSpan, orientW, orientX, orientY, orientZ, oCT, posErr, orientErr, from, to,
702  linearDegX, linearDegY, linearDegZ));
703  break;
704  }
705 
706  default:
707  throw std::runtime_error("Error in parsing xml: RecGenPointType not valid");
708  break;
709  }
710  mTimestampVisibleXmlPoints[p] = timestampVisible;
711  p->mTrack = sharedTrack;
712  auto itToSucceed = mPoints.insert(std::make_tuple(from,to,p));
713  if(!itToSucceed.second)
714  {
715  std::stringstream ss;
716  ss << "At: " << name << "," << id << " point intervals overlapping";
717  throw std::runtime_error(ss.str());
718  }
719  }
720  }
721  }
722 
723  }
724  }
725  void dynamicReconfCallback(asr_ism::recordGenConfig &config, uint32_t level)
726  {
727  mShowFrom = config.from;
728  mShowTo = config.to;
729  }
730 
731  RecordGenerator(std::string dbFile,
732  std::string patternName = "pattern0", bool doPublish = false):
733  mNextId(0),
734  mTimestep(0),
735  mDbFile(dbFile),
736  mFilterViaVgOut(false),
737  mPatternName(patternName),
738  mShowCurrent(false),
739  mDoPublish(doPublish),
740  mShowFrom(0),
741  mShowTo(0)
742  {
743  dynamic_reconfigure::Server<asr_ism::recordGenConfig>::CallbackType f = boost::bind(&RecordGenerator::dynamicReconfCallback, this, _1, _2);
744  mDynReconfServer.setCallback(f);
745  rec.reset(new ISM::Recorder(mDbFile));
746  //TODO drop tables?
747  //rec->dropTables();
748  }
749 
750  void filterViaVgOut(boost::filesystem::path vgOutFile)
751  {
752  mFilterViaVgOut = true;
753  std::ifstream is(vgOutFile.string());
754  std::string line;
755  std::string type;
756  std::string pattern;
757  bool foundRep = true;
758  while(std::getline(is, line))
759  {
760  if(line.find("Ref is:") != std::string::npos)
761  {
762  line.erase(line.begin(), line.begin() + line.find_first_of(":") + 1);
763  mPatternToRef[pattern] = line;
764  }
765  if(line.find("Id for pattern:") != std::string::npos)
766  {
767  line.erase(line.begin(), line.begin() + line.find_first_of(":") + 1);
768  pattern = line;
769  } else if(line.find("Id for type:") != std::string::npos)
770  {
771  line.erase(line.begin(), line.begin() + line.find_first_of(":") + 1);
772  type = line;
773  } else if(line.find("These trackIds are in the same Orient Grid:") != std::string::npos)
774  {
775  foundRep = false;
776  } else if(!foundRep)
777  {
778  uint32_t rep = 0;
779  try
780  {
781  rep = boost::lexical_cast<uint32_t>(line);
782  } catch(boost::bad_lexical_cast& e)
783  {
784  continue;
785  }
786  mPatternTypeToVoxelReps[std::make_pair(pattern,type)].insert(rep);
787  foundRep = true;
788  }
789  }
790  for(auto& patternTypeToVoxelReps : mPatternTypeToVoxelReps)
791  {
792  std::string pattern = patternTypeToVoxelReps.first.first;
793  std::string type = patternTypeToVoxelReps.first.second;
794  for(uint32_t rep : patternTypeToVoxelReps.second)
795  {
796  std::cout << "Rep for p: " << pattern << " t: " << type << " is: " << rep << std::endl;
797  }
798  }
799  }
800 
801  void applyAndGotoNextTimestep()
802  {
803  for(auto& fromToPoint : mPoints)
804  {
805  if(std::get<0>(fromToPoint) <= mTimestep &&
806  std::get<1>(fromToPoint) >= mTimestep)
807  {
808  auto& p = std::get<2>(fromToPoint);
809  std::vector<unsigned int>& visTs = mTimestampVisibleXmlPoints[p];
810 
811  if(std::find(visTs.begin(), visTs.end(), mTimestep) != visTs.end())
812  {
813  p->forceInvisible(false);
814  } else
815  {
816  p->forceInvisible(true);
817  }
818  p->applyAndGotoNextTimestep();
819  }
820  }
821  mTimestep++;
822  }
823 
824  void apply()
825  {
826  for(auto& fromToPoint : mPoints)
827  {
828  if(std::get<0>(fromToPoint) <= mTimestep &&
829  std::get<1>(fromToPoint) >= mTimestep)
830  {
831  auto& p = std::get<2>(fromToPoint);
832  std::vector<unsigned int>& visTs = mTimestampVisibleXmlPoints[p];
833 
834  if(std::find(visTs.begin(), visTs.end(), mTimestep) != visTs.end())
835  {
836  p->forceInvisible(false);
837  } else
838  {
839  p->forceInvisible(true);
840  }
841  p->apply();
842  }
843  }
844  }
845 
846  void applyAndGotoNextTimestep(const uint32_t n)
847  {
848  for(unsigned int i = 0; i < n; ++i)
849  {
850  applyAndGotoNextTimestep();
851  }
852  }
853 
854  std::vector<ISM::ObjectPtr> getCurrentVisibleObjects()
855  {
856  std::vector<ISM::ObjectPtr> result;
857  for(auto& fromToPoint : mPoints)
858  {
859  if(std::get<0>(fromToPoint) <= mTimestep &&
860  std::get<1>(fromToPoint) >= mTimestep)
861  {
862  auto& p = std::get<2>(fromToPoint);
863  if(p->isVisible())
864  {
865  result.push_back(p->mCurrentO);
866  }
867  }
868  }
869  return result;
870  }
871 
872  std::vector<ISM::ObjectPtr> getCurrentInvisibleObjects()
873  {
874  std::vector<ISM::ObjectPtr> result;
875  for(auto& fromToPoint : mPoints)
876  {
877  if(std::get<0>(fromToPoint) <= mTimestep &&
878  std::get<1>(fromToPoint) >= mTimestep)
879  {
880  auto& p = std::get<2>(fromToPoint);
881  if(!p->isVisible())
882  {
883  result.push_back(p->mCurrentO);
884  }
885  }
886  }
887  return result;
888  }
889 
890  //void showFromTo(std_msgs::UInt32MultiArray::ConstPtr& msg)
891  void showFromTo(const std_msgs::Float64MultiArray::ConstPtr& msg)
892  {
893  mShowFrom = *msg->data.begin();
894  mShowTo = *(msg->data.begin() + 1);
895  mShowCurrent = false;
896  }
897 
898  std::vector<ISM::ObjectPtr> getObjects(unsigned int from, unsigned int to)
899  {
900  std::vector<ISM::ObjectPtr> result;
901  static unsigned int oldTo = to;
902  if(!mDoPublish)
903  {
904  if(to == oldTo)
905  {
906  return result;
907  } else
908  {
909  oldTo = to;
910  }
911  ROS_INFO_STREAM("from is " << from << " << to: " <<to );
912  }
913  if(mTimestep < to)
914  {
915  //TODO
916  applyAndGotoNextTimestep(to - mTimestep);
917  }
918 
919  if(mTimestep == to)
920  {
921  apply();
922  }
923  if(from <= to)
924  {
925  for(auto& t : mTracks)
926  {
927  bool isRef = false;
928  if(mFilterViaVgOut)
929  {
930  std::vector<std::string> isRefInPatterns;
931  for(auto& patternToRef : mPatternToRef)
932  {
933  if(patternToRef.second == t->type)
934  {
935  isRefInPatterns.push_back(patternToRef.first);
936  }
937  }
938  if(isRefInPatterns.size() > 0)
939  {
940  isRef = true;
941  for(auto& patternTypeToReps : mPatternTypeToVoxelReps)
942  {
943  if(patternTypeToReps.first.second != t->type
944  && H::contains(isRefInPatterns, patternTypeToReps.first.first))
945  {
946  for(uint32_t repTs : patternTypeToReps.second)
947  {
948  if(from <= repTs && repTs <= to && !H::contains(result,
949  t->objects[repTs]))
950  {
951  result.push_back(t->objects[repTs]);
952  }
953  }
954  }
955  }
956  }
957  }
958  if((!isRef && mFilterViaVgOut) || !mFilterViaVgOut)
959  {
960  if(t->objects.size() > to)
961  {
962  {
963  //TODO why did i do this?
964  result.insert(result.end(), t->objects.begin() + from, t->objects.begin() + to + 1);
965  }
966  } else if(t->objects.size() > from)
967  {
968  result.insert(result.end(), t->objects.begin() + from, t->objects.end());
969  }
970  }
971  }
972  }
973  /*
974  ROS_INFO_STREAM("-------");
975  for(auto& o : result)
976  {
977  if(o)
978  {
979  ROS_INFO_STREAM(o);
980  }
981  }
982  ROS_INFO_STREAM("-------");
983  */
984  return result;
985  }
986 
987  std::vector<ISM::ObjectPtr> getOldObjects()
988  {
989  if(mTimestep == 0)
990  {
991  return std::vector<ISM::ObjectPtr>();
992  }
993  return getObjects(0, mTimestep -1);
994  }
995 
996  void writeToDb()
997  {
998  uint32_t maxTimestep = std::get<1>(*mPoints.rbegin());
999  if(maxTimestep > mTimestep)
1000  {
1001  applyAndGotoNextTimestep(maxTimestep - mTimestep);
1002  apply();
1003  }
1004  ROS_INFO_STREAM("Writing to database");
1005  apply();
1006  ISM::TracksPtr tracks(new ISM::Tracks(mTracks));
1007  auto objectSets = tracks->toObjectSetVector();
1008  for (auto objectSet : objectSets) {
1009  rec->insert(objectSet, mPatternName);
1010  }
1011  ROS_INFO_STREAM("Finished");
1012  }
1013 
1014  void writeToDb(const std_msgs::Empty)
1015  {
1016  writeToDb();
1017  }
1018 
1019 private:
1020  int32_t mNextId;
1021  std::vector<int32_t> mIdXmlPoints;
1022  std::map<RecordGenPointPtr, std::vector<unsigned int> > mTimestampVisibleXmlPoints;
1023  dynamic_reconfigure::Server<asr_ism::recordGenConfig> mDynReconfServer;
1024 
1025 public:
1026  unsigned int mTimestep;
1027  std::string mDbFile;
1028  bool mFilterViaVgOut;
1029  std::map<std::pair<std::string, std::string>, std::set<uint32_t>> mPatternTypeToVoxelReps;
1030  std::map<std::string, std::string> mPatternToRef;
1031  std::string mPatternName;
1032 
1033  std::set<std::tuple<uint32_t,uint32_t,RecordGenPointPtr>, cmpFromToPointTuple> mPoints;
1034  std::vector<ISM::TrackPtr> mTracks;
1035  ISM::RecorderPtr rec;
1036  bool mShowCurrent;
1037  bool mDoPublish;
1038  uint32_t mShowFrom;
1039  uint32_t mShowTo;
1040 };
1041 
1042 std_msgs::ColorRGBA getColorForTypeId(std::map<std::pair<std::string,std::string>, std_msgs::ColorRGBA >&
1043  typeIdToColor, std::vector<std_msgs::ColorRGBA>& allColors, uint32_t& availColorIndex,
1044  std::string& type, std::string& id)
1045 {
1046  std_msgs::ColorRGBA result;
1047  if(typeIdToColor.find(std::make_pair(type,id)) != typeIdToColor.end())
1048  {
1049  result = typeIdToColor[std::make_pair(type,id)];
1050  } else
1051  {
1052  if(allColors.size() < availColorIndex + 1)
1053  {
1054  std::uniform_real_distribution<float> urd;
1055  std::default_random_engine re;
1056  result = VIZ::VizHelperRVIZ::createColorRGBA(urd(re), urd(re), urd(re), 1);
1057  allColors.push_back(result);
1058  } else
1059  {
1060  result = allColors[availColorIndex];
1061  }
1062  typeIdToColor[std::make_pair(type, id)] = result;
1063  ++availColorIndex;
1064  }
1065  return result;
1066 }
1067 
1068 int main(int argc, char** argv) {
1069  ros::init(argc, argv, "recordGen");
1070  ros::NodeHandle n("~");
1071  ros::Rate r(1);
1072  std::string baseFrame = "/camera_left_frame";
1073  std::string visualizationTopic = "/visualization_marker";
1074  std::string dbFile;//("/media/share/data/own/gen1.sqlite");
1075  double bin_size = 0.03;
1076  double maxAngleDeviation = 10;
1077  bool useXml = false;
1078  bool useVgOut = false;
1079  boost::filesystem::path xmlFile;
1080  boost::filesystem::path vgOutFile;
1081  bool detailedBinNS;
1082  bool doPublish;
1083 
1084  if (!n.getParam("dbfilename", dbFile)) {
1085  throw std::runtime_error("dbfilename not set");
1086  }
1087  ROS_INFO_STREAM("dbfilename: " << dbFile);
1088 
1089  std::string temp;
1090  if (n.getParam("xml", temp)) {
1091  useXml = true;
1092  xmlFile = temp;
1093  }
1094  xmlFile = temp;
1095  ROS_INFO_STREAM("xml: " << temp);
1096 
1097  if (n.getParam("vgOut", temp)) {
1098  vgOutFile = temp;
1099  if(boost::filesystem::is_regular_file(vgOutFile))
1100  {
1101  useVgOut = true;
1102  } else
1103  {
1104  useVgOut = false;
1105  }
1106  }
1107  vgOutFile = temp;
1108  if(useVgOut)
1109  {
1110  ROS_INFO_STREAM("vgOut: " << vgOutFile);
1111  }
1112 
1113  if (!n.getParam("detailedBinNS", detailedBinNS)) {
1114  detailedBinNS = false;
1115  }
1116  ROS_INFO_STREAM("detailedBinNS: " << detailedBinNS);
1117 
1118  if (!n.getParam("doPublish", doPublish)) {
1119  doPublish = false;
1120  }
1121  ROS_INFO_STREAM("doPublish: " << doPublish);
1122 
1123  if (!n.getParam("bin_size", bin_size)) {
1124  bin_size = 0.03;
1125  }
1126  ROS_INFO_STREAM("bin_size: " << bin_size);
1127 
1128  RecordGenerator rg(dbFile, "pattern0", doPublish);
1129  if(useXml)
1130  {
1131  rg.parseXml(xmlFile);
1132  }
1133  if(useVgOut)
1134  {
1135  rg.filterViaVgOut(vgOutFile);
1136  }
1137 
1138  ros::Publisher visPub = n.advertise<visualization_msgs::MarkerArray>(visualizationTopic, 100);
1139 
1140 
1141  ISM::VotingSpace vs = ISM::VotingSpace(bin_size, maxAngleDeviation);
1142  //std_msgs::ColorRGBA regularBinColor = VizHelperRVIZ::createColorRGBA(0.0, 1.0, 0.0, 1.0);
1143  //std_msgs::ColorRGBA regularObjectColor = VizHelperRVIZ::createColorRGBA(0.0, 1.0, 0.0, 1.0);
1144  //const double sphereRadius = 0.005;
1145  //const double markerLifetime = ros::Duration().toSec();
1146 
1147  /*
1148  ros::Subscriber s0 = n.subscribe<std_msgs::Float64MultiArray>("addPoint", 1000, &RecordGenerator::addPoint, &rg);
1149  ros::Subscriber s1 = n.subscribe<std_msgs::UInt32>("applyAndGotoNextTimestep2", 1000, &RecordGenerator::applyAndGotoNextTimestep, &rg);
1150  ros::Subscriber s3 = n.subscribe<std_msgs::Float64MultiArray>("changeParameter", 1000, &RecordGenerator::changeParameter, &rg);
1151  ros::Subscriber s4 = n.subscribe<std_msgs::String>("toogleInvis", 1000, &RecordGenerator::toogleInvisible, &rg);
1152  */
1153  ros::Subscriber s1 = n.subscribe<std_msgs::Float64MultiArray>("showFromTo", 1000, &RecordGenerator::showFromTo, &rg);
1154  //ros::Subscriber s1 = n.subscribe<std_msgs::UInt32MultiArray>("showFromTo", 1000, &RecordGenerator::showFromTo, &rg);
1155  ros::Subscriber s2 = n.subscribe<std_msgs::Empty>("writeToDb", 1000, &RecordGenerator::writeToDb, &rg);
1156 
1157  std::vector<std_msgs::ColorRGBA> allColors;
1158  allColors.push_back(VIZ::VizHelperRVIZ::createColorRGBA(0.0,0.0,0.0,1.0));
1159  allColors.push_back(VIZ::VizHelperRVIZ::createColorRGBA(1.0,0.0,0.0,1.0));
1160  allColors.push_back(VIZ::VizHelperRVIZ::createColorRGBA(0.0,1.0,0.0,1.0));
1161  allColors.push_back(VIZ::VizHelperRVIZ::createColorRGBA(0.0,0.0,1.0,1.0));
1162  allColors.push_back(VIZ::VizHelperRVIZ::createColorRGBA(0.5,0.2,0.9,1.0));
1163  allColors.push_back(VIZ::VizHelperRVIZ::createColorRGBA(0.3,0.8,0.8,1.0));
1164  allColors.push_back(VIZ::VizHelperRVIZ::createColorRGBA(0.3,0.5,0.5,1.0));
1165  allColors.push_back(VIZ::VizHelperRVIZ::createColorRGBA(0.5,0.3,0.1,1.0));
1166 
1167  std::map<std::pair<std::string,std::string>, std_msgs::ColorRGBA > typeIdToColor;
1168  //uint32_t availColorIndex = 0;
1169 
1170  /*
1171  boost::function< geometry_msgs::Point (int,int,int) > pointF = boost::bind(genCuboidPoint, _1, _2, _3, x, y, z, xwitdh, ywidth, zwidth);
1172 std_msgs::ColorRGBA getColor(std::map<std::pair<std::string,std::string>, std_msgs::ColorRGBA >&
1173  typeIdToColor, std::vector<std_msgs::ColorRGBA>& allColors, uint32_t& availColorIndex,
1174  const std::string& type, const std::string& id)
1175  */
1176 
1177  /* boost::function < std_msgs::ColorRGBA (const std::string,const std::string) > getColor =
1178  boost::bind(getColorForTypeId, boost::ref(typeIdToColor), boost::ref(allColors),
1179  boost::ref(availColorIndex), _1, _2);
1180 */
1181  while (ros::ok()) {
1182 
1184  /* std::map<std::tuple<int,int,int>,bool> binDrawn;
1185 
1186  MarkerArray oBs;
1187  MarkerArray bins;
1188  ros::spinOnce();
1189  //TODO
1190  if(rg.mShowCurrent)
1191  {
1192  for(ISM::ObjectPtr& o : rg.getCurrentVisibleObjects())
1193  {
1194  ISM::PointPtr point = o->pose->point;
1195  std::stringstream ss;
1196  ss << o->getType() << " With X:" << point->eigen.x() << "Y:" << point->eigen.y() << "Z:" << point->eigen.z();
1197  oBs.markers.push_back(VIZ::VizHelperRVIZ::createSphereMarker(point, baseFrame, ss.str(),
1198  boost::lexical_cast<int32_t>(o->getId()), sphereRadius, regularObjectColor, markerLifetime));
1199  //TODO: look for better solution than hard-code binSize; Problem introduced because of deletion of calcBinNum in VotingSpace
1200  int x = vs.discretizeToBins(point->eigen.x(), 0.1);
1201  int y = vs.discretizeToBins(point->eigen.y(), 0.1);
1202  int z = vs.discretizeToBins(point->eigen.z(), 0.1);
1203  std::tuple<int,int,int> t = std::make_tuple(x, y, z);
1204  if(binDrawn[t] == false)
1205  {
1206  std::stringstream ss;
1207  ss << "\nObject" << o->getId() << "Is in Bin: "<< "X:" << x << " Y:" << y << " Z:" << z;
1208  bins.markers.push_back(VIZ::VizHelperRVIZ::getBinMarker(x, y, z, bin_size, ss.str(), 0, baseFrame,
1209  regularBinColor));
1210  binDrawn[t] = true;
1211  }
1212  }
1213  for(ISM::ObjectPtr& o : rg.getCurrentInvisibleObjects())
1214  {
1215  ISM::PointPtr point = o->pose->point;
1216  std::stringstream ss;
1217  ss << "Invis: " << o->getType() << " With X:" << point->eigen.x() << "Y:" << point->eigen.y() << "Z:" << point->eigen.z();
1218  visualization_msgs::Marker s = VizHelperRVIZ::createSphereMarker(point, baseFrame, ss.str(),
1219  boost::lexical_cast<int32_t>(o->getId()), sphereRadius, regularObjectColor, markerLifetime);
1220  s.color.g = 0.0f;
1221  s.color.b = 1.0f;
1222  oBs.markers.push_back(s);
1223  //TODO: look for better solution than hard-code binSize; Problem introduced because of deletion of calcBinNum in VotingSpace
1224  int x = vs.discretizeToBins(point->eigen.x(), 0.1);
1225  int y = vs.discretizeToBins(point->eigen.y(), 0.1);
1226  int z = vs.discretizeToBins(point->eigen.z(), 0.1);
1227  std::tuple<int,int,int> t = std::make_tuple(x, y, z);
1228  if(binDrawn[t] == false)
1229  {
1230  std::stringstream ss;
1231  ss << "\nObject" << o->getId() << "Is in Bin: "<< "X:" << x << " Y:" << y << " Z:" << z;
1232  bins.markers.push_back(VizHelperRVIZ::getBinMarker(x,y,z,bin_size,ss.str(), 0, baseFrame,
1233  regularBinColor));
1234  binDrawn[t] = true;
1235  }
1236  }
1237  int id = 0;
1238  for(ISM::ObjectPtr& o : rg.getOldObjects())
1239  {
1240  if(!o)
1241  continue;
1242  ISM::PointPtr point = o->pose->point;
1243  std::stringstream ss;
1244  //ss << "Old " << o->getType() << " With X:" << point->eigen.x() << "Y:" << point->eigen.y() << "Z:" << point->eigen.z();
1245  ss << "Old " << o->getType();
1246  //visualization_msgs::Marker m = objectToSphere(o, baseFrame, ss.str(), boost::lexical_cast<int32_t>(o->getId()));
1247  visualization_msgs::Marker m = VizHelperRVIZ::createSphereMarker(point, baseFrame, ss.str(),
1248  boost::lexical_cast<int32_t>(id++), sphereRadius, regularObjectColor, markerLifetime);
1249  m.color.r = 1.0f;
1250  oBs.markers.push_back(m);
1251  //TODO: look for better solution than hard-code binSize; Problem introduced because of deletion of calcBinNum in VotingSpace
1252  int x = vs.discretizeToBins(point->eigen.x(), 0.1);
1253  int y = vs.discretizeToBins(point->eigen.y(), 0.1);
1254  int z = vs.discretizeToBins(point->eigen.z(), 0.1);
1255  std::tuple<int,int,int> t = std::make_tuple(x, y, z);
1256  if(binDrawn[t] == false)
1257  {
1258  std::stringstream ss;
1259  ss << "\nObject" << o->getId() << "Is in Bin: "<< "X:" << x << " Y:" << y << " Z:" << z;
1260  bins.markers.push_back(VizHelperRVIZ::getBinMarker(x,y,z,bin_size,ss.str(),0,baseFrame,
1261  regularBinColor));
1262  binDrawn[t] = true;
1263  }
1264  }
1265  } else
1266  {
1267  int objectMarkerId = 0;
1268  int binMarkerId = 0;
1269 
1270  for(ISM::ObjectPtr& o : rg.getObjects(rg.mShowFrom, rg.mShowTo))
1271  {
1272  if(!rg.mDoPublish)
1273  {
1274  break;
1275  }
1276  if(!o)
1277  {
1278  continue;
1279  }
1280  ISM::PointPtr point = o->pose->point;
1281  std::stringstream ss;
1282  //ss << o->getType() << " With X:" << point->eigen.x() << "Y:" << point->eigen.y() << "Z:" << point->eigen.z();
1283  ss << o->getType() << " " << o->getId();
1284  auto sphereWithOrient = VIZ::VizHelperRVIZ::createSphereMarkerWithOrientation(
1285  o->pose, baseFrame, ss.str(), objectMarkerId + 1, sphereRadius,
1286  getColor(o->getType(), o->getId()), markerLifetime, objectMarkerId + 2);
1287  objectMarkerId += 4;
1288  oBs.markers.insert(oBs.markers.end(), sphereWithOrient.markers.begin(),
1289  sphereWithOrient.markers.end());
1290  //TODO: look for better solution than hard-code binSize; Problem introduced because of deletion of calcBinNum in VotingSpace
1291  int x = vs.discretizeToBins(point->eigen.x(), 0.1);
1292  int y = vs.discretizeToBins(point->eigen.y(), 0.1);
1293  int z = vs.discretizeToBins(point->eigen.z(), 0.1);
1294  std::tuple<int,int,int> t = std::make_tuple(x, y, z);
1295  if(binDrawn[t] == false)
1296  {
1297  std::stringstream ss;
1298  //ss << "\nObject" << o->getId() << "Is in Bin: "<< "X:" << x << " Y:" << y << " Z:" << z;
1299  if(detailedBinNS)
1300  {
1301  ss << "Bin: "<< "X:" << x << " Y:" << y << " Z:" << z;
1302  } else
1303  {
1304  ss << "Bins";
1305  }
1306  bins.markers.push_back(VIZ::VizHelperRVIZ::getBinMarker(x, y, z, bucketSize, ss.str(),
1307  binMarkerId++, baseFrame, regularBinColor));
1308  binDrawn[t] = true;
1309  }
1310  }
1311 
1312  }
1313  visPub.publish(oBs);
1314  visPub.publish(bins);*/
1315 
1316  r.sleep();
1317  }
1318 
1319 }
RecordGenerator::mPatternToRef
std::map< std::string, std::string > mPatternToRef
Definition: recordGen.cpp:1030
RecordGenPoint::mCurrentOrientX
double mCurrentOrientX
Definition: recordGen.cpp:342
RecordGenPoint::applyAndGotoNextTimestep
virtual void applyAndGotoNextTimestep()
Definition: recordGen.cpp:322
RecordGenerator::mTracks
std::vector< ISM::TrackPtr > mTracks
Definition: recordGen.cpp:1034
orientCalcTypeFromString
OrientCalcType orientCalcTypeFromString(const std::string &s)
Definition: recordGen.cpp:119
RecordGenPoint::mRandomGenOrient
boost::mt19937 mRandomGenOrient
Definition: recordGen.cpp:358
ros::NodeHandle
RecordGenerator::rec
ISM::RecorderPtr rec
Definition: recordGen.cpp:1035
RecordGenPoint::RecordGenPoint
RecordGenPoint(std::string name, int32_t id, RecGenPointType type, double posX, double posY, double posZ, double orientW, double orientX, double orientY, double orientZ, OrientCalcType orientCalcType, double posErr, double orientDerivation, unsigned int from, unsigned int to, double linearDegX, double linearDegY, double linearDegZ)
Definition: recordGen.cpp:137
RecordGenerator::parseXml
void parseXml(path xmlFile)
Definition: recordGen.cpp:608
RecordGenPoint::mCurrentO
ISM::ObjectPtr mCurrentO
Definition: recordGen.cpp:336
RecordGenerator::mDynReconfServer
dynamic_reconfigure::Server< asr_ism::recordGenConfig > mDynReconfServer
Definition: recordGen.cpp:1023
RecordGenPoint::mTrack
ISM::TrackPtr mTrack
Definition: recordGen.cpp:335
f
f
values
std::vector< double > values
RecordGenPointPtr
boost::shared_ptr< RecordGenPoint > RecordGenPointPtr
Definition: recordGen.cpp:588
RecordGenPoint::mCurrentOrientW
double mCurrentOrientW
Definition: recordGen.cpp:341
FixRecordGenPoint::calcCurrObPos
virtual void calcCurrObPos()
Definition: recordGen.cpp:571
RecordGenPoint::mFrom
unsigned int mFrom
Definition: recordGen.cpp:352
ros::NodeHandle::subscribe
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
RecordGenPoint::mLinearDegZ
double mLinearDegZ
Definition: recordGen.cpp:356
RecordGenPoint::~RecordGenPoint
virtual ~RecordGenPoint()
Definition: recordGen.cpp:186
RecordGenerator::applyAndGotoNextTimestep
void applyAndGotoNextTimestep(const uint32_t n)
Definition: recordGen.cpp:846
s
XmlRpcServer s
MidSplinePoint::mTimeSpan
double mTimeSpan
Definition: recordGen.cpp:480
MidSplinePointAlt::calcCurrObPos
virtual void calcCurrObPos()
Definition: recordGen.cpp:497
MidSplinePoint::isVisible
virtual bool isVisible()
Definition: recordGen.cpp:439
ros::init
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
RecordGenPoint::mInitOrientW
double mInitOrientW
Definition: recordGen.cpp:345
RecordGenerator::mDbFile
std::string mDbFile
Definition: recordGen.cpp:1027
RecordGenPoint::mUniform
boost::uniform_real mUniform
Definition: recordGen.cpp:359
H
Definition: recordGen.cpp:85
FromToPointTuple
std::tuple< uint32_t, uint32_t, RecordGenPointPtr > FromToPointTuple
Definition: recordGen.cpp:589
recGenPointTypeFromString
RecGenPointType recGenPointTypeFromString(const std::string &s)
Definition: recordGen.cpp:95
RecordGenerator::getCurrentInvisibleObjects
std::vector< ISM::ObjectPtr > getCurrentInvisibleObjects()
Definition: recordGen.cpp:872
MidSplinePoint::MidSplinePoint
MidSplinePoint(std::string name, int32_t id, double xs, double ys, double zs, double xe, double ye, double ze, double degree, double timeSpan, double orientW, double orientX, double orientY, double orientZ, OrientCalcType orientCalcType, double posErr, double orientErr, unsigned int from, unsigned int to, double linearDegX, double linearDegY, double linearDegZ)
Definition: recordGen.cpp:400
FixRecordGenPoint::isVisible
virtual bool isVisible()
Definition: recordGen.cpp:561
RecordGenerator::mTimestep
unsigned int mTimestep
Definition: recordGen.cpp:1026
RecordGenerator::applyAndGotoNextTimestep
void applyAndGotoNextTimestep()
Definition: recordGen.cpp:801
RecordGenerator::mPatternTypeToVoxelReps
std::map< std::pair< std::string, std::string >, std::set< uint32_t > > mPatternTypeToVoxelReps
Definition: recordGen.cpp:1029
frand
double frand(double min, double max)
Definition: recordGen.cpp:72
RecordGenerator::showFromTo
void showFromTo(const std_msgs::Float64MultiArray::ConstPtr &msg)
Definition: recordGen.cpp:891
RecordGenPoint::calcCurrObOrient
virtual void calcCurrObOrient()
Definition: recordGen.cpp:191
MidSplinePoint::setNewParams
void setNewParams(double xs, double ys, double zs, double xe, double ye, double ze, double degree, double timeSpan)
Definition: recordGen.cpp:418
boost::shared_ptr
MidSplinePointAlt::MidSplinePointAlt
MidSplinePointAlt(std::string name, int32_t id, double xs, double ys, double zs, double xe, double ye, double ze, double degree, double timeSpan, double orientW, double orientX, double orientY, double orientZ, OrientCalcType orientCalcType, double posErr, double orientErr, unsigned int from, unsigned int to, double linearDegX, double linearDegY, double linearDegZ)
Definition: recordGen.cpp:486
RecordGenerator::filterViaVgOut
void filterViaVgOut(boost::filesystem::path vgOutFile)
Definition: recordGen.cpp:750
RecordGenPoint::mType
RecGenPointType mType
Definition: recordGen.cpp:337
RecordGenerator::mShowFrom
uint32_t mShowFrom
Definition: recordGen.cpp:1038
RecordGenPoint::apply
virtual void apply()
Definition: recordGen.cpp:264
RecordGenPoint::mCurrentPosY
double mCurrentPosY
Definition: recordGen.cpp:339
RecordGenPoint::mVariantGen
boost::variate_generator< boost::mt19937, boost::uniform_real<> > * mVariantGen
Definition: recordGen.cpp:361
MidSplinePoint::~MidSplinePoint
virtual ~MidSplinePoint()
Definition: recordGen.cpp:435
RecordGenPoint::mInitOrientZ
double mInitOrientZ
Definition: recordGen.cpp:348
RecordGenPoint::mCurrentPosX
double mCurrentPosX
Definition: recordGen.cpp:338
RecordGenerator::mPoints
std::set< std::tuple< uint32_t, uint32_t, RecordGenPointPtr >, cmpFromToPointTuple > mPoints
Definition: recordGen.cpp:1033
RecordGenPoint::mCurrentOrientY
double mCurrentOrientY
Definition: recordGen.cpp:343
ros::ok
ROSCPP_DECL bool ok()
RecGenPointType
RecGenPointType
Definition: recordGen.cpp:77
RecordGenerator::mShowTo
uint32_t mShowTo
Definition: recordGen.cpp:1039
MidSplinePoint::calcSpline
void calcSpline()
Definition: recordGen.cpp:377
x
TFSIMD_FORCE_INLINE const tfScalar & x() const
RecordGenerator::dynamicReconfCallback
void dynamicReconfCallback(asr_ism::recordGenConfig &config, uint32_t level)
Definition: recordGen.cpp:725
MidSplinePoint::calcCurrObPos
virtual void calcCurrObPos()
Definition: recordGen.cpp:449
ros::NodeHandle::advertise
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
main
int main(int argc, char **argv)
Definition: recordGen.cpp:1068
RecordGenerator::mIdXmlPoints
std::vector< int32_t > mIdXmlPoints
Definition: recordGen.cpp:1021
RecordGenPoint::mOrientCalcType
OrientCalcType mOrientCalcType
Definition: recordGen.cpp:349
getColorForTypeId
std_msgs::ColorRGBA getColorForTypeId(std::map< std::pair< std::string, std::string >, std_msgs::ColorRGBA > &typeIdToColor, std::vector< std_msgs::ColorRGBA > &allColors, uint32_t &availColorIndex, std::string &type, std::string &id)
Definition: recordGen.cpp:1042
ros::Rate::sleep
bool sleep()
H::contains
auto contains(const C &v, const T &x) -> decltype(end(v), true)
Definition: recordGen.cpp:88
RecordGenPoint::mTo
unsigned int mTo
Definition: recordGen.cpp:353
RecordGenPoint::mCurrentOrientZ
double mCurrentOrientZ
Definition: recordGen.cpp:344
RecordGenerator::writeToDb
void writeToDb(const std_msgs::Empty)
Definition: recordGen.cpp:1014
ROS_INFO_STREAM
#define ROS_INFO_STREAM(args)
FixRecordGenPoint::FixRecordGenPoint
FixRecordGenPoint(std::string name, int32_t id, double x, double y, double z, double orientW, double orientX, double orientY, double orientZ, OrientCalcType orientCalcType, double posErr, double orientErr, unsigned int from, unsigned int to, double linearDegX, double linearDegY, double linearDegZ)
Definition: recordGen.cpp:524
ros::NodeHandle::getParam
bool getParam(const std::string &key, std::string &s) const
RecordGenPoint::mCurrentPosZ
double mCurrentPosZ
Definition: recordGen.cpp:340
RecordGenPoint::mCurrentApplied
bool mCurrentApplied
Definition: recordGen.cpp:332
FixRecordGenPoint::~FixRecordGenPoint
virtual ~FixRecordGenPoint()
Definition: recordGen.cpp:549
RecordGenPoint::mNormal
boost::normal_distribution mNormal
Definition: recordGen.cpp:360
RecordGenPoint::mLinearDegY
double mLinearDegY
Definition: recordGen.cpp:355
RecordGenerator::mPatternName
std::string mPatternName
Definition: recordGen.cpp:1031
RecordGenerator::getOldObjects
std::vector< ISM::ObjectPtr > getOldObjects()
Definition: recordGen.cpp:987
RecordGenerator::getCurrentVisibleObjects
std::vector< ISM::ObjectPtr > getCurrentVisibleObjects()
Definition: recordGen.cpp:854
RecordGenPoint::mOrientDer
double mOrientDer
Definition: recordGen.cpp:351
RecordGenPoint::mName
std::string mName
Definition: recordGen.cpp:333
MidSplinePoint::forceInvisible
virtual void forceInvisible(bool b)
Definition: recordGen.cpp:444
MidSplinePoint::setCol
void setCol(Eigen::MatrixXd &m, int col, Eigen::Vector3d &v, double time)
Definition: recordGen.cpp:369
RecordGenPoint::mInitOrientX
double mInitOrientX
Definition: recordGen.cpp:346
RecordGenerator::mTimestampVisibleXmlPoints
std::map< RecordGenPointPtr, std::vector< unsigned int > > mTimestampVisibleXmlPoints
Definition: recordGen.cpp:1022
MidSplinePoint::mSpline
Eigen::Spline< double, 4 > mSpline
Definition: recordGen.cpp:478
RecordGenPoint::mLinearDegX
double mLinearDegX
Definition: recordGen.cpp:354
OrientCalcType
OrientCalcType
Definition: recordGen.cpp:111
RecordGenerator::getObjects
std::vector< ISM::ObjectPtr > getObjects(unsigned int from, unsigned int to)
Definition: recordGen.cpp:898
RecordGenPoint::mInitOrientY
double mInitOrientY
Definition: recordGen.cpp:347
RecordGenPoint::mRandomGen
boost::mt19937 mRandomGen
Definition: recordGen.cpp:357
RecordGenerator::RecordGenerator
RecordGenerator(std::string dbFile, std::string patternName="pattern0", bool doPublish=false)
Definition: recordGen.cpp:731
RecordGenPoint::mVarGenOrient
boost::variate_generator< boost::mt19937, boost::normal_distribution<> > * mVarGenOrient
Definition: recordGen.cpp:362
cmpFromToPointTuple::operator()
bool operator()(const FromToPointTuple &a, const FromToPointTuple &b) const
Definition: recordGen.cpp:593
FixRecordGenPoint::setNewParams
void setNewParams(double x, double y, double z)
Definition: recordGen.cpp:553
RecordGenPoint::mTimestep
unsigned int mTimestep
Definition: recordGen.cpp:331
FixRecordGenPoint::forceInvisible
virtual void forceInvisible(bool b)
Definition: recordGen.cpp:566

asr_ism
Author(s): Borella Jocelyn, Hanselmann Fabian, Heller Florian, Heizmann Heinrich, Kübler Marcel, Mehlhaus Jonas, Meißner Pascal, Qattan Mohamad, Reckling Reno, Stroh Daniel
autogenerated on Thu Jan 9 2020 07:20:58