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 #ifndef __HectorThermalSelfFilter_h_
00030 #define __HectorThermalSelfFilter_h_
00031
00032 #include <pcl_conversions/pcl_conversions.h>
00033 #include <tf/transform_listener.h>
00034 #include <robot_self_filter/self_see_filter.h>
00035
00036 class HectorThermalSelfFilter{
00037 public:
00038
00039 HectorThermalSelfFilter(ros::NodeHandle& nh, tf::TransformListener* tfl_in)
00040 : tfL_(tfl_in)
00041 {
00042 self_filter_ = new filters::SelfFilter<pcl::PointCloud<pcl::PointXYZI> > (nh);
00043
00044
00045 self_filter_->getSelfMask()->getLinkNames(frames_);
00046 if (frames_.empty()){
00047 ROS_ERROR ("No valid frames have been passed into the self filter.");
00048 }else{
00049 ROS_INFO ("Self filter uses the following links to filter:");
00050
00051 for (size_t i = 0; i < frames_.size(); ++i){
00052 ROS_INFO("Filtered frame %u : %s", static_cast<unsigned int>(i), frames_[i].c_str());
00053 }
00054 }
00055 }
00056
00057 virtual ~HectorThermalSelfFilter(){
00058 delete self_filter_;
00059 }
00060
00061 bool pointBelongsToRobot(const geometry_msgs::Point& point_in, const std_msgs::Header& header)
00062 {
00063 pcl::PointCloud<pcl::PointXYZI> cloud_in;
00064 pcl_conversions::toPCL(header, cloud_in.header);
00065
00066 pcl::PointXYZI point;
00067 point.x = point_in.x;
00068 point.y = point_in.y;
00069 point.z = point_in.z;
00070
00071 cloud_in.push_back(point);
00072
00073
00074 pcl::PointCloud<pcl::PointXYZI> cloud_filtered;
00075
00076 if (waitForRelevantTransforms(header)){
00077 self_filter_->updateWithSensorFrame (cloud_in, cloud_filtered, header.frame_id);
00078
00079 if(cloud_filtered.size() > 0){
00080 return false;
00081 }else{
00082 return true;
00083 }
00084 }else{
00085 return false;
00086 }
00087 }
00088
00089
00090 bool waitForRelevantTransforms(const std_msgs::Header& header)
00091 {
00092 try{
00093 size_t num_frames = frames_.size();
00094 ros::Duration waitDuration(0.5);
00095
00096 for (size_t i = 0; i < num_frames; ++i){
00097 tfL_->waitForTransform(header.frame_id, frames_[i], header.stamp, waitDuration);
00098 }
00099 } catch (tf::TransformException ex) {
00100 ROS_ERROR("Self filter failed waiting for necessary transforms: %s", ex.what());
00101 return false;
00102 }
00103 return true;
00104 }
00105
00106
00107
00108
00109
00110 protected:
00111 filters::SelfFilter<pcl::PointCloud<pcl::PointXYZI> > *self_filter_;
00112 std::vector<std::string> frames_;
00113
00114 tf::TransformListener* tfL_;
00115
00116
00117 };
00118
00119 #endif