filter_pointcloud2_z_value_alg.cpp

Go to the documentation of this file.

#include "filter_pointcloud2_z_value_alg.h"

FilterPointcloud2ZValueAlgorithm::FilterPointcloud2ZValueAlgorithm(void)
{
}

FilterPointcloud2ZValueAlgorithm::~FilterPointcloud2ZValueAlgorithm(void)
{
}

void FilterPointcloud2ZValueAlgorithm::config_update(Config& new_cfg, uint32_t level)
{
    this->lock();

    // save the current configuration
    this->config_=new_cfg;

    this->unlock();
}

// FilterPointcloud2ZValueAlgorithm Public API

bool FilterPointcloud2ZValueAlgorithm::apply_z_value_filter(const sensor_msgs::PointCloud2::ConstPtr& msg)
{
    nan_pc_ = *msg;
    max_pc_ = *msg;

    for (unsigned int rr=0; rr<msg->height; ++rr){
        for (unsigned int cc=0; cc<msg->width; ++cc){
            int idx0 = rr*msg->width + cc;
            float *nan_step = (float*)&nan_pc_.data[idx0 * nan_pc_.point_step];
            float *max_step = (float*)&max_pc_.data[idx0 * max_pc_.point_step];

            // Bounding Box Threshold
            //std::cout << std::endl << " z_e: " << nan_step[2];
            if ( nan_step[2] > z_thr_)
            {
                nan_step[2] = std::numeric_limits<float>::quiet_NaN ();
                max_step[2] = 7.5f;
                nan_pc_.is_dense = false;
            }
        }
    }
    return true;
}

sensor_msgs::PointCloud2* FilterPointcloud2ZValueAlgorithm::get_nan_pc(void)
{
  return &nan_pc_;
}

sensor_msgs::PointCloud2* FilterPointcloud2ZValueAlgorithm::get_max_pc(void)
{
  return &max_pc_;
}

void FilterPointcloud2ZValueAlgorithm::set_z_thr(double z_thr)
{
  z_thr_ = z_thr;
}