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_OCCUPANCY_MAP_MONITOR_OCCUPANCY_MAP_UPDATER_
00038 #define MOVEIT_OCCUPANCY_MAP_MONITOR_OCCUPANCY_MAP_UPDATER_
00039
00040 #include <moveit/occupancy_map_monitor/occupancy_map.h>
00041 #include <geometric_shapes/shapes.h>
00042 #include <boost/shared_ptr.hpp>
00043 #include <Eigen/Core>
00044 #include <Eigen/Geometry>
00045
00046 namespace occupancy_map_monitor
00047 {
00048
00049 typedef unsigned int ShapeHandle;
00050 typedef std::map<ShapeHandle, Eigen::Affine3d, std::less<ShapeHandle>,
00051 Eigen::aligned_allocator<std::pair<const ShapeHandle, Eigen::Affine3d> > > ShapeTransformCache;
00052 typedef boost::function<bool(const std::string &target_frame, const ros::Time &target_time, ShapeTransformCache &cache)> TransformCacheProvider;
00053
00054 class OccupancyMapMonitor;
00055
00058 class OccupancyMapUpdater
00059 {
00060 public:
00061
00062 OccupancyMapUpdater(const std::string &type);
00063 virtual ~OccupancyMapUpdater();
00064
00066 void setMonitor(OccupancyMapMonitor *monitor);
00067
00069 virtual bool setParams(XmlRpc::XmlRpcValue ¶ms) = 0;
00070
00072 virtual bool initialize() = 0;
00073
00074 virtual void start() = 0;
00075
00076 virtual void stop() = 0;
00077
00078 virtual ShapeHandle excludeShape(const shapes::ShapeConstPtr &shape) = 0;
00079
00080 virtual void forgetShape(ShapeHandle handle) = 0;
00081
00082 const std::string& getType() const
00083 {
00084 return type_;
00085 }
00086
00087 void setTransformCacheCallback(const TransformCacheProvider &transform_callback)
00088 {
00089 transform_provider_callback_ = transform_callback;
00090 }
00091
00092 void publishDebugInformation(bool flag)
00093 {
00094 debug_info_ = flag;
00095 }
00096
00097 protected:
00098
00099 OccupancyMapMonitor *monitor_;
00100 std::string type_;
00101 OccMapTreePtr tree_;
00102 TransformCacheProvider transform_provider_callback_;
00103 ShapeTransformCache transform_cache_;
00104 bool debug_info_;
00105
00106 bool updateTransformCache(const std::string &target_frame, const ros::Time &target_time);
00107
00108 static void readXmlParam(XmlRpc::XmlRpcValue ¶ms, const std::string ¶m_name, double *value);
00109 static void readXmlParam(XmlRpc::XmlRpcValue ¶ms, const std::string ¶m_name, unsigned int *value);
00110
00111 };
00112
00113 typedef boost::shared_ptr<OccupancyMapUpdater> OccupancyMapUpdaterPtr;
00114 typedef boost::shared_ptr<const OccupancyMapUpdater> OccupancyMapUpdaterConstPtr;
00115
00116 }
00117
00118 #endif