#include "geometry_msgs/PolygonStamped.h"
#include <iostream>

int main()
{
  geometry_msgs::PolygonStamped poly_stamped;

  poly_stamped.header.seq = 1;
  poly_stamped.header.stamp.sec = 1625079042;
  poly_stamped.header.stamp.nsec = 123456789;
  poly_stamped.header.frame_id = "map";
  poly_stamped.polygon.points.resize(3);
  poly_stamped.polygon.points[0].x = 1.1;
  poly_stamped.polygon.points[0].y = 2.5;
  poly_stamped.polygon.points[0].z = 0.8;
  poly_stamped.polygon.points[1].x = 2.1;
  poly_stamped.polygon.points[1].y = 2.3;
  poly_stamped.polygon.points[1].z = 0.3;
  poly_stamped.polygon.points[2].x = 3.2;
  poly_stamped.polygon.points[2].y = 5.9;
  poly_stamped.polygon.points[2].z = 4.6;

  std::cout << "PolygonStamped:" << std::endl;
  std::cout << " Header:" << std::endl;
  std::cout << "  seq: " << poly_stamped.header.seq << std::endl;
  std::cout << "  stamp: " << poly_stamped.header.stamp.sec << "s " << poly_stamped.header.stamp.nsec << "ns" << std::endl;
  std::cout << "  frame_id: " << poly_stamped.header.frame_id << std::endl;
  std::cout << " Polygon points:" << std::endl;
  for (size_t i = 0; i < poly_stamped.polygon.points.size(); ++i)
  {
    std::cout << "  Point " << i << ": ("
              << poly_stamped.polygon.points[i].x << ", "
              << poly_stamped.polygon.points[i].y << ", "
              << poly_stamped.polygon.points[i].z << ")" << std::endl;
  }


  return 0;
}