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 <pr2_navigation_self_filter/bodies.h>
00035 #include <tf/transform_listener.h>
00036 #include <boost/bind.hpp>
00037 #include <string>
00038 #include <vector>
00039
00040 namespace robot_self_filter
00041 {
00042
00044 enum
00045 {
00046 INSIDE = 0,
00047 OUTSIDE = 1,
00048 SHADOW = 2,
00049 };
00050
00051 struct LinkInfo
00052 {
00053 std::string name;
00054 double padding;
00055 double scale;
00056 };
00057
00058
00062 class SelfMask
00063 {
00064 protected:
00065
00066 struct SeeLink
00067 {
00068 SeeLink(void)
00069 {
00070 body = unscaledBody = NULL;
00071 }
00072
00073 std::string name;
00074 bodies::Body *body;
00075 bodies::Body *unscaledBody;
00076 btTransform constTransf;
00077 double volume;
00078 };
00079
00080 struct SortBodies
00081 {
00082 bool operator()(const SeeLink &b1, const SeeLink &b2)
00083 {
00084 return b1.volume > b2.volume;
00085 }
00086 };
00087
00088 public:
00089
00091 SelfMask(tf::TransformListener &tf, const std::vector<LinkInfo> &links) : tf_(tf)
00092 {
00093 configure(links);
00094 }
00095
00098 ~SelfMask(void)
00099 {
00100 freeMemory();
00101 }
00102
00106 void maskContainment(const sensor_msgs::PointCloud& data_in, std::vector<int> &mask);
00107
00117 void maskIntersection(const sensor_msgs::PointCloud& data_in, const std::string &sensor_frame, const double min_sensor_dist,
00118 std::vector<int> &mask, const boost::function<void(const btVector3&)> &intersectionCallback = NULL);
00119
00126 void maskIntersection(const sensor_msgs::PointCloud& data_in, const btVector3 &sensor, const double min_sensor_dist,
00127 std::vector<int> &mask, const boost::function<void(const btVector3&)> &intersectionCallback = NULL);
00128
00131 void assumeFrame(const roslib::Header& header);
00132
00135 void assumeFrame(const roslib::Header& header, const std::string &sensor_frame, const double min_sensor_dist);
00136
00139 void assumeFrame(const roslib::Header& header, const btVector3 &sensor_pos, const double min_sensor_dist);
00140
00143 int getMaskContainment(double x, double y, double z) const;
00144
00147 int getMaskContainment(const btVector3 &pt) const;
00148
00152 int getMaskIntersection(double x, double y, double z, const boost::function<void(const btVector3&)> &intersectionCallback = NULL) const;
00153
00157 int getMaskIntersection(const btVector3 &pt, const boost::function<void(const btVector3&)> &intersectionCallback = NULL) const;
00158
00160 void getLinkNames(std::vector<std::string> &frames) const;
00161
00162 private:
00163
00165 void freeMemory(void);
00166
00168 bool configure(const std::vector<LinkInfo> &links);
00169
00171 void computeBoundingSpheres(void);
00172
00174 void maskAuxContainment(const sensor_msgs::PointCloud& data_in, std::vector<int> &mask);
00175
00177 void maskAuxIntersection(const sensor_msgs::PointCloud& data_in, std::vector<int> &mask, const boost::function<void(const btVector3&)> &callback);
00178
00179 tf::TransformListener &tf_;
00180 ros::NodeHandle nh_;
00181
00182 btVector3 sensor_pos_;
00183 double min_sensor_dist_;
00184
00185 std::vector<SeeLink> bodies_;
00186 std::vector<double> bspheresRadius2_;
00187 std::vector<bodies::BoundingSphere> bspheres_;
00188
00189 };
00190
00191 }
00192
00193 #endif