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 #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 tf::Transform 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 tf::Vector3&)> &intersectionCallback = NULL);
00117
00124 void maskIntersection (const pcl::PointCloud<pcl::PointXYZ>& data_in, const tf::Vector3 &sensor, const double min_sensor_dist,
00125 std::vector<int> &mask, const boost::function<void(const tf::Vector3&)> &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 tf::Vector3 &sensor_pos, const double min_sensor_dist);
00138
00141 int getMaskContainment (double x, double y, double z) const;
00142
00145 int getMaskContainment (const tf::Vector3 &pt) const;
00146
00150 int getMaskIntersection (double x, double y, double z, const boost::function<void(const tf::Vector3&)> &intersectionCallback = NULL) const;
00151
00155 int getMaskIntersection (const tf::Vector3 &pt, const boost::function<void(const tf::Vector3&)> &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 tf::Vector3&)> &callback);
00175
00176 tf::TransformListener &tf_;
00177 ros::NodeHandle nh_;
00178
00179 tf::Vector3 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