$search
00001 /* 00002 * Copyright (c) 2008, Willow Garage, Inc. 00003 * All rights reserved. 00004 * 00005 * Redistribution and use in source and binary forms, with or without 00006 * modification, are permitted provided that the following conditions are met: 00007 * 00008 * * Redistributions of source code must retain the above copyright 00009 * notice, this list of conditions and the following disclaimer. 00010 * * Redistributions in binary form must reproduce the above copyright 00011 * notice, this list of conditions and the following disclaimer in the 00012 * documentation and/or other materials provided with the distribution. 00013 * * Neither the name of the Willow Garage, Inc. nor the names of its 00014 * contributors may be used to endorse or promote products derived from 00015 * this software without specific prior written permission. 00016 * 00017 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 00018 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 00019 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 00020 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 00021 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 00022 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 00023 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 00024 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 00025 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 00026 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00027 * POSSIBILITY OF SUCH DAMAGE. 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_color 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::PointXYZRGB>& data_in, std::vector<int> &mask); 00105 00115 void maskIntersection (const pcl::PointCloud<pcl::PointXYZRGB>& 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::PointXYZRGB>& 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::PointXYZRGB>& data_in, std::vector<int> &mask); 00172 00174 void maskAuxIntersection (const pcl::PointCloud<pcl::PointXYZRGB>& 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