00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2011, Willow Garage, Inc. 00005 * All rights reserved. 00006 * 00007 * Redistribution and use in source and binary forms, with or without 00008 * modification, are permitted provided that the following conditions 00009 * are met: 00010 * 00011 * * Redistributions of source code must retain the above copyright 00012 * notice, this list of conditions and the following disclaimer. 00013 * * Redistributions in binary form must reproduce the above 00014 * copyright notice, this list of conditions and the following 00015 * disclaimer in the documentation and/or other materials provided 00016 * with the distribution. 00017 * * Neither the name of Willow Garage nor the names of its 00018 * contributors may be used to endorse or promote products derived 00019 * from this software without specific prior written permission. 00020 * 00021 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 * POSSIBILITY OF SUCH DAMAGE. 00033 *********************************************************************/ 00034 00035 /* Author: Ioan Sucan, Jon Binney */ 00036 00037 #ifndef MOVEIT_PERCEPTION_OCCUPANCY_MAP_MONITOR_ 00038 #define MOVEIT_PERCEPTION_OCCUPANCY_MAP_MONITOR_ 00039 00040 #include <vector> 00041 #include <string> 00042 #include <ros/ros.h> 00043 #include <tf/tf.h> 00044 #include <pluginlib/class_loader.h> 00045 00046 #include <moveit_msgs/SaveMap.h> 00047 #include <moveit_msgs/LoadMap.h> 00048 #include <moveit/occupancy_map_monitor/occupancy_map.h> 00049 #include <moveit/occupancy_map_monitor/occupancy_map_updater.h> 00050 00051 #include <boost/thread/mutex.hpp> 00052 00053 namespace occupancy_map_monitor 00054 { 00055 00056 class OccupancyMapMonitor 00057 { 00058 public: 00059 00060 OccupancyMapMonitor(const boost::shared_ptr<tf::Transformer> &tf, 00061 const std::string &map_frame = "", double map_resolution = 0.0); 00062 OccupancyMapMonitor(double map_resolution = 0.0); 00063 00064 ~OccupancyMapMonitor(); 00065 00067 void startMonitor(); 00068 00069 void stopMonitor(); 00070 00073 const OccMapTreePtr& getOcTreePtr() 00074 { 00075 return tree_; 00076 } 00077 00080 const OccMapTreeConstPtr& getOcTreePtr() const 00081 { 00082 return tree_const_; 00083 } 00084 00085 const std::string& getMapFrame() const 00086 { 00087 return map_frame_; 00088 } 00089 00090 void setMapFrame(const std::string &frame); 00091 00092 double getMapResolution() const 00093 { 00094 return map_resolution_; 00095 } 00096 00097 const boost::shared_ptr<tf::Transformer>& getTFClient() const 00098 { 00099 return tf_; 00100 } 00101 00102 void addUpdater(const OccupancyMapUpdaterPtr &updater); 00103 00105 ShapeHandle excludeShape(const shapes::ShapeConstPtr &shape); 00106 00108 void forgetShape(ShapeHandle handle); 00109 00111 void setUpdateCallback(const boost::function<void()> &update_callback) 00112 { 00113 tree_->setUpdateCallback(update_callback); 00114 } 00115 00116 void setTransformCacheCallback(const TransformCacheProvider &transform_cache_callback); 00117 00118 void publishDebugInformation(bool flag); 00119 00120 bool isActive() const 00121 { 00122 return active_; 00123 } 00124 00125 private: 00126 00127 void initialize(); 00128 00130 bool saveMapCallback(moveit_msgs::SaveMap::Request& request, moveit_msgs::SaveMap::Response& response); 00131 00133 bool loadMapCallback(moveit_msgs::LoadMap::Request& request, moveit_msgs::LoadMap::Response& response); 00134 00135 bool getShapeTransformCache(std::size_t index, const std::string &target_frame, const ros::Time &target_time, ShapeTransformCache &cache) const; 00136 00137 boost::shared_ptr<tf::Transformer> tf_; 00138 std::string map_frame_; 00139 double map_resolution_; 00140 boost::mutex parameters_lock_; 00141 00142 OccMapTreePtr tree_; 00143 OccMapTreeConstPtr tree_const_; 00144 00145 boost::scoped_ptr<pluginlib::ClassLoader<OccupancyMapUpdater> > updater_plugin_loader_; 00146 std::vector<OccupancyMapUpdaterPtr> map_updaters_; 00147 std::vector<std::map<ShapeHandle, ShapeHandle> > mesh_handles_; 00148 TransformCacheProvider transform_cache_callback_; 00149 bool debug_info_; 00150 00151 std::size_t mesh_handle_count_; 00152 00153 ros::NodeHandle root_nh_; 00154 ros::NodeHandle nh_; 00155 ros::ServiceServer save_map_srv_; 00156 ros::ServiceServer load_map_srv_; 00157 00158 bool active_; 00159 00160 }; 00161 00162 } 00163 00164 #endif