Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
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 class OccupancyMapMonitor
00056 {
00057 public:
00058 OccupancyMapMonitor(const boost::shared_ptr<tf::Transformer>& tf, const std::string& map_frame = "",
00059 double map_resolution = 0.0);
00060 OccupancyMapMonitor(double map_resolution = 0.0);
00061 OccupancyMapMonitor(const boost::shared_ptr<tf::Transformer>& tf, ros::NodeHandle& nh,
00062 const std::string& map_frame = "", 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 void initialize();
00127
00129 bool saveMapCallback(moveit_msgs::SaveMap::Request& request, moveit_msgs::SaveMap::Response& response);
00130
00132 bool loadMapCallback(moveit_msgs::LoadMap::Request& request, moveit_msgs::LoadMap::Response& response);
00133
00134 bool getShapeTransformCache(std::size_t index, const std::string& target_frame, const ros::Time& target_time,
00135 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 #endif