wrapper.cpp

#include <ros/ros.h>

#include "std_msgs/String.h"

#include "geometry_msgs/Transform.h"
#include <sensor_msgs/PointCloud2.h>
// PCL specific includes, but mostly aren't needed
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <boost/make_shared.hpp>

#include "ethzasl_icp_mapper/MatchClouds.h"

#include "tf/tf.h"

// Global variables
sensor_msgs::PointCloud2 call_1_ptr;
sensor_msgs::PointCloud2 call_2_ptr;

void callback (const sensor_msgs::PointCloud2ConstPtr& msg)
{
  call_1_ptr = *msg;
}

void callback_r (const sensor_msgs::PointCloud2ConstPtr& msg)
{
  call_2_ptr = *msg;
}



int main (int argc, char** argv)
{
  // Initialize ROS
  ros::init (argc, argv, "wrapper");
  ros::NodeHandle nh;

  ros::Publisher resp_pub;

  ros::Subscriber front_sub = nh.subscribe<sensor_msgs::PointCloud2> ("/front_merged_cloud", 1, callback);
  ros::Subscriber rear_sub = nh.subscribe<sensor_msgs::PointCloud2> ("/rear_merged_cloud", 1, callback_r);
  bool latchOn = true;
  resp_pub = nh.advertise<geometry_msgs::Transform>("/lidars_transform", 1, latchOn);



  while(front_sub.getNumPublishers() == 0)
  {
    ROS_INFO("Waiting for PointCloud2 PUBLISHERS");
    ros::Duration(0.1).sleep();
  }
  ROS_INFO("Got PUBLISHERS");

  ros::Rate rate(2);

  while (ros::ok())
  {
    ethzasl_icp_mapper::MatchClouds srv;

    srv.request.reference = call_1_ptr;
    srv.request.readings  = call_2_ptr;

    ros::ServiceClient client = nh.serviceClient<ethzasl_icp_mapper::MatchClouds>("/match_clouds");
    ros::Publisher resp_pub = nh.advertise<geometry_msgs::Transform>("/lidars_transform", true);

    if (rear_sub.getNumPublishers() != 0 && client.call(srv))
    {

      if (client.call(srv) == true)
      {

          geometry_msgs::Transform resp = srv.response.transform;

          sleep(1.5); // in case

          ROS_INFO("Got a Transform:");
          ROS_INFO("x: %f, y: %f, z: %f", (float)resp.translation.x, (float)resp.translation.y, (float)resp.translation.z);

          sleep(1.5); // in another case
          resp_pub.publish(resp);

          return true;

      }

      else
      {
        ROS_WARN("Failed to call service");
        return 1;
      }
    }
    // Spin
    rate.sleep();
    ros::spinOnce();
  }
  return 0;
}