loop_closure.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  *
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2008, Willow Garage, Inc.
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/or other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of the Willow Garage nor the names of its
00019  *     contributors may be used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
00034  *
00035  *********************************************************************/
00036 
00037 
00038 /*
00039  * Implement the loop-closure exploration algorithm, from Stachniss, et
00040  * al., IROS 2004.
00041  */
00042 
00043 #include <float.h>
00044 
00045 #include <ros/console.h>
00046 #include <explore/loop_closure.h>
00047 #include <visualization_msgs/Marker.h>
00048 #include <geometry_msgs/Point.h>
00049 
00050 using namespace explore;
00051 
00052 LoopClosure::LoopClosure(double addition_dist_min, 
00053                          double loop_dist_min, 
00054                          double loop_dist_max, 
00055                          double slam_entropy_max_,
00056                          double graph_update_frequency, 
00057                          actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction>& move_base_client,
00058                          costmap_2d::Costmap2DROS& costmap,
00059                          boost::mutex& control_mutex) :
00060         curr_node_(NULL), 
00061         addition_dist_min_(addition_dist_min), 
00062         loop_dist_min_(loop_dist_min),
00063         loop_dist_max_(loop_dist_max),
00064         slam_entropy_max_(slam_entropy_max_),
00065         graph_update_frequency_(graph_update_frequency),
00066         move_base_client_(move_base_client),
00067         nh_(),
00068         marker_id_(0),
00069         costmap_(costmap),
00070         control_mutex_(control_mutex),
00071         planner_(NULL),
00072         slam_entropy_(0.0),
00073         slam_entropy_time_(ros::Time::now().toSec())
00074 {
00075   marker_publisher_ = nh_.advertise<visualization_msgs::MarkerArray>("visualization_marker_array",1);
00076   entropy_subscriber_ = nh_.subscribe("slam_entropy", 1, &LoopClosure::entropyCallback, this);
00077   planner_ = new navfn::NavfnROS(std::string("loop_closure_planner"), &costmap_);
00078 }
00079 
00080 LoopClosure::~LoopClosure()
00081 {
00082   delete planner_;
00083 }
00084 
00085 void
00086 LoopClosure::entropyCallback(const std_msgs::Float64::ConstPtr& entropy)
00087 {
00088   slam_entropy_ = entropy->data;
00089   slam_entropy_time_ = ros::Time::now().toSec();
00090   ROS_DEBUG("Entropy is: %f (%f)", slam_entropy_, slam_entropy_max_);
00091 }
00092 
00093 void 
00094 LoopClosure::updateGraph(const tf::Pose& pose)
00095 {
00096   // Try to add a node
00097   addNode(pose);
00098 
00099   double time = ros::Time::now().toSec();
00100   if ((time - slam_entropy_time_) > 5.0f) {
00101     ROS_WARN("Entropy has not been updated. Loop closure opportunities may be ignored.");
00102     slam_entropy_time_ = time;
00103   }
00104 
00105   // Look for an opportunity to close a loop
00106   if(slam_entropy_ > 0.0 && slam_entropy_ > slam_entropy_max_)
00107   {
00108     ROS_DEBUG("Entropy is high enough (%.3f > %.3f); checking for loop closure opportunities",
00109               slam_entropy_, slam_entropy_max_);
00110     std::vector<GraphNode*> candidates;
00111     if(checkLoopClosure(pose, candidates))
00112     {
00113       // We found some. Time to control the robot
00114       control_mutex_.lock();
00115       // Control the robot
00116       ROS_INFO("Taking control of the robot for loop closure goals.");
00117 
00118       double min_path_length = DBL_MAX;
00119       GraphNode* entry_point = candidates[0];
00120 
00121       // Take the candidate node that is closest to our current position as the entry point
00122       for(unsigned int i = 0; i < candidates.size(); ++i){
00123         if(candidates[i]->path_length_ < min_path_length){
00124           entry_point = candidates[i];
00125           min_path_length = candidates[i]->path_length_;
00126         }
00127       }
00128       
00129       // We may end up going to several nodes; the first one is the entry
00130       // point.
00131       GraphNode* curr_target = entry_point;
00132 
00133       //the node that we'll connect to the target
00134       GraphNode* connect_node = curr_node_;
00135       
00136       move_base_msgs::MoveBaseGoal goal;
00137       goal.target_pose.header.frame_id = "map";
00138       goal.target_pose.header.stamp = ros::Time::now();
00139       // We retrace our steps as long as entropy is higher than what it was
00140       // last time we were here.
00141       do
00142       {
00143         ROS_INFO("Loop closure: Sending the robot to %.3f %.3f (%d)",
00144                  curr_target->pose_.getOrigin().x(),
00145                  curr_target->pose_.getOrigin().y(),
00146                  curr_target->id_);
00147         // TODO: pick a better orientation on the goal pose
00148         tf::poseTFToMsg(curr_target->pose_, goal.target_pose.pose);
00149         move_base_client_.sendGoal(goal);
00150         ros::Rate r(graph_update_frequency_);
00151 
00152 
00153         while(!move_base_client_.getState().isDone()){
00154           //get the current pose of the robot and check if
00155           tf::Stamped<tf::Pose> robot_pose;
00156           costmap_.getRobotPose(robot_pose);
00157 
00158           addNode(robot_pose);
00159 
00160           // Compute shortest distances from current node to all nodes.  Distances
00161           // are stored in nodes themselves.
00162           dijkstra(curr_node_->id_);
00163 
00164           //now we'll check if we are still far enough topologically from the loop closure target
00165           //if we are, we'll update the node that we'll connect with the loop closure
00166           if(curr_target->dijkstra_d_ > loop_dist_max_){
00167             connect_node = curr_node_;
00168           }
00169 
00170           visualizeGraph();
00171           r.sleep();
00172         }
00173         // TODO: inspect the result
00174 
00175         // Now go to the node that we went to last time we were here
00176         curr_target = curr_target->next_;
00177       } while((curr_target != NULL) && (slam_entropy_ > entry_point->slam_entropy_));
00178 
00179       // We need to connect the last node we added to the entry point in order to log that we've closed the loop
00180       if(connect_node){
00181         graph_[connect_node->id_].push_back(entry_point->id_);
00182         graph_[entry_point->id_].push_back(connect_node->id_);
00183         ROS_INFO("Adding edge from connector node to entry point");
00184       }
00185 
00186       control_mutex_.unlock();
00187       ROS_INFO("Entropy threshold satisfied (%.3f <= %.3f); loop closure terminated",
00188                slam_entropy_, entry_point->slam_entropy_);
00189     }
00190   }
00191 
00192   //actually display the graph in rviz
00193   visualizeGraph();
00194 }
00195 
00196 void 
00197 LoopClosure::addNode(const tf::Pose& pose)
00198 {
00199   bool add = false;
00200   // If there are no nodes yet, add one now
00201   if(curr_node_ == NULL)
00202     add = true;
00203   else
00204   {
00205     geometry_msgs::PoseStamped planner_start;
00206     geometry_msgs::Pose temp_pose;
00207     tf::poseTFToMsg(pose, temp_pose);
00208     planner_start.header.stamp = ros::Time::now();
00209     planner_start.header.frame_id = costmap_.getGlobalFrameID();
00210     planner_start.pose = temp_pose;
00211 
00212     //we'll only compute the potential once from our position
00213     planner_->computePotential(planner_start.pose.position);
00214 
00215     add = true;
00216     
00217     // How far are we from the closest node?
00218     double mind = DBL_MAX;
00219     GraphNode* closest_node = NULL;
00220     for(int i = nodes_.size() - 1; i >= 0; --i)
00221     {
00222       GraphNode* index_node = nodes_[i];
00223 
00224       //we need to convert from tf to message types for the planner
00225       geometry_msgs::PoseStamped planner_end;
00226 
00227       tf::poseTFToMsg(index_node->pose_, temp_pose);
00228       planner_end.header.stamp = ros::Time::now();
00229       planner_end.header.frame_id = costmap_.getGlobalFrameID();
00230       planner_end.pose = temp_pose;
00231 
00232       //now we'll make a plan from one point to the other
00233       std::vector<geometry_msgs::PoseStamped> plan;
00234       bool valid_plan = planner_->getPlanFromPotential(planner_end, plan) && !plan.empty();
00235 
00236       // Graph distance check satisfied; compute map distance
00237       if(valid_plan){
00238         double path_length = 0.0;
00239 
00240         //compute the path length
00241         if(plan.size() > 1){
00242           //double dist = distance(plan[0], plan[1]);
00243           //path_length = dist * (plan.size() - 1);
00244 
00245           for(unsigned int j = 0; j < plan.size() - 1; ++j){
00246             double dist = distance(plan[j], plan[j + 1]);
00247             path_length += dist;
00248           }
00249 
00250           if(path_length < mind){
00251             mind = path_length;
00252             closest_node = index_node;
00253 
00254             //we'll also update our current node here because we always want to add ourselves as a node to the 
00255             //closest node
00256             curr_node_ = closest_node;
00257           }
00258         }
00259 
00260       }
00261     }
00262     if(mind < addition_dist_min_)
00263     {
00264       // We found a close-enough node, so we won't add one, unless we lost visibility
00265       add = false;
00266 
00267       /*
00268       for(int i = nodes_.size() - 1; i >= 0; --i)
00269       {
00270         GraphNode* index_node = nodes_[i];
00271 
00272         //we also need to check if we have visibility from our current node to the previous node
00273         bool is_visible = true;
00274         VisibilityChecker checker(visibility_map.getCharMap(), is_visible);
00275 
00276         //get map coordinates
00277         bool in_bounds = true;
00278         unsigned int x0, y0;
00279         if(!visibility_map.worldToMap(pose.getOrigin().x(), pose.getOrigin().y(), x0, y0)){
00280           ROS_WARN("Attempting to check visibility from a point off the map... this should never happen");
00281           in_bounds = false;
00282         }
00283 
00284         unsigned int x1, y1;
00285         if(!visibility_map.worldToMap(index_node->pose_.getOrigin().x(), index_node->pose_.getOrigin().y(), x1, y1)){
00286           ROS_WARN("Attempting to check visibility to a point off the map... this should never happen");
00287           in_bounds = false;
00288         }
00289 
00290         if(in_bounds){
00291           //raytrace a line with our visibility checker
00292           raytraceLine(checker, x0, y0, x1, y1, visibility_map.getSizeInCellsX());
00293 
00294           //check if we have visibility to the node
00295           if(is_visible){
00296             add = false;
00297             break;
00298           }
00299         }
00300       }
00301       */
00302     
00303       
00304       // Update current node to be this one.
00305       curr_node_ = closest_node;
00306 
00307       // Store the current slam entropy; we compare to it later to decide
00308       // to terminate loop closure.
00309       if(slam_entropy_ > 0.0)
00310         curr_node_->slam_entropy_ = slam_entropy_;
00311     }
00312   }
00313 
00314   if(add)
00315   {
00316     ROS_INFO("Adding node at %.3f, %.3f", pose.getOrigin().x(), pose.getOrigin().y());
00317     GraphNode* n = new GraphNode(nodes_.size(), pose);
00318     nodes_.push_back(n);
00319     // Add an empty adjacency list for the new node
00320     std::vector<int> edges;
00321     graph_.push_back(edges);
00322 
00323     if(curr_node_)
00324     {
00325       // Update both adjacency lists
00326       graph_[curr_node_->id_].push_back(n->id_);
00327       graph_[n->id_].push_back(curr_node_->id_);
00328       curr_node_->next_ = n;
00329     }
00330     curr_node_ = n;
00331 
00332     // Store the current slam entropy; we compare to it later to decide
00333     // to terminate loop closure.
00334     if(slam_entropy_ > 0.0)
00335       curr_node_->slam_entropy_ = slam_entropy_;
00336   }
00337 }
00338 
00339 bool
00340 LoopClosure::checkLoopClosure(const tf::Pose& pose, std::vector<GraphNode*>& candidates)
00341 {
00342   bool ret = false;
00343 
00344   if(curr_node_ == NULL || nodes_.size() < 2)
00345     return ret;
00346 
00347   // Compute shortest distances from current node to all nodes.  Distances
00348   // are stored in nodes themselves.
00349   dijkstra(curr_node_->id_);
00350 
00351   //we'll only compute the potential once
00352   geometry_msgs::Pose temp_pose;
00353   geometry_msgs::PoseStamped planner_start;
00354 
00355   tf::poseTFToMsg(pose, temp_pose);
00356   planner_start.header.stamp = ros::Time::now();
00357   planner_start.header.frame_id = costmap_.getGlobalFrameID();
00358   planner_start.pose = temp_pose;
00359 
00360   planner_->computePotential(planner_start.pose.position);
00361 
00362   //get a copy of the costmap that we can do raytracing on
00363   costmap_2d::Costmap2D visibility_map;
00364   costmap_.getCostmapCopy(visibility_map);
00365 
00366   for(std::vector<GraphNode*>::iterator it = nodes_.begin();
00367       it != nodes_.end();
00368       ++it)
00369   {
00370 
00371     ROS_DEBUG("Checking %d (%.3f, %.3f)", (*it)->id_, (*it)->pose_.getOrigin().x(), (*it)->pose_.getOrigin().y());
00372     ROS_DEBUG("Graph distance is %.3f (threshold is %.3f)", (*it)->dijkstra_d_, loop_dist_max_);
00373 
00374     if((*it)->dijkstra_d_ > loop_dist_max_)
00375     {
00376       ROS_DEBUG("graph dist check satisfied");
00377       //we need to convert from tf to message types for the planner
00378       geometry_msgs::PoseStamped planner_end;
00379 
00380       tf::poseTFToMsg((*it)->pose_, temp_pose);
00381       planner_end.header.stamp = ros::Time::now();
00382       planner_end.header.frame_id = costmap_.getGlobalFrameID();
00383       planner_end.pose = temp_pose;
00384 
00385       //now we'll make a plan from one point to the other
00386       std::vector<geometry_msgs::PoseStamped> plan;
00387       bool valid_plan = planner_->getPlanFromPotential(planner_end, plan) && !plan.empty();
00388 
00389       // Graph distance check satisfied; compute map distance
00390       if(valid_plan){
00391         double path_length = 0.0;
00392 
00393         //compute the path length
00394         if(plan.size() > 1){
00395           //double dist = distance(plan[0], plan[1]);
00396           //path_length = dist * (plan.size() - 1);
00397 
00398           for(unsigned int j = 0; j < plan.size() - 1; ++j){
00399             double dist = distance(plan[j], plan[j + 1]);
00400             path_length += dist;
00401           }
00402 
00403           ROS_DEBUG("path_length: %.3f (threshold is %.3f)", path_length, loop_dist_min_);
00404           if(path_length < loop_dist_min_)
00405           {
00406             //also check that there is visibility from one node to the other
00407             bool is_visible = true;
00408             VisibilityChecker checker(visibility_map.getCharMap(), is_visible);
00409 
00410             //get map coordinates
00411             bool in_bounds = true;
00412             unsigned int x0, y0;
00413             if(!visibility_map.worldToMap(pose.getOrigin().x(), pose.getOrigin().y(), x0, y0)){
00414               ROS_WARN("Attempting to check visibility from a point off the map... this should never happen");
00415               in_bounds = false;
00416             }
00417 
00418             unsigned int x1, y1;
00419             if(!visibility_map.worldToMap((*it)->pose_.getOrigin().x(), (*it)->pose_.getOrigin().y(), x1, y1)){
00420               ROS_WARN("Attempting to check visibility to a point off the map... this should never happen");
00421               in_bounds = false;
00422             }
00423 
00424             if(in_bounds){
00425               //raytrace a line with our visibility checker
00426               raytraceLine(checker, x0, y0, x1, y1, visibility_map.getSizeInCellsX());
00427 
00428               //check if we have visibility to the node
00429               if(is_visible){
00430                 //update the path distance on the node we're about to push back
00431                 (*it)->path_length_ = path_length;
00432                 // Both checks satisfied
00433                 candidates.push_back(*it);
00434                 ROS_INFO("added loop closure candidate %d (%.3f, %.3f)", 
00435                     (*it)->id_, (*it)->pose_.getOrigin().x(), (*it)->pose_.getOrigin().y());
00436                 ret = true;
00437               }
00438             }
00439           }
00440         }
00441       }
00442     }
00443   }
00444 
00445   return ret;
00446 }
00447 
00448 // Poor-man's (inefficient) dijkstra implementation.
00449 void
00450 LoopClosure::dijkstra(int source)
00451 {
00452   std::list<GraphNode*> Q;
00453   for(std::vector<GraphNode*>::iterator it = nodes_.begin();
00454       it != nodes_.end();
00455       ++it)
00456   {
00457     if((*it)->id_ == source)
00458       (*it)->dijkstra_d_ = 0.0;
00459     else
00460       (*it)->dijkstra_d_ = DBL_MAX;
00461     Q.push_back(*it);
00462   }
00463   while(!Q.empty())
00464   {
00465     std::list<GraphNode*>::iterator it = Q.end();
00466     for(std::list<GraphNode*>::iterator iit = Q.begin();
00467         iit != Q.end();
00468         ++iit)
00469     {
00470       if((it == Q.end()) || ((*iit)->dijkstra_d_ < (*it)->dijkstra_d_))
00471       {
00472         it = iit;
00473       }
00474     }
00475     GraphNode* n = *it;
00476     if(n->dijkstra_d_ == DBL_MAX)
00477       break;
00478     Q.erase(it);
00479 
00480     for(std::vector<int>::iterator git = graph_[n->id_].begin();
00481         git != graph_[n->id_].end();
00482         ++git)
00483     {
00484       GraphNode* nn = nodes_[*git];
00485       double d = (n->pose_.getOrigin() - nn->pose_.getOrigin()).length();
00486       double alt = n->dijkstra_d_ + d;
00487       if(alt < nn->dijkstra_d_)
00488       {
00489         nn->dijkstra_d_ = alt;
00490       }
00491     }
00492   }
00493 }
00494 
00495 void LoopClosure::visualizeNode(const tf::Pose& pose, visualization_msgs::MarkerArray& markers)
00496 {
00497   visualization_msgs::Marker marker;
00498   marker.header.frame_id = "map";
00499   marker.header.stamp = ros::Time::now();
00500   marker.action = visualization_msgs::Marker::ADD;
00501   marker.ns = "loop_closure";
00502   marker.id = marker_id_++;
00503   marker.type = visualization_msgs::Marker::SPHERE;
00504   marker.pose.position.x = pose.getOrigin().x();
00505   marker.pose.position.y = pose.getOrigin().y();
00506   marker.scale.x = 0.5;
00507   marker.scale.y = 0.5;
00508   marker.scale.z = 0.5;
00509   marker.color.a = 1.0;
00510   marker.color.r = 1.0;
00511   marker.color.g = 0.0;
00512   marker.color.b = 0.0;
00513   //marker.lifetime = ros::Duration(5);
00514   markers.markers.push_back(marker);
00515   //marker_publisher_.publish(marker);
00516 }
00517 
00518 void LoopClosure::visualizeEdge(const tf::Pose& pose1, const tf::Pose& pose2, visualization_msgs::MarkerArray& markers)
00519 {
00520   visualization_msgs::Marker marker;
00521   marker.header.frame_id = "map";
00522   marker.header.stamp = ros::Time::now();
00523   marker.action = visualization_msgs::Marker::ADD;
00524   marker.ns = "loop_closure";
00525   marker.id = marker_id_++;
00526   marker.type = visualization_msgs::Marker::LINE_STRIP;
00527   geometry_msgs::Point p;
00528   p.x = pose1.getOrigin().x();
00529   p.y = pose1.getOrigin().y();
00530   marker.points.push_back(p);
00531   p.x = pose2.getOrigin().x();
00532   p.y = pose2.getOrigin().y();
00533   marker.points.push_back(p);
00534   marker.scale.x = 0.25;
00535   marker.scale.y = 0.25;
00536   marker.scale.z = 0.25;
00537   marker.color.a = 1.0;
00538   marker.color.r = 1.0;
00539   marker.color.g = 0.0;
00540   marker.color.b = 0.0;
00541   //marker.lifetime = ros::Duration(5);
00542   markers.markers.push_back(marker);
00543   //marker_publisher_.publish(marker);
00544 }
00545 
00546 void LoopClosure::visualizeGraph()
00547 {
00548   visualization_msgs::MarkerArray markers;
00549   marker_id_ = 0;
00550   //first we'll go through each node in the graph and  add a marker for it
00551   for(unsigned int i = 0; i < nodes_.size(); ++i){
00552     visualizeNode(nodes_[i]->pose_, markers);
00553   }
00554 
00555   //we'll also add all the edges in the graph... there may be duplicates here, but that should be ok
00556   for(unsigned int i = 0; i < graph_.size(); ++i){
00557     for(unsigned int j = 0; j < graph_[i].size(); ++j){
00558       visualizeEdge(nodes_[i]->pose_, nodes_[graph_[i][j]]->pose_, markers);
00559     }
00560   }
00561 
00562   marker_publisher_.publish(markers);
00563 }
00564 
00565 


explore
Author(s): Charles DuHadway (maintained by Benjamin Pitzer)
autogenerated on Sun Oct 5 2014 23:56:48