get_geometry_map.cpp

Go to the documentation of this file.

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

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

// ROS message includes
#include <cob_3d_mapping_msgs/GetGeometryMap.h>
#include <cob_3d_mapping_msgs/ShapeArray.h>

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


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

  ros::NodeHandle nh;

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

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

  //build message
  cob_3d_mapping_msgs::GetGeometryMapRequest req;
  cob_3d_mapping_msgs::GetGeometryMapResponse resp;

  if (ros::service::call("/geometry_map/get_map", req,resp))
  {
    ROS_INFO("Service call finished.");
  }
  else
  {
    ROS_INFO("Service call failed.");
    return 0;
  }

  rosbag::Bag bag;
  bag.open(argv[1], rosbag::bagmode::Write);
  bag.write("/geometry_map/map_array", resp.map.header.stamp, resp.map);

  bag.close();

  //exit
  return 0;
}