get_point_map.cpp

Go to the documentation of this file.

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

// ROS includes
#include <ros/ros.h>

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

// PCL includes
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/conversions.h>
#include <pcl_conversions/pcl_conversions.h>

int main (int argc, char **argv)
{
  if(argc<1) {
    ROS_ERROR("Please specify output file\nrosrun cob_3d_mapping_point_map get_map_client myfile.pcd");
    return -1;
  }
  ros::init(argc, argv, "get_point_map");

  ros::NodeHandle nh;

  ROS_INFO("Waiting for service server to start.");
  ros::service::waitForService("/point_map/get_map"); //will wait for infinite time

  ROS_INFO("Server started, polling map.");

  //build message
  cob_3d_mapping_msgs::GetPointMapRequest req;
  cob_3d_mapping_msgs::GetPointMapResponse resp;

  if (ros::service::call("/point_map/get_map", req,resp))
  {
    ROS_INFO("Service call finished.");
  }
  else
  {
    ROS_INFO("[get point map]: Service call failed.");
    return 0;
  }

  pcl::PointCloud<pcl::PointXYZRGB> map;
  pcl::PCLPointCloud2 map2;
  pcl_conversions::toPCL(resp.map, map2);
  pcl::fromPCLPointCloud2(map2, map);
  //pcl::fromROSMsg(resp.map, map);
  pcl::io::savePCDFile(argv[1],map,false);

  //exit
  return 0;
}