ROS: C++实现节点发布多边形，通过RVIZ查看

C++实现节点发布多边形，通过RVIZ查看

例子主要使用visualization_msgs::Marker 这个消息，通过节点发布这个消息，然后在rviz上订阅这个topic，即可以显示出画的多边形。

#include <ros/ros.h> 
#include <iostream>
#include <visualization_msgs/Marker.h>
#include <string>

struct Point {
public:
    float x;
    float y;

    Point() 
    {
      x = 0.0;
      y = 0.0;
    };
    Point(const float xx, const float yy):x(xx), y(yy) 
    {};

};


int main (int argc, char** argv) 
{ 
  //初始化节点 
  ros::init(argc, argv, "serial_example_node"); 
  //声明节点句柄 
  ros::NodeHandle nh; 

  ros::Publisher polygon_pub = nh.advertise<visualization_msgs::Marker>("polygon_marker", 1, true);
  
  std::vector<Point> polygon;
  polygon.push_back(Point(-1.8578, -4.1203));
  polygon.push_back(Point(-3.453, -4.0748));
  polygon.push_back(Point(-3.4025, -2.58));
  polygon.push_back(Point(-1.8183, -2.5885));

    visualization_msgs::Marker marker;
    marker.header.frame_id = "map";
    marker.header.stamp = ros::Time::now();
    marker.ns = "polygon";
    marker.id = 0;
    marker.type = visualization_msgs::Marker::LINE_STRIP;
    marker.action = visualization_msgs::Marker::ADD;
    marker.pose.orientation.w = 1.0;
    marker.scale.x = 0.1;
    marker.color.r = 1.0;
    marker.color.a = 1.0;
    
      for(int i = 0; i < polygon.size(); i++)
      {
        geometry_msgs::Point p;

        p.x = polygon[i].x;
        p.y = polygon[i].y;
        p.z = 0.0;
        marker.points.push_back(p);
      }

      geometry_msgs::Point p;

      p.x = polygon[0].x;
      p.y = polygon[0].y;
      p.z = 0.0;
      marker.points.push_back(p);

  //指定循环的频率 
  ros::Rate loop_rate(5); 
  while(ros::ok()) 
  { 
    polygon_pub.publish(marker);

  //处理ROS的信息，比如订阅消息,并调用回调函数 
  ros::spinOnce(); 
  loop_rate.sleep(); 
  } 
} 

节点启动后，启动rviz订阅topic即可，如下图。

下图中红色矩形框即为显示的多边形。