map.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2017-9, Ubiquity Robotics
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * 1. Redistributions of source code must retain the above copyright notice,
9  * this list of conditions and the following disclaimer.
10  * 2. Redistributions in binary form must reproduce the above copyright notice,
11  * this list of conditions and the following disclaimer in the documentation
12  * and/or other materials provided with the distribution.
13  *
14  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
15  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
16  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
17  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
18  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
19  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
20  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
21  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
22  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
23  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
24  * POSSIBILITY OF SUCH DAMAGE.
25  *
26  * The views and conclusions contained in the software and documentation are
27  * those of the authors and should not be interpreted as representing official
28  * policies, either expressed or implied, of the FreeBSD Project.
29  *
30  */
31 
32 #include <fiducial_slam/helpers.h>
33 #include <fiducial_slam/map.h>
34 
36 #include <tf2/LinearMath/Vector3.h>
37 #include <string>
38 
39 #include <std_msgs/ColorRGBA.h>
40 #include <std_msgs/String.h>
41 
42 #include <geometry_msgs/Point.h>
43 #include <geometry_msgs/PoseWithCovarianceStamped.h>
44 #include <geometry_msgs/TransformStamped.h>
45 #include <visualization_msgs/Marker.h>
46 
47 #include <boost/filesystem.hpp>
48 
49 
50 static double systematic_error = 0.01;
51 
52 // Constructor for observation
54  this->fid = fid;
55 
56  T_camFid = camFid;
58  T_fidCam.transform = T_camFid.transform.inverse();
59 }
60 
61 // Update a fiducial position in map with a new estimate
63  pose.update(newPose);
64  numObs++;
65 }
66 
67 // Create a fiducial from an estimate of its position in the map
69  this->id = id;
70  this->pose = pose;
71  this->lastPublished = ros::Time(0);
72  this->numObs = 0;
73  this->visible = false;
74 }
75 
76 // Constructor for map
77 
78 Map::Map(ros::NodeHandle &nh) : tfBuffer(ros::Duration(30.0)) {
79  frameNum = 0;
80  initialFrameNum = 0;
81  originFid = -1;
82  isInitializingMap = false;
83  havePose = false;
84  fiducialToAdd = -1;
85 
86  listener = make_unique<tf2_ros::TransformListener>(tfBuffer);
87 
88  robotPosePub =
89  ros::Publisher(nh.advertise<geometry_msgs::PoseWithCovarianceStamped>("/fiducial_pose", 1));
91  nh.advertise<geometry_msgs::PoseWithCovarianceStamped>("/fiducial_slam/camera_pose", 1));
92 
93  markerPub = ros::Publisher(nh.advertise<visualization_msgs::Marker>("/fiducials", 100));
94  mapPub = ros::Publisher(nh.advertise<fiducial_msgs::FiducialMapEntryArray>("/fiducial_map", 1));
95 
96  clearSrv = nh.advertiseService("clear_map", &Map::clearCallback, this);
97  addSrv = nh.advertiseService("add_fiducial", &Map::addFiducialCallback, this);
98 
99  nh.param<std::string>("map_frame", mapFrame, "map");
100  nh.param<std::string>("odom_frame", odomFrame, "odom");
101  nh.param<std::string>("base_frame", baseFrame, "base_footprint");
102 
103  nh.param<float>("tf_publish_interval", tfPublishInterval, 1.0);
104  nh.param<bool>("publish_tf", publishPoseTf, true);
105  nh.param<double>("systematic_error", systematic_error, 0.01);
106  nh.param<double>("future_date_transforms", future_date_transforms, 0.1);
107  nh.param<bool>("publish_6dof_pose", publish_6dof_pose, false);
108  nh.param<bool>("read_only_map", readOnly, false);
109 
110  std::fill(covarianceDiagonal.begin(), covarianceDiagonal.end(), 0);
111  overridePublishedCovariance = nh.getParam("covariance_diagonal", covarianceDiagonal);
112  if (overridePublishedCovariance) {
113  // Check to make sure that the diagonal is non-zero
114  for (auto variance : covarianceDiagonal) {
115  if (variance = 0) {
116  ROS_WARN("ignoring covariance_diagonal because it has 0 values");
117  std::fill(covarianceDiagonal.begin(), covarianceDiagonal.end(), 0);
118  break;
119  }
120  }
121  }
122 
123  // threshold of object error for using multi-fidicial pose
124  // set -ve to never use
125  nh.param<double>("multi_error_theshold", multiErrorThreshold, -1);
126 
127  nh.param<std::string>("map_file", mapFilename,
128  std::string(getenv("HOME")) + "/.ros/slam/map.txt");
129 
130  boost::filesystem::path mapPath(mapFilename);
131  boost::filesystem::path dir = mapPath.parent_path();
132  boost::filesystem::create_directories(dir);
133 
134  std::string initialMap;
135  nh.param<std::string>("initial_map_file", initialMap, "");
136 
137  if (!initialMap.empty()) {
138  loadMap(initialMap);
139  } else {
140  loadMap();
141  }
142 
143  publishMarkers();
144 }
145 
146 // Update map with a set of observations
147 
148 void Map::update(std::vector<Observation> &obs, const ros::Time &time) {
149  ROS_INFO("Updating map with %d observations. Map has %d fiducials", (int)obs.size(),
150  (int)fiducials.size());
151 
152  frameNum++;
153 
154  if (obs.size() > 0 && fiducials.size() == 0) {
155  isInitializingMap = true;
156  }
157 
158  if (isInitializingMap) {
159  autoInit(obs, time);
160  } else {
162  T_mapCam.frame_id_ = mapFrame;
163 
164  if (updatePose(obs, time, T_mapCam) > 0 && obs.size() > 1 && !readOnly) {
165  updateMap(obs, time, T_mapCam);
166  }
167  }
168 
169  handleAddFiducial(obs);
170 
171  publishMap();
172 }
173 
174 // update estimates of observed fiducials from previously estimated
175 // camera pose
176 
177 void Map::updateMap(const std::vector<Observation> &obs, const ros::Time &time,
178  const tf2::Stamped<TransformWithVariance> &T_mapCam) {
179  for (auto &map_pair : fiducials) {
180  Fiducial &f = map_pair.second;
181  f.visible = false;
182  }
183 
184  for (const Observation &o : obs) {
185  // This should take into account the variances from both
186  tf2::Stamped<TransformWithVariance> T_mapFid = T_mapCam * o.T_camFid;
187  T_mapFid.frame_id_ = mapFrame;
188 
189  // New scope for logging vars
190  {
191  tf2::Vector3 trans = T_mapFid.transform.getOrigin();
192 
193  ROS_INFO("Estimate of %d %lf %lf %lf var %lf %lf", o.fid, trans.x(), trans.y(),
194  trans.z(), o.T_camFid.variance, T_mapFid.variance);
195 
196  if (std::isnan(trans.x()) || std::isnan(trans.y()) || std::isnan(trans.z())) {
197  ROS_WARN("Skipping NAN estimate\n");
198  continue;
199  };
200  }
201 
202  if (fiducials.find(o.fid) == fiducials.end()) {
203  ROS_INFO("New fiducial %d", o.fid);
204  fiducials[o.fid] = Fiducial(o.fid, T_mapFid);
205  }
206  Fiducial &f = fiducials[o.fid];
207  f.visible = true;
208  if (f.pose.variance != 0) {
209  f.update(T_mapFid);
210  f.numObs++;
211  }
212 
213  for (const Observation &observation : obs) {
214  int fid = observation.fid;
215  if (f.id != fid) {
216  f.links.insert(fid);
217  }
218  }
219  publishMarker(fiducials[o.fid]);
220  }
221 }
222 
223 // lookup specified transform
224 
225 bool Map::lookupTransform(const std::string &from, const std::string &to, const ros::Time &time,
226  tf2::Transform &T) const {
227  geometry_msgs::TransformStamped transform;
228 
229  try {
230  transform = tfBuffer.lookupTransform(from, to, time, ros::Duration(0.5));
231 
232  tf2::fromMsg(transform.transform, T);
233  return true;
234  } catch (tf2::TransformException &ex) {
235  ROS_WARN("%s", ex.what());
236  return false;
237  }
238 }
239 
240 // update pose estimate of robot. We combine the camera->base_link
241 // tf to each estimate so we can evaluate how good they are. A good
242 // estimate would have z == roll == pitch == 0.
243 int Map::updatePose(std::vector<Observation> &obs, const ros::Time &time,
245  int numEsts = 0;
249 
250  if (obs.size() == 0) {
251  return 0;
252  }
253 
254  if (lookupTransform(obs[0].T_camFid.frame_id_, baseFrame, time, T_camBase.transform)) {
255  tf2::Vector3 c = T_camBase.transform.getOrigin();
256  ROS_INFO("camera->base %lf %lf %lf", c.x(), c.y(), c.z());
257  T_camBase.variance = 1.0;
258  } else {
259  ROS_ERROR("Cannot determine tf from camera to robot\n");
260  }
261 
262  if (lookupTransform(baseFrame, obs[0].T_camFid.frame_id_, time, T_baseCam.transform)) {
263  tf2::Vector3 c = T_baseCam.transform.getOrigin();
264  ROS_INFO("base->camera %lf %lf %lf", c.x(), c.y(), c.z());
265  T_baseCam.variance = 1.0;
266  } else {
267  ROS_ERROR("Cannot determine tf from robot to camera\n");
268  return numEsts;
269  }
270 
271  for (Observation &o : obs) {
272  if (fiducials.find(o.fid) != fiducials.end()) {
273  const Fiducial &fid = fiducials[o.fid];
274 
275  tf2::Stamped<TransformWithVariance> p = fid.pose * o.T_fidCam;
276 
277  p.frame_id_ = mapFrame;
278  p.stamp_ = o.T_fidCam.stamp_;
279 
280  p.setData(p * T_camBase);
281  auto position = p.transform.getOrigin();
282  double roll, pitch, yaw;
283  p.transform.getBasis().getRPY(roll, pitch, yaw);
284 
285  // Create variance according to how well the robot is upright on the ground
286  // TODO: Create variance for each DOF
287  // TODO: Take into account position according to odom
288  auto cam_f = o.T_camFid.transform.getOrigin();
289  double s1 = std::pow(position.z() / cam_f.z(), 2) *
290  (std::pow(cam_f.x(), 2) + std::pow(cam_f.y(), 2));
291  double s2 = position.length2() * std::pow(std::sin(roll), 2);
292  double s3 = position.length2() * std::pow(std::sin(pitch), 2);
293  p.variance = s1 + s2 + s3 + systematic_error;
294  o.T_camFid.variance = p.variance;
295 
296  ROS_INFO("Pose %d %lf %lf %lf %lf %lf %lf %lf", o.fid, position.x(), position.y(),
297  position.z(), roll, pitch, yaw, p.variance);
298 
299  // drawLine(fid.pose.getOrigin(), o.position);
300 
301  if (std::isnan(position.x()) || std::isnan(position.y()) || std::isnan(position.z())) {
302  ROS_WARN("Skipping NAN estimate\n");
303  continue;
304  };
305 
306  // compute base_link pose based on this estimate
307 
308  if (numEsts == 0) {
309  T_mapBase = p;
310  } else {
311  T_mapBase.setData(averageTransforms(T_mapBase, p));
312  T_mapBase.stamp_ = p.stamp_;
313  }
314  numEsts++;
315  }
316  }
317 
318  if (numEsts == 0) {
319  ROS_INFO("Finished frame - no estimates\n");
320  return numEsts;
321  }
322 
323  // New scope for logging vars
324  {
325  tf2::Vector3 trans = T_mapBase.transform.getOrigin();
326  double r, p, y;
327  T_mapBase.transform.getBasis().getRPY(r, p, y);
328 
329  ROS_INFO("Pose ALL %lf %lf %lf %lf %lf %lf %f", trans.x(), trans.y(), trans.z(), r, p, y,
330  T_mapBase.variance);
331  }
332 
333  tf2::Stamped<TransformWithVariance> basePose = T_mapBase;
334  basePose.frame_id_ = mapFrame;
335  auto robotPose = toPose(basePose);
336 
338  for (int i = 0; i <= 5; i++) {
339  robotPose.pose.covariance[i * 6 + i] = covarianceDiagonal[i]; // Fill the diagonal
340  }
341  }
342 
343  T_mapCam = T_mapBase * T_baseCam;
344 
345  robotPosePub.publish(robotPose);
346 
347  tf2::Stamped<TransformWithVariance> outPose = basePose;
348  outPose.frame_id_ = mapFrame;
349  std::string outFrame = baseFrame;
350 
351  if (!odomFrame.empty()) {
352  tf2::Transform odomTransform;
353  if (lookupTransform(odomFrame, baseFrame, outPose.stamp_, odomTransform)) {
354  outPose.setData(basePose * odomTransform.inverse());
355  outFrame = odomFrame;
356 
357  tf2::Vector3 c = odomTransform.getOrigin();
358  ROS_INFO("odom %lf %lf %lf", c.x(), c.y(), c.z());
359  }
360  else {
361  // Don't publish anything if map->odom was requested and is unavailaable
362  return numEsts;
363  }
364  }
365 
366  // Make outgoing transform make sense - ie only consist of x, y, yaw
367  // This can be disabled via the publish_6dof_pose param, mainly for debugging
368  if (!publish_6dof_pose) {
369  tf2::Vector3 translation = outPose.transform.getOrigin();
370  translation.setZ(0);
371  outPose.transform.setOrigin(translation);
372  double roll, pitch, yaw;
373  outPose.transform.getBasis().getRPY(roll, pitch, yaw);
374  outPose.transform.getBasis().setRPY(0, 0, yaw);
375  }
376 
377  poseTf = toMsg(outPose);
378  poseTf.child_frame_id = outFrame;
379  havePose = true;
380 
381  if (publishPoseTf) {
382  publishTf();
383  }
384 
385  ROS_INFO("Finished frame. Estimates %d\n", numEsts);
386  return numEsts;
387 }
388 
389 // Publish map -> odom tf
390 
395 }
396 
397 // publish latest tf if enough time has elapsed
398 
399 void Map::update() {
400  ros::Time now = ros::Time::now();
401  if (publishPoseTf && havePose && tfPublishInterval != 0.0 &&
402  (now - tfPublishTime).toSec() > tfPublishInterval) {
403  publishTf();
404  tfPublishTime = now;
405  }
406  publishMarkers();
407 }
408 
409 // Find closest fiducial to camera
410 
411 static int findClosestObs(const std::vector<Observation> &obs) {
412  double smallestDist = -1;
413  int closestIdx = -1;
414 
415  for (size_t i = 0; i < obs.size(); i++) {
416  const Observation &o = obs[i];
417  double d = o.T_camFid.transform.getOrigin().length2();
418  if (smallestDist < 0 || d < smallestDist) {
419  smallestDist = d;
420  closestIdx = i;
421  }
422  }
423 
424  return closestIdx;
425 }
426 
427 // Initialize a map from the closest observed fiducial
428 // Figure out the closest marker, and then figure out the
429 // pose of that marker such that base_link is at the origin of the
430 // map frame
431 
432 void Map::autoInit(const std::vector<Observation> &obs, const ros::Time &time) {
433  ROS_INFO("Auto init map %d", frameNum);
434 
435  tf2::Transform T_baseCam;
436 
437  if (fiducials.size() == 0) {
438  int idx = findClosestObs(obs);
439 
440  if (idx == -1) {
441  ROS_WARN("Could not find a fiducial to initialize map from");
442  return;
443  }
444  const Observation &o = obs[idx];
445  originFid = o.fid;
446 
447  ROS_INFO("Initializing map from fiducial %d", o.fid);
448 
450 
451  if (lookupTransform(baseFrame, o.T_camFid.frame_id_, o.T_camFid.stamp_, T_baseCam)) {
452  T.setData(T_baseCam * T);
453  }
454 
455  fiducials[o.fid] = Fiducial(o.fid, T);
456  } else {
457  for (const Observation &o : obs) {
458  if (o.fid == originFid) {
460 
461  tf2::Vector3 trans = T.transform.getOrigin();
462  ROS_INFO("Estimate of %d from base %lf %lf %lf err %lf", o.fid, trans.x(),
463  trans.y(), trans.z(), o.T_camFid.variance);
464 
465  if (lookupTransform(baseFrame, o.T_camFid.frame_id_, o.T_camFid.stamp_,
466  T_baseCam)) {
467  T.setData(T_baseCam * T);
468  }
469 
470  fiducials[originFid].update(T);
471  break;
472  }
473  }
474  }
475 
476  if (frameNum - initialFrameNum > 10 && originFid != -1) {
477  isInitializingMap = false;
478 
479  fiducials[originFid].pose.variance = 0.0;
480  }
481 }
482 
483 // Attempt to add the specified fiducial to the map
484 
485 void Map::handleAddFiducial(const std::vector<Observation> &obs) {
486 
487  if (fiducialToAdd == -1) {
488  return;
489  }
490 
491  if (fiducials.find(fiducialToAdd) != fiducials.end()) {
492  ROS_INFO("Fiducial %d is already in map - ignoring add request",
493  fiducialToAdd);
494  fiducialToAdd = -1;
495  return;
496  }
497 
498  for (const Observation &o : obs) {
499  if (o.fid == fiducialToAdd) {
500  ROS_INFO("Adding fiducial_id %d to map", fiducialToAdd);
501 
502 
504 
505  // Take into account position of camera on base
506  tf2::Transform T_baseCam;
507  if (lookupTransform(baseFrame, o.T_camFid.frame_id_,
508  o.T_camFid.stamp_, T_baseCam)) {
509  T.setData(T_baseCam * T);
510  }
511 
512  // Take into account position of robot in the world if known
513  tf2::Transform T_mapBase;
514  if (lookupTransform(mapFrame, baseFrame, ros::Time(0), T_mapBase)) {
515  T.setData(T_mapBase * T);
516  }
517  else {
518  ROS_INFO("Placing robot at the origin");
519  }
520 
521  fiducials[o.fid] = Fiducial(o.fid, T);
522  fiducials[originFid].pose.variance = 0.0;
523  isInitializingMap = false;
524 
525  fiducialToAdd = -1;
526  return;
527  }
528  }
529 
530  ROS_INFO("Unable to add fiducial %d to map", fiducialToAdd);
531 }
532 
533 // save map to file
534 
535 bool Map::saveMap() { return saveMap(mapFilename); }
536 
537 bool Map::saveMap(std::string filename) {
538  ROS_INFO("Saving map with %d fiducials to file %s\n", (int)fiducials.size(), filename.c_str());
539 
540  FILE *fp = fopen(filename.c_str(), "w");
541  if (fp == NULL) {
542  ROS_WARN("Could not open %s for write\n", filename.c_str());
543  return false;
544  }
545 
546  for (auto &map_pair : fiducials) {
547  Fiducial &f = map_pair.second;
548  tf2::Vector3 trans = f.pose.transform.getOrigin();
549  double rx, ry, rz;
550  f.pose.transform.getBasis().getRPY(rx, ry, rz);
551 
552  fprintf(fp, "%d %lf %lf %lf %lf %lf %lf %lf %d", f.id, trans.x(), trans.y(), trans.z(),
553  rad2deg(rx), rad2deg(ry), rad2deg(rz), f.pose.variance, f.numObs);
554 
555  for (const auto linked_fid : f.links) {
556  fprintf(fp, " %d", linked_fid);
557  }
558  fprintf(fp, "\n");
559  }
560  fclose(fp);
561  return true;
562 }
563 
564 // Load map from file
565 
566 bool Map::loadMap() { return loadMap(mapFilename); }
567 
568 bool Map::loadMap(std::string filename) {
569  int numRead = 0;
570 
571  ROS_INFO("Load map %s", filename.c_str());
572 
573  FILE *fp = fopen(filename.c_str(), "r");
574  if (fp == NULL) {
575  ROS_WARN("Could not open %s for read\n", filename.c_str());
576  return false;
577  }
578 
579  const int BUFSIZE = 2048;
580  char linebuf[BUFSIZE];
581  char linkbuf[BUFSIZE];
582 
583  while (!feof(fp)) {
584  if (fgets(linebuf, BUFSIZE - 1, fp) == NULL) break;
585 
586  int id;
587  double tx, ty, tz, rx, ry, rz, var;
588  int numObs = 0;
589 
590  linkbuf[0] = '\0';
591  int nElems = sscanf(linebuf, "%d %lf %lf %lf %lf %lf %lf %lf %d%[^\t\n]*s", &id, &tx, &ty,
592  &tz, &rx, &ry, &rz, &var, &numObs, linkbuf);
593  if (nElems == 9 || nElems == 10) {
594  tf2::Vector3 tvec(tx, ty, tz);
595  tf2::Quaternion q;
596  q.setRPY(deg2rad(rx), deg2rad(ry), deg2rad(rz));
597 
598  auto twv = TransformWithVariance(tvec, q, var);
599  // TODO: figure out what the timestamp in Fiducial should be
600  Fiducial f =
602  f.numObs = numObs;
603 
604  std::istringstream ss(linkbuf);
605  std::string s;
606  while (getline(ss, s, ' ')) {
607  if (!s.empty()) {
608  f.links.insert(stoi(s));
609  }
610  }
611  fiducials[id] = f;
612  numRead++;
613  } else {
614  ROS_WARN("Invalid line: %s", linebuf);
615  }
616  }
617 
618  fclose(fp);
619  ROS_INFO("Load map %s read %d entries", filename.c_str(), numRead);
620  return true;
621 }
622 
623 // Publish the map
624 
626  fiducial_msgs::FiducialMapEntryArray fmea;
627  std::map<int, Fiducial>::iterator it;
628 
629  for (const auto &map_pair : fiducials) {
630  const Fiducial &f = map_pair.second;
631 
632  fiducial_msgs::FiducialMapEntry fme;
633  fme.fiducial_id = f.id;
634 
635  tf2::Vector3 t = f.pose.transform.getOrigin();
636  fme.x = t.x();
637  fme.y = t.y();
638  fme.z = t.z();
639 
640  double rx, ry, rz;
641  f.pose.transform.getBasis().getRPY(rx, ry, rz);
642  fme.rx = rx;
643  fme.ry = ry;
644  fme.rz = rz;
645 
646  fmea.fiducials.push_back(fme);
647  }
648 
649  mapPub.publish(fmea);
650 }
651 
652 // Publish the next marker visualization messages that hasn't been
653 // published recently
654 
656  ros::Time now = ros::Time::now();
657  std::map<int, Fiducial>::iterator it;
658 
659  for (auto &map_pair : fiducials) {
660  Fiducial &f = map_pair.second;
661  if ((now - f.lastPublished).toSec() > 1.0) {
662  publishMarker(f);
663  }
664  }
665 }
666 
667 // Publish visualization messages for a single fiducial
668 
671 
672  // Flattened cube
673  visualization_msgs::Marker marker;
674  marker.type = visualization_msgs::Marker::CUBE;
675  marker.action = visualization_msgs::Marker::ADD;
676  toMsg(fid.pose.transform, marker.pose);
677 
678  marker.scale.x = 0.15;
679  marker.scale.y = 0.15;
680  marker.scale.z = 0.01;
681  if (fid.visible) {
682  marker.color.r = 1.0f;
683  marker.color.g = 0.0f;
684  marker.color.b = 0.0f;
685  marker.color.a = 1.0f;
686  } else {
687  marker.color.r = 0.0f;
688  marker.color.g = 1.0f;
689  marker.color.b = 0.0f;
690  marker.color.a = 1.0f;
691  }
692  marker.id = fid.id;
693  marker.ns = "fiducial";
694  marker.header.frame_id = mapFrame;
695  markerPub.publish(marker);
696 
697  // cylinder scaled by stddev
698  visualization_msgs::Marker cylinder;
699  cylinder.type = visualization_msgs::Marker::CYLINDER;
700  cylinder.action = visualization_msgs::Marker::ADD;
701  cylinder.header.frame_id = mapFrame;
702  cylinder.color.r = 0.0f;
703  cylinder.color.g = 0.0f;
704  cylinder.color.b = 1.0f;
705  cylinder.color.a = 0.5f;
706  cylinder.id = fid.id + 10000;
707  cylinder.ns = "sigma";
708  cylinder.scale.x = cylinder.scale.y = std::max(std::sqrt(fid.pose.variance), 0.1);
709  cylinder.scale.z = 0.01;
710  cylinder.pose.position.x = marker.pose.position.x;
711  cylinder.pose.position.y = marker.pose.position.y;
712  cylinder.pose.position.z = marker.pose.position.z;
713  cylinder.pose.position.z += (marker.scale.z / 2.0) + 0.05;
714  markerPub.publish(cylinder);
715 
716  // Text
717  visualization_msgs::Marker text;
718  text.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
719  text.action = visualization_msgs::Marker::ADD;
720  text.header.frame_id = mapFrame;
721  text.color.r = text.color.g = text.color.b = text.color.a = 1.0f;
722  text.id = fid.id;
723  text.scale.x = text.scale.y = text.scale.z = 0.1;
724  text.pose.position.x = marker.pose.position.x;
725  text.pose.position.y = marker.pose.position.y;
726  text.pose.position.z = marker.pose.position.z;
727  text.pose.position.z += (marker.scale.z / 2.0) + 0.1;
728  text.id = fid.id + 30000;
729  text.ns = "text";
730  text.text = std::to_string(fid.id);
731  markerPub.publish(text);
732 
733  // Links
734  visualization_msgs::Marker links;
735  links.type = visualization_msgs::Marker::LINE_LIST;
736  links.action = visualization_msgs::Marker::ADD;
737  links.header.frame_id = mapFrame;
738  links.color.r = 0.0f;
739  links.color.g = 0.0f;
740  links.color.b = 1.0f;
741  links.color.a = 1.0f;
742  links.id = fid.id + 40000;
743  links.ns = "links";
744  links.scale.x = links.scale.y = links.scale.z = 0.02;
745  links.pose.position.x = 0;
746  links.pose.position.y = 0;
747  links.pose.position.z = 0;
748 
749  geometry_msgs::Point gp0, gp1;
750  tf2::Vector3 p0 = fid.pose.transform.getOrigin();
751  gp0.x = p0.x();
752  gp0.y = p0.y();
753  gp0.z = p0.z();
754 
755  std::map<int, int>::iterator lit;
756  for (const auto linked_fid : fid.links) {
757  // only draw links in one direction
758  if (fid.id < linked_fid) {
759  if (fiducials.find(linked_fid) != fiducials.end()) {
760  tf2::Vector3 p1 = fiducials[linked_fid].pose.transform.getOrigin();
761  gp1.x = p1.x();
762  gp1.y = p1.y();
763  gp1.z = p1.z();
764  links.points.push_back(gp0);
765  links.points.push_back(gp1);
766  }
767  }
768  }
769 
770  markerPub.publish(links);
771 }
772 
773 // Publish a line marker between two points
774 
775 void Map::drawLine(const tf2::Vector3 &p0, const tf2::Vector3 &p1) {
776  static int lid = 60000;
777  visualization_msgs::Marker line;
778  line.type = visualization_msgs::Marker::LINE_LIST;
779  line.action = visualization_msgs::Marker::ADD;
780  line.header.frame_id = mapFrame;
781  line.color.r = 1.0f;
782  line.color.g = 0.0f;
783  line.color.b = 0.0f;
784  line.color.a = 1.0f;
785  line.id = lid++;
786  line.ns = "lines";
787  line.scale.x = line.scale.y = line.scale.z = 0.01;
788  line.pose.position.x = 0;
789  line.pose.position.y = 0;
790  geometry_msgs::Point gp0, gp1;
791  gp0.x = p0.x();
792  gp0.y = p0.y();
793  gp0.z = p0.z();
794  gp1.x = p1.x();
795  gp1.y = p1.y();
796  gp1.z = p1.z();
797  line.points.push_back(gp0);
798  line.points.push_back(gp1);
799 
800  markerPub.publish(line);
801 }
802 
803 // Service to clear the map and enable auto initialization
804 
805 bool Map::clearCallback(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res) {
806  ROS_INFO("Clearing fiducial map from service call");
807 
808  fiducials.clear();
810  originFid = -1;
811 
812  return true;
813 }
814 
815 // Service to add a fiducial to the map
816 
817 bool Map::addFiducialCallback(fiducial_slam::AddFiducial::Request &req,
818  fiducial_slam::AddFiducial::Response &res)
819 {
820  ROS_INFO("Request to add fiducial %d to map", req.fiducial_id);
821  fiducialToAdd = req.fiducial_id;
822 
823  return true;
824 }
d
ros::Time lastPublished
Definition: map.h:82
ros::Publisher cameraPosePub
Definition: map.h:101
void setRPY(const tf2Scalar &roll, const tf2Scalar &pitch, const tf2Scalar &yaw)
constexpr double deg2rad(double deg)
Definition: helpers.h:11
double future_date_transforms
Definition: map.h:114
void update()
Definition: map.cpp:399
tf2::Stamped< TransformWithVariance > T_fidCam
Definition: map.h:65
void publishMarkers()
Definition: map.cpp:655
std::string odomFrame
Definition: map.h:111
bool clearCallback(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
Definition: map.cpp:805
void autoInit(const std::vector< Observation > &obs, const ros::Time &time)
Definition: map.cpp:432
void publish(const boost::shared_ptr< M > &message) const
void publishTf()
Definition: map.cpp:391
constexpr double rad2deg(double rad)
Definition: helpers.h:14
ros::ServiceServer clearSrv
Definition: map.h:103
bool publishPoseTf
Definition: map.h:129
XmlRpcServer s
tf2_ros::Buffer tfBuffer
Definition: map.h:95
virtual geometry_msgs::TransformStamped lookupTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, const ros::Duration timeout) const
static int findClosestObs(const std::vector< Observation > &obs)
Definition: map.cpp:411
int id
Definition: map.h:76
bool havePose
Definition: map.h:127
void publishMap()
Definition: map.cpp:625
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
Map(ros::NodeHandle &nh)
Definition: map.cpp:78
TFSIMD_FORCE_INLINE const tfScalar & y() const
bool loadMap()
Definition: map.cpp:566
#define ROS_WARN(...)
geometry_msgs::TransformStamped t
std::vector< double > covarianceDiagonal
Definition: map.h:125
geometry_msgs::TransformStamped toMsg(const tf2::Stamped< TransformWithVariance > &in)
void handleAddFiducial(const std::vector< Observation > &obs)
Definition: map.cpp:485
std::unique_ptr< tf2_ros::TransformListener > listener
Definition: map.h:96
Definition: map.h:74
geometry_msgs::TransformStamped poseTf
Definition: map.h:131
void updateMap(const std::vector< Observation > &obs, const ros::Time &time, const tf2::Stamped< TransformWithVariance > &cameraPose)
Definition: map.cpp:177
bool isInitializingMap
Definition: map.h:118
ros::ServiceServer addSrv
Definition: map.h:104
bool visible
Definition: map.h:78
TF2SIMD_FORCE_INLINE Vector3 & getOrigin()
std::map< int, Fiducial > fiducials
Definition: map.h:133
#define ROS_INFO(...)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
ros::Time stamp_
TransformWithVariance averageTransforms(const TransformWithVariance &t1, const TransformWithVariance &t2)
std::set< int > links
Definition: map.h:79
void fromMsg(const A &, B &b)
Transform inverse() const
std::string mapFrame
Definition: map.h:110
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
list roll
Definition: fit_plane.py:42
geometry_msgs::PoseWithCovarianceStamped toPose(const tf2::Stamped< TransformWithVariance > &in)
void sendTransform(const geometry_msgs::TransformStamped &transform)
static double systematic_error
Definition: map.cpp:50
void update(const tf2::Stamped< TransformWithVariance > &newPose)
Definition: map.cpp:62
bool saveMap()
Definition: map.cpp:535
ros::Publisher mapPub
Definition: map.h:99
std::string frame_id_
ros::Time tfPublishTime
Definition: map.h:130
float tfPublishInterval
Definition: map.h:128
int fid
Definition: map.h:64
ros::Publisher robotPosePub
Definition: map.h:100
int initialFrameNum
Definition: map.h:121
bool readOnly
Definition: map.h:119
int fiducialToAdd
Definition: map.h:134
bool overridePublishedCovariance
Definition: map.h:124
void publishMarker(Fiducial &fid)
Definition: map.cpp:669
list pitch
Definition: fit_plane.py:43
bool getParam(const std::string &key, std::string &s) const
int originFid
Definition: map.h:122
Observation()
Definition: map.h:68
static Time now()
void drawLine(const tf2::Vector3 &p0, const tf2::Vector3 &p1)
Definition: map.cpp:775
int updatePose(std::vector< Observation > &obs, const ros::Time &time, tf2::Stamped< TransformWithVariance > &cameraPose)
Definition: map.cpp:243
tf2_ros::TransformBroadcaster broadcaster
Definition: map.h:94
int frameNum
Definition: map.h:120
Fiducial()
Definition: map.h:86
tf2::Stamped< TransformWithVariance > pose
Definition: map.h:81
bool addFiducialCallback(fiducial_slam::AddFiducial::Request &req, fiducial_slam::AddFiducial::Response &res)
Definition: map.cpp:817
bool publish_6dof_pose
Definition: map.h:115
int numObs
Definition: map.h:77
double multiErrorThreshold
Definition: map.h:116
std::string mapFilename
Definition: map.h:109
#define ROS_ERROR(...)
tf2::Stamped< TransformWithVariance > T_camFid
Definition: map.h:66
void setData(const T &input)
bool lookupTransform(const std::string &from, const std::string &to, const ros::Time &time, tf2::Transform &T) const
Definition: map.cpp:225
ros::Publisher markerPub
Definition: map.h:98
std::string baseFrame
Definition: map.h:113


fiducial_slam
Author(s): Jim Vaughan
autogenerated on Fri May 1 2020 04:04:05