loop_closure.h
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 #ifndef EXPLORE_LOOP_CLOSURE_H
00038 #define EXPLORE_LOOP_CLOSURE_H
00039 
00040 
00041 /*
00042  * Implement the loop-closure exploration algorithm, from Stachniss, et
00043  * al., IROS 2004.
00044  */
00045 
00046 #include <tf/transform_datatypes.h>
00047 #include <actionlib/client/simple_action_client.h>
00048 #include <move_base_msgs/MoveBaseAction.h>
00049 #include <costmap_2d/costmap_2d_ros.h>
00050 #include <std_msgs/Float64.h>
00051 #include <navfn/navfn_ros.h>
00052 
00053 #include <ros/node_handle.h>
00054 
00055 #include <boost/thread/mutex.hpp>
00056 #include <boost/function.hpp>
00057 
00058 #include <visualization_msgs/MarkerArray.h>
00059 
00060 namespace explore
00061 {
00062 
00063 class GraphNode
00064 {
00065   public:
00066     GraphNode(int id, const tf::Pose& pose) : 
00067             id_(id), pose_(pose), next_(NULL), dijkstra_d_(0.0), slam_entropy_(0.0), path_length_(DBL_MAX) {}
00068 
00069     // id of this node in the graph
00070     int id_;
00071     // Pose of this node, in map coords
00072     tf::Pose pose_;
00073     // The node to which the robot went after this one, last time it was
00074     // here.
00075     GraphNode* next_;
00076     // Dijkstra distance
00077     double dijkstra_d_;
00078     // SLAM entropy last time we were here
00079     double slam_entropy_;
00080     //the last path distance computed for the node
00081     double path_length_;
00082 };
00083 
00084 class LoopClosure
00085 {
00086   public:
00087     LoopClosure(double addition_dist_min, 
00088                 double loop_dist_min, 
00089                 double loop_dist_max, 
00090                 double slam_entropy_max_,
00091                 double graph_update_frequency,
00092                 actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction>& move_base_client,
00093                 costmap_2d::Costmap2DROS& costmap,
00094                 boost::mutex& control_mutex);
00095     ~LoopClosure();
00096 
00097     // Call this periodically to let loop closure update its graph.  It may
00098     // take control of the robot, and not return until it's done.
00099     // @param pose : current robot pose
00100     void updateGraph(const tf::Pose& pose);
00101 
00102 
00103   private:
00104     // Consider adding a node (if distance and visibility constraints are
00105     // satisfied)
00106     void addNode(const tf::Pose& pose);
00107     bool checkLoopClosure(const tf::Pose& pose, std::vector<GraphNode*>& candidates);
00108     void dijkstra(int source);
00109     void entropyCallback(const std_msgs::Float64::ConstPtr& entropy);
00110 
00111     void visualizeNode(const tf::Pose& pose, visualization_msgs::MarkerArray& markers);
00112     void visualizeEdge(const tf::Pose& pose1, const tf::Pose& pose2, visualization_msgs::MarkerArray& markers);
00113     void visualizeGraph();
00114 
00115     inline double distance(const geometry_msgs::PoseStamped& p1, const geometry_msgs::PoseStamped& p2){
00116       double x1 = p1.pose.position.x;
00117       double x2 = p2.pose.position.x;
00118       double y1 = p1.pose.position.y;
00119       double y2 = p2.pose.position.y;
00120       return sqrt((x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2));
00121     }
00122 
00123     inline int sign(int x){
00124       return x > 0 ? 1.0 : -1.0;
00125     }
00126 
00136     template <class ActionType>
00137       inline void raytraceLine(ActionType at, unsigned int x0, unsigned int y0, unsigned int x1, unsigned int y1, unsigned int size_x, unsigned int max_length = UINT_MAX){
00138         int dx = x1 - x0;
00139         int dy = y1 - y0;
00140 
00141         unsigned int abs_dx = abs(dx);
00142         unsigned int abs_dy = abs(dy);
00143 
00144         int offset_dx = sign(dx);
00145         int offset_dy = sign(dy) * size_x;
00146 
00147         unsigned int offset = y0 * size_x + x0;
00148 
00149         //we need to chose how much to scale our dominant dimension, based on the maximum length of the line
00150         double dist = sqrt((x0 - x1) * (x0 - x1) + (y0 - y1) * (y0 - y1));
00151         double scale = std::min(1.0,  max_length / dist);
00152 
00153         //if x is dominant
00154         if(abs_dx >= abs_dy){
00155           int error_y = abs_dx / 2;
00156           bresenham2D(at, abs_dx, abs_dy, error_y, offset_dx, offset_dy, offset, (unsigned int)(scale * abs_dx));
00157           return;
00158         }
00159 
00160         //otherwise y is dominant
00161         int error_x = abs_dy / 2;
00162         bresenham2D(at, abs_dy, abs_dx, error_x, offset_dy, offset_dx, offset, (unsigned int)(scale * abs_dy));
00163 
00164       }
00165 
00169     template <class ActionType>
00170       inline void bresenham2D(ActionType at, unsigned int abs_da, unsigned int abs_db, int error_b, int offset_a, int offset_b, 
00171           unsigned int offset, unsigned int max_length){
00172         unsigned int end = std::min(max_length, abs_da);
00173         for(unsigned int i = 0; i < end; ++i){
00174           at(offset);
00175           offset += offset_a;
00176           error_b += abs_db;
00177           if((unsigned int)error_b >= abs_da){
00178             offset += offset_b;
00179             error_b -= abs_da;
00180           }
00181         }
00182         at(offset);
00183       }
00184 
00185     class VisibilityChecker {
00186       public:
00187         VisibilityChecker(const unsigned char* costmap, bool& is_visible) : costmap_(costmap), is_visible_(is_visible) {}
00188         inline void operator()(unsigned int offset){
00189           unsigned char cost = costmap_[offset];
00190           if(cost >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE){
00191             is_visible_ = false;
00192           }
00193         }
00194 
00195       public:
00196         const unsigned char* costmap_;
00197         bool& is_visible_;
00198     };
00199     
00200     //The node we're currently associated with
00201     GraphNode* curr_node_;
00202     // Travel distance required to drop a new node.
00203     double addition_dist_min_;
00204     // Minimum distance in map space to try loop closure
00205     double loop_dist_min_;
00206     // Maximum distance in graph space to try loop closure
00207     double loop_dist_max_;
00208     // Maximum slam entropy we'll tolerate before trying loop closure
00209     double slam_entropy_max_;
00210 
00211     //Freqeuncy at which to update the graph when actively closing a loop
00212     double graph_update_frequency_;
00213 
00214     // Action client for commanding move base
00215     actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction>& move_base_client_;
00216     ros::NodeHandle nh_;
00217     // List of nodes.
00218     std::vector<GraphNode*> nodes_;
00219     // Adjacency list representation of graph
00220     std::vector<std::vector<int> > graph_;
00221 
00222     ros::Subscriber entropy_subscriber_;
00223     ros::Publisher marker_publisher_;
00224     int marker_id_;
00225     costmap_2d::Costmap2DROS& costmap_;
00226     // Mutex to lock when commanding the robot
00227     boost::mutex& control_mutex_;
00228     navfn::NavfnROS* planner_;
00229     double slam_entropy_;
00230     double slam_entropy_time_;
00231 };
00232 
00233 }
00234 
00235 #endif


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