Go to the documentation of this file.00001
00002
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
00030
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
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
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
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
00126
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
00137 filtered_scan.ranges[count] = (!c) ? input_scan.ranges[count] : std::numeric_limits<float>::quiet_NaN();
00138 } else {
00139
00140 filtered_scan.ranges[count] = input_scan.ranges[count];
00141 }
00142 current_angle += input_scan.angle_increment;
00143 }
00144
00145
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