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 "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   // Set solver to be used in loop closure
00164   solver_ = new SpaSolver();
00165   mapper_->SetScanSolver(solver_);
00166 }
00167 
00168 SlamKarto::~SlamKarto()
00169 {
00170   if(transform_thread_)
00171   {
00172     transform_thread_->join();
00173     delete transform_thread_;
00174   }
00175   if (scan_filter_)
00176     delete scan_filter_;
00177   if (scan_filter_sub_)
00178     delete scan_filter_sub_;
00179   if (solver_)
00180     delete solver_;
00181   if (mapper_)
00182     delete mapper_;
00183   if (dataset_)
00184     delete dataset_;
00185   // TODO: delete the pointers in the lasers_ map; not sure whether or not
00186   // I'm supposed to do that.
00187 }
00188 
00189 void 
00190 SlamKarto::publishLoop(double transform_publish_period)
00191 {
00192   if(transform_publish_period == 0)
00193     return;
00194 
00195   ros::Rate r(1.0 / transform_publish_period);
00196   while(ros::ok())
00197   {
00198     publishTransform();
00199     r.sleep();
00200   }
00201 }
00202 
00203 void 
00204 SlamKarto::publishTransform()
00205 {
00206   boost::mutex::scoped_lock(map_to_odom_mutex_);
00207   ros::Time tf_expiration = ros::Time::now() + ros::Duration(0.05);
00208   tfB_->sendTransform(tf::StampedTransform (map_to_odom_, ros::Time::now(), map_frame_, odom_frame_));
00209 }
00210 
00211 karto::LaserRangeFinder*
00212 SlamKarto::getLaser(const sensor_msgs::LaserScan::ConstPtr& scan)
00213 {
00214   // Check whether we know about this laser yet
00215   if(lasers_.find(scan->header.frame_id) == lasers_.end())
00216   {
00217     // New laser; need to create a Karto device for it.
00218 
00219     // Get the laser's pose, relative to base.
00220     tf::Stamped<tf::Pose> ident;
00221     tf::Stamped<btTransform> laser_pose;
00222     ident.setIdentity();
00223     ident.frame_id_ = scan->header.frame_id;
00224     ident.stamp_ = scan->header.stamp;
00225     try
00226     {
00227       tf_.transformPose(base_frame_, ident, laser_pose);
00228     }
00229     catch(tf::TransformException e)
00230     {
00231       ROS_WARN("Failed to compute laser pose, aborting initialization (%s)",
00232                e.what());
00233       return NULL;
00234     }
00235 
00236     double yaw = tf::getYaw(laser_pose.getRotation());
00237 
00238     ROS_INFO("laser %s's pose wrt base: %.3f %.3f %.3f",
00239              scan->header.frame_id.c_str(),
00240              laser_pose.getOrigin().x(),
00241              laser_pose.getOrigin().y(),
00242              yaw);
00243     // To account for lasers that are mounted upside-down, we determine the
00244     // min, max, and increment angles of the laser in the base frame.
00245     tf::Quaternion q;
00246     q.setRPY(0.0, 0.0, scan->angle_min);
00247     tf::Stamped<tf::Quaternion> min_q(q, scan->header.stamp,
00248                                       scan->header.frame_id);
00249     q.setRPY(0.0, 0.0, scan->angle_max);
00250     tf::Stamped<tf::Quaternion> max_q(q, scan->header.stamp,
00251                                       scan->header.frame_id);
00252     try
00253     {
00254       tf_.transformQuaternion(base_frame_, min_q, min_q);
00255       tf_.transformQuaternion(base_frame_, max_q, max_q);
00256     }
00257     catch(tf::TransformException& e)
00258     {
00259       ROS_WARN("Unable to transform min/max laser angles into base frame: %s",
00260                e.what());
00261       return false;
00262     }
00263 
00264     double angle_min = tf::getYaw(min_q);
00265     double angle_max = tf::getYaw(max_q);
00266     bool inverse =  lasers_inverted_[scan->header.frame_id] = angle_max < angle_min;
00267     if (inverse)
00268       ROS_INFO("laser is mounted upside-down");
00269 
00270 
00271     // Create a laser range finder device and copy in data from the first
00272     // scan
00273     std::string name = scan->header.frame_id;
00274     karto::LaserRangeFinder* laser = 
00275       karto::LaserRangeFinder::CreateLaserRangeFinder(karto::LaserRangeFinder_Custom, karto::Name(name));
00276     laser->SetOffsetPose(karto::Pose2(laser_pose.getOrigin().x(),
00277                                       laser_pose.getOrigin().y(),
00278                                       yaw));
00279     laser->SetMinimumRange(scan->range_min);
00280     laser->SetMaximumRange(scan->range_max);
00281     laser->SetMinimumAngle(scan->angle_min);
00282     laser->SetMaximumAngle(scan->angle_max);
00283     laser->SetAngularResolution(scan->angle_increment);
00284     // TODO: expose this, and many other parameters
00285     //laser_->SetRangeThreshold(12.0);
00286 
00287     // Store this laser device for later
00288     lasers_[scan->header.frame_id] = laser;
00289 
00290     // Add it to the dataset, which seems to be necessary
00291     dataset_->Add(laser);
00292   }
00293 
00294   return lasers_[scan->header.frame_id];
00295 }
00296 
00297 bool
00298 SlamKarto::getOdomPose(karto::Pose2& karto_pose, const ros::Time& t)
00299 {
00300   // Get the robot's pose
00301   tf::Stamped<tf::Pose> ident (btTransform(tf::createQuaternionFromRPY(0,0,0),
00302                                            btVector3(0,0,0)), t, base_frame_);
00303   tf::Stamped<btTransform> odom_pose;
00304   try
00305   {
00306     tf_.transformPose(odom_frame_, ident, odom_pose);
00307   }
00308   catch(tf::TransformException e)
00309   {
00310     ROS_WARN("Failed to compute odom pose, skipping scan (%s)", e.what());
00311     return false;
00312   }
00313   double yaw = tf::getYaw(odom_pose.getRotation());
00314 
00315   karto_pose = 
00316           karto::Pose2(odom_pose.getOrigin().x(),
00317                        odom_pose.getOrigin().y(),
00318                        yaw);
00319   return true;
00320 }
00321 
00322 void
00323 SlamKarto::publishGraphVisualization()
00324 {
00325   std::vector<float> graph;
00326   solver_->getGraph(graph);
00327 
00328   visualization_msgs::MarkerArray marray;
00329 
00330   visualization_msgs::Marker m;
00331   m.header.frame_id = "map";
00332   m.header.stamp = ros::Time::now();
00333   m.id = 0;
00334   m.ns = "karto";
00335   m.type = visualization_msgs::Marker::SPHERE;
00336   m.pose.position.x = 0.0;
00337   m.pose.position.y = 0.0;
00338   m.pose.position.z = 0.0;
00339   m.scale.x = 0.1;
00340   m.scale.y = 0.1;
00341   m.scale.z = 0.1;
00342   m.color.r = 1.0;
00343   m.color.g = 0;
00344   m.color.b = 0.0;
00345   m.color.a = 1.0;
00346   m.lifetime = ros::Duration(0);
00347 
00348   visualization_msgs::Marker edge;
00349   edge.header.frame_id = "map";
00350   edge.header.stamp = ros::Time::now();
00351   edge.action = visualization_msgs::Marker::ADD;
00352   edge.ns = "karto";
00353   edge.id = 0;
00354   edge.type = visualization_msgs::Marker::LINE_STRIP;
00355   edge.scale.x = 0.1;
00356   edge.scale.y = 0.1;
00357   edge.scale.z = 0.1;
00358   edge.color.a = 1.0;
00359   edge.color.r = 0.0;
00360   edge.color.g = 0.0;
00361   edge.color.b = 1.0;
00362 
00363   m.action = visualization_msgs::Marker::ADD;
00364   uint id = 0;
00365   for (uint i=0; i<graph.size()/2; i++) 
00366   {
00367     m.id = id;
00368     m.pose.position.x = graph[2*i];
00369     m.pose.position.y = graph[2*i+1];
00370     marray.markers.push_back(visualization_msgs::Marker(m));
00371     id++;
00372 
00373     if(i>0)
00374     {
00375       edge.points.clear();
00376 
00377       geometry_msgs::Point p;
00378       p.x = graph[2*(i-1)];
00379       p.y = graph[2*(i-1)+1];
00380       edge.points.push_back(p);
00381       p.x = graph[2*i];
00382       p.y = graph[2*i+1];
00383       edge.points.push_back(p);
00384       edge.id = id;
00385 
00386       marray.markers.push_back(visualization_msgs::Marker(edge));
00387       id++;
00388     }
00389   }
00390 
00391   m.action = visualization_msgs::Marker::DELETE;
00392   for (; id < marker_count_; id++) 
00393   {
00394     m.id = id;
00395     marray.markers.push_back(visualization_msgs::Marker(m));
00396   }
00397 
00398   marker_count_ = marray.markers.size();
00399 
00400   marker_publisher_.publish(marray);
00401 }
00402 
00403 void
00404 SlamKarto::laserCallback(const sensor_msgs::LaserScan::ConstPtr& scan)
00405 {
00406   laser_count_++;
00407   if ((laser_count_ % throttle_scans_) != 0)
00408     return;
00409 
00410   static ros::Time last_map_update(0,0);
00411 
00412   // Check whether we know about this laser yet
00413   karto::LaserRangeFinder* laser = getLaser(scan);
00414 
00415   if(!laser)
00416   {
00417     ROS_WARN("Failed to create laser device for %s; discarding scan",
00418              scan->header.frame_id.c_str());
00419     return;
00420   }
00421 
00422   karto::Pose2 odom_pose;
00423   if(addScan(laser, scan, odom_pose))
00424   {
00425     ROS_DEBUG("added scan at pose: %.3f %.3f %.3f", 
00426               odom_pose.GetX(),
00427               odom_pose.GetY(),
00428               odom_pose.GetHeading());
00429 
00430     publishGraphVisualization();
00431 
00432     if(!got_map_ || 
00433        (scan->header.stamp - last_map_update) > map_update_interval_)
00434     {
00435       if(updateMap())
00436       {
00437         last_map_update = scan->header.stamp;
00438         got_map_ = true;
00439         ROS_DEBUG("Updated the map");
00440       }
00441     }
00442   }
00443 }
00444 
00445 bool
00446 SlamKarto::updateMap()
00447 {
00448   boost::mutex::scoped_lock(map_mutex_);
00449 
00450   karto::OccupancyGrid* occ_grid = 
00451           karto::OccupancyGrid::CreateFromScans(mapper_->GetAllProcessedScans(), resolution_);
00452 
00453   if(!occ_grid)
00454     return false;
00455 
00456   if(!got_map_) {
00457     map_.map.info.resolution = resolution_;
00458     map_.map.info.origin.position.x = 0.0;
00459     map_.map.info.origin.position.y = 0.0;
00460     map_.map.info.origin.position.z = 0.0;
00461     map_.map.info.origin.orientation.x = 0.0;
00462     map_.map.info.origin.orientation.y = 0.0;
00463     map_.map.info.origin.orientation.z = 0.0;
00464     map_.map.info.origin.orientation.w = 1.0;
00465   } 
00466 
00467   // Translate to ROS format
00468   kt_int32s width = occ_grid->GetWidth();
00469   kt_int32s height = occ_grid->GetHeight();
00470   karto::Vector2<kt_double> offset = occ_grid->GetCoordinateConverter()->GetOffset();
00471 
00472   if(map_.map.info.width != (unsigned int) width || 
00473      map_.map.info.height != (unsigned int) height ||
00474      map_.map.info.origin.position.x != offset.GetX() ||
00475      map_.map.info.origin.position.y != offset.GetY())
00476   {
00477     map_.map.info.origin.position.x = offset.GetX();
00478     map_.map.info.origin.position.y = offset.GetY();
00479     map_.map.info.width = width;
00480     map_.map.info.height = height;
00481     map_.map.data.resize(map_.map.info.width * map_.map.info.height);
00482   }
00483 
00484   for (kt_int32s y=0; y<height; y++)
00485   {
00486     for (kt_int32s x=0; x<width; x++) 
00487     {
00488       // Getting the value at position x,y
00489       kt_int8u value = occ_grid->GetValue(karto::Vector2<kt_int32s>(x, y));
00490 
00491       switch (value)
00492       {
00493         case karto::GridStates_Unknown:
00494           map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = -1;
00495           break;
00496         case karto::GridStates_Occupied:
00497           map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = 100;
00498           break;
00499         case karto::GridStates_Free:
00500           map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = 0;
00501           break;
00502         default:
00503           ROS_WARN("Encountered unknown cell value at %d, %d", x, y);
00504           break;
00505       }
00506     }
00507   }
00508   
00509   // Set the header information on the map
00510   map_.map.header.stamp = ros::Time::now();
00511   map_.map.header.frame_id = map_frame_;
00512 
00513   sst_.publish(map_.map);
00514   sstm_.publish(map_.map.info);
00515 
00516   delete occ_grid;
00517 
00518   return true;
00519 }
00520 
00521 bool
00522 SlamKarto::addScan(karto::LaserRangeFinder* laser,
00523                    const sensor_msgs::LaserScan::ConstPtr& scan, 
00524                    karto::Pose2& karto_pose)
00525 {
00526   if(!getOdomPose(karto_pose, scan->header.stamp))
00527      return false;
00528   
00529   // Create a vector of doubles for karto
00530   std::vector<kt_double> readings;
00531 
00532   if (lasers_inverted_[scan->header.frame_id]) {
00533     for(std::vector<float>::const_reverse_iterator it = scan->ranges.rbegin();
00534       it != scan->ranges.rend();
00535       ++it)
00536     {
00537       readings.push_back(*it);
00538     }
00539   } else {
00540     for(std::vector<float>::const_iterator it = scan->ranges.begin();
00541       it != scan->ranges.end();
00542       ++it)
00543     {
00544       readings.push_back(*it);
00545     }
00546   }
00547   
00548   // create localized range scan
00549   karto::LocalizedRangeScan* range_scan = 
00550     new karto::LocalizedRangeScan(laser->GetName(), readings);
00551   range_scan->SetOdometricPose(karto_pose);
00552   range_scan->SetCorrectedPose(karto_pose);
00553 
00554   // Add the localized range scan to the mapper
00555   bool processed;
00556   if((processed = mapper_->Process(range_scan)))
00557   {
00558     //std::cout << "Pose: " << range_scan->GetOdometricPose() << " Corrected Pose: " << range_scan->GetCorrectedPose() << std::endl;
00559     
00560     karto::Pose2 corrected_pose = range_scan->GetCorrectedPose();
00561 
00562     // Compute the map->odom transform
00563     tf::Stamped<tf::Pose> odom_to_map;
00564     try
00565     {
00566       tf_.transformPose(odom_frame_,tf::Stamped<tf::Pose> (btTransform(tf::createQuaternionFromRPY(0, 0, corrected_pose.GetHeading()),
00567                                                                     btVector3(corrected_pose.GetX(), corrected_pose.GetY(), 0.0)).inverse(),
00568                                                                     scan->header.stamp, base_frame_),odom_to_map);
00569     }
00570     catch(tf::TransformException e)
00571     {
00572       ROS_ERROR("Transform from base_link to odom failed\n");
00573       odom_to_map.setIdentity();
00574     }
00575 
00576     map_to_odom_mutex_.lock();
00577     map_to_odom_ = tf::Transform(tf::Quaternion( odom_to_map.getRotation() ),
00578                                  tf::Point(      odom_to_map.getOrigin() ) ).inverse();
00579     map_to_odom_mutex_.unlock();
00580 
00581 
00582     // Add the localized range scan to the dataset (for memory management)
00583     dataset_->Add(range_scan);
00584   }
00585   else
00586     delete range_scan;
00587 
00588   return processed;
00589 }
00590 
00591 bool 
00592 SlamKarto::mapCallback(nav_msgs::GetMap::Request  &req,
00593                        nav_msgs::GetMap::Response &res)
00594 {
00595   boost::mutex::scoped_lock(map_mutex_);
00596   if(got_map_ && map_.map.info.width && map_.map.info.height)
00597   {
00598     res = map_;
00599     return true;
00600   }
00601   else
00602     return false;
00603 }
00604 
00605 int
00606 main(int argc, char** argv)
00607 {
00608   ros::init(argc, argv, "slam_karto");
00609 
00610   SlamKarto kn;
00611 
00612   ros::spin();
00613 
00614   return 0;
00615 }


karto
Author(s): SRI International (package maintained by Brian Gerkey)
autogenerated on Thu Jan 2 2014 12:02:39