localization_monitor_node.cpp

Go to the documentation of this file.

00001 /*
00002  * Copyright (c) 2008, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  *
00029  */
00030 
00069 #include <flirtlib_ros/localization_monitor.h>
00070 #include <flirtlib_ros/conversions.h>
00071 #include <flirtlib_ros/common.h>
00072 #include <flirtlib_ros/ExecutiveState.h>
00073 #include <boost/thread.hpp>
00074 #include <ros/ros.h>
00075 #include <tf/transform_listener.h>
00076 #include <tf/exceptions.h>
00077 #include <geometry_msgs/PoseArray.h>
00078 #include <geometry_msgs/PoseWithCovarianceStamped.h>
00079 #include <mongo_ros/message_collection.h>
00080 #include <visualization_msgs/Marker.h>
00081 #include <move_base_msgs/MoveBaseActionResult.h>
00082 #include <std_srvs/Empty.h>
00083 
00084 
00085 namespace flirtlib_ros
00086 {
00087 
00088 namespace sm=sensor_msgs;
00089 namespace gm=geometry_msgs;
00090 namespace nm=nav_msgs;
00091 namespace mr=mongo_ros;
00092 namespace vm=visualization_msgs;
00093 namespace mbm=move_base_msgs;
00094 
00095 using std::string;
00096 using std::vector;
00097 using boost::format;
00098 using boost::shared_ptr;
00099 
00100 typedef boost::mutex::scoped_lock Lock;
00101 typedef mr::MessageWithMetadata<RefScanRos>::ConstPtr DBScan;
00102 typedef std::pair<InterestPoint*, InterestPoint*> Correspondence;
00103 typedef vector<Correspondence> Correspondences;
00104 typedef vector<RefScan> RefScans;
00105 typedef mr::MessageWithMetadata<nm::OccupancyGrid> MapWithMetadata;
00106 
00107 class Node
00108 {
00109 public:
00110 
00111   Node ();
00112 
00113   // Update given new laser scan.
00114   void scanCB (sm::LaserScan::ConstPtr scan);
00115   
00116   // Save the map the first time it comes in
00117   void mapCB (const nm::OccupancyGrid& g);
00118   
00119   // Used to count successful navigations, as an indicator that we're
00120   // well localized.
00121   void navCB (const mbm::MoveBaseActionResult& m);
00122   
00123   // Publish the executive state periodically
00124   void publishExecState (const ros::TimerEvent& e);
00125   
00126   // Reset the executive state
00127   bool resetExecState (std_srvs::Empty::Request& req,
00128                        std_srvs::Empty::Response& resp);
00129   
00130 private:
00131   
00132   // Update given that we're well localized
00133   void updateLocalized (sm::LaserScan::ConstPtr scan, const gm::Pose& p);
00134   
00135   // Update given that we're not well localized
00136   void updateUnlocalized (sm::LaserScan::ConstPtr scan);
00137   
00138   // Republish the set of reference scan poses.
00139   void publishRefScans () const;
00140   
00141   // Get saved occupancy grid from db
00142   MapWithMetadata::ConstPtr getSavedGrid() const;
00143   
00144   // Compensate for base movement between when the scan was taken and now
00145   tf::Transform compensateOdometry (const tf::Pose& sensor_pose,
00146                                     const string& frame, const ros::Time& t1,
00147                                     const ros::Time& t2);
00148   
00149   /************************************************************
00150    * Needed during init
00151    ************************************************************/
00152   
00153   boost::mutex mutex_;
00154   ros::NodeHandle nh_;
00155   
00156   /************************************************************
00157    * Parameters
00158    ************************************************************/ 
00159 
00160   // Minimum number of feature matches needed for succesful scan match
00161   unsigned min_num_matches_; 
00162   
00163   // Minimum number of navigations to happen before we'll save new scans
00164   unsigned min_successful_navs_;
00165   
00166   // Name of the database
00167   string db_name_;
00168   
00169   // Host where the db is running
00170   string db_host_;
00171   
00172   // Rate at which we'll consider new scans
00173   ros::Rate update_rate_;
00174   
00175   // Bound on badness to consider the robot well localized
00176   double badness_threshold_;
00177   
00178   // Odometric frame
00179   string odom_frame_;
00180   
00181   // Whether we continually publish localizations on initialpose 
00182   // (if false, we'll just publish one until reset)
00183   bool continual_publish_;
00184   
00185   /************************************************************
00186    * Mutable state
00187    ************************************************************/
00188   
00189   // Local copy of db scans. 
00190   vector<RefScan> ref_scans_;
00191   
00192   // Offset between laser and base frames
00193   tf::Transform laser_offset_;
00194   
00195   // Will we publish a localization next time we find a good match
00196   bool publishing_loc_;
00197   
00198   // How many successful navigations have we observed
00199   unsigned successful_navs_;
00200   
00201   // The last measurement of localization badness
00202   double localization_badness_;
00203   
00204   /************************************************************
00205    * Associated objects
00206    ************************************************************/
00207   
00208   // Flirtlib detector and descriptor
00209   FlirtlibFeatures features_;
00210 
00211   // Evaluates localization badness based on the scan and the static map
00212   shared_ptr<ScanPoseEvaluator> evaluator_;
00213   
00214   // Db collection of scans
00215   mr::MessageCollection<RefScanRos> scans_;
00216   
00217   tf::TransformListener tf_;
00218   ros::Subscriber scan_sub_;
00219   ros::Subscriber map_sub_;
00220   ros::Publisher marker_pub_;
00221   ros::Publisher ref_scan_pose_pub_;
00222   ros::Publisher pose_est_pub_;
00223   ros::Publisher match_pose_pub_;
00224   ros::Publisher exec_state_pub_;
00225   ros::Publisher pose_pub_;
00226   ros::Subscriber nav_sub_;
00227   ros::NodeHandle pnh_;
00228   ros::ServiceServer reset_srv_;
00229   ros::Timer pub_timer_;
00230 };
00231 
00232 // Reset to the executive state upon startup
00233 bool Node::resetExecState (std_srvs::Empty::Request& req,
00234                            std_srvs::Empty::Response& resp)
00235 {
00236   Lock l(mutex_);
00237   publishing_loc_ = true;
00238   successful_navs_ = 0;
00239   localization_badness_ = -1;
00240   return true;
00241 }
00242 
00243 
00244 // Constructor sets up ros comms and flirtlib objects, loads scans from db,
00245 // and figures out the offset between the base and the laser frames
00246 Node::Node () :
00247   min_num_matches_(getPrivateParam<int>("min_num_matches", 8)),
00248   min_successful_navs_(getPrivateParam<int>("min_successful_navs", 1)),
00249   db_name_(getPrivateParam<string>("db_name", "flirtlib_place_rec")),
00250   db_host_(getPrivateParam<string>("db_host", "localhost")),
00251   update_rate_(getPrivateParam<double>("update_rate", 1.0)),
00252   badness_threshold_(0.25),
00253   odom_frame_(getPrivateParam<string>("odom_frame", "odom_combined")),
00254   continual_publish_(getPrivateParam<bool>("continual_publish", false)),
00255   publishing_loc_(true),
00256   successful_navs_(0), localization_badness_(-1),
00257   features_(ros::NodeHandle("~")), scans_(db_name_, "scans", db_host_),
00258   scan_sub_(nh_.subscribe("base_scan", 10, &Node::scanCB, this)),
00259   map_sub_(nh_.subscribe("map", 10, &Node::mapCB, this)),
00260   marker_pub_(nh_.advertise<vm::Marker>("visualization_marker", 10)),
00261   ref_scan_pose_pub_(nh_.advertise<gm::PoseArray>("ref_scan_poses", 10, true)),
00262   pose_est_pub_(nh_.advertise<gm::PoseStamped>("pose_estimate", 1)),
00263   match_pose_pub_(nh_.advertise<gm::PoseArray>("match_poses", 1)),
00264   exec_state_pub_(nh_.advertise<ExecutiveState>("startup_loc_state", 1)),
00265   pose_pub_(nh_.advertise<gm::PoseWithCovarianceStamped>("initialpose", 1)),
00266   nav_sub_(nh_.subscribe("move_base/result", 1, &Node::navCB, this)), pnh_("~"),
00267   reset_srv_(pnh_.advertiseService("reset_state", &Node::resetExecState, this)),
00268   pub_timer_(nh_.createTimer(ros::Duration(1.0), &Node::publishExecState, this))
00269 {
00270   ROS_DEBUG_NAMED("init", "Waiting for laser offset");
00271   laser_offset_ = getLaserOffset(tf_);
00272 
00273   ROS_DEBUG_NAMED("init", "Loading scans from db");
00274   BOOST_FOREACH (const mr::MessageWithMetadata<RefScanRos>::ConstPtr m,
00275                  scans_.queryResults(mr::Query(), false)) 
00276     ref_scans_.push_back(fromRos(*m));
00277   publishRefScans();
00278   ROS_INFO("Localization monitor initialized with %zu scans", ref_scans_.size());
00279 }
00280 
00281 // Publish the executive state
00282 void Node::publishExecState (const ros::TimerEvent& e)
00283 {
00284   Lock l(mutex_);
00285   ExecutiveState s;
00286   s.publishing_localization = publishing_loc_;
00287   s.successful_navs = successful_navs_;
00288   s.localization_badness = localization_badness_;
00289   exec_state_pub_.publish(s);
00290 }
00291 
00292 // Update counter of successful navs observed
00293 void Node::navCB (const mbm::MoveBaseActionResult& res)
00294 {
00295   if (res.status.status == actionlib_msgs::GoalStatus::SUCCEEDED)
00296   {
00297     successful_navs_++;
00298     if (successful_navs_==min_successful_navs_)
00299     {
00300       ROS_INFO ("Have now observed %u successful navigations.  Can "
00301                 "now start saving scans", successful_navs_);
00302     }
00303   }
00304 }
00305 
00306 // Publish the poses of the reference scans (for visualization)
00307 void Node::publishRefScans () const
00308 {
00309   gm::PoseArray poses;
00310   poses.header.stamp = ros::Time::now();
00311   poses.header.frame_id = "/map";
00312   BOOST_FOREACH (const RefScan& s, ref_scans_) 
00313     poses.poses.push_back(s.pose);
00314   ref_scan_pose_pub_.publish(poses);
00315 }
00316 
00318 unsigned getSum (const nm::OccupancyGrid& g)
00319 {
00320   unsigned sum=0;
00321   BOOST_FOREACH (const unsigned val, g.data) 
00322     sum += val;
00323   return sum;
00324 }
00325 
00326 
00327 // Use the map to initialize the localization evaluator
00328 void Node::mapCB (const nm::OccupancyGrid& g)
00329 {
00330   const unsigned my_sum = getSum(g);
00331   mr::MessageCollection<nm::OccupancyGrid> coll(db_name_, "grid", db_host_);
00332   ROS_INFO("Received map");
00333   if (coll.count()>0)
00334   {
00335     MapWithMetadata::ConstPtr saved_map =
00336       coll.pullAllResults(mr::Query(), true)[0];
00337     const double res = saved_map->lookupDouble("resolution");
00338     const unsigned height = saved_map->lookupInt("height");
00339     const unsigned width = saved_map->lookupInt("width");
00340     const unsigned sum = saved_map->lookupInt("sum");
00341     if (my_sum!=sum || height!=g.info.height || width!=g.info.width ||
00342         fabs(res-g.info.resolution)>1e-3)
00343     {
00344       ROS_FATAL("Db map is %ux%u at %.4f m/cell, with sum %u, which "
00345                 "doesn't match current map of size %ux%u at %.4f m/cell"
00346                 " with sum %u", height, width, res, sum, g.info.height,
00347                 g.info.width, g.info.resolution, my_sum);
00348       ROS_BREAK();
00349     }
00350   }
00351   else
00352   {
00353     ROS_WARN("Didn't find an OccupancyGrid in the database.  Assuming this is "
00354              "a new db and saving current map with size %ux%u and sum %u",
00355              g.info.height, g.info.width, my_sum);
00356     mr::Metadata m = mr::Metadata().\
      append("height", g.info.height).append("width", g.info.width).\
      append("resolution", g.info.resolution).append("sum", my_sum);
00357     coll.insert(g, m);
00358   }
00359   ROS_INFO("Initializing scan evaluator distance field");
00360   Lock l(mutex_);
00361   evaluator_.reset(new ScanPoseEvaluator(g, badness_threshold_*1.5));
00362   ROS_INFO("Scan evaluator initialized");
00363 }
00364 
00365 
00366 
00367 // If we're not well localized, try to localize by matching against the scans in the db
00368   void Node::updateUnlocalized (sm::LaserScan::ConstPtr scan)
00369   {
00370     // Extract features from curent scan
00371     gm::Pose current = getCurrentPose(tf_, "base_footprint");
00372     ROS_INFO("Not well localized");
00373     InterestPointVec pts = features_.extractFeatures(scan);
00374     marker_pub_.publish(interestPointMarkers(pts, current));
00375       
00376     // Set up 
00377     gm::PoseArray match_poses;
00378     match_poses.header.frame_id = "/map";
00379     match_poses.header.stamp = ros::Time::now();
00380     unsigned best_num_matches = 0;
00381     gm::Pose best_pose;
00382   
00383     // Iterate over reference scans and match against each one, looking for the
00384     // one with the largest number of feature matches
00385     BOOST_FOREACH (const RefScan& ref_scan, ref_scans_) 
00386     {
00387       Correspondences matches;
00388       OrientedPoint2D trans;
00389       features_.ransac_->matchSets(ref_scan.raw_pts, pts, trans, matches);
00390       const unsigned num_matches = matches.size();
00391       if (num_matches > min_num_matches_) 
00392       {
00393         ROS_INFO_NAMED ("match", "Found %d matches with ref scan at "
00394                         "%.2f, %.2f, %.2f", num_matches,
00395                         ref_scan.pose.position.x, ref_scan.pose.position.y,
00396                         tf::getYaw(ref_scan.pose.orientation));
00397         match_poses.poses.push_back(ref_scan.pose);
00398         const gm::Pose laser_pose = transformPose(ref_scan.pose, trans);
00399         if (num_matches > best_num_matches)
00400         {
00401           best_num_matches = num_matches;
00402           best_pose = laser_pose;
00403         }
00404       }
00405     }
00406 
00407     // Only proceed if there are a sufficient number of feature matches
00408     if (best_num_matches<min_num_matches_)
00409       return;
00410 
00411     match_pose_pub_.publish(match_poses);
00412     const double badness = (*evaluator_)(*scan, best_pose);
00413     ROS_INFO ("Badness is %.2f", badness);
00414   
00415     // Do a further check of scan badness before committing to this
00416     // We'll always publish a visualization, but we'll only publish on the
00417     // initialpose topic once (so after that, this node will go into a state where
00418     // all it does is possibly save new scans).
00419     if (badness < badness_threshold_)
00420     {
00421       ROS_INFO("Found a good match");
00422     
00423       const ros::Time now = ros::Time::now();
00424       gm::PoseStamped estimated_pose;
00425       try {
00426         tf::Pose adjusted_pose(compensateOdometry(poseMsgToTf(best_pose),
00427                                                   scan->header.frame_id,
00428                                                   scan->header.stamp, now));
00429         tf::poseTFToMsg(adjusted_pose*laser_offset_, estimated_pose.pose);
00430       }
00431       catch (tf::TransformException& e)
00432       {
00433         ROS_INFO ("Discarding match due to tf exception: %s", e.what());
00434         return;
00435       }
00436     
00437 
00438       estimated_pose.header.frame_id = "/map";
00439       estimated_pose.header.stamp = now;
00440       pose_est_pub_.publish(estimated_pose);
00441     
00442       if (!continual_publish_)
00443         publishing_loc_ = false;
00444       gm::PoseWithCovarianceStamped initial_pose;
00445       initial_pose.header.frame_id = "/map";
00446       initial_pose.header.stamp = scan->header.stamp;
00447       initial_pose.pose.pose = estimated_pose.pose;
00448       pose_pub_.publish(initial_pose);
00449     }
00450   }
00451 
00454 tf::Transform Node::compensateOdometry (const tf::Pose& pose,
00455                                         const string& frame,
00456                                         const ros::Time& t1,
00457                                         const ros::Time& t2)
00458 {
00459   tf_.waitForTransform(odom_frame_, frame, t1, ros::Duration(0.1));
00460   tf_.waitForTransform(odom_frame_, frame, t2, ros::Duration(0.1));
00461 
00462   tf::StampedTransform current_odom, prev_odom;
00463   tf_.lookupTransform(odom_frame_, frame, t2, current_odom);
00464   tf_.lookupTransform(odom_frame_, frame, t1, prev_odom);
00465   tf::Transform current_to_prev = prev_odom.inverse()*current_odom;
00466   return pose*current_to_prev;
00467 }
00468                                         
00469 
00470 // Search the db for a nearby scan.  If none is found, and also we've
00471 // successfully navigated previously (as a further cue that we're well
00472 // localized), then add this scan to the db, and remove any
00473 // nearby old scans.
00474 void Node::updateLocalized (sm::LaserScan::ConstPtr scan,
00475                             const gm::Pose& current)
00476 {
00477   ROS_INFO("Well localized");
00478   const double DPOS = 0.7;
00479   const double DTHETA = 1;
00480   const double DT = 86400; // one day
00481   
00482   // Query the db for nearby scans
00483   const double t = ros::Time::now().toSec();
00484   const double x = current.position.x;
00485   const double x0 = x - DPOS;
00486   const double x1 = x + DPOS;
00487   const double y = current.position.y;
00488   const double y0 = y - DPOS;
00489   const double y1 = y + DPOS;
00490   const double th = tf::getYaw(current.orientation);
00491   const double th0 = th - DTHETA;
00492   const double th1 = th + DTHETA;
00493   const double min_time = t-DT;
00494   format f("{x : {$gt: %.4f, $lt: %.4f}, y : {$gt: %.4f, $lt: %.4f}, "
00495            "theta: {$gt: %.4f, $lt: %.4f}, creation_time: {$gt: %.8f} }");
00496   string str = (f % x0 % x1 % y0 % y1 % th0 % th1 % min_time).str();
00497   mongo::Query q = mongo::fromjson(str);
00498   vector<DBScan> scans = scans_.pullAllResults(q, true);
00499   
00500   // Possibly save this new scan
00501   if (scans.size()<1 && successful_navs_>=min_successful_navs_) 
00502   {
00503     // First remove old scans that are nearby
00504     format old("{x : {$gt: %.4f, $lt: %.4f}, y : {$gt: %.4f, $lt: %.4f}, "
00505                "theta: {$gt: %.4f, $lt: %.4f} }");
00506     const string old_query_str = (old % x0 % x1 % y0 % y1 % th0 % th1).str();
00507     const mongo::Query old_query = mongo::fromjson(old_query_str);
00508     const vector<DBScan> old_scans = scans_.pullAllResults(old_query, true);
00509     scans_.removeMessages(old_query);
00510     BOOST_FOREACH (const DBScan& old_scan, old_scans) 
00511     {
00512       ROS_INFO ("Removed old scan at (%.4f, %.4f, %.f) taken at %.4f",
00513                 old_scan->lookupDouble("x"), old_scan->lookupDouble("y"),
00514                 old_scan->lookupDouble("theta"),
00515                 old_scan->lookupDouble("creation_time"));
00516     }
00517 
00518     // Now add this new scan
00519     InterestPointVec pts = features_.extractFeatures(scan);
00520     sm::LaserScan::Ptr stripped_scan(new sm::LaserScan(*scan));
00521     stripped_scan->ranges.clear();
00522     stripped_scan->intensities.clear();
00523     RefScan ref_scan(stripped_scan, current, pts);
00524     ref_scans_.push_back(ref_scan);
00525     scans_.insert(toRos(ref_scan),
00526                   mr::Metadata("x", x, "y", y, "theta", th));
00527     ROS_DEBUG_NAMED ("save_scan", "Saved scan at %.2f, %.2f, %.2f", x, y, th);
00528     
00529     publishRefScans();
00530   }
00531 }
00532 
00533 
00534 // Given current scan and pose, update state.  If well localized, we'll 
00535 // consider adding this new scan to the db.  If not, we'll try to localize
00536 // using the db.
00537 void Node::scanCB (sm::LaserScan::ConstPtr scan)
00538 {
00539   // First, look up the sensor pose at the time of the scan
00540   const std::string frame = scan->header.frame_id;
00541   tf_.waitForTransform("/map", frame, scan->header.stamp, ros::Duration(0.5));
00542   tf::StampedTransform trans;
00543   try
00544   {
00545     tf_.lookupTransform("/map", frame, scan->header.stamp, trans);
00546   }
00547   catch (tf::TransformException& e)
00548   {
00549     ROS_INFO_STREAM ("Skipping scan due to tf exception " << e.what());
00550     return;
00551   }
00552   
00553   // Have we initialized the evaluator yet?
00554   {
00555   Lock l(mutex_);
00556   if (!evaluator_)
00557   {
00558     ROS_INFO_STREAM ("Skipping scan as evaluator not yet initialized");
00559     return;
00560   }
00561     
00562   // Evaluate how well this scan is localized
00563   const gm::Pose pose = tfTransformToPose(trans);
00564   const double dist = (*evaluator_)(*scan, pose);
00565   localization_badness_ = dist;
00566   ROS_INFO ("Localization badness is %.2f", dist);
00567   
00568   if (dist < badness_threshold_)
00569     updateLocalized(scan, pose);
00570   else if (publishing_loc_)
00571     updateUnlocalized(scan);
00572   
00573   }
00574   // Make this callback happen at a bounded rate
00575   update_rate_.sleep();
00576 }
00577 
00578 } // namespace
00579 
00580 int main (int argc, char** argv)
00581 {
00582   ros::init(argc, argv, "localization_monitor_node");
00583   flirtlib_ros::Node node;
00584   ros::spin();
00585   return 0;
00586 }
00587