visualize_polygon.hpp

Go to the documentation of this file.

#ifndef VISUALIZE_POLYGON
#define VISUALIZE_POLYGON
#include <visualization_msgs/Marker.h>
#include <geometry_msgs/Point.h>

template <class PointCloudType>
visualization_msgs::Marker visualize_polygon(const PointCloudType& cropping_polygon)
{
  visualization_msgs::Marker polygon;
  polygon.header = cropping_polygon.header;
  polygon.id = 3;
  polygon.type = visualization_msgs::Marker::LINE_LIST;
  polygon.action = visualization_msgs::Marker::ADD;
  polygon.pose.orientation.w = 1.0;
  polygon.scale.x = 0.01;
  polygon.color.a = 0.5;
  polygon.color.r = 0.5;
  polygon.color.b = 1.0;
  for(unsigned int i = 0; i < cropping_polygon.size(); i++)
  {
    //Start line
    geometry_msgs::Point p;
    p.x = cropping_polygon[i].x;
    p.y = cropping_polygon[i].y;
    p.z = cropping_polygon[i].z;
    polygon.points.push_back(p);
    //End of line
    unsigned int j = (i+1) %cropping_polygon.size();
    p.x = cropping_polygon[j].x;
    p.y = cropping_polygon[j].y;
    p.z = cropping_polygon[j].z;
    polygon.points.push_back(p);
  }
  return polygon;
}

#endif