pointcloud_to_octomap_alg.cpp

Go to the documentation of this file.

#include "pointcloud_to_octomap_alg.h"
#include <limits>
PointcloudToOctomapAlgorithm::PointcloudToOctomapAlgorithm(void)
{
}

PointcloudToOctomapAlgorithm::~PointcloudToOctomapAlgorithm(void)
{
}

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

  // save the current configuration
  this->config_=new_cfg;
  
  this->unlock();
}

// PointcloudToOctomapAlgorithm Public API

bool PointcloudToOctomapAlgorithm::isRunning(const std::string& file_name, const sensor_msgs::PointCloud2::ConstPtr& msg, const octomap::point3d& sensor_origin, const octomap::pose6d& frame_origin)
{
    octomap::Pointcloud scan;
    for(unsigned int rr=0; rr<msg->height; ++rr){
        for(unsigned int cc=0; cc<msg->width; ++cc){
            unsigned int idx = rr*msg->width + cc;
            float *step = (float*)&msg->data[idx*msg->point_step];
            if (!isnan(step[0]) && !isnan(step[1]) && !isnan(step[2])){
                scan.push_back(step[0], step[1], step[2]);
            }
        }
    }

    octomap::ColorOcTree occupancy_tree(0.004f);
    occupancy_tree.insertScan(scan, sensor_origin, frame_origin);
    occupancy_tree.write(file_name);
    //pcl::io::savePCDFile ( file_name, *cloud, origin, orientation, binary_mode );
    return true;
}