graph_slam: graph_mapper.h Source 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 
00039 #ifndef GRAPH_SLAM_GRAPH_MAPPER_H
00040 #define GRAPH_SLAM_GRAPH_MAPPER_H
00041 
00042 #include <graph_slam/localization_buffer.h>
00043 #include <pose_graph/constraint_graph.h>
00044 #include <pose_graph/graph_db.h>
00045 #include <pose_graph/visualization.h>
00046 #include <pose_graph/diff_publisher.h>
00047 #include <graph_mapping_msgs/SaveGraph.h>
00048 #include <graph_mapping_msgs/GetPoses.h>
00049 #include <warehouse/warehouse_client.h>
00050 #include <tf/transform_listener.h>
00051 #include <tf/transform_broadcaster.h>
00052 #include <geometry_msgs/PoseWithCovarianceStamped.h>
00053 #include <ros/ros.h>
00054 #include <boost/thread.hpp>
00055 #include <boost/optional.hpp>
00056 
00057 
00058 namespace graph_slam
00059 {
00060 
00061 class Odometer;
00062 
00064 class GraphMapper
00065 {
00066 public:
00067 
00073   GraphMapper ();
00074 
00076   virtual ~GraphMapper ();
00077 
00078 protected:
00079 
00081   pose_graph::ConstraintGraph graphSnapshot () const;
00082 
00083   ros::NodeHandle param_nh_; // Needs to exist before parameters
00084   warehouse::WarehouseClient db_;
00085 
00086   /****************************************
00087    * Parameters
00088    ****************************************/
00089   
00090   const double angle_threshold_;
00091   const double pos_threshold_;
00092   const std::string base_frame_, fixed_frame_, optimization_frame_;
00093   const ros::Duration update_duration_;
00094   const ros::Duration constraint_wait_;
00095   const std::string optimization_algorithm_;
00096   const ros::Duration loc_transform_wait_;
00097   const ros::Duration ref_transform_timeout_;
00098   const bool add_new_nodes_; // If this is false, we'll just localize wrt existing graph
00099   
00100   /****************************************
00101    * State
00102    ****************************************/
00103 
00104   boost::shared_ptr<tf::TransformListener> tf_;
00105   pose_graph::ConstraintGraph graph_;
00106   LocalizationBuffer localizations_;
00107   bool initialized_;
00108   boost::optional<tf::StampedTransform> ref_transform_; // Todo: this no longer needs to be optional
00109   ros::Time last_node_addition_time_;
00110   bool optimize_flag_; // Does the graph need to be reoptimized
00111   pose_graph::CachedNodeMap<geometry_msgs::Pose> ff_poses_;
00112   warehouse::Collection<graph_mapping_msgs::ConstraintGraphMessage> saved_graph_;
00113   std::set<unsigned> last_optimized_comp_;
00114   pose_graph::NodePoseMap last_optimization_response_;
00115   
00116 
00117 private:
00118 
00119   /****************************************
00120    * Entry points
00121    ****************************************/
00122   
00123   void updateGraph (const ros::TimerEvent& e);
00124   void visualize (const ros::TimerEvent& e) const;
00125   void addConstraint (graph_mapping_msgs::GraphConstraint::ConstPtr constraint);
00126   void saveGraphTimed (const ros::TimerEvent& e);
00127   void updateLocalization
00128   (graph_mapping_msgs::LocalizationDistribution::ConstPtr l);
00129   bool getPoses (graph_mapping_msgs::GetPoses::Request& req,
00130                  graph_mapping_msgs::GetPoses::Response& resp);
00131   void publishRefTransform (const ros::TimerEvent& e);
00132 
00133   /****************************************
00134    * Modifying functions.  Must hold
00135    * lock while calling these.
00136    ****************************************/
00137   
00138   void addNodeAt (const ros::Time& t);
00139   void saveGraph ();
00140   void optimize ();
00141   void optimizeGraphComponent (unsigned n);
00142 
00143   /****************************************
00144    * Const functions
00145    ****************************************/
00146   
00147   tf::Pose fixedFramePoseAt (const ros::Time& t,
00148                              const ros::Duration& wait) const;
00149   bool findNodeNear (const geometry_msgs::PoseStamped& l) const;
00150 
00151   /****************************************
00152    * Virtual functions
00153    ****************************************/
00154 
00157   virtual boost::optional<ros::Time> newNodeTime ();
00158 
00159   /****************************************
00160    * Associated objects
00161    ****************************************/
00162 
00163   mutable boost::mutex mutex_; // Mutable because const operations may need to acquire a lock
00164   ros::NodeHandle nh_;
00165   tf::TransformBroadcaster tfb_;
00166   pose_graph::ConstraintGraphVisualizer visualizer_;
00167   boost::scoped_ptr<Odometer> update_odometer_;
00168   ros::Subscriber constraint_sub_, loc_sub_;
00169   pose_graph::DiffPublisher diff_pub_;
00170   ros::Publisher marker_pub_, pose_pub_;
00171   ros::ServiceServer get_poses_srv_;
00172   ros::Timer update_timer_, vis_timer_, save_timer_, ref_pub_timer_;
00173 };
00174 
00175 
00176 } // namespace
00177 
00178 #endif // include guard