footprint_filter.h
Go to the documentation of this file.
00001 /*
00002  * LICENSE: https://github.com/utexas-bwi/segbot/blob/devel/LICENSE
00003  */
00004 
00005 #ifndef LASER_SCAN_FOOTPRINT_FILTER_H
00006 #define LASER_SCAN_FOOTPRINT_FILTER_H
00007 
00008 #include <tf/transform_listener.h>
00009 #include <filters/filter_base.h>
00010 #include <sensor_msgs/LaserScan.h>
00011 #include <geometry_msgs/PolygonStamped.h>
00012 #include <XmlRpcException.h>
00013 
00014 namespace segbot_sensors
00015 {
00016   class SegbotFootprintFilter : public filters::FilterBase<sensor_msgs::LaserScan>
00017   {
00018     public:
00019       std::vector<tf::Point> footprint_;
00020       std::string footprint_frame_;
00021       bool get_transformation_;
00022       boost::shared_ptr<tf::TransformListener> listener_;
00023       ros::Publisher footprint_publisher_;
00024 
00025       bool configure() {
00026 
00027         listener_.reset(new tf::TransformListener());
00028 
00029         // Get the frame of reference here. The sensor should not move w.r.t to 
00030         // this frame of reference
00031         get_transformation_ = true;
00032         getParam("footprint_frame", footprint_frame_);
00033         if (footprint_frame_.empty()) {
00034           ROS_WARN("No footprint frame provided, assuming footprint is in the frame of reference of the sensor.");
00035           get_transformation_ = false;
00036           return false;
00037         } else {
00038           ros::NodeHandle private_nh("~");
00039           std::string tf_prefix = tf::getPrefixParam(private_nh);
00040           std::string resolved_frame = tf::resolve(tf_prefix, footprint_frame_);
00041           ROS_INFO("Resolved footprint frame from %s to %s.", footprint_frame_.c_str(), resolved_frame.c_str());
00042           footprint_frame_ = resolved_frame;
00043         }
00044 
00045         XmlRpc::XmlRpcValue footprint_list;
00046         getParam("footprint", footprint_list);
00047         ROS_ASSERT(footprint_list.getType() == XmlRpc::XmlRpcValue::TypeArray);
00048 
00049         try {
00050           for (int32_t i = 0; i < footprint_list.size() - 1; i+=2) {
00051             float valx, valy;
00052             if (footprint_list[i].getType() == XmlRpc::XmlRpcValue::TypeDouble) 
00053               valx = static_cast<double>(footprint_list[i]);
00054             else if (footprint_list[i].getType() == XmlRpc::XmlRpcValue::TypeInt)
00055               valx = static_cast<int>(footprint_list[i]);
00056             else {
00057               ROS_FATAL("Footprint should be a list of numeric values");
00058               return false;
00059             }
00060             if (footprint_list[i+1].getType() == XmlRpc::XmlRpcValue::TypeDouble) 
00061               valy = static_cast<double>(footprint_list[i+1]);
00062             else if (footprint_list[i+1].getType() == XmlRpc::XmlRpcValue::TypeInt)
00063               valy = static_cast<int>(footprint_list[i+1]);
00064             else {
00065               ROS_FATAL("Footprint should be a list of numeric values");
00066               return false;
00067             }
00068             footprint_.push_back(tf::Point(valx, valy, 0));
00069           }
00070         } catch (XmlRpc::XmlRpcException ex) {
00071           ROS_FATAL_STREAM(ex.getMessage());
00072           return false;
00073         }
00074         
00075         if(footprint_.size() < 3) {
00076           ROS_FATAL("Footprint needs to contain atleast 3 vertices.");
00077           return false;
00078         }
00079 
00080         // Get ready to publish footprint
00081         ros::NodeHandle n("~");
00082         footprint_publisher_ = n.advertise<geometry_msgs::PolygonStamped>("footprint", 1);
00083 
00084         return true;
00085       }
00086 
00087       virtual ~SegbotFootprintFilter(){}
00088 
00089       bool update(const sensor_msgs::LaserScan& input_scan, sensor_msgs::LaserScan& filtered_scan){
00090 
00091         // Get the transformation the first time only
00092         if (get_transformation_) {
00093           try {
00094             tf::StampedTransform transform;
00095             listener_->waitForTransform(input_scan.header.frame_id, footprint_frame_, ros::Time(0), ros::Duration(10.0) );
00096             listener_->lookupTransform(input_scan.header.frame_id, footprint_frame_, ros::Time(0), transform);
00097             for (uint32_t i = 0; i < footprint_.size(); ++i) {
00098               footprint_[i] = transform * footprint_[i]; 
00099             }
00100             ROS_INFO("Transformation from %s to %s received. Transforming footprint", footprint_frame_.c_str(), input_scan.header.frame_id.c_str());
00101           } catch (tf::TransformException ex) {
00102             ROS_ERROR("Error getting transformation. The filtered scan is probably incorrect: %s",ex.what());
00103           }
00104           get_transformation_ = false;
00105         }
00106 
00107         // Publish polygon corresponding to footprint - visualization
00108         geometry_msgs::PolygonStamped polygon_msg;
00109         polygon_msg.header.frame_id = input_scan.header.frame_id;
00110         polygon_msg.header.stamp = ros::Time::now();
00111         polygon_msg.polygon.points.resize(footprint_.size());
00112         for (uint32_t i = 0; i < footprint_.size(); ++i) {
00113           polygon_msg.polygon.points[i].x = footprint_[i].x();
00114           polygon_msg.polygon.points[i].y = footprint_[i].y();
00115           polygon_msg.polygon.points[i].z = 0;
00116         }
00117         footprint_publisher_.publish(polygon_msg);
00118 
00119         filtered_scan.ranges.resize(input_scan.ranges.size());
00120         double current_angle = input_scan.angle_min;
00121 
00122         for(unsigned int count = 0; count < input_scan.ranges.size(); ++count){
00123           if (input_scan.range_min <= input_scan.ranges[count] && 
00124               input_scan.ranges[count] <= input_scan.range_max) {
00125             // Valid measurement, check if it lies in the footprint
00126             // http://www.ecse.rpi.edu/Homepages/wrf/Research/Short_Notes/pnpoly.html
00127             float testx = cosf(current_angle) * input_scan.ranges[count];
00128             float testy = sinf(current_angle) * input_scan.ranges[count];
00129             int i, j, c = 0;
00130             int nvert = footprint_.size(); 
00131             for (i = 0, j = nvert-1; i < nvert; j = i++) {
00132               if ( ((footprint_[i].y()>testy) != (footprint_[j].y()>testy)) &&
00133                (testx < (footprint_[j].x()-footprint_[i].x()) * (testy-footprint_[i].y()) / (footprint_[j].y()-footprint_[i].y()) + footprint_[i].x()) )
00134                  c = !c;
00135             }
00136             // in accordance with REP 117, invalidate all readings that are within the robot foot print
00137             filtered_scan.ranges[count] = (!c) ? input_scan.ranges[count] : std::numeric_limits<float>::quiet_NaN();
00138           } else {
00139             // Let all inifinte values pass through
00140             filtered_scan.ranges[count] = input_scan.ranges[count];
00141           }
00142           current_angle += input_scan.angle_increment;
00143         }
00144 
00145         //make sure to set all the needed fields on the filtered scan
00146         filtered_scan.header.frame_id = input_scan.header.frame_id;
00147         filtered_scan.header.stamp = input_scan.header.stamp;
00148         filtered_scan.angle_min = input_scan.angle_min;
00149         filtered_scan.angle_max = input_scan.angle_max;
00150         filtered_scan.angle_increment = input_scan.angle_increment;
00151         filtered_scan.time_increment = input_scan.time_increment;
00152         filtered_scan.scan_time = input_scan.scan_time;
00153         filtered_scan.range_min = input_scan.range_min;
00154         filtered_scan.range_max = input_scan.range_max;
00155         filtered_scan.intensities = input_scan.intensities;
00156 
00157         return true;
00158 
00159       }
00160   };
00161 }
00162 #endif
00163 


segbot_sensors
Author(s): Piyush Khandelwal
autogenerated on Fri Aug 28 2015 13:03:00