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 #ifndef ROBOT_SELF_FILTER_SELF_MASK_
00031 #define ROBOT_SELF_FILTER_SELF_MASK_
00032
00033 #include <sensor_msgs/PointCloud.h>
00034 #include <geometric_shapes/bodies.h>
00035 #include <tf/transform_listener.h>
00036 #include <boost/bind.hpp>
00037 #include <string>
00038 #include <vector>
00039
00040 #include <pcl/point_cloud.h>
00041 #include <pcl/point_types.h>
00042
00043 namespace robot_self_filter
00044 {
00046 enum
00047 {
00048 INSIDE = 0,
00049 OUTSIDE = 1,
00050 SHADOW = 2,
00051 };
00052
00053 struct LinkInfo
00054 {
00055 std::string name;
00056 double padding;
00057 double scale;
00058 };
00059
00061 class SelfMask
00062 {
00063 protected:
00064
00065 struct SeeLink
00066 {
00067 SeeLink(void)
00068 {
00069 body = unscaledBody = NULL;
00070 }
00071
00072 std::string name;
00073 bodies::Body *body;
00074 bodies::Body *unscaledBody;
00075 btTransform constTransf;
00076 double volume;
00077 };
00078
00079 struct SortBodies
00080 {
00081 bool operator()(const SeeLink &b1, const SeeLink &b2)
00082 {
00083 return (b1.volume > b2.volume);
00084 }
00085 };
00086
00087 public:
00088
00090 SelfMask(tf::TransformListener &tf, const std::vector<LinkInfo> &links) : tf_(tf)
00091 {
00092 configure (links);
00093 }
00094
00096 ~SelfMask(void)
00097 {
00098 freeMemory ();
00099 }
00100
00104 void maskContainment (const pcl::PointCloud<pcl::PointXYZ>& data_in, std::vector<int> &mask);
00105
00115 void maskIntersection (const pcl::PointCloud<pcl::PointXYZ>& data_in, const std::string &sensor_frame, const double min_sensor_dist,
00116 std::vector<int> &mask, const boost::function<void(const btVector3&)> &intersectionCallback = NULL);
00117
00124 void maskIntersection (const pcl::PointCloud<pcl::PointXYZ>& data_in, const btVector3 &sensor, const double min_sensor_dist,
00125 std::vector<int> &mask, const boost::function<void(const btVector3&)> &intersectionCallback = NULL);
00126
00129 void assumeFrame (const std::string &frame_id, const ros::Time &stamp);
00130
00133 void assumeFrame (const std::string &frame_id, const ros::Time &stamp, const std::string &sensor_frame, const double min_sensor_dist);
00134
00137 void assumeFrame (const std::string &frame_id, const ros::Time &stamp, const btVector3 &sensor_pos, const double min_sensor_dist);
00138
00141 int getMaskContainment (double x, double y, double z) const;
00142
00145 int getMaskContainment (const btVector3 &pt) const;
00146
00150 int getMaskIntersection (double x, double y, double z, const boost::function<void(const btVector3&)> &intersectionCallback = NULL) const;
00151
00155 int getMaskIntersection (const btVector3 &pt, const boost::function<void(const btVector3&)> &intersectionCallback = NULL) const;
00156
00158 void getLinkNames (std::vector<std::string> &frames) const;
00159
00160 private:
00162 void freeMemory (void);
00163
00165 bool configure (const std::vector<LinkInfo> &links);
00166
00168 void computeBoundingSpheres (void);
00169
00171 void maskAuxContainment (const pcl::PointCloud<pcl::PointXYZ>& data_in, std::vector<int> &mask);
00172
00174 void maskAuxIntersection (const pcl::PointCloud<pcl::PointXYZ>& data_in, std::vector<int> &mask, const boost::function<void(const btVector3&)> &callback);
00175
00176 tf::TransformListener &tf_;
00177 ros::NodeHandle nh_;
00178
00179 btVector3 sensor_pos_;
00180 double min_sensor_dist_;
00181
00182 std::vector<SeeLink> bodies_;
00183 std::vector<double> bspheresRadius2_;
00184 std::vector<bodies::BoundingSphere> bspheres_;
00185 };
00186 }
00187
00188 #endif