$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 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 template 00062 <typename tpCloudType> 00063 class SelfMask 00064 { 00065 protected: 00066 00067 struct SeeLink 00068 { 00069 SeeLink(void) 00070 { 00071 body = unscaledBody = NULL; 00072 } 00073 00074 std::string name; 00075 bodies::Body *body; 00076 bodies::Body *unscaledBody; 00077 btTransform constTransf; 00078 double volume; 00079 }; 00080 00081 struct SortBodies 00082 { 00083 bool operator()(const SeeLink &b1, const SeeLink &b2) 00084 { 00085 return (b1.volume > b2.volume); 00086 } 00087 }; 00088 00089 public: 00090 00092 SelfMask(tf::TransformListener &tf, const std::vector<LinkInfo> &links) : tf_(tf) 00093 { 00094 configure (links); 00095 } 00096 00098 ~SelfMask(void) 00099 { 00100 freeMemory (); 00101 } 00102 00106 void maskContainment (const tpCloudType& data_in, std::vector<int> &mask); 00107 00117 void maskIntersection (const tpCloudType& 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 tpCloudType& 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 std::string &frame_id, const ros::Time &stamp); 00132 00135 void assumeFrame (const std::string &frame_id, const ros::Time &stamp, const std::string &sensor_frame, const double min_sensor_dist); 00136 00139 void assumeFrame (const std::string &frame_id, const ros::Time &stamp, 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: 00164 void freeMemory (void); 00165 00167 bool configure (const std::vector<LinkInfo> &links); 00168 00170 void computeBoundingSpheres (void); 00171 00173 void maskAuxContainment (const tpCloudType& data_in, std::vector<int> &mask); 00174 00176 void maskAuxIntersection (const tpCloudType& data_in, std::vector<int> &mask, const boost::function<void(const btVector3&)> &callback); 00177 00178 tf::TransformListener &tf_; 00179 ros::NodeHandle nh_; 00180 00181 btVector3 sensor_pos_; 00182 double min_sensor_dist_; 00183 00184 std::vector<SeeLink> bodies_; 00185 std::vector<double> bspheresRadius2_; 00186 std::vector<bodies::BoundingSphere> bspheres_; 00187 }; 00188 } 00189 00190 #include "self_mask.hxx" 00191 00192 #endif