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
00032
00033
00034
00035 #include "PointCloudFilter.h"
00036 #include <sensor_msgs/point_cloud2_iterator.h>
00037
00038 namespace mobility_base_pointcloud_filter
00039 {
00040
00041 PointCloudFilter::PointCloudFilter(ros::NodeHandle nh, ros::NodeHandle priv_nh) : nh_(nh)
00042 {
00043 if (!nh.getParam("width", width_)) {
00044 width_ = 0.14 * M_PI;
00045 }
00046 ros::SubscriberStatusCallback connect_cb = boost::bind(&PointCloudFilter::connectCb, this);
00047 pub_ = nh.advertise<sensor_msgs::PointCloud2>("points_filtered", 10, connect_cb, connect_cb);
00048 }
00049
00050 PointCloudFilter::~PointCloudFilter()
00051 {
00052 }
00053
00054 void PointCloudFilter::connectCb()
00055 {
00056 boost::lock_guard<boost::mutex> lock(connect_mutex_);
00057 if (!pub_.getNumSubscribers()) {
00058 sub_.shutdown();
00059 } else if (!sub_) {
00060 sub_ = nh_.subscribe("points", 10, &PointCloudFilter::recvCallback, this);
00061 }
00062 }
00063
00064 static inline float normalizeAnglePositive(float angle)
00065 {
00066 return fmodf(fmodf(angle, 2.0*M_PI) + 2.0*M_PI, 2.0*M_PI);
00067 }
00068
00069 void PointCloudFilter::recvCallback(const sensor_msgs::PointCloud2Ptr& msg)
00070 {
00071 #if 0
00072 ros::WallTime tic = ros::WallTime::now();
00073 #endif
00074
00075 if (width_ == 0.0) {
00076 pub_.publish(msg);
00077 return;
00078 }
00079
00080 const float ANGLE_WIDTH = std::min(fabs(width_), M_PI_4);
00081 const float ANGLE_START = M_PI_4;
00082 const float ANGLE_INC = M_PI_2;
00083
00084
00085 #if 0 // atan2() is slow
00086 for (sensor_msgs::PointCloud2Iterator<float> it(*msg, "x"); it != it.end(); it.operator ++()) {
00087 const float angle = normalizeAnglePositive(atan2f(it[1], it[0]));
00088 for (unsigned int j = 0; j < 4; j++) {
00089 const float a1 = ANGLE_START + (j * ANGLE_INC) - (ANGLE_WIDTH / 2);
00090 const float a2 = ANGLE_START + (j * ANGLE_INC) + (ANGLE_WIDTH / 2);
00091 if ((a2 > angle) && (angle > a1)) {
00092 it[2] = NAN;
00093 break;
00094 }
00095 }
00096 }
00097 #elif 1 // unit vector is ~3x faster
00098 const float UNIT_X_LOW = cosf(ANGLE_START + (ANGLE_WIDTH / 2));
00099 const float UNIT_X_HI = cosf(ANGLE_START - (ANGLE_WIDTH / 2));
00100 for (sensor_msgs::PointCloud2Iterator<float> it(*msg, "x"); it != it.end(); it.operator ++()) {
00101 const float x = it[0];
00102 const float y = it[1];
00103 const float unit_x = fabsf(x / sqrtf(x * x + y * y));
00104 if ((UNIT_X_HI > unit_x) && (unit_x > UNIT_X_LOW)) {
00105 it[2] = NAN;
00106 }
00107 }
00108 #endif
00109
00110 pub_.publish(msg);
00111
00112 #if 0
00113 ros::WallTime toc = ros::WallTime::now();
00114 ROS_INFO("PointCloudFilter completed in %04uus", (unsigned int)((toc - tic).toNSec() / 1000));
00115 #endif
00116 }
00117
00118 }