MultiMapper.cpp
Go to the documentation of this file.
1 #include <visualization_msgs/Marker.h>
2 #include <nav2d_msgs/RobotPose.h>
4 
5 // compute linear index for given map coords
6 #define MAP_IDX(sx, i, j) ((sx) * (j) + (i))
7 
8 
10 {
11  // Get parameters from the ROS parameter server
12  ros::NodeHandle robotNode;
13  robotNode.param("robot_id", mRobotID, 1);
14  robotNode.param("scan_input_topic", mScanInputTopic, std::string("karto_in"));
15  robotNode.param("scan_output_topic", mScanOutputTopic, std::string("karto_out"));
16  robotNode.param("laser_frame", mLaserFrame, std::string("laser"));
17  robotNode.param("robot_frame", mRobotFrame, std::string("robot"));
18  robotNode.param("odometry_frame", mOdometryFrame, std::string("odometry_base"));
19  robotNode.param("offset_frame", mOffsetFrame, std::string("odometry_offset"));
20  robotNode.param("map_frame", mMapFrame, std::string("map"));
21  robotNode.param("map_service", mMapService, std::string("get_map"));
22  robotNode.param("laser_topic", mLaserTopic, std::string("scan"));
23  robotNode.param("map_topic", mMapTopic, std::string("map"));
24 
25  ros::NodeHandle mapperNode("~/");
26  mapperNode.param("grid_resolution", mMapResolution, 0.05);
27  mapperNode.param("range_threshold", mRangeThreshold, 30.0);
28  mapperNode.param("map_update_rate", mMapUpdateRate, 1);
29  mapperNode.param("publish_pose_graph", mPublishPoseGraph, true);
30  mapperNode.param("max_covariance", mMaxCovariance, 0.05);
31  mapperNode.param("min_map_size", mMinMapSize, 50);
32 
33  // Apply tf_prefix to all used frame-id's
39 
40  // Initialize Publisher/Subscribers
42  mScanPublisher = robotNode.advertise<nav2d_msgs::LocalizedScan>(mScanOutputTopic, 100, true);
44  mMapPublisher = robotNode.advertise<nav_msgs::OccupancyGrid>(mMapTopic, 1, true);
46  mInitialPoseSubscriber = robotNode.subscribe("initialpose", 1, &MultiMapper::receiveInitialPose, this);
47  mOtherRobotsPublisher = robotNode.advertise<nav2d_msgs::RobotPose>("others", 10, true);
48 
49  mVerticesPublisher = mapperNode.advertise<visualization_msgs::Marker>("vertices", 1, true);
50  mEdgesPublisher = mapperNode.advertise<visualization_msgs::Marker>("edges", 1, true);
51  mPosePublisher = robotNode.advertise<geometry_msgs::PoseStamped>("localization_result", 1, true);
52 
53  // Initialize KARTO-Mapper
54  mMapper = new karto::OpenMapper(true);
55 
56  double param_d;
57  bool param_b;
58  int param_i;
59 
60  if(mapperNode.getParam("UseScanMatching", param_b))
61  mMapper->SetParameters("UseScanMatching", param_b);
62 
63  if(mapperNode.getParam("UseScanBarycenter", param_b))
64  mMapper->SetParameters("UseScanBarycenter", param_b);
65 
66  if(mapperNode.getParam("MinimumTravelDistance", param_d))
67  mMapper->SetParameters("MinimumTravelDistance", param_d);
68 
69  if(mapperNode.getParam("MinimumTravelHeading", param_d))
70  mMapper->SetParameters("MinimumTravelHeading", param_d);
71 
72  if(mapperNode.getParam("ScanBufferSize", param_i))
73  mMapper->SetParameters("ScanBufferSize", param_i);
74 
75  if(mapperNode.getParam("ScanBufferMaximumScanDistance", param_d))
76  mMapper->SetParameters("ScanBufferMaximumScanDistance", param_d);
77 
78  if(mapperNode.getParam("UseResponseExpansion", param_b))
79  mMapper->SetParameters("UseResponseExpansion", param_b);
80 
81  if(mapperNode.getParam("DistanceVariancePenalty", param_d))
82  mMapper->SetParameters("DistanceVariancePenalty", param_d);
83 
84  if(mapperNode.getParam("MinimumDistancePenalty", param_d))
85  mMapper->SetParameters("MinimumDistancePenalty", param_d);
86 
87  if(mapperNode.getParam("AngleVariancePenalty", param_d))
88  mMapper->SetParameters("AngleVariancePenalty", param_d);
89 
90  if(mapperNode.getParam("MinimumAnglePenalty", param_d))
91  mMapper->SetParameters("MinimumAnglePenalty", param_d);
92 
93  if(mapperNode.getParam("LinkMatchMinimumResponseFine", param_d))
94  mMapper->SetParameters("LinkMatchMinimumResponseFine", param_d);
95 
96  if(mapperNode.getParam("LinkScanMaximumDistance", param_d))
97  mMapper->SetParameters("LinkScanMaximumDistance", param_d);
98 
99  if(mapperNode.getParam("CorrelationSearchSpaceDimension", param_d))
100  mMapper->SetParameters("CorrelationSearchSpaceDimension", param_d);
101 
102  if(mapperNode.getParam("CorrelationSearchSpaceResolution", param_d))
103  mMapper->SetParameters("CorrelationSearchSpaceResolution", param_d);
104 
105  if(mapperNode.getParam("CorrelationSearchSpaceSmearDeviation", param_d))
106  mMapper->SetParameters("CorrelationSearchSpaceSmearDeviation", param_d);
107 
108  if(mapperNode.getParam("CoarseSearchAngleOffset", param_d))
109  mMapper->SetParameters("CoarseSearchAngleOffset", param_d);
110 
111  if(mapperNode.getParam("FineSearchAngleOffset", param_d))
112  mMapper->SetParameters("FineSearchAngleOffset", param_d);
113 
114  if(mapperNode.getParam("CoarseAngleResolution", param_d))
115  mMapper->SetParameters("CoarseAngleResolution", param_d);
116 
117  if(mapperNode.getParam("LoopSearchSpaceDimension", param_d))
118  mMapper->SetParameters("LoopSearchSpaceDimension", param_d);
119 
120  if(mapperNode.getParam("LoopSearchSpaceResolution", param_d))
121  mMapper->SetParameters("LoopSearchSpaceResolution", param_d);
122 
123  if(mapperNode.getParam("LoopSearchSpaceSmearDeviation", param_d))
124  mMapper->SetParameters("LoopSearchSpaceSmearDeviation", param_d);
125 
126  if(mapperNode.getParam("LoopSearchMaximumDistance", param_d))
127  mMapper->SetParameters("LoopSearchMaximumDistance", param_d);
128 
129  if(mapperNode.getParam("LoopMatchMinimumChainSize", param_i))
130  mMapper->SetParameters("LoopMatchMinimumChainSize", param_i);
131 
132  if(mapperNode.getParam("LoopMatchMaximumVarianceCoarse", param_d))
133  mMapper->SetParameters("LoopMatchMaximumVarianceCoarse", param_d);
134 
135  if(mapperNode.getParam("LoopMatchMinimumResponseCoarse", param_d))
136  mMapper->SetParameters("LoopMatchMinimumResponseCoarse", param_d);
137 
138  if(mapperNode.getParam("LoopMatchMinimumResponseFine", param_d))
139  mMapper->SetParameters("LoopMatchMinimumResponseFine", param_d);
140 
141  mMapper->Message += karto::delegate(this, &MultiMapper::onMessage);
142 
143  mLaser = NULL;
144 
145  // Initialize Variables
148  mNodesAdded = 0;
149  mMapChanged = true;
151 
152  if(mRobotID == 1)
153  {
154  // I am the number one, so start mapping right away.
155  mState = ST_MAPPING;
156  ROS_INFO("Inititialized robot 1, starting to map now.");
157  mSelfLocalizer = NULL;
158 
159  geometry_msgs::PoseStamped locResult;
160  locResult.header.stamp = ros::Time::now();
161  locResult.header.frame_id = mMapFrame.c_str();
162  locResult.pose.position.x = 0;
163  locResult.pose.position.y = 0;
164  locResult.pose.position.z = 0;
165  locResult.pose.orientation = tf::createQuaternionMsgFromYaw(0);
166  mPosePublisher.publish(locResult);
167  }else
168  {
169  // I am not number one, so wait to receive a map from number one.
171  ROS_INFO("Initialized robot %d, waiting for map from robot 1 now.", mRobotID);
173  }
174 }
175 
177 {
178 
179 }
180 
182 {
183  mMapper->SetScanSolver(scanSolver);
184 }
185 
186 void MultiMapper::setRobotPose(double x, double y, double yaw)
187 {
188  tf::Transform transform;
189  transform.setOrigin(tf::Vector3(x, y, 0));
191  transform = transform.inverse();
192 
193  tf::Stamped<tf::Pose> pose_in, pose_out;
194  pose_in.setData(transform);
195  pose_in.frame_id_ = mRobotFrame;
196  pose_in.stamp_ = ros::Time(0);
197  mTransformListener.transformPose(mOdometryFrame, pose_in, pose_out);
198 
199  transform = pose_out;
200  mOdometryOffset = transform.inverse();
201 
202  if(mSelfLocalizer)
203  {
204  delete mSelfLocalizer;
205  mSelfLocalizer = NULL;
206  }
207 
208  // Publish the new pose (to inform other nodes, that we are localized now)
209  geometry_msgs::PoseStamped locResult;
210  locResult.header.stamp = ros::Time::now();
211  locResult.header.frame_id = mMapFrame.c_str();
212  locResult.pose.position.x = x;
213  locResult.pose.position.y = y;
214  locResult.pose.position.z = 0;
215  locResult.pose.orientation = tf::createQuaternionMsgFromYaw(yaw);
216  mPosePublisher.publish(locResult);
217 
218  // Publish via tf
219  mState = ST_MAPPING;
221 }
222 
223 karto::LocalizedRangeScan* MultiMapper::createFromRosMessage(const sensor_msgs::LaserScan& scan, const karto::Identifier& robot)
224 {
225  // Implementing REP 117: Informational Distance Measurements
226  // http://www.ros.org/reps/rep-0117.html
227  karto::RangeReadingsList readings;
228  std::vector<float>::const_iterator it;
229  for(it = scan.ranges.begin(); it != scan.ranges.end(); it++)
230  {
231  if(*it >= scan.range_min && *it <= scan.range_max)
232  {
233  // This is a valid measurement.
234  readings.Add(*it);
235  }else if( !std::isfinite(*it) && *it < 0)
236  {
237  // Object too close to measure.
238  readings.Add(scan.range_max);
239  }else if( !std::isfinite(*it) && *it > 0)
240  {
241  // No objects detected in range.
242  readings.Add(scan.range_max);
243  }else if( std::isnan(*it) )
244  {
245  // This is an erroneous, invalid, or missing measurement.
246  ROS_WARN_THROTTLE(1,"Laser scan contains nan-values!");
247  readings.Add(scan.range_max);
248  }else
249  {
250  // The sensor reported these measurements as valid, but they are
251  // discarded per the limits defined by minimum_range and maximum_range.
252  ROS_WARN_THROTTLE(1, "Laser reading not between range_min and range_max!");
253  readings.Add(scan.range_max);
254  }
255  }
256  return new karto::LocalizedRangeScan(robot, readings);
257 }
258 
259 void MultiMapper::receiveLaserScan(const sensor_msgs::LaserScan::ConstPtr& scan)
260 {
261  // Ignore own readings until map has been received
263  {
264  return;
265  }
266 
267  if(!mLaser)
268  {
269  // Create a laser range finder device and copy in data from the first scan
270  char name[10];
271  sprintf(name, "robot_%d", mRobotID);
272 
273  // Add the laser to the mapper
274  try
275  {
277  mLaser->SetMinimumRange(scan->range_min);
278  mLaser->SetMaximumRange(scan->range_max);
279  mLaser->SetMinimumAngle(scan->angle_min);
280  mLaser->SetMaximumAngle(scan->angle_max);
281  mLaser->SetAngularResolution(scan->angle_increment);
282  mLaser->SetRangeThreshold(mRangeThreshold);
284  }
285  catch(karto::Exception e)
286  {
287  ROS_ERROR("Could not add new Laser to Mapper: %s", e.GetErrorMessage().ToCString());
288  return;
289  }
290  }
291 
292  if(mState == ST_LOCALIZING)
293  {
294  mSelfLocalizer->process(scan);
296  {
297  // Localization finished, kill the localizer and start mapping
298  ROS_INFO("Localization finished on robot %d, now starting to map.", mRobotID);
301  }
302  }else
303  if(mState == ST_MAPPING)
304  {
305  // get the odometry pose from tf
306  tf::StampedTransform tfPose;
307  try
308  {
309  mTransformListener.lookupTransform(mOffsetFrame, mLaserFrame, scan->header.stamp, tfPose);
310  }
311  catch(tf::TransformException e)
312  {
313  try
314  {
316  }
317  catch(tf::TransformException e)
318  {
319  ROS_WARN("Failed to compute odometry pose, skipping scan (%s)", e.what());
320  return;
321  }
322  }
323  karto::Pose2 kartoPose = karto::Pose2(tfPose.getOrigin().x(), tfPose.getOrigin().y(), tf::getYaw(tfPose.getRotation()));
324 
325  // create localized laser scan
326  karto::LocalizedLaserScanPtr laserScan = createFromRosMessage(*scan, mLaser->GetIdentifier());
327  laserScan->SetOdometricPose(kartoPose);
328  laserScan->SetCorrectedPose(kartoPose);
329 
330  bool success;
331  try
332  {
333  success = mMapper->Process(laserScan);
334  }
335  catch(karto::Exception e)
336  {
337  ROS_ERROR("%s", e.GetErrorMessage().ToCString());
338  success = false;
339  }
340 
341  if(success)
342  {
343  // Compute the map->odom transform
344  karto::Pose2 corrected_pose = laserScan->GetCorrectedPose();
345  tf::Pose map_in_robot(tf::createQuaternionFromYaw(corrected_pose.GetHeading()), tf::Vector3(corrected_pose.GetX(), corrected_pose.GetY(), 0.0));
346  map_in_robot = map_in_robot.inverse();
347  tf::Stamped<tf::Pose> map_in_odom;
348  bool ok = true;
349  try
350  {
351  mTransformListener.transformPose(mOffsetFrame, tf::Stamped<tf::Pose>(map_in_robot, ros::Time(0) /*scan->header.stamp*/, mLaserFrame), map_in_odom);
352  }
353  catch(tf::TransformException e)
354  {
355  ROS_WARN("Transform from %s to %s failed! (%s)", mLaserFrame.c_str(), mOffsetFrame.c_str(), e.what());
356  ok = false;
357  }
358  if(ok)
359  {
360  mMapToOdometry = tf::Transform(tf::Quaternion( map_in_odom.getRotation() ), tf::Point(map_in_odom.getOrigin() ) ).inverse();
362  v.setZ(0);
364  }
365  mNodesAdded++;
366  mMapChanged = true;
367 
369  if(mMapUpdateRate > 0 && d.toSec() > mMapUpdateRate)
370  {
371  sendMap();
372  }
373 
374  // Send the scan to the other robots via com-layer (DDS)
375  ROS_DEBUG("Robot %d: Sending scan (uniqueID: %d, Sensor: %s, stateID: %d)", mRobotID, laserScan->GetUniqueId(), laserScan->GetSensorIdentifier().ToString().ToCString(), laserScan->GetStateId());
376  sendLocalizedScan(scan, laserScan->GetOdometricPose());
377 
378  // Publish via extra topic
379  nav2d_msgs::RobotPose other;
380  other.header.stamp = ros::Time::now();
381  other.header.frame_id = mMapFrame;
382  other.robot_id = mRobotID;
383  other.pose.x = laserScan->GetCorrectedPose().GetX();
384  other.pose.y = laserScan->GetCorrectedPose().GetY();
385  other.pose.theta = laserScan->GetCorrectedPose().GetHeading();
387  }
388  }
389 }
390 
391 bool MultiMapper::getMap(nav_msgs::GetMap::Request &req, nav_msgs::GetMap::Response &res)
392 {
394  {
395  ROS_INFO("Still waiting for map from robot 1.");
396  return false;
397  }
398 
399  if(sendMap())
400  {
401  res.map = mGridMap;
402  return true;
403  }else
404  {
405  ROS_WARN("Serving map request failed!");
406  return false;
407  }
408 }
409 
411 {
412  if(!updateMap()) return false;
413 
414  // Publish the map
417 
418  // Publish the pose-graph
420  {
421  // Publish the vertices
423  visualization_msgs::Marker marker;
424  marker.header.frame_id = mMapFrame;
425  marker.header.stamp = ros::Time();
426  marker.id = 0;
427  marker.type = visualization_msgs::Marker::SPHERE_LIST;
428  marker.action = visualization_msgs::Marker::ADD;
429  marker.pose.position.x = 0;
430  marker.pose.position.y = 0;
431  marker.pose.position.z = 0;
432  marker.pose.orientation.x = 0.0;
433  marker.pose.orientation.y = 0.0;
434  marker.pose.orientation.z = 0.0;
435  marker.pose.orientation.w = 1.0;
436  marker.scale.x = 0.1;
437  marker.scale.y = 0.1;
438  marker.scale.z = 0.1;
439  marker.color.a = 1.0;
440  marker.color.r = 0.0;
441  marker.color.g = 1.0;
442  marker.color.b = 0.0;
443  marker.points.resize(vertices.Size());
444 
445  for(int i = 0; i < vertices.Size(); i++)
446  {
447  marker.points[i].x = vertices[i]->GetVertexObject()->GetCorrectedPose().GetX();
448  marker.points[i].y = vertices[i]->GetVertexObject()->GetCorrectedPose().GetY();
449  marker.points[i].z = 0;
450  }
451  mVerticesPublisher.publish(marker);
452 
453  // Publish the edges
455  marker.header.frame_id = mMapFrame;
456  marker.header.stamp = ros::Time();
457  marker.id = 0;
458  marker.type = visualization_msgs::Marker::LINE_LIST;
459  marker.scale.x = 0.01;
460  marker.color.a = 1.0;
461  marker.color.r = 1.0;
462  marker.color.g = 0.0;
463  marker.color.b = 0.0;
464  marker.points.resize(edges.Size() * 2);
465 
466  for(int i = 0; i < edges.Size(); i++)
467  {
468  marker.points[2*i].x = edges[i]->GetSource()->GetVertexObject()->GetCorrectedPose().GetX();
469  marker.points[2*i].y = edges[i]->GetSource()->GetVertexObject()->GetCorrectedPose().GetY();
470  marker.points[2*i].z = 0;
471 
472  marker.points[2*i+1].x = edges[i]->GetTarget()->GetVertexObject()->GetCorrectedPose().GetX();
473  marker.points[2*i+1].y = edges[i]->GetTarget()->GetVertexObject()->GetCorrectedPose().GetY();
474  marker.points[2*i+1].z = 0;
475  }
476  mEdgesPublisher.publish(marker);
477  }
478  return true;
479 }
480 
482 {
483  if(!mMapChanged) return true;
484 
487 
488  if(!kartoGrid)
489  {
490  ROS_WARN("Failed to get occupancy map from Karto-Mapper.");
491  return false;
492  }
493 
494  // Translate to ROS format
495  unsigned int width = kartoGrid->GetWidth();
496  unsigned int height = kartoGrid->GetHeight();
497  karto::Vector2<kt_double> offset = kartoGrid->GetCoordinateConverter()->GetOffset();
498 
499  if( mGridMap.info.width != width ||
500  mGridMap.info.height != height ||
501  mGridMap.info.origin.position.x != offset.GetX() ||
502  mGridMap.info.origin.position.y != offset.GetY())
503  {
504  mGridMap.info.resolution = mMapResolution;
505  mGridMap.info.origin.position.x = offset.GetX();
506  mGridMap.info.origin.position.y = offset.GetY();
507  mGridMap.info.width = width;
508  mGridMap.info.height = height;
509  mGridMap.data.resize(mGridMap.info.width * mGridMap.info.height);
510  }
511 
512  for (unsigned int y = 0; y < height; y++)
513  {
514  for (unsigned int x = 0; x < width; x++)
515  {
516  // Getting the value at position x,y
517  kt_int8u value = kartoGrid->GetValue(karto::Vector2<kt_int32s>(x, y));
518 
519  switch (value)
520  {
522  mGridMap.data[MAP_IDX(mGridMap.info.width, x, y)] = -1;
523  break;
525  mGridMap.data[MAP_IDX(mGridMap.info.width, x, y)] = 100;
526  break;
528  mGridMap.data[MAP_IDX(mGridMap.info.width, x, y)] = 0;
529  break;
530  default:
531  ROS_WARN("Encountered unknown cell value at %d, %d", x, y);
532  break;
533  }
534  }
535  }
536 
537  // Set the header information on the map
538  mGridMap.header.stamp = ros::Time::now();
539  mGridMap.header.frame_id = mMapFrame.c_str();
540  mMapChanged = false;
541  return true;
542 }
543 
544 void MultiMapper::receiveLocalizedScan(const nav2d_msgs::LocalizedScan::ConstPtr& scan)
545 {
546  // Ignore my own scans
547  if(scan->robot_id == mRobotID) return;
548 
549  // Get the robot id
550  char robot[10];
551  sprintf(robot, "robot_%d", scan->robot_id);
552 
553  // Get the scan pose
554  karto::Pose2 scanPose(scan->x, scan->y, scan->yaw);
555 
556  // create localized laser scan
557  karto::LocalizedLaserScanPtr localizedScan = createFromRosMessage(scan->scan, robot);
558  localizedScan->SetOdometricPose(scanPose);
559  localizedScan->SetCorrectedPose(scanPose);
560 
561  // feed the localized scan to the Karto-Mapper
562  bool added = false;
563  try
564  {
565  added = mMapper->Process(localizedScan);
566  }
567  catch(karto::Exception e1)
568  {
569  if(mOtherLasers.find(scan->robot_id) == mOtherLasers.end())
570  {
571  try
572  {
574  laser->SetMinimumRange(scan->scan.range_min);
575  laser->SetMaximumRange(scan->scan.range_max);
576  laser->SetMinimumAngle(scan->scan.angle_min);
577  laser->SetMaximumAngle(scan->scan.angle_max);
578  laser->SetAngularResolution(scan->scan.angle_increment);
579  laser->SetRangeThreshold(mRangeThreshold);
580  mMapper->Process(laser);
581  mOtherLasers.insert(std::pair<int,karto::LaserRangeFinderPtr>(scan->robot_id,laser));
582 
583  added = mMapper->Process(localizedScan);
584  }
585  catch(karto::Exception e2)
586  {
587  ROS_ERROR("%s", e2.GetErrorMessage().ToCString());
588  }
589  }else
590  {
591  ROS_ERROR("%s", e1.GetErrorMessage().ToCString());
592  }
593  }
594  if(added)
595  {
596  mNodesAdded++;
597  mMapChanged = true;
598  ROS_DEBUG("Robot %d: Received scan (uniqueID: %d, Sensor: %s, stateID: %d)", mRobotID, localizedScan->GetUniqueId(), localizedScan->GetSensorIdentifier().ToString().ToCString(), localizedScan->GetStateId());
599 
600  // Publish via extra topic
601  nav2d_msgs::RobotPose other;
602  other.header.stamp = ros::Time::now();
603  other.header.frame_id = mMapFrame;
604  other.robot_id = scan->robot_id;
605  other.pose.x = localizedScan->GetCorrectedPose().GetX();
606  other.pose.y = localizedScan->GetCorrectedPose().GetY();
607  other.pose.theta = localizedScan->GetCorrectedPose().GetHeading();
609 
610  // Send the map via topic
612  if(mMapUpdateRate > 0 && d.toSec() > mMapUpdateRate)
613  {
614  sendMap();
615  if(mState == ST_LOCALIZING)
616  {
618  }
619  }
620  }else
621  {
622  ROS_DEBUG("Discarded Scan from Robot %d!", scan->robot_id);
623  }
624 
626  {
627  sendMap();
631  ROS_INFO("Received a map, now starting to localize.");
633  }
634 }
635 
636 void MultiMapper::sendLocalizedScan(const sensor_msgs::LaserScan::ConstPtr& scan, const karto::Pose2& pose)
637 {
638  nav2d_msgs::LocalizedScan rosScan;
639  rosScan.robot_id = mRobotID;
640  rosScan.laser_type = 0;
641  rosScan.x = pose.GetX();
642  rosScan.y = pose.GetY();
643  rosScan.yaw = pose.GetHeading();
644 
645  rosScan.scan.angle_min = scan->angle_min;
646  rosScan.scan.angle_max = scan->angle_max;
647  rosScan.scan.range_min = scan->range_min;
648  rosScan.scan.range_max = scan->range_max;
649  rosScan.scan.angle_increment = scan->angle_increment;
650  rosScan.scan.time_increment = scan->time_increment;
651  rosScan.scan.scan_time = scan->scan_time;
652 
653  unsigned int nReadings = scan->ranges.size();
654  rosScan.scan.ranges.resize(nReadings);
655  for(unsigned int i = 0; i < nReadings; i++)
656  {
657  rosScan.scan.ranges[i] = scan->ranges[i];
658  }
659 
660 // rosScan.scan = *scan;
661  mScanPublisher.publish(rosScan);
662 }
663 
664 void MultiMapper::receiveInitialPose(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& pose)
665 {
666  double x = pose->pose.pose.position.x;
667  double y = pose->pose.pose.position.y;
668  double yaw = tf::getYaw(pose->pose.pose.orientation);
669  ROS_INFO("Received initial pose (%.2f, %.2f, %.2f) on robot %d, now starting to map.",x,y,yaw,mRobotID);
670  try
671  {
672  setRobotPose(x,y,yaw);
673  }
674  catch(tf::TransformException e)
675  {
676  ROS_ERROR("Failed to set pose on robot %d! (%s)", mRobotID, e.what());
677  return;
678  }
679 }
680 
682 {
683  ROS_DEBUG("OpenMapper: %s\n", args.GetEventMessage().ToCString());
684 }
685 
687 {
688  if(mState == ST_MAPPING)
689  {
692  }
693 }
d
#define ST_WAITING_FOR_MAP
Definition: MultiMapper.h:19
void SetOdometricPose(const Pose2 &rOdometricPose)
Definition: SensorData.h:389
bool getMap(nav_msgs::GetMap::Request &req, nav_msgs::GetMap::Response &res)
virtual kt_size_t Size() const
Definition: List.h:214
nav_msgs::OccupancyGrid mGridMap
Definition: MultiMapper.h:57
kt_int32s GetStateId() const
Definition: SensorData.h:87
void setScanSolver(karto::ScanSolver *scanSolver)
void sendLocalizedScan(const sensor_msgs::LaserScan::ConstPtr &scan, const karto::Pose2 &pose)
TFSIMD_FORCE_INLINE void setRotation(const Quaternion &q)
ros::Subscriber mLaserSubscriber
Definition: MultiMapper.h:66
kt_double GetHeading() const
Definition: Geometry.h:2274
const VertexList & GetVertices() const
Definition: OpenMapper.h:536
void process(const sensor_msgs::LaserScan::ConstPtr &scan)
void publish(const boost::shared_ptr< M > &message) const
virtual void SetCorrectedPose(const Pose2 &rCorrectedPose)
Definition: SensorData.h:585
void setData(const T &input)
ros::Subscriber mScanSubscriber
Definition: MultiMapper.h:67
std::string mRobotFrame
Definition: MultiMapper.h:90
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
static LaserRangeFinder * CreateLaserRangeFinder(LaserRangeFinderType type, const Identifier &rName)
Definition: Sensor.cpp:229
std::string mScanOutputTopic
Definition: MultiMapper.h:98
const T & GetY() const
Definition: Geometry.h:369
static double getYaw(const Quaternion &bt_q)
#define MAP_IDX(sx, i, j)
Definition: MultiMapper.cpp:6
std::string mLaserTopic
Definition: MultiMapper.h:94
std::string mOffsetFrame
Definition: MultiMapper.h:92
SelfLocalizer * mSelfLocalizer
Definition: MultiMapper.h:49
kt_double GetX() const
Definition: Geometry.h:2220
TFSIMD_FORCE_INLINE void setZ(tfScalar z)
void publishParticleCloud()
bool mMapChanged
Definition: MultiMapper.h:74
virtual void Add(const T &rValue)
Definition: List.h:111
bool sendMap()
TFSIMD_FORCE_INLINE const tfScalar & getY() const
void SetScanSolver(ScanSolver *pSolver)
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
std::string resolve(const std::string &frame_name)
std::string mMapTopic
Definition: MultiMapper.h:95
TFSIMD_FORCE_INLINE const tfScalar & y() const
#define ROS_WARN(...)
std::string mScanInputTopic
Definition: MultiMapper.h:97
ros::WallTime mLastMapUpdate
Definition: MultiMapper.h:86
void publishTransform()
const String & GetErrorMessage() const
Definition: Exception.cpp:56
virtual kt_bool Process(Object *pObject)
karto::SmartPointer< karto::OpenMapper > mMapper
Definition: MultiMapper.h:72
ros::Publisher mVerticesPublisher
Definition: MultiMapper.h:62
void setIdentity()
const String & GetEventMessage() const
Definition: OpenMapper.h:74
const LocalizedLaserScanList GetAllProcessedScans() const
uint8_t kt_int8u
Definition: Types.h:91
const T & GetX() const
Definition: Geometry.h:351
bool initialize()
TFSIMD_FORCE_INLINE const tfScalar & x() const
karto::LaserRangeFinderPtr mLaser
Definition: MultiMapper.h:71
ros::Publisher mMapPublisher
Definition: MultiMapper.h:60
void receiveLaserScan(const sensor_msgs::LaserScan::ConstPtr &scan)
const EdgeList & GetEdges() const
Definition: OpenMapper.h:527
const Identifier & GetSensorIdentifier() const
Definition: SensorData.h:141
static Quaternion createQuaternionFromYaw(double yaw)
ros::Publisher mScanPublisher
Definition: MultiMapper.h:61
void convertMap(const nav_msgs::OccupancyGrid &map_msg)
ros::Publisher mEdgesPublisher
Definition: MultiMapper.h:63
#define ROS_WARN_THROTTLE(period,...)
tf::TransformListener mTransformListener
Definition: MultiMapper.h:52
#define ROS_INFO(...)
kt_int32s GetUniqueId() const
Definition: SensorData.h:105
bool param(const std::string &param_name, T &param_val, const T &default_val) const
karto::LocalizedRangeScan * createFromRosMessage(const sensor_msgs::LaserScan &scan, const karto::Identifier &robot)
TFSIMD_FORCE_INLINE const tfScalar & y() const
#define ST_LOCALIZING
Definition: MultiMapper.h:20
void sendTransform(const StampedTransform &transform)
ros::Publisher mPosePublisher
Definition: MultiMapper.h:64
const String & ToString() const
Definition: Identifier.cpp:93
int mNodesAdded
Definition: MultiMapper.h:84
tf::Transform mOdometryOffset
Definition: MultiMapper.h:55
TFSIMD_FORCE_INLINE const tfScalar & x() const
Transform inverse() const
static Delegate< TObj, TArgs, true > delegate(TObj *pObj, void(TObj::*NotifyMethod)(const void *, TArgs &))
Definition: Event.h:1048
#define ST_MAPPING
Definition: MultiMapper.h:21
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
ros::ServiceServer mMapServer
Definition: MultiMapper.h:59
TFSIMD_FORCE_INLINE Vector3 & getOrigin()
void lookupTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, StampedTransform &transform) const
void onMessage(const void *sender, karto::MapperEventArguments &args)
std::string mMapService
Definition: MultiMapper.h:96
ros::Time stamp_
int mMapUpdateRate
Definition: MultiMapper.h:82
std::string mLaserFrame
Definition: MultiMapper.h:89
ros::Subscriber mInitialPoseSubscriber
Definition: MultiMapper.h:68
static geometry_msgs::Quaternion createQuaternionMsgFromYaw(double yaw)
std::string frame_id_
static WallTime now()
static OccupancyGrid * CreateFromScans(const LocalizedLaserScanList &rScans, kt_double resolution)
void setRobotPose(double x, double y, double yaw)
void transformPose(const std::string &target_frame, const geometry_msgs::PoseStamped &stamped_in, geometry_msgs::PoseStamped &stamped_out) const
Quaternion getRotation() const
std::map< int, karto::LaserRangeFinderPtr > mOtherLasers
Definition: MultiMapper.h:73
bool getParam(const std::string &key, std::string &s) const
void receiveLocalizedScan(const nav2d_msgs::LocalizedScan::ConstPtr &scan)
std::string mMapFrame
Definition: MultiMapper.h:93
virtual MapperGraph * GetGraph() const
TFSIMD_FORCE_INLINE const tfScalar & getX() const
static Time now()
kt_double GetY() const
Definition: Geometry.h:2238
const Pose2 & GetOdometricPose() const
Definition: SensorData.h:380
const char * ToCString() const
Definition: String.cpp:72
ros::Publisher mOtherRobotsPublisher
Definition: MultiMapper.h:65
tf::Transform mMapToOdometry
Definition: MultiMapper.h:54
#define ROS_ERROR(...)
double mRangeThreshold
Definition: MultiMapper.h:79
std::string mOdometryFrame
Definition: MultiMapper.h:91
bool mPublishPoseGraph
Definition: MultiMapper.h:83
const Pose2 & GetCorrectedPose() const
Definition: SensorData.h:402
TFSIMD_FORCE_INLINE void setOrigin(const Vector3 &origin)
tf::Transform getBestPose()
tf::TransformBroadcaster mTransformBroadcaster
Definition: MultiMapper.h:53
int mMinMapSize
Definition: MultiMapper.h:85
void receiveInitialPose(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &pose)
double getCovariance()
bool updateMap()
double mMapResolution
Definition: MultiMapper.h:78
double mMaxCovariance
Definition: MultiMapper.h:80
#define ROS_DEBUG(...)


nav2d_karto
Author(s): Sebastian Kasperski
autogenerated on Thu Jun 6 2019 19:20:24