set_point_map.cpp

Go to the documentation of this file.

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

// ROS includes
#include <ros/ros.h>
#include <actionlib/client/simple_action_client.h>
#include <actionlib/client/terminal_state.h>

// ROS message includes
#include <cob_3d_mapping_msgs/SetPointMap.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 path to map file\nrosrun cob_env_model set_point_map myfile.pcd");
    return -1;
  }
  ros::init(argc, argv, "set_point_map");

  ros::NodeHandle nh;

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

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

  //create message
  cob_3d_mapping_msgs::SetPointMapRequest req;

  pcl::PointCloud<pcl::PointXYZRGB> map;
  if(pcl::io::loadPCDFile(argv[1], map)!=0) {
    ROS_ERROR("Couldn't open pcd file. Sorry.");
    return -1;
  }

  pcl::PCLPointCloud2 map2;
  pcl::toPCLPointCloud2(map, map2);
  sensor_msgs::PointCloud2 cloud_msg;
  pcl_conversions::fromPCL(map2, cloud_msg);
  //pcl::toROSMsg(map,req.map);
  req.map.header.frame_id ="/map";
  cob_3d_mapping_msgs::SetPointMapResponse resp;

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

  return 0;
}