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