47 pub_ = nh.
advertise<sensor_msgs::PointCloud2>(
"points_filtered", 10, connect_cb, connect_cb);
66 return fmodf(fmodf(angle, 2.0*M_PI) + 2.0*M_PI, 2.0*M_PI);
80 const float ANGLE_WIDTH = std::min(fabs(
width_), M_PI_4);
81 const float ANGLE_START = M_PI_4;
82 const float ANGLE_INC = M_PI_2;
85 #if 0 // atan2() is slow 88 for (
unsigned int j = 0; j < 4; j++) {
89 const float a1 = ANGLE_START + (j * ANGLE_INC) - (ANGLE_WIDTH / 2);
90 const float a2 = ANGLE_START + (j * ANGLE_INC) + (ANGLE_WIDTH / 2);
91 if ((a2 > angle) && (angle > a1)) {
97 #elif 1 // unit vector is ~3x faster 98 const float UNIT_X_LOW = cosf(ANGLE_START + (ANGLE_WIDTH / 2));
99 const float UNIT_X_HI = cosf(ANGLE_START - (ANGLE_WIDTH / 2));
101 const float x = it[0];
102 const float y = it[1];
103 const float unit_x = fabsf(x / sqrtf(x * x + y * y));
104 if ((UNIT_X_HI > unit_x) && (unit_x > UNIT_X_LOW)) {
114 ROS_INFO(
"PointCloudFilter completed in %04uus", (
unsigned int)((toc - tic).toNSec() / 1000));
boost::function< void(const SingleSubscriberPublisher &)> SubscriberStatusCallback
void publish(const boost::shared_ptr< M > &message) const
PointCloudFilter(ros::NodeHandle nh, ros::NodeHandle priv_nh)
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
void recvCallback(const sensor_msgs::PointCloud2Ptr &msg)
boost::mutex connect_mutex_
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
uint32_t getNumSubscribers() const
bool getParam(const std::string &key, std::string &s) const
PointCloud2Iterator< T > end() const
static float normalizeAnglePositive(float angle)