pose_storage_service.cpp

#include "ros/ros.h"
#include "panda_teaching/GetCollectedPoses.h"
#include "panda_teaching/SavePose.h"
#include <tf2_ros/transform_listener.h>
#include <geometry_msgs/TransformStamped.h>
#include <geometry_msgs/Twist.h>

std::vector<double> x_transform, y_transform, z_transform, x_rotation, y_rotation, z_rotation, w_rotation;

bool getCollectedPoses(panda_teaching::GetCollectedPoses::Request  &req,
             panda_teaching::GetCollectedPoses::Response &res){

    res.x_transform = x_transform;
    res.y_transform = y_transform;
    res.z_transform = z_transform;

    res.x_rotation = x_rotation;
    res.y_rotation = y_rotation;
    res.z_rotation = z_rotation;
    res.w_rotation = w_rotation;

    return true;
}

bool saveCollectedPose(panda_teaching::SavePose::Request  &req,
         panda_teaching::SavePose::Response &res){

  std::cout << "retrieved data: " << req << std::endl;
  x_transform.push_back((long) req.x_transform);
  y_transform.push_back((long) req.y_transform);
  z_transform.push_back((long) req.z_transform);

  x_rotation.push_back((long) req.x_rotation);
  y_rotation.push_back((long) req.y_rotation);
  z_rotation.push_back((long) req.z_rotation);
  w_rotation.push_back((long) req.w_rotation);

  return true;
}

int main(int argc, char **argv){

  ros::init(argc, argv, "pose_service_service");
  ros::NodeHandle n;

  ros::ServiceServer retrieve_service = n.advertiseService("GetCollectedPoses", getCollectedPoses);
  ros::ServiceServer save_service = n.advertiseService("SaveCollectedPose", saveCollectedPose);

  ROS_INFO("Ready to collect.");
  ros::spin();

  return 0;
}