point_map_nodelet.cpp

Go to the documentation of this file.

//##################
//#### includes ####

// standard includes
//--
#include <sstream>
#include <fstream>


// ROS includes
#include <ros/ros.h>
#include <ros/console.h>
//#include <actionlib/server/simple_action_server.h>
#include <pluginlib/class_list_macros.h>
#include <dynamic_reconfigure/server.h>
#include <cob_3d_mapping_point_map/point_map_nodeletConfig.h>

// PCL includes
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl_ros/point_cloud.h>
#include <nodelet/nodelet.h>
//#include <pcl/filters/extract_indices.h>
//#include <visualization_msgs/Marker.h>

// ROS message includes
#include <cob_3d_mapping_msgs/SetPointMap.h>
#include <cob_3d_mapping_msgs/GetPointMap.h>
#include <cob_srvs/Trigger.h>


//####################
//#### node class ####
class PointMapNodelet : public nodelet::Nodelet//, protected Reconfigurable_Node<cob_3d_mapping_point_map::point_map_nodeletConfig>
{
  typedef pcl::PointXYZRGB Point;

public:
  // Constructor
  PointMapNodelet()
  : ctr_(0),
    is_running_(false),
    map_frame_id_("/map")
    {

    map_.header.frame_id = map_frame_id_;
    }


  // Destructor
  ~PointMapNodelet()
  {
    // void
  }

  void
  dynReconfCallback(cob_3d_mapping_point_map::point_map_nodeletConfig &config, uint32_t level)
  {
    file_path_ = config.file_path;
    save_ = config.save;
    voxel_leafsize_ = config.voxel_leafsize;
    map_frame_id_ = config.map_frame_id;
    map_.header.frame_id = map_frame_id_;
  }


  void
  onInit()
  {
    n_ = getNodeHandle();

    config_server_ = boost::shared_ptr<dynamic_reconfigure::Server<cob_3d_mapping_point_map::point_map_nodeletConfig> >(new dynamic_reconfigure::Server<cob_3d_mapping_point_map::point_map_nodeletConfig>(getPrivateNodeHandle()));
    config_server_->setCallback(boost::bind(&PointMapNodelet::dynReconfCallback, this, _1, _2));

    point_cloud_sub_ = n_.subscribe("point_cloud2", 1, &PointMapNodelet::updateCallback, this);
    map_pub_ = n_.advertise<pcl::PointCloud<Point> >("map",1);
    clear_map_server_ = n_.advertiseService("clear_map", &PointMapNodelet::clearMap, this);
    get_map_server_ = n_.advertiseService("get_map", &PointMapNodelet::getMap, this);
    set_map_server_ = n_.advertiseService("set_map", &PointMapNodelet::setMap, this);

    //n_.param("aggregate_point_map/file_path" ,file_path_ ,std::string("~/pcl_daten/table/icp/map_"));
    //n_.param("aggregate_point_map/save_",save_ , false);
    //n_.param("aggregate_point_map/voxel_leafsize" ,voxel_leafsize_, 0.03);
  }

  void
  updateCallback(const pcl::PointCloud<Point>::Ptr& pc)
  {
    ROS_DEBUG("PointCloudSubCallback");
    if (pc->size() < 1)
    {
      ROS_WARN("[point_map] Incoming point cloud is empty, skipping map update.");
      return;
    }
    if(pc->header.frame_id != map_frame_id_)
    {
      ROS_ERROR("[point_map] Frame ID of incoming point cloud (%s) does not match map frame ID (%s), aborting...", pc->header.frame_id.c_str(), map_frame_id_.c_str());
      return;
    }

    updateMap(pc);
    map_pub_.publish(map_);

    ROS_DEBUG("[point_map] Updated map has %d points", map_.size());

    if(save_)
    {
      std::stringstream ss;
      ss << file_path_ << "/map_" << ctr_ << ".pcd";
      pcl::io::savePCDFileASCII (ss.str(), map_);
    }
    ctr_++;
    return;
  }

  void
  updateMap(const pcl::PointCloud<Point>::Ptr& pc)
  {
    map_ += *pc;
    downsampleMap();
  }


  /*void
  actionCallback(const cob_3d_mapping_msgs::TriggerGoalConstPtr &goal)
  {
    //boost::mutex::scoped_lock l(m_mutex_actionCallback);

    cob_3d_mapping_msgs::TriggerResult result;
    if(goal->start && !is_running_)
    {
      ROS_INFO("Starting mapping...");
      point_cloud_sub_ = n_.subscribe("point_cloud2", 1, &PointMapNodelet::updateCallback, this);
      //point_cloud_sub_.subscribe(n_, "point_cloud2", 1);
      //transform_sub_.subscribe(n_, "transform_reg", 1);
      is_running_ = true;
    }
    else if(!goal->start && is_running_)
    {
      ROS_INFO("Stopping mapping...");
      point_cloud_sub_.shutdown();//unsubscribe();
      //transform_sub_.unsubscribe();
      //first_ = true;
      is_running_ = false;
    }
    as_->setSucceeded(result);
  }*/

  bool
  clearMap(cob_srvs::Trigger::Request &req,
           cob_srvs::Trigger::Response &res)
  {
    //TODO: add mutex
    ROS_INFO("Clearing point map...");
    map_.clear();
    Point p;
    map_.points.push_back(p);
    map_pub_.publish(map_);
    return true;
  }

  bool
  getMap(cob_3d_mapping_msgs::GetPointMap::Request &req,
         cob_3d_mapping_msgs::GetPointMap::Response &res)
  {
    pcl::toROSMsg(map_, res.map);
    return true;
  }

  bool
  setMap(cob_3d_mapping_msgs::SetPointMap::Request &req,
                  cob_3d_mapping_msgs::SetPointMap::Response &res)
  {
    pcl::fromROSMsg(req.map, map_);
    downsampleMap();
    map_pub_.publish(map_);
    //ROS_WARN("not needed");
    return true;
  }


  void
  downsampleMap()
  {
    pcl::VoxelGrid<Point> vox_filter;
    vox_filter.setInputCloud(map_.makeShared());
    vox_filter.setLeafSize(voxel_leafsize_,voxel_leafsize_,voxel_leafsize_);
    vox_filter.filter(map_);
  }


  ros::NodeHandle n_;


protected:
  ros::Subscriber point_cloud_sub_;     //subscriber for input pc
  ros::Publisher map_pub_;              //publisher for map
  ros::ServiceServer clear_map_server_;
  ros::ServiceServer get_map_server_;
  ros::ServiceServer set_map_server_;

  boost::shared_ptr<dynamic_reconfigure::Server<cob_3d_mapping_point_map::point_map_nodeletConfig> > config_server_;

  unsigned int ctr_;
  bool is_running_;

  // Parameters from launch file or dynamic config
  std::string file_path_;
  bool save_;
  std::string map_frame_id_;
  double voxel_leafsize_;

  pcl::PointCloud<Point> map_;

};

PLUGINLIB_DECLARE_CLASS(cob_3d_mapping_point_map, PointMapNodelet, PointMapNodelet, nodelet::Nodelet)