$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 <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 tf::Transform 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 tf::Vector3&)> &intersectionCallback = NULL); 00119 00126 void maskIntersection(const sensor_msgs::PointCloud& data_in, const tf::Vector3 &sensor, const double min_sensor_dist, 00127 std::vector<int> &mask, const boost::function<void(const tf::Vector3&)> &intersectionCallback = NULL); 00128 00131 void assumeFrame(const std_msgs::Header& header); 00132 00135 void assumeFrame(const std_msgs::Header& header, const std::string &sensor_frame, const double min_sensor_dist); 00136 00139 void assumeFrame(const std_msgs::Header& header, const tf::Vector3 &sensor_pos, const double min_sensor_dist); 00140 00143 int getMaskContainment(double x, double y, double z) const; 00144 00147 int getMaskContainment(const tf::Vector3 &pt) const; 00148 00152 int getMaskIntersection(double x, double y, double z, const boost::function<void(const tf::Vector3&)> &intersectionCallback = NULL) const; 00153 00157 int getMaskIntersection(const tf::Vector3 &pt, const boost::function<void(const tf::Vector3&)> &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 tf::Vector3&)> &callback); 00178 00179 tf::TransformListener &tf_; 00180 ros::NodeHandle nh_; 00181 00182 tf::Vector3 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