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
00030
00031 #include <cart_state_estimator/polygon_point_cloud_filter.h>
00032 #include "pluginlib/class_list_macros.h"
00033 #include <geometry_msgs/PolygonStamped.h>
00034
00035 PLUGINLIB_DECLARE_CLASS(cart_state_estimator, PolygonPointCloudFilter,
00036 cart_state_estimator::PolygonPointCloudFilter, filters::FilterBase<sensor_msgs::PointCloud>)
00037
00038 namespace cart_state_estimator
00039 {
00040
00041 using sensor_msgs::PointCloud;
00042 using geometry_msgs::Point32;
00043 using XmlRpc::XmlRpcValue;
00044 using geometry_msgs::PolygonStamped;
00045
00046 bool PolygonPointCloudFilter::configure ()
00047 {
00048 filter_polygon_pub_ = nh_.advertise<PolygonStamped>("filter_polygon", 10);
00049 XmlRpcValue polygon;
00050 if (!getParam("polygon", polygon)) {
00051 ROS_ERROR ("PolygonPointCloudFilter needs polygon to be set");
00052 return false;
00053 }
00054 else if ((polygon.getType() == XmlRpcValue::TypeArray) &&
00055 (polygon.size() >= 3)) {
00056 poly_.resize(polygon.size());
00057 for (int i=0; i< polygon.size(); i++) {
00058 XmlRpcValue pt = polygon[i];
00059 ROS_ASSERT_MSG (pt.getType() == XmlRpcValue::TypeArray && pt.size() == 2,
00060 "Invalid xml rpc value for polygon parameter");
00061 poly_[i].x = (double) pt[0];
00062 poly_[i].y = (double) pt[1];
00063 ROS_INFO_STREAM ("Adding point " << poly_[i]);
00064 }
00065 }
00066 else {
00067 ROS_ERROR ("Invalid xml rpc value for polygon");
00068 return false;
00069 }
00070 if (!getParam("frame", frame_)) {
00071 ROS_ERROR ("PolygonPointCloudFilter needs frame to be set");
00072 }
00073 return true;
00074 }
00075
00076 bool PolygonPointCloudFilter::update (const PointCloud& input_scan, PointCloud& filtered_scan)
00077 {
00078 if (&input_scan == &filtered_scan) {
00079 ROS_ERROR ("This filter does not currently support in place copying");
00080 return false;
00081 }
00082 PointCloud laser_cloud;
00083
00084 try {
00085 tf_.waitForTransform(input_scan.header.frame_id, frame_, input_scan.header.stamp, ros::Duration(0.2));
00086 tf_.transformPointCloud(frame_, input_scan, laser_cloud);
00087 }
00088 catch (tf::TransformException& ex) {
00089 ROS_ERROR ("Transform unavailable %s", ex.what());
00090 return false;
00091 }
00092
00093 const unsigned num_channels = input_scan.channels.size();
00094
00095 filtered_scan.header = input_scan.header;
00096 filtered_scan.points.resize (input_scan.points.size());
00097 filtered_scan.channels.resize(num_channels);
00098 for (unsigned d=0; d<num_channels; d++) {
00099 filtered_scan.channels[d].values.resize (input_scan.points.size());
00100 filtered_scan.channels[d].name = input_scan.channels[d].name;
00101 }
00102
00103 int num_pts = 0;
00104 for (unsigned i=0; i<laser_cloud.points.size(); i++) {
00105 ROS_DEBUG_STREAM_NAMED ("polygon_filter_internal", "Looking at point " << laser_cloud.points[i]);
00106 if (!inPolygon(laser_cloud.points[i])) {
00107 ROS_DEBUG_STREAM_NAMED ("polygon_filter_internal", "Not in polygon");
00108 filtered_scan.points[num_pts] = input_scan.points[i];
00109 for (unsigned d=0; d<num_channels; d++)
00110 filtered_scan.channels[d].values[num_pts] = input_scan.channels[d].values[i];
00111 num_pts++;
00112 }
00113 else
00114 ROS_DEBUG_STREAM_NAMED ("polygon_filter_internal", "In polygon");
00115 }
00116 ROS_DEBUG_NAMED ("polygon_filter", "In update, received a cloud of size %zu, of which we're keeping %d",
00117 laser_cloud.points.size(), num_pts);
00118
00119
00120 filtered_scan.points.resize(num_pts);
00121 for (unsigned d=0; d<num_channels; d++)
00122 filtered_scan.channels[d].values.resize(num_pts);
00123
00124
00125 {
00126 PolygonStamped poly;
00127 poly.polygon.points = poly_;
00128 poly.header.frame_id = frame_;
00129 poly.header.stamp = ros::Time::now();
00130 filter_polygon_pub_.publish(poly);
00131 }
00132
00133 return true;
00134 }
00135
00136
00137 bool PolygonPointCloudFilter::inPolygon (const Point32& p) const
00138 {
00139 bool c=false;
00140 const int num_vertices = poly_.size();
00141 for (int i=0; i<num_vertices; i++) {
00142 const int j = i ? i-1 : num_vertices-1;
00143 if ( ((poly_[i].y>p.y) != (poly_[j].y>p.y)) &&
00144 (p.x < (poly_[j].x-poly_[i].x) * (p.y-poly_[i].y) / (poly_[j].y-poly_[i].y) + poly_[i].x))
00145 c = !c;
00146 }
00147 return c;
00148 }
00149
00150
00151
00152 }
00153