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 <pcl/point_types.h>
00034 #include <pcl_ros/point_cloud.h>
00035 #include <sensor_msgs/PointCloud2.h>
00036 #include <pr2_navigation_self_filter/bodies.h>
00037 #include <tf/transform_listener.h>
00038 #include <boost/bind.hpp>
00039 #include <string>
00040 #include <vector>
00041
00042 namespace robot_self_filter
00043 {
00044 typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
00045
00047 enum
00048 {
00049 INSIDE = 0,
00050 OUTSIDE = 1,
00051 SHADOW = 2,
00052 };
00053
00054 struct LinkInfo
00055 {
00056 std::string name;
00057 double padding;
00058 double scale;
00059 };
00060
00061
00065 class SelfMask
00066 {
00067 protected:
00068
00069 struct SeeLink
00070 {
00071 SeeLink(void)
00072 {
00073 body = unscaledBody = NULL;
00074 }
00075
00076 std::string name;
00077 bodies::Body *body;
00078 bodies::Body *unscaledBody;
00079 tf::Transform constTransf;
00080 double volume;
00081 };
00082
00083 struct SortBodies
00084 {
00085 bool operator()(const SeeLink &b1, const SeeLink &b2)
00086 {
00087 return b1.volume > b2.volume;
00088 }
00089 };
00090
00091 public:
00092
00094 SelfMask(tf::TransformListener &tf, const std::vector<LinkInfo> &links) : tf_(tf)
00095 {
00096 configure(links);
00097 }
00098
00101 ~SelfMask(void)
00102 {
00103 freeMemory();
00104 }
00105
00109 void maskContainment(const PointCloud& data_in, std::vector<int> &mask);
00110
00120 void maskIntersection(const PointCloud& data_in, const std::string &sensor_frame, const double min_sensor_dist,
00121 std::vector<int> &mask, const boost::function<void(const tf::Vector3&)> &intersectionCallback = NULL);
00122
00129 void maskIntersection(const PointCloud& data_in, const tf::Vector3 &sensor, const double min_sensor_dist,
00130 std::vector<int> &mask, const boost::function<void(const tf::Vector3&)> &intersectionCallback = NULL);
00131
00134 void assumeFrame(const std_msgs::Header& header);
00135
00138 void assumeFrame(const std_msgs::Header& header, const std::string &sensor_frame, const double min_sensor_dist);
00139
00142 void assumeFrame(const std_msgs::Header& header, const tf::Vector3 &sensor_pos, const double min_sensor_dist);
00143
00146 int getMaskContainment(double x, double y, double z) const;
00147
00150 int getMaskContainment(const tf::Vector3 &pt) const;
00151
00155 int getMaskIntersection(double x, double y, double z, const boost::function<void(const tf::Vector3&)> &intersectionCallback = NULL) const;
00156
00160 int getMaskIntersection(const tf::Vector3 &pt, const boost::function<void(const tf::Vector3&)> &intersectionCallback = NULL) const;
00161
00163 void getLinkNames(std::vector<std::string> &frames) const;
00164
00165 private:
00166
00168 void freeMemory(void);
00169
00171 bool configure(const std::vector<LinkInfo> &links);
00172
00174 void computeBoundingSpheres(void);
00175
00177 void maskAuxContainment(const PointCloud& data_in, std::vector<int> &mask);
00178
00180 void maskAuxIntersection(const PointCloud& data_in, std::vector<int> &mask, const boost::function<void(const tf::Vector3&)> &callback);
00181
00182 tf::TransformListener &tf_;
00183 ros::NodeHandle nh_;
00184
00185 tf::Vector3 sensor_pos_;
00186 double min_sensor_dist_;
00187
00188 std::vector<SeeLink> bodies_;
00189 std::vector<double> bspheresRadius2_;
00190 std::vector<bodies::BoundingSphere> bspheres_;
00191
00192 };
00193
00194 }
00195
00196 #endif