MultiMapper.cpp
Go to the documentation of this file.
00001 #include <visualization_msgs/Marker.h>
00002 #include <nav2d_msgs/RobotPose.h>
00003 #include <nav2d_karto/MultiMapper.h>
00004 
00005 // compute linear index for given map coords
00006 #define MAP_IDX(sx, i, j) ((sx) * (j) + (i))
00007 
00008 
00009 MultiMapper::MultiMapper()
00010 {
00011         // Get parameters from the ROS parameter server
00012         ros::NodeHandle robotNode;
00013         robotNode.param("robot_id", mRobotID, 1);
00014         robotNode.param("scan_input_topic", mScanInputTopic, std::string("karto_in"));
00015         robotNode.param("scan_output_topic", mScanOutputTopic, std::string("karto_out"));
00016         robotNode.param("laser_frame", mLaserFrame, std::string("laser"));
00017         robotNode.param("robot_frame", mRobotFrame, std::string("robot"));
00018         robotNode.param("odometry_frame", mOdometryFrame, std::string("odometry_base"));
00019         robotNode.param("offset_frame", mOffsetFrame, std::string("odometry_offset"));
00020         robotNode.param("map_frame", mMapFrame, std::string("map"));
00021         robotNode.param("map_service", mMapService, std::string("get_map"));
00022         robotNode.param("laser_topic", mLaserTopic, std::string("scan"));
00023         robotNode.param("map_topic", mMapTopic, std::string("map"));
00024         
00025         ros::NodeHandle mapperNode("~/");
00026         mapperNode.param("grid_resolution", mMapResolution, 0.05);
00027         mapperNode.param("range_threshold", mRangeThreshold, 30.0);
00028         mapperNode.param("map_update_rate", mMapUpdateRate, 1);
00029         mapperNode.param("publish_pose_graph", mPublishPoseGraph, true);
00030         mapperNode.param("max_covariance", mMaxCovariance, 0.05);
00031         mapperNode.param("min_map_size", mMinMapSize, 50);
00032 
00033         // Apply tf_prefix to all used frame-id's
00034         mLaserFrame = mTransformListener.resolve(mLaserFrame);
00035         mRobotFrame = mTransformListener.resolve(mRobotFrame);
00036         mOdometryFrame = mTransformListener.resolve(mOdometryFrame);
00037         mOffsetFrame = mTransformListener.resolve(mOffsetFrame);
00038         mMapFrame = mTransformListener.resolve(mMapFrame);
00039 
00040         // Initialize Publisher/Subscribers
00041         mScanSubscriber = robotNode.subscribe(mScanInputTopic, 100, &MultiMapper::receiveLocalizedScan, this);
00042         mScanPublisher = robotNode.advertise<nav2d_msgs::LocalizedScan>(mScanOutputTopic, 100, true);
00043         mMapServer = robotNode.advertiseService(mMapService, &MultiMapper::getMap, this);
00044         mMapPublisher = robotNode.advertise<nav_msgs::OccupancyGrid>(mMapTopic, 1, true);
00045         mLaserSubscriber = robotNode.subscribe(mLaserTopic, 100, &MultiMapper::receiveLaserScan, this);
00046         mInitialPoseSubscriber = robotNode.subscribe("initialpose", 1, &MultiMapper::receiveInitialPose, this);
00047         mOtherRobotsPublisher = robotNode.advertise<nav2d_msgs::RobotPose>("others", 10, true);
00048 
00049         mVerticesPublisher = mapperNode.advertise<visualization_msgs::Marker>("vertices", 1, true);
00050         mEdgesPublisher = mapperNode.advertise<visualization_msgs::Marker>("edges", 1, true);
00051         mPosePublisher = robotNode.advertise<geometry_msgs::PoseStamped>("localization_result", 1, true);
00052 
00053         // Initialize KARTO-Mapper
00054         mMapper = new karto::OpenMapper(true);
00055         
00056         double param_d;
00057         bool param_b;
00058         int param_i;
00059         
00060         if(mapperNode.getParam("UseScanMatching", param_b))
00061                 mMapper->SetParameters("UseScanMatching", param_b);
00062                 
00063         if(mapperNode.getParam("UseScanBarycenter", param_b))
00064                 mMapper->SetParameters("UseScanBarycenter", param_b);
00065                 
00066         if(mapperNode.getParam("MinimumTravelDistance", param_d))
00067                 mMapper->SetParameters("MinimumTravelDistance", param_d);
00068         
00069         if(mapperNode.getParam("MinimumTravelHeading", param_d))
00070                 mMapper->SetParameters("MinimumTravelHeading", param_d);
00071                 
00072         if(mapperNode.getParam("ScanBufferSize", param_i))
00073                 mMapper->SetParameters("ScanBufferSize", param_i);
00074                 
00075         if(mapperNode.getParam("ScanBufferMaximumScanDistance", param_d))
00076                 mMapper->SetParameters("ScanBufferMaximumScanDistance", param_d);
00077                 
00078         if(mapperNode.getParam("UseResponseExpansion", param_b))
00079                 mMapper->SetParameters("UseResponseExpansion", param_b);
00080                 
00081         if(mapperNode.getParam("DistanceVariancePenalty", param_d))
00082                 mMapper->SetParameters("DistanceVariancePenalty", param_d);
00083                 
00084         if(mapperNode.getParam("MinimumDistancePenalty", param_d))
00085                 mMapper->SetParameters("MinimumDistancePenalty", param_d);
00086                 
00087         if(mapperNode.getParam("AngleVariancePenalty", param_d))
00088                 mMapper->SetParameters("AngleVariancePenalty", param_d);
00089                 
00090         if(mapperNode.getParam("MinimumAnglePenalty", param_d))
00091                 mMapper->SetParameters("MinimumAnglePenalty", param_d);
00092                 
00093         if(mapperNode.getParam("LinkMatchMinimumResponseFine", param_d))
00094                 mMapper->SetParameters("LinkMatchMinimumResponseFine", param_d);
00095                 
00096         if(mapperNode.getParam("LinkScanMaximumDistance", param_d))
00097                 mMapper->SetParameters("LinkScanMaximumDistance", param_d);
00098                 
00099         if(mapperNode.getParam("CorrelationSearchSpaceDimension", param_d))
00100                 mMapper->SetParameters("CorrelationSearchSpaceDimension", param_d);
00101                 
00102         if(mapperNode.getParam("CorrelationSearchSpaceResolution", param_d))
00103                 mMapper->SetParameters("CorrelationSearchSpaceResolution", param_d);
00104                 
00105         if(mapperNode.getParam("CorrelationSearchSpaceSmearDeviation", param_d))
00106                 mMapper->SetParameters("CorrelationSearchSpaceSmearDeviation", param_d);
00107                 
00108         if(mapperNode.getParam("CoarseSearchAngleOffset", param_d))
00109                 mMapper->SetParameters("CoarseSearchAngleOffset", param_d);
00110                 
00111         if(mapperNode.getParam("FineSearchAngleOffset", param_d))
00112                 mMapper->SetParameters("FineSearchAngleOffset", param_d);
00113                 
00114         if(mapperNode.getParam("CoarseAngleResolution", param_d))
00115                 mMapper->SetParameters("CoarseAngleResolution", param_d);
00116                 
00117         if(mapperNode.getParam("LoopSearchSpaceDimension", param_d))
00118                 mMapper->SetParameters("LoopSearchSpaceDimension", param_d);
00119                 
00120         if(mapperNode.getParam("LoopSearchSpaceResolution", param_d))
00121                 mMapper->SetParameters("LoopSearchSpaceResolution", param_d);
00122                 
00123         if(mapperNode.getParam("LoopSearchSpaceSmearDeviation", param_d))
00124                 mMapper->SetParameters("LoopSearchSpaceSmearDeviation", param_d);
00125                 
00126         if(mapperNode.getParam("LoopSearchMaximumDistance", param_d))
00127                 mMapper->SetParameters("LoopSearchMaximumDistance", param_d);
00128                 
00129         if(mapperNode.getParam("LoopMatchMinimumChainSize", param_i))
00130                 mMapper->SetParameters("LoopMatchMinimumChainSize", param_i);
00131                 
00132         if(mapperNode.getParam("LoopMatchMaximumVarianceCoarse", param_d))
00133                 mMapper->SetParameters("LoopMatchMaximumVarianceCoarse", param_d);
00134                 
00135         if(mapperNode.getParam("LoopMatchMinimumResponseCoarse", param_d))
00136                 mMapper->SetParameters("LoopMatchMinimumResponseCoarse", param_d);
00137                 
00138         if(mapperNode.getParam("LoopMatchMinimumResponseFine", param_d))
00139                 mMapper->SetParameters("LoopMatchMinimumResponseFine", param_d);
00140         
00141         mMapper->Message += karto::delegate(this, &MultiMapper::onMessage);
00142         
00143         mLaser = NULL;
00144         
00145         // Initialize Variables
00146         mMapToOdometry.setIdentity();
00147         mOdometryOffset.setIdentity();
00148         mNodesAdded = 0;
00149         mMapChanged = true;
00150         mLastMapUpdate = ros::WallTime(0);
00151         
00152         if(mRobotID == 1)
00153         {
00154                 // I am the number one, so start mapping right away.
00155                 mState = ST_MAPPING;
00156                 ROS_INFO("Inititialized robot 1, starting to map now.");
00157                 mSelfLocalizer = NULL;
00158                 
00159                 geometry_msgs::PoseStamped locResult;
00160                 locResult.header.stamp = ros::Time::now();
00161                 locResult.header.frame_id = mMapFrame.c_str();
00162                 locResult.pose.position.x = 0;
00163                 locResult.pose.position.y = 0;
00164                 locResult.pose.position.z = 0;
00165                 locResult.pose.orientation = tf::createQuaternionMsgFromYaw(0);
00166                 mPosePublisher.publish(locResult);
00167         }else
00168         {
00169                 // I am not number one, so wait to receive a map from number one.
00170                 mState = ST_WAITING_FOR_MAP;
00171                 ROS_INFO("Initialized robot %d, waiting for map from robot 1 now.", mRobotID);
00172                 mSelfLocalizer = new SelfLocalizer();
00173         }
00174 }
00175 
00176 MultiMapper::~MultiMapper()
00177 {
00178 
00179 }
00180 
00181 void MultiMapper::setScanSolver(karto::ScanSolver* scanSolver)
00182 {
00183         mMapper->SetScanSolver(scanSolver);
00184 }
00185 
00186 void MultiMapper::setRobotPose(double x, double y, double yaw)
00187 {
00188         tf::Transform transform;
00189         transform.setOrigin(tf::Vector3(x, y, 0));
00190         transform.setRotation(tf::createQuaternionFromYaw(yaw));
00191         transform = transform.inverse();
00192         
00193         tf::Stamped<tf::Pose> pose_in, pose_out;
00194         pose_in.setData(transform);
00195         pose_in.frame_id_ = mRobotFrame;
00196         pose_in.stamp_ = ros::Time(0);
00197         mTransformListener.transformPose(mOdometryFrame, pose_in, pose_out);
00198         
00199         transform = pose_out;
00200         mOdometryOffset = transform.inverse();
00201 
00202         if(mSelfLocalizer)
00203         {
00204                 delete mSelfLocalizer;
00205                 mSelfLocalizer = NULL;
00206         }
00207         
00208         // Publish the new pose (to inform other nodes, that we are localized now)
00209         geometry_msgs::PoseStamped locResult;
00210         locResult.header.stamp = ros::Time::now();
00211         locResult.header.frame_id = mMapFrame.c_str();
00212         locResult.pose.position.x = x;
00213         locResult.pose.position.y = y;
00214         locResult.pose.position.z = 0;
00215         locResult.pose.orientation = tf::createQuaternionMsgFromYaw(yaw);
00216         mPosePublisher.publish(locResult);
00217         
00218         // Publish via tf
00219         mState = ST_MAPPING;
00220         publishTransform();
00221 }
00222 
00223 karto::LocalizedRangeScan* MultiMapper::createFromRosMessage(const sensor_msgs::LaserScan& scan, const karto::Identifier& robot)
00224 {
00225         // Implementing REP 117: Informational Distance Measurements
00226         // http://www.ros.org/reps/rep-0117.html
00227         karto::RangeReadingsList readings;
00228         std::vector<float>::const_iterator it;
00229         for(it = scan.ranges.begin(); it != scan.ranges.end(); it++)
00230         {
00231                 if(*it >= scan.range_min && *it <= scan.range_max)
00232                 {
00233                         // This is a valid measurement.
00234                         readings.Add(*it);
00235                 }else if( !std::isfinite(*it) && *it < 0)
00236                 {
00237                         // Object too close to measure.
00238                         readings.Add(scan.range_max);
00239                 }else if( !std::isfinite(*it) && *it > 0)
00240                 {
00241                         // No objects detected in range.
00242                         readings.Add(scan.range_max);
00243                 }else if( std::isnan(*it) )
00244                 {
00245                         // This is an erroneous, invalid, or missing measurement.
00246                         ROS_WARN_THROTTLE(1,"Laser scan contains nan-values!");
00247                         readings.Add(scan.range_max);
00248                 }else
00249                 {
00250                         // The sensor reported these measurements as valid, but they are
00251                         // discarded per the limits defined by minimum_range and maximum_range.
00252                         ROS_WARN_THROTTLE(1, "Laser reading not between range_min and range_max!");
00253                         readings.Add(scan.range_max);
00254                 }
00255         }
00256         return new karto::LocalizedRangeScan(robot, readings);
00257 }
00258 
00259 void MultiMapper::receiveLaserScan(const sensor_msgs::LaserScan::ConstPtr& scan)
00260 {
00261         // Ignore own readings until map has been received
00262         if(mState == ST_WAITING_FOR_MAP)
00263         {
00264                 return;
00265         }
00266         
00267         if(!mLaser)
00268         {
00269                 // Create a laser range finder device and copy in data from the first scan
00270                 char name[10];
00271                 sprintf(name, "robot_%d", mRobotID);
00272 
00273                 // Add the laser to the mapper
00274                 try
00275                 {
00276                         mLaser = karto::LaserRangeFinder::CreateLaserRangeFinder(karto::LaserRangeFinder_Custom, name);         
00277                         mLaser->SetMinimumRange(scan->range_min);
00278                         mLaser->SetMaximumRange(scan->range_max);
00279                         mLaser->SetMinimumAngle(scan->angle_min);
00280                         mLaser->SetMaximumAngle(scan->angle_max);
00281                         mLaser->SetAngularResolution(scan->angle_increment);
00282                         mLaser->SetRangeThreshold(mRangeThreshold);
00283                         mMapper->Process(mLaser);
00284                 }
00285                 catch(karto::Exception e)
00286                 {
00287                         ROS_ERROR("Could not add new Laser to Mapper: %s", e.GetErrorMessage().ToCString());
00288                         return;
00289                 }
00290         }
00291         
00292         if(mState == ST_LOCALIZING)
00293         {
00294                 mSelfLocalizer->process(scan);
00295                 if(mSelfLocalizer->getCovariance() < mMaxCovariance)
00296                 {
00297                         // Localization finished, kill the localizer and start mapping
00298                         ROS_INFO("Localization finished on robot %d, now starting to map.", mRobotID);
00299                         tf::Transform p = mSelfLocalizer->getBestPose();
00300                         setRobotPose(p.getOrigin().getX(), p.getOrigin().getY(), tf::getYaw(p.getRotation()));
00301                 }
00302         }else 
00303         if(mState == ST_MAPPING)
00304         {
00305                 // get the odometry pose from tf
00306                 tf::StampedTransform tfPose;
00307                 try
00308                 {
00309                         mTransformListener.lookupTransform(mOffsetFrame, mLaserFrame, scan->header.stamp, tfPose);
00310                 }
00311                 catch(tf::TransformException e)
00312                 {
00313                         try
00314                         {
00315                                 mTransformListener.lookupTransform(mOffsetFrame, mLaserFrame, ros::Time(0), tfPose);
00316                         }
00317                         catch(tf::TransformException e)
00318                         {
00319                                 ROS_WARN("Failed to compute odometry pose, skipping scan (%s)", e.what());
00320                                 return;
00321                         }
00322                 }
00323                 karto::Pose2 kartoPose = karto::Pose2(tfPose.getOrigin().x(), tfPose.getOrigin().y(), tf::getYaw(tfPose.getRotation()));
00324                 
00325                 // create localized laser scan
00326                 karto::LocalizedLaserScanPtr laserScan = createFromRosMessage(*scan, mLaser->GetIdentifier());
00327                 laserScan->SetOdometricPose(kartoPose);
00328                 laserScan->SetCorrectedPose(kartoPose);
00329                 
00330                 bool success;
00331                 try
00332                 {
00333                         success = mMapper->Process(laserScan);
00334                 }
00335                 catch(karto::Exception e)
00336                 {
00337                         ROS_ERROR("%s", e.GetErrorMessage().ToCString());
00338                         success = false;
00339                 }
00340                 
00341                 if(success)
00342                 {
00343                         // Compute the map->odom transform
00344                         karto::Pose2 corrected_pose = laserScan->GetCorrectedPose();
00345                         tf::Pose map_in_robot(tf::createQuaternionFromYaw(corrected_pose.GetHeading()), tf::Vector3(corrected_pose.GetX(), corrected_pose.GetY(), 0.0));
00346                         map_in_robot = map_in_robot.inverse();
00347                         tf::Stamped<tf::Pose> map_in_odom;
00348                         bool ok = true;
00349                         try
00350                         {
00351                                 mTransformListener.transformPose(mOffsetFrame, tf::Stamped<tf::Pose>(map_in_robot, ros::Time(0) /*scan->header.stamp*/, mLaserFrame), map_in_odom);
00352                         }
00353                         catch(tf::TransformException e)
00354                         {
00355                                 ROS_WARN("Transform from %s to %s failed! (%s)", mLaserFrame.c_str(), mOffsetFrame.c_str(), e.what());
00356                                 ok = false;
00357                         }
00358                         if(ok) 
00359                         {
00360                                 mMapToOdometry = tf::Transform(tf::Quaternion( map_in_odom.getRotation() ), tf::Point(map_in_odom.getOrigin() ) ).inverse();
00361                                 tf::Vector3 v = mMapToOdometry.getOrigin();
00362                                 v.setZ(0);
00363                                 mMapToOdometry.setOrigin(v);
00364                         }
00365                         mNodesAdded++;
00366                         mMapChanged = true;
00367 
00368                         ros::WallDuration d = ros::WallTime::now() - mLastMapUpdate;
00369                         if(mMapUpdateRate > 0 && d.toSec() > mMapUpdateRate)
00370                         {
00371                                 sendMap();
00372                         }
00373 
00374                         // Send the scan to the other robots via com-layer (DDS)
00375                         ROS_DEBUG("Robot %d: Sending scan (uniqueID: %d, Sensor: %s, stateID: %d)", mRobotID, laserScan->GetUniqueId(), laserScan->GetSensorIdentifier().ToString().ToCString(), laserScan->GetStateId());
00376                         sendLocalizedScan(scan, laserScan->GetOdometricPose());
00377                         
00378                         // Publish via extra topic
00379                         nav2d_msgs::RobotPose other;
00380                         other.header.stamp = ros::Time::now();
00381                         other.header.frame_id = mMapFrame;
00382                         other.robot_id = mRobotID;
00383                         other.pose.x = laserScan->GetCorrectedPose().GetX();
00384                         other.pose.y = laserScan->GetCorrectedPose().GetY();
00385                         other.pose.theta = laserScan->GetCorrectedPose().GetHeading();
00386                         mOtherRobotsPublisher.publish(other);
00387                 }
00388         }
00389 }
00390 
00391 bool MultiMapper::getMap(nav_msgs::GetMap::Request  &req, nav_msgs::GetMap::Response &res)
00392 {
00393         if(mState == ST_WAITING_FOR_MAP && mNodesAdded < mMinMapSize)
00394         {
00395                 ROS_INFO("Still waiting for map from robot 1.");
00396                 return false;
00397         }
00398         
00399         if(sendMap())
00400         {
00401                 res.map = mGridMap;
00402                 return true;
00403         }else
00404         {
00405                 ROS_WARN("Serving map request failed!");
00406                 return false;
00407         }
00408 }
00409 
00410 bool MultiMapper::sendMap()
00411 {
00412         if(!updateMap()) return false;
00413         
00414         // Publish the map
00415         mMapPublisher.publish(mGridMap);
00416         mLastMapUpdate = ros::WallTime::now();
00417 
00418         // Publish the pose-graph
00419         if(mPublishPoseGraph)
00420         {
00421                 // Publish the vertices
00422                 karto::MapperGraph::VertexList vertices = mMapper->GetGraph()->GetVertices();
00423                 visualization_msgs::Marker marker;
00424                 marker.header.frame_id = mMapFrame;
00425                 marker.header.stamp = ros::Time();
00426                 marker.id = 0;
00427                 marker.type = visualization_msgs::Marker::SPHERE_LIST;
00428                 marker.action = visualization_msgs::Marker::ADD;
00429                 marker.pose.position.x = 0;
00430                 marker.pose.position.y = 0;
00431                 marker.pose.position.z = 0;
00432                 marker.pose.orientation.x = 0.0;
00433                 marker.pose.orientation.y = 0.0;
00434                 marker.pose.orientation.z = 0.0;
00435                 marker.pose.orientation.w = 1.0;
00436                 marker.scale.x = 0.1;
00437                 marker.scale.y = 0.1;
00438                 marker.scale.z = 0.1;
00439                 marker.color.a = 1.0;
00440                 marker.color.r = 0.0;
00441                 marker.color.g = 1.0;
00442                 marker.color.b = 0.0;
00443                 marker.points.resize(vertices.Size());
00444                 
00445                 for(int i = 0; i < vertices.Size(); i++)
00446                 {
00447                         marker.points[i].x = vertices[i]->GetVertexObject()->GetCorrectedPose().GetX();
00448                         marker.points[i].y = vertices[i]->GetVertexObject()->GetCorrectedPose().GetY();
00449                         marker.points[i].z = 0;
00450                 }
00451                 mVerticesPublisher.publish(marker);
00452                 
00453                 // Publish the edges
00454                 karto::MapperGraph::EdgeList edges = mMapper->GetGraph()->GetEdges();
00455                 marker.header.frame_id = mMapFrame;
00456                 marker.header.stamp = ros::Time();
00457                 marker.id = 0;
00458                 marker.type = visualization_msgs::Marker::LINE_LIST;
00459                 marker.scale.x = 0.01;
00460                 marker.color.a = 1.0;
00461                 marker.color.r = 1.0;
00462                 marker.color.g = 0.0;
00463                 marker.color.b = 0.0;
00464                 marker.points.resize(edges.Size() * 2);
00465                 
00466                 for(int i = 0; i < edges.Size(); i++)
00467                 {
00468                         marker.points[2*i].x = edges[i]->GetSource()->GetVertexObject()->GetCorrectedPose().GetX();
00469                         marker.points[2*i].y = edges[i]->GetSource()->GetVertexObject()->GetCorrectedPose().GetY();
00470                         marker.points[2*i].z = 0;
00471 
00472                         marker.points[2*i+1].x = edges[i]->GetTarget()->GetVertexObject()->GetCorrectedPose().GetX();
00473                         marker.points[2*i+1].y = edges[i]->GetTarget()->GetVertexObject()->GetCorrectedPose().GetY();
00474                         marker.points[2*i+1].z = 0;
00475                 }
00476                 mEdgesPublisher.publish(marker);
00477         }
00478         return true;
00479 }
00480 
00481 bool MultiMapper::updateMap()
00482 {
00483         if(!mMapChanged) return true;
00484         
00485         const karto::LocalizedLaserScanList allScans = mMapper->GetAllProcessedScans();
00486         karto::OccupancyGridPtr kartoGrid = karto::OccupancyGrid::CreateFromScans(allScans, mMapResolution);
00487 
00488         if(!kartoGrid)
00489         {
00490                 ROS_WARN("Failed to get occupancy map from Karto-Mapper.");
00491                 return false; 
00492         }
00493 
00494         // Translate to ROS format
00495         unsigned int width = kartoGrid->GetWidth();
00496         unsigned int height = kartoGrid->GetHeight();
00497         karto::Vector2<kt_double> offset = kartoGrid->GetCoordinateConverter()->GetOffset();
00498 
00499         if(     mGridMap.info.width != width || 
00500                 mGridMap.info.height != height || 
00501                 mGridMap.info.origin.position.x != offset.GetX() || 
00502                 mGridMap.info.origin.position.y != offset.GetY())
00503         {
00504                 mGridMap.info.resolution = mMapResolution;
00505                 mGridMap.info.origin.position.x = offset.GetX();
00506                 mGridMap.info.origin.position.y = offset.GetY();
00507                 mGridMap.info.width = width;
00508                 mGridMap.info.height = height;
00509                 mGridMap.data.resize(mGridMap.info.width * mGridMap.info.height);
00510         }
00511         
00512         for (unsigned int y = 0; y < height; y++)
00513         {
00514                 for (unsigned int x = 0; x < width; x++) 
00515                 {
00516                         // Getting the value at position x,y
00517                         kt_int8u value = kartoGrid->GetValue(karto::Vector2<kt_int32s>(x, y));
00518 
00519                         switch (value)
00520                         {
00521                         case karto::GridStates_Unknown:
00522                                 mGridMap.data[MAP_IDX(mGridMap.info.width, x, y)] = -1;
00523                                 break;
00524                         case karto::GridStates_Occupied:
00525                                 mGridMap.data[MAP_IDX(mGridMap.info.width, x, y)] = 100;
00526                                 break;
00527                         case karto::GridStates_Free:
00528                                 mGridMap.data[MAP_IDX(mGridMap.info.width, x, y)] = 0;
00529                                 break;
00530                         default:
00531                                 ROS_WARN("Encountered unknown cell value at %d, %d", x, y);
00532                                 break;
00533                         }
00534                 }
00535         }
00536 
00537         // Set the header information on the map
00538         mGridMap.header.stamp = ros::Time::now();
00539         mGridMap.header.frame_id = mMapFrame.c_str();
00540         mMapChanged = false;
00541         return true;
00542 }
00543 
00544 void MultiMapper::receiveLocalizedScan(const nav2d_msgs::LocalizedScan::ConstPtr& scan)
00545 {
00546         // Ignore my own scans
00547         if(scan->robot_id == mRobotID) return;
00548         
00549         // Get the robot id
00550         char robot[10];
00551         sprintf(robot, "robot_%d", scan->robot_id);
00552         
00553         // Get the scan pose
00554         karto::Pose2 scanPose(scan->x, scan->y, scan->yaw);
00555         
00556         // create localized laser scan
00557         karto::LocalizedLaserScanPtr localizedScan = createFromRosMessage(scan->scan, robot);
00558         localizedScan->SetOdometricPose(scanPose);
00559         localizedScan->SetCorrectedPose(scanPose);
00560         
00561         // feed the localized scan to the Karto-Mapper
00562         bool added = false;
00563         try
00564         {
00565                 added = mMapper->Process(localizedScan);
00566         }
00567         catch(karto::Exception e1)
00568         {
00569                 if(mOtherLasers.find(scan->robot_id) == mOtherLasers.end())
00570                 {
00571                         try
00572                         {
00573                                 karto::LaserRangeFinderPtr laser = karto::LaserRangeFinder::CreateLaserRangeFinder(karto::LaserRangeFinder_Custom, robot);
00574                                 laser->SetMinimumRange(scan->scan.range_min);
00575                                 laser->SetMaximumRange(scan->scan.range_max);
00576                                 laser->SetMinimumAngle(scan->scan.angle_min);
00577                                 laser->SetMaximumAngle(scan->scan.angle_max);
00578                                 laser->SetAngularResolution(scan->scan.angle_increment);
00579                                 laser->SetRangeThreshold(mRangeThreshold);
00580                                 mMapper->Process(laser);
00581                                 mOtherLasers.insert(std::pair<int,karto::LaserRangeFinderPtr>(scan->robot_id,laser));
00582                         
00583                                 added = mMapper->Process(localizedScan);
00584                         }
00585                         catch(karto::Exception e2)
00586                         {
00587                                 ROS_ERROR("%s", e2.GetErrorMessage().ToCString());
00588                         }
00589                 }else
00590                 {
00591                         ROS_ERROR("%s", e1.GetErrorMessage().ToCString());
00592                 }
00593         }
00594         if(added)
00595         {
00596                 mNodesAdded++;
00597                 mMapChanged = true;
00598                 ROS_DEBUG("Robot %d: Received scan (uniqueID: %d, Sensor: %s, stateID: %d)", mRobotID, localizedScan->GetUniqueId(), localizedScan->GetSensorIdentifier().ToString().ToCString(), localizedScan->GetStateId());
00599                 
00600                 // Publish via extra topic
00601                 nav2d_msgs::RobotPose other;
00602                 other.header.stamp = ros::Time::now();
00603                 other.header.frame_id = mMapFrame;
00604                 other.robot_id = scan->robot_id;
00605                 other.pose.x = localizedScan->GetCorrectedPose().GetX();
00606                 other.pose.y = localizedScan->GetCorrectedPose().GetY();
00607                 other.pose.theta = localizedScan->GetCorrectedPose().GetHeading();
00608                 mOtherRobotsPublisher.publish(other);
00609                 
00610                 // Send the map via topic
00611                 ros::WallDuration d = ros::WallTime::now() - mLastMapUpdate;
00612                 if(mMapUpdateRate > 0 && d.toSec() > mMapUpdateRate)
00613                 {
00614                         sendMap();
00615                         if(mState == ST_LOCALIZING)
00616                         {
00617                                 mSelfLocalizer->convertMap(mGridMap);
00618                         }
00619                 }
00620         }else
00621         {
00622                 ROS_DEBUG("Discarded Scan from Robot %d!", scan->robot_id);
00623         }
00624 
00625         if(mState == ST_WAITING_FOR_MAP && mNodesAdded >= mMinMapSize)
00626         {
00627                 sendMap();
00628                 mSelfLocalizer->convertMap(mGridMap);
00629                 mSelfLocalizer->initialize();
00630                 mState = ST_LOCALIZING;
00631                 ROS_INFO("Received a map, now starting to localize.");
00632                 mSelfLocalizer->publishParticleCloud();
00633         }
00634 }
00635 
00636 void MultiMapper::sendLocalizedScan(const sensor_msgs::LaserScan::ConstPtr& scan, const karto::Pose2& pose)
00637 {
00638         nav2d_msgs::LocalizedScan rosScan;
00639         rosScan.robot_id = mRobotID;
00640         rosScan.laser_type = 0;
00641         rosScan.x = pose.GetX();
00642         rosScan.y = pose.GetY();
00643         rosScan.yaw = pose.GetHeading();
00644         
00645         rosScan.scan.angle_min = scan->angle_min;
00646         rosScan.scan.angle_max = scan->angle_max;
00647         rosScan.scan.range_min = scan->range_min;
00648         rosScan.scan.range_max = scan->range_max;
00649         rosScan.scan.angle_increment = scan->angle_increment;
00650         rosScan.scan.time_increment = scan->time_increment;
00651         rosScan.scan.scan_time = scan->scan_time;
00652         
00653         unsigned int nReadings = scan->ranges.size();
00654         rosScan.scan.ranges.resize(nReadings);
00655         for(unsigned int i = 0; i < nReadings; i++)
00656         {
00657                 rosScan.scan.ranges[i] = scan->ranges[i];
00658         }
00659 
00660 //      rosScan.scan = *scan;
00661         mScanPublisher.publish(rosScan);
00662 }
00663 
00664 void MultiMapper::receiveInitialPose(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& pose)
00665 {
00666         double x = pose->pose.pose.position.x;
00667         double y = pose->pose.pose.position.y;
00668         double yaw = tf::getYaw(pose->pose.pose.orientation);
00669         ROS_INFO("Received initial pose (%.2f, %.2f, %.2f) on robot %d, now starting to map.",x,y,yaw,mRobotID);
00670         try
00671         {
00672                 setRobotPose(x,y,yaw);
00673         }
00674         catch(tf::TransformException e)
00675         {
00676                 ROS_ERROR("Failed to set pose on robot %d! (%s)", mRobotID, e.what());
00677                 return;
00678         }
00679 }
00680 
00681 void MultiMapper::onMessage(const void* sender, karto::MapperEventArguments& args)
00682 {
00683         ROS_DEBUG("OpenMapper: %s\n", args.GetEventMessage().ToCString());
00684 }
00685 
00686 void MultiMapper::publishTransform()
00687 {
00688         if(mState == ST_MAPPING)
00689         {
00690                 mTransformBroadcaster.sendTransform(tf::StampedTransform (mOdometryOffset, ros::Time::now() , mOffsetFrame, mOdometryFrame));
00691                 mTransformBroadcaster.sendTransform(tf::StampedTransform (mMapToOdometry, ros::Time::now() , mMapFrame, mOffsetFrame));
00692         }
00693 }


nav2d_karto
Author(s): Sebastian Kasperski
autogenerated on Sun Apr 2 2017 03:53:08