logging.cpp

Go to the documentation of this file.

#include "toposens_pointcloud/logging.h"

namespace toposens_pointcloud
{
Logging::Logging(ros::NodeHandle nh, ros::NodeHandle private_nh)
{
  int pcd_save_interval = 0;
  std::string target_frame;
  private_nh.param<int>("pcd_save_interval", pcd_save_interval, 0);
  private_nh.param<std::string>("target_frame", target_frame, "toposens");
  private_nh.param<std::string>("pcd_path", pcd_path_, "");

  if (pcd_path_ == "") pcd_path_ = std::string(get_current_dir_name()) + "/" + "toposens.pcd";

  cloud_sub_ = nh.subscribe(kPointCloudTopic, 100, &Logging::_accumulate, this);

  timer_ = nh.createTimer(ros::Duration(pcd_save_interval), &Logging::save,
                          this);  // continuous timer

  store_ = boost::make_shared<XYZINCloud>();
  pcl_conversions::toPCL(ros::Time::now(), store_->header.stamp);
  store_->header.frame_id = target_frame;
  store_->height = 1;
}

Logging::~Logging() { timer_.stop(); };

void Logging::save(const ros::TimerEvent &event)
{
  boost::mutex::scoped_lock lock(store_mutex_);

  if (!store_->width)
  {
    ROS_WARN("No pointcloud data to save.");
    return;
  }
  pcl_conversions::toPCL(ros::Time::now(), store_->header.stamp);
  try
  {
    if (pcl::io::savePCDFile(pcd_path_, *store_) == 0)
    {
      ROS_INFO("Saved latest point cloud data (%s)", pcd_path_.c_str());
    }
  }
  catch (pcl::io::IOException &e)
  {
    ROS_ERROR("%s", e.what());
  }
}

void Logging::_accumulate(const XYZINCloud::ConstPtr &msg)
{
  boost::mutex::scoped_lock lock(store_mutex_);

  for (auto it = msg->points.begin(); it != msg->points.end(); ++it)
  {
    pcl::PointXYZINormal pt = *it;
    store_->points.push_back(pt);
  }

  store_->width = store_->points.size();
}

}  // namespace toposens_pointcloud