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::PointXYZ> > (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::PointXYZ> cloud_in;
00064 pcl_conversions::toPCL(header, cloud_in.header);
00065 cloud_in.push_back(pcl::PointXYZ(point_in.x, point_in.y, point_in.z));
00066
00067 pcl::PointCloud<pcl::PointXYZ> cloud_filtered;
00068
00069 if (waitForRelevantTransforms(header)){
00070 self_filter_->updateWithSensorFrame (cloud_in, cloud_filtered, header.frame_id);
00071
00072 if(cloud_filtered.size() > 0){
00073 return false;
00074 }else{
00075 return true;
00076 }
00077 }else{
00078 return false;
00079 }
00080 }
00081
00082
00083 bool waitForRelevantTransforms(const std_msgs::Header& header)
00084 {
00085 try{
00086 size_t num_frames = frames_.size();
00087 ros::Duration waitDuration(0.5);
00088
00089 for (size_t i = 0; i < num_frames; ++i){
00090 tfL_->waitForTransform(header.frame_id, frames_[i], header.stamp, waitDuration);
00091 }
00092 } catch (tf::TransformException ex) {
00093 ROS_ERROR("Self filter failed waiting for necessary transforms: %s", ex.what());
00094 return false;
00095 }
00096 return true;
00097 }
00098
00099
00100
00101
00102
00103 protected:
00104 filters::SelfFilter<pcl::PointCloud<pcl::PointXYZ> > *self_filter_;
00105 std::vector<std::string> frames_;
00106
00107 tf::TransformListener* tfL_;
00108
00109
00110 };
00111
00112 #endif