slam_karto.cpp
Go to the documentation of this file.
00001 /*
00002  * slam_karto
00003  * Copyright (c) 2008, Willow Garage, Inc.
00004  *
00005  * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE
00006  * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY
00007  * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS
00008  * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED.
00009  *
00010  * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO
00011  * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS
00012  * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND
00013  * CONDITIONS.
00014  *
00015  */
00016 
00017 /* Author: Brian Gerkey */
00018 
00027 #include "ros/ros.h"
00028 #include "ros/console.h"
00029 #include "message_filters/subscriber.h"
00030 #include "tf/transform_broadcaster.h"
00031 #include "tf/transform_listener.h"
00032 #include "tf/message_filter.h"
00033 #include "visualization_msgs/MarkerArray.h"
00034 
00035 #include "nav_msgs/MapMetaData.h"
00036 #include "sensor_msgs/LaserScan.h"
00037 #include "nav_msgs/GetMap.h"
00038 
00039 #include "open_karto/Mapper.h"
00040 
00041 #include "spa_solver.h"
00042 
00043 #include <boost/thread.hpp>
00044 
00045 #include <string>
00046 #include <map>
00047 #include <vector>
00048 
00049 // compute linear index for given map coords
00050 #define MAP_IDX(sx, i, j) ((sx) * (j) + (i))
00051 
00052 class SlamKarto
00053 {
00054   public:
00055     SlamKarto();
00056     ~SlamKarto();
00057 
00058     void laserCallback(const sensor_msgs::LaserScan::ConstPtr& scan);
00059     bool mapCallback(nav_msgs::GetMap::Request  &req,
00060                      nav_msgs::GetMap::Response &res);
00061 
00062   private:
00063     bool getOdomPose(karto::Pose2& karto_pose, const ros::Time& t);
00064     karto::LaserRangeFinder* getLaser(const sensor_msgs::LaserScan::ConstPtr& scan);
00065     bool addScan(karto::LaserRangeFinder* laser,
00066                  const sensor_msgs::LaserScan::ConstPtr& scan,
00067                  karto::Pose2& karto_pose);
00068     bool updateMap();
00069     void publishTransform();
00070     void publishLoop(double transform_publish_period);
00071     void publishGraphVisualization();
00072 
00073     // ROS handles
00074     ros::NodeHandle node_;
00075     tf::TransformListener tf_;
00076     tf::TransformBroadcaster* tfB_;
00077     message_filters::Subscriber<sensor_msgs::LaserScan>* scan_filter_sub_;
00078     tf::MessageFilter<sensor_msgs::LaserScan>* scan_filter_;
00079     ros::Publisher sst_;
00080     ros::Publisher marker_publisher_;
00081     ros::Publisher sstm_;
00082     ros::ServiceServer ss_;
00083 
00084     // The map that will be published / send to service callers
00085     nav_msgs::GetMap::Response map_;
00086 
00087     // Storage for ROS parameters
00088     std::string odom_frame_;
00089     std::string map_frame_;
00090     std::string base_frame_;
00091     int throttle_scans_;
00092     ros::Duration map_update_interval_;
00093     double resolution_;
00094     boost::mutex map_mutex_;
00095     boost::mutex map_to_odom_mutex_;
00096 
00097     // Karto bookkeeping
00098     karto::Mapper* mapper_;
00099     karto::Dataset* dataset_;
00100     SpaSolver* solver_;
00101     std::map<std::string, karto::LaserRangeFinder*> lasers_;
00102     std::map<std::string, bool> lasers_inverted_;
00103 
00104     // Internal state
00105     bool got_map_;
00106     int laser_count_;
00107     boost::thread* transform_thread_;
00108     tf::Transform map_to_odom_;
00109     unsigned marker_count_;
00110     bool inverted_laser_;
00111 };
00112 
00113 SlamKarto::SlamKarto() :
00114         got_map_(false),
00115         laser_count_(0),
00116         transform_thread_(NULL),
00117         marker_count_(0)
00118 {
00119   map_to_odom_.setIdentity();
00120   // Retrieve parameters
00121   ros::NodeHandle private_nh_("~");
00122   if(!private_nh_.getParam("odom_frame", odom_frame_))
00123     odom_frame_ = "odom";
00124   if(!private_nh_.getParam("map_frame", map_frame_))
00125     map_frame_ = "map";
00126   if(!private_nh_.getParam("base_frame", base_frame_))
00127     base_frame_ = "base_link";
00128   if(!private_nh_.getParam("throttle_scans", throttle_scans_))
00129     throttle_scans_ = 1;
00130   double tmp;
00131   if(!private_nh_.getParam("map_update_interval", tmp))
00132     tmp = 5.0;
00133   map_update_interval_.fromSec(tmp);
00134   if(!private_nh_.getParam("resolution", resolution_))
00135   {
00136     // Compatibility with slam_gmapping, which uses "delta" to mean
00137     // resolution
00138     if(!private_nh_.getParam("delta", resolution_))
00139       resolution_ = 0.05;
00140   }
00141   double transform_publish_period;
00142   private_nh_.param("transform_publish_period", transform_publish_period, 0.05);
00143 
00144   // Set up advertisements and subscriptions
00145   tfB_ = new tf::TransformBroadcaster();
00146   sst_ = node_.advertise<nav_msgs::OccupancyGrid>("map", 1, true);
00147   sstm_ = node_.advertise<nav_msgs::MapMetaData>("map_metadata", 1, true);
00148   ss_ = node_.advertiseService("dynamic_map", &SlamKarto::mapCallback, this);
00149   scan_filter_sub_ = new message_filters::Subscriber<sensor_msgs::LaserScan>(node_, "scan", 5);
00150   scan_filter_ = new tf::MessageFilter<sensor_msgs::LaserScan>(*scan_filter_sub_, tf_, odom_frame_, 5);
00151   scan_filter_->registerCallback(boost::bind(&SlamKarto::laserCallback, this, _1));
00152   marker_publisher_ = node_.advertise<visualization_msgs::MarkerArray>("visualization_marker_array",1);
00153 
00154   // Create a thread to periodically publish the latest map->odom
00155   // transform; it needs to go out regularly, uninterrupted by potentially
00156   // long periods of computation in our main loop.
00157   transform_thread_ = new boost::thread(boost::bind(&SlamKarto::publishLoop, this, transform_publish_period));
00158 
00159   // Initialize Karto structures
00160   mapper_ = new karto::Mapper();
00161   dataset_ = new karto::Dataset();
00162 
00163   // Setting General Parameters from the Parameter Server
00164   bool use_scan_matching;
00165   if(private_nh_.getParam("use_scan_matching", use_scan_matching))
00166     mapper_->setParamUseScanMatching(use_scan_matching);
00167   
00168   bool use_scan_barycenter;
00169   if(private_nh_.getParam("use_scan_barycenter", use_scan_barycenter))
00170     mapper_->setParamUseScanBarycenter(use_scan_barycenter);
00171 
00172   double minimum_travel_distance;
00173   if(private_nh_.getParam("minimum_travel_distance", minimum_travel_distance))
00174     mapper_->setParamMinimumTravelDistance(minimum_travel_distance);
00175 
00176   double minimum_travel_heading;
00177   if(private_nh_.getParam("minimum_travel_heading", minimum_travel_heading))
00178     mapper_->setParamMinimumTravelHeading(minimum_travel_heading);
00179 
00180   int scan_buffer_size;
00181   if(private_nh_.getParam("scan_buffer_size", scan_buffer_size))
00182     mapper_->setParamScanBufferSize(scan_buffer_size);
00183 
00184   double scan_buffer_maximum_scan_distance;
00185   if(private_nh_.getParam("scan_buffer_maximum_scan_distance", scan_buffer_maximum_scan_distance))
00186     mapper_->setParamScanBufferMaximumScanDistance(scan_buffer_maximum_scan_distance);
00187 
00188   double link_match_minimum_response_fine;
00189   if(private_nh_.getParam("link_match_minimum_response_fine", link_match_minimum_response_fine))
00190     mapper_->setParamLinkMatchMinimumResponseFine(link_match_minimum_response_fine);
00191 
00192   double link_scan_maximum_distance;
00193   if(private_nh_.getParam("link_scan_maximum_distance", link_scan_maximum_distance))
00194     mapper_->setParamLinkScanMaximumDistance(link_scan_maximum_distance);
00195 
00196   double loop_search_maximum_distance;
00197   if(private_nh_.getParam("loop_search_maximum_distance", loop_search_maximum_distance))
00198     mapper_->setParamLoopSearchMaximumDistance(loop_search_maximum_distance);
00199 
00200   bool do_loop_closing;
00201   if(private_nh_.getParam("do_loop_closing", do_loop_closing))
00202     mapper_->setParamDoLoopClosing(do_loop_closing);
00203 
00204   int loop_match_minimum_chain_size;
00205   if(private_nh_.getParam("loop_match_minimum_chain_size", loop_match_minimum_chain_size))
00206     mapper_->setParamLoopMatchMinimumChainSize(loop_match_minimum_chain_size);
00207 
00208   double loop_match_maximum_variance_coarse;
00209   if(private_nh_.getParam("loop_match_maximum_variance_coarse", loop_match_maximum_variance_coarse))
00210     mapper_->setParamLoopMatchMaximumVarianceCoarse(loop_match_maximum_variance_coarse);
00211 
00212   double loop_match_minimum_response_coarse;
00213   if(private_nh_.getParam("loop_match_minimum_response_coarse", loop_match_minimum_response_coarse))
00214     mapper_->setParamLoopMatchMinimumResponseCoarse(loop_match_minimum_response_coarse);
00215 
00216   double loop_match_minimum_response_fine;
00217   if(private_nh_.getParam("loop_match_minimum_response_fine", loop_match_minimum_response_fine))
00218     mapper_->setParamLoopMatchMinimumResponseFine(loop_match_minimum_response_fine);
00219 
00220   // Setting Correlation Parameters from the Parameter Server
00221 
00222   double correlation_search_space_dimension;
00223   if(private_nh_.getParam("correlation_search_space_dimension", correlation_search_space_dimension))
00224     mapper_->setParamCorrelationSearchSpaceDimension(correlation_search_space_dimension);
00225 
00226   double correlation_search_space_resolution;
00227   if(private_nh_.getParam("correlation_search_space_resolution", correlation_search_space_resolution))
00228     mapper_->setParamCorrelationSearchSpaceResolution(correlation_search_space_resolution);
00229 
00230   double correlation_search_space_smear_deviation;
00231   if(private_nh_.getParam("correlation_search_space_smear_deviation", correlation_search_space_smear_deviation))
00232     mapper_->setParamCorrelationSearchSpaceSmearDeviation(correlation_search_space_smear_deviation);
00233 
00234   // Setting Correlation Parameters, Loop Closure Parameters from the Parameter Server
00235 
00236   double loop_search_space_dimension;
00237   if(private_nh_.getParam("loop_search_space_dimension", loop_search_space_dimension))
00238     mapper_->setParamLoopSearchSpaceDimension(loop_search_space_dimension);
00239 
00240   double loop_search_space_resolution;
00241   if(private_nh_.getParam("loop_search_space_resolution", loop_search_space_resolution))
00242     mapper_->setParamLoopSearchSpaceResolution(loop_search_space_resolution);
00243 
00244   double loop_search_space_smear_deviation;
00245   if(private_nh_.getParam("loop_search_space_smear_deviation", loop_search_space_smear_deviation))
00246     mapper_->setParamLoopSearchSpaceSmearDeviation(loop_search_space_smear_deviation);
00247 
00248   // Setting Scan Matcher Parameters from the Parameter Server
00249 
00250   double distance_variance_penalty;
00251   if(private_nh_.getParam("distance_variance_penalty", distance_variance_penalty))
00252     mapper_->setParamDistanceVariancePenalty(distance_variance_penalty);
00253 
00254   double angle_variance_penalty;
00255   if(private_nh_.getParam("angle_variance_penalty", angle_variance_penalty))
00256     mapper_->setParamAngleVariancePenalty(angle_variance_penalty);
00257 
00258   double fine_search_angle_offset;
00259   if(private_nh_.getParam("fine_search_angle_offset", fine_search_angle_offset))
00260     mapper_->setParamFineSearchAngleOffset(fine_search_angle_offset);
00261 
00262   double coarse_search_angle_offset;
00263   if(private_nh_.getParam("coarse_search_angle_offset", coarse_search_angle_offset))
00264     mapper_->setParamCoarseSearchAngleOffset(coarse_search_angle_offset);
00265 
00266   double coarse_angle_resolution;
00267   if(private_nh_.getParam("coarse_angle_resolution", coarse_angle_resolution))
00268     mapper_->setParamCoarseAngleResolution(coarse_angle_resolution);
00269 
00270   double minimum_angle_penalty;
00271   if(private_nh_.getParam("minimum_angle_penalty", minimum_angle_penalty))
00272     mapper_->setParamMinimumAnglePenalty(minimum_angle_penalty);
00273 
00274   double minimum_distance_penalty;
00275   if(private_nh_.getParam("minimum_distance_penalty", minimum_distance_penalty))
00276     mapper_->setParamMinimumDistancePenalty(minimum_distance_penalty);
00277 
00278   bool use_response_expansion;
00279   if(private_nh_.getParam("use_response_expansion", use_response_expansion))
00280     mapper_->setParamUseResponseExpansion(use_response_expansion);
00281 
00282   // Set solver to be used in loop closure
00283   solver_ = new SpaSolver();
00284   mapper_->SetScanSolver(solver_);
00285 }
00286 
00287 SlamKarto::~SlamKarto()
00288 {
00289   if(transform_thread_)
00290   {
00291     transform_thread_->join();
00292     delete transform_thread_;
00293   }
00294   if (scan_filter_)
00295     delete scan_filter_;
00296   if (scan_filter_sub_)
00297     delete scan_filter_sub_;
00298   if (solver_)
00299     delete solver_;
00300   if (mapper_)
00301     delete mapper_;
00302   if (dataset_)
00303     delete dataset_;
00304   // TODO: delete the pointers in the lasers_ map; not sure whether or not
00305   // I'm supposed to do that.
00306 }
00307 
00308 void
00309 SlamKarto::publishLoop(double transform_publish_period)
00310 {
00311   if(transform_publish_period == 0)
00312     return;
00313 
00314   ros::Rate r(1.0 / transform_publish_period);
00315   while(ros::ok())
00316   {
00317     publishTransform();
00318     r.sleep();
00319   }
00320 }
00321 
00322 void
00323 SlamKarto::publishTransform()
00324 {
00325   boost::mutex::scoped_lock lock(map_to_odom_mutex_);
00326   ros::Time tf_expiration = ros::Time::now() + ros::Duration(0.05);
00327   tfB_->sendTransform(tf::StampedTransform (map_to_odom_, ros::Time::now(), map_frame_, odom_frame_));
00328 }
00329 
00330 karto::LaserRangeFinder*
00331 SlamKarto::getLaser(const sensor_msgs::LaserScan::ConstPtr& scan)
00332 {
00333   // Check whether we know about this laser yet
00334   if(lasers_.find(scan->header.frame_id) == lasers_.end())
00335   {
00336     // New laser; need to create a Karto device for it.
00337 
00338     // Get the laser's pose, relative to base.
00339     tf::Stamped<tf::Pose> ident;
00340     tf::Stamped<tf::Transform> laser_pose;
00341     ident.setIdentity();
00342     ident.frame_id_ = scan->header.frame_id;
00343     ident.stamp_ = scan->header.stamp;
00344     try
00345     {
00346       tf_.transformPose(base_frame_, ident, laser_pose);
00347     }
00348     catch(tf::TransformException e)
00349     {
00350       ROS_WARN("Failed to compute laser pose, aborting initialization (%s)",
00351                e.what());
00352       return NULL;
00353     }
00354 
00355     double yaw = tf::getYaw(laser_pose.getRotation());
00356 
00357     ROS_INFO("laser %s's pose wrt base: %.3f %.3f %.3f",
00358              scan->header.frame_id.c_str(),
00359              laser_pose.getOrigin().x(),
00360              laser_pose.getOrigin().y(),
00361              yaw);
00362     // To account for lasers that are mounted upside-down,
00363     // we create a point 1m above the laser and transform it into the laser frame
00364     // if the point's z-value is <=0, it is upside-down
00365 
00366     tf::Vector3 v;
00367     v.setValue(0, 0, 1 + laser_pose.getOrigin().z());
00368     tf::Stamped<tf::Vector3> up(v, scan->header.stamp, base_frame_);
00369 
00370     try
00371     {
00372       tf_.transformPoint(scan->header.frame_id, up, up);
00373       ROS_DEBUG("Z-Axis in sensor frame: %.3f", up.z());
00374     }
00375     catch (tf::TransformException& e)
00376     {
00377       ROS_WARN("Unable to determine orientation of laser: %s", e.what());
00378       return NULL;
00379     }
00380 
00381     bool inverse = lasers_inverted_[scan->header.frame_id] = up.z() <= 0;
00382     if (inverse)
00383       ROS_INFO("laser is mounted upside-down");
00384 
00385 
00386     // Create a laser range finder device and copy in data from the first
00387     // scan
00388     std::string name = scan->header.frame_id;
00389     karto::LaserRangeFinder* laser = 
00390       karto::LaserRangeFinder::CreateLaserRangeFinder(karto::LaserRangeFinder_Custom, karto::Name(name));
00391     laser->SetOffsetPose(karto::Pose2(laser_pose.getOrigin().x(),
00392                                       laser_pose.getOrigin().y(),
00393                                       yaw));
00394     laser->SetMinimumRange(scan->range_min);
00395     laser->SetMaximumRange(scan->range_max);
00396     laser->SetMinimumAngle(scan->angle_min);
00397     laser->SetMaximumAngle(scan->angle_max);
00398     laser->SetAngularResolution(scan->angle_increment);
00399     // TODO: expose this, and many other parameters
00400     //laser_->SetRangeThreshold(12.0);
00401 
00402     // Store this laser device for later
00403     lasers_[scan->header.frame_id] = laser;
00404 
00405     // Add it to the dataset, which seems to be necessary
00406     dataset_->Add(laser);
00407   }
00408 
00409   return lasers_[scan->header.frame_id];
00410 }
00411 
00412 bool
00413 SlamKarto::getOdomPose(karto::Pose2& karto_pose, const ros::Time& t)
00414 {
00415   // Get the robot's pose
00416   tf::Stamped<tf::Pose> ident (tf::Transform(tf::createQuaternionFromRPY(0,0,0),
00417                                            tf::Vector3(0,0,0)), t, base_frame_);
00418   tf::Stamped<tf::Transform> odom_pose;
00419   try
00420   {
00421     tf_.transformPose(odom_frame_, ident, odom_pose);
00422   }
00423   catch(tf::TransformException e)
00424   {
00425     ROS_WARN("Failed to compute odom pose, skipping scan (%s)", e.what());
00426     return false;
00427   }
00428   double yaw = tf::getYaw(odom_pose.getRotation());
00429 
00430   karto_pose = 
00431           karto::Pose2(odom_pose.getOrigin().x(),
00432                        odom_pose.getOrigin().y(),
00433                        yaw);
00434   return true;
00435 }
00436 
00437 void
00438 SlamKarto::publishGraphVisualization()
00439 {
00440   std::vector<float> graph;
00441   solver_->getGraph(graph);
00442 
00443   visualization_msgs::MarkerArray marray;
00444 
00445   visualization_msgs::Marker m;
00446   m.header.frame_id = "map";
00447   m.header.stamp = ros::Time::now();
00448   m.id = 0;
00449   m.ns = "karto";
00450   m.type = visualization_msgs::Marker::SPHERE;
00451   m.pose.position.x = 0.0;
00452   m.pose.position.y = 0.0;
00453   m.pose.position.z = 0.0;
00454   m.scale.x = 0.1;
00455   m.scale.y = 0.1;
00456   m.scale.z = 0.1;
00457   m.color.r = 1.0;
00458   m.color.g = 0;
00459   m.color.b = 0.0;
00460   m.color.a = 1.0;
00461   m.lifetime = ros::Duration(0);
00462 
00463   visualization_msgs::Marker edge;
00464   edge.header.frame_id = "map";
00465   edge.header.stamp = ros::Time::now();
00466   edge.action = visualization_msgs::Marker::ADD;
00467   edge.ns = "karto";
00468   edge.id = 0;
00469   edge.type = visualization_msgs::Marker::LINE_STRIP;
00470   edge.scale.x = 0.1;
00471   edge.scale.y = 0.1;
00472   edge.scale.z = 0.1;
00473   edge.color.a = 1.0;
00474   edge.color.r = 0.0;
00475   edge.color.g = 0.0;
00476   edge.color.b = 1.0;
00477 
00478   m.action = visualization_msgs::Marker::ADD;
00479   uint id = 0;
00480   for (uint i=0; i<graph.size()/2; i++) 
00481   {
00482     m.id = id;
00483     m.pose.position.x = graph[2*i];
00484     m.pose.position.y = graph[2*i+1];
00485     marray.markers.push_back(visualization_msgs::Marker(m));
00486     id++;
00487 
00488     if(i>0)
00489     {
00490       edge.points.clear();
00491 
00492       geometry_msgs::Point p;
00493       p.x = graph[2*(i-1)];
00494       p.y = graph[2*(i-1)+1];
00495       edge.points.push_back(p);
00496       p.x = graph[2*i];
00497       p.y = graph[2*i+1];
00498       edge.points.push_back(p);
00499       edge.id = id;
00500 
00501       marray.markers.push_back(visualization_msgs::Marker(edge));
00502       id++;
00503     }
00504   }
00505 
00506   m.action = visualization_msgs::Marker::DELETE;
00507   for (; id < marker_count_; id++) 
00508   {
00509     m.id = id;
00510     marray.markers.push_back(visualization_msgs::Marker(m));
00511   }
00512 
00513   marker_count_ = marray.markers.size();
00514 
00515   marker_publisher_.publish(marray);
00516 }
00517 
00518 void
00519 SlamKarto::laserCallback(const sensor_msgs::LaserScan::ConstPtr& scan)
00520 {
00521   laser_count_++;
00522   if ((laser_count_ % throttle_scans_) != 0)
00523     return;
00524 
00525   static ros::Time last_map_update(0,0);
00526 
00527   // Check whether we know about this laser yet
00528   karto::LaserRangeFinder* laser = getLaser(scan);
00529 
00530   if(!laser)
00531   {
00532     ROS_WARN("Failed to create laser device for %s; discarding scan",
00533              scan->header.frame_id.c_str());
00534     return;
00535   }
00536 
00537   karto::Pose2 odom_pose;
00538   if(addScan(laser, scan, odom_pose))
00539   {
00540     ROS_DEBUG("added scan at pose: %.3f %.3f %.3f", 
00541               odom_pose.GetX(),
00542               odom_pose.GetY(),
00543               odom_pose.GetHeading());
00544 
00545     publishGraphVisualization();
00546 
00547     if(!got_map_ || 
00548        (scan->header.stamp - last_map_update) > map_update_interval_)
00549     {
00550       if(updateMap())
00551       {
00552         last_map_update = scan->header.stamp;
00553         got_map_ = true;
00554         ROS_DEBUG("Updated the map");
00555       }
00556     }
00557   }
00558 }
00559 
00560 bool
00561 SlamKarto::updateMap()
00562 {
00563   boost::mutex::scoped_lock lock(map_mutex_);
00564 
00565   karto::OccupancyGrid* occ_grid = 
00566           karto::OccupancyGrid::CreateFromScans(mapper_->GetAllProcessedScans(), resolution_);
00567 
00568   if(!occ_grid)
00569     return false;
00570 
00571   if(!got_map_) {
00572     map_.map.info.resolution = resolution_;
00573     map_.map.info.origin.position.x = 0.0;
00574     map_.map.info.origin.position.y = 0.0;
00575     map_.map.info.origin.position.z = 0.0;
00576     map_.map.info.origin.orientation.x = 0.0;
00577     map_.map.info.origin.orientation.y = 0.0;
00578     map_.map.info.origin.orientation.z = 0.0;
00579     map_.map.info.origin.orientation.w = 1.0;
00580   } 
00581 
00582   // Translate to ROS format
00583   kt_int32s width = occ_grid->GetWidth();
00584   kt_int32s height = occ_grid->GetHeight();
00585   karto::Vector2<kt_double> offset = occ_grid->GetCoordinateConverter()->GetOffset();
00586 
00587   if(map_.map.info.width != (unsigned int) width || 
00588      map_.map.info.height != (unsigned int) height ||
00589      map_.map.info.origin.position.x != offset.GetX() ||
00590      map_.map.info.origin.position.y != offset.GetY())
00591   {
00592     map_.map.info.origin.position.x = offset.GetX();
00593     map_.map.info.origin.position.y = offset.GetY();
00594     map_.map.info.width = width;
00595     map_.map.info.height = height;
00596     map_.map.data.resize(map_.map.info.width * map_.map.info.height);
00597   }
00598 
00599   for (kt_int32s y=0; y<height; y++)
00600   {
00601     for (kt_int32s x=0; x<width; x++) 
00602     {
00603       // Getting the value at position x,y
00604       kt_int8u value = occ_grid->GetValue(karto::Vector2<kt_int32s>(x, y));
00605 
00606       switch (value)
00607       {
00608         case karto::GridStates_Unknown:
00609           map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = -1;
00610           break;
00611         case karto::GridStates_Occupied:
00612           map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = 100;
00613           break;
00614         case karto::GridStates_Free:
00615           map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = 0;
00616           break;
00617         default:
00618           ROS_WARN("Encountered unknown cell value at %d, %d", x, y);
00619           break;
00620       }
00621     }
00622   }
00623   
00624   // Set the header information on the map
00625   map_.map.header.stamp = ros::Time::now();
00626   map_.map.header.frame_id = map_frame_;
00627 
00628   sst_.publish(map_.map);
00629   sstm_.publish(map_.map.info);
00630 
00631   delete occ_grid;
00632 
00633   return true;
00634 }
00635 
00636 bool
00637 SlamKarto::addScan(karto::LaserRangeFinder* laser,
00638                    const sensor_msgs::LaserScan::ConstPtr& scan, 
00639                    karto::Pose2& karto_pose)
00640 {
00641   if(!getOdomPose(karto_pose, scan->header.stamp))
00642      return false;
00643   
00644   // Create a vector of doubles for karto
00645   std::vector<kt_double> readings;
00646 
00647   if (lasers_inverted_[scan->header.frame_id]) {
00648     for(std::vector<float>::const_reverse_iterator it = scan->ranges.rbegin();
00649       it != scan->ranges.rend();
00650       ++it)
00651     {
00652       readings.push_back(*it);
00653     }
00654   } else {
00655     for(std::vector<float>::const_iterator it = scan->ranges.begin();
00656       it != scan->ranges.end();
00657       ++it)
00658     {
00659       readings.push_back(*it);
00660     }
00661   }
00662   
00663   // create localized range scan
00664   karto::LocalizedRangeScan* range_scan = 
00665     new karto::LocalizedRangeScan(laser->GetName(), readings);
00666   range_scan->SetOdometricPose(karto_pose);
00667   range_scan->SetCorrectedPose(karto_pose);
00668 
00669   // Add the localized range scan to the mapper
00670   bool processed;
00671   if((processed = mapper_->Process(range_scan)))
00672   {
00673     //std::cout << "Pose: " << range_scan->GetOdometricPose() << " Corrected Pose: " << range_scan->GetCorrectedPose() << std::endl;
00674     
00675     karto::Pose2 corrected_pose = range_scan->GetCorrectedPose();
00676 
00677     // Compute the map->odom transform
00678     tf::Stamped<tf::Pose> odom_to_map;
00679     try
00680     {
00681       tf_.transformPose(odom_frame_,tf::Stamped<tf::Pose> (tf::Transform(tf::createQuaternionFromRPY(0, 0, corrected_pose.GetHeading()),
00682                                                                     tf::Vector3(corrected_pose.GetX(), corrected_pose.GetY(), 0.0)).inverse(),
00683                                                                     scan->header.stamp, base_frame_),odom_to_map);
00684     }
00685     catch(tf::TransformException e)
00686     {
00687       ROS_ERROR("Transform from base_link to odom failed\n");
00688       odom_to_map.setIdentity();
00689     }
00690 
00691     map_to_odom_mutex_.lock();
00692     map_to_odom_ = tf::Transform(tf::Quaternion( odom_to_map.getRotation() ),
00693                                  tf::Point(      odom_to_map.getOrigin() ) ).inverse();
00694     map_to_odom_mutex_.unlock();
00695 
00696 
00697     // Add the localized range scan to the dataset (for memory management)
00698     dataset_->Add(range_scan);
00699   }
00700   else
00701     delete range_scan;
00702 
00703   return processed;
00704 }
00705 
00706 bool 
00707 SlamKarto::mapCallback(nav_msgs::GetMap::Request  &req,
00708                        nav_msgs::GetMap::Response &res)
00709 {
00710   boost::mutex::scoped_lock lock(map_mutex_);
00711   if(got_map_ && map_.map.info.width && map_.map.info.height)
00712   {
00713     res = map_;
00714     return true;
00715   }
00716   else
00717     return false;
00718 }
00719 
00720 int
00721 main(int argc, char** argv)
00722 {
00723   ros::init(argc, argv, "slam_karto");
00724 
00725   SlamKarto kn;
00726 
00727   ros::spin();
00728 
00729   return 0;
00730 }


slam_karto
Author(s): Brian Gerkey
autogenerated on Sat Jun 8 2019 19:23:06