ROS RViz基本学习笔记（一）

目标：利用ROS RViz写出一个可以实现可视化的视觉自定位3D展示

环境： ubuntu16.04,ROS_kinetic

前期工作：配置好相关环境，可以跑通RViz/Tutorials

链接：http://wiki.ros.org/rviz/Tutorials/Markers%3A%20Basic%20Shapes

一 、初始化 

catkin_create_pkg mapping roscpp visualization_msgs

cd src/mapping/src

gedit mapping.cpp

二、程序部分

#include <ros/ros.h>
#include <visualization_msgs/Marker.h>
#include <cmath>
main(int argc, char** argv){
    ros::init(argc , argv , "mapping");
    ros::NodeHandle n;
    ros::Rate per(10);

    ros::Publisher pub = n.advertise<visualization_msgs::Marker>("mapping_marker",1000);

    while(ros::ok()){
        visualization_msgs::Marker points,lines[11],circle;
        visualization_msgs::Marker door[2];

        points.header.frame_id = circle.header.frame_id = door[0].header.frame_id = door[1].header.frame_id = "/my_frame";
        for(int lines_num = 0;lines_num < 11;lines_num++){
             lines[lines_num].header.frame_id = "/my_frame";
            }

        points.header.stamp = circle.header.stamp = door[0].header.stamp = door[1].header.stamp = ros::Time::now();
        for(int lines_num = 0;lines_num < 11;lines_num++){
             lines[lines_num].header.stamp = ros::Time::now();
            }

        points.ns = circle.ns = door[0].ns = door[1].ns = "mapping";
        for(int lines_num = 0;lines_num < 11;lines_num++){
             lines[lines_num].ns = "mapping";
            }

        points.action = circle.action = door[0].action = door[1].action = visualization_msgs::Marker::ADD;
        for(int lines_num = 0;lines_num < 11;lines_num++){
             lines[lines_num].action = visualization_msgs::Marker::ADD;
            }

        points.id = 0;
        circle.id = 1;
        door[0].id = 2;
        door[1].id = 3;
        for(int lines_num = 0;lines_num < 11;lines_num++){
             lines[lines_num].id = 4+lines_num;
            }

        points.type = visualization_msgs::Marker::POINTS;
        for(int lines_num = 0;lines_num < 11;lines_num++){
             lines[lines_num].type = visualization_msgs::Marker::LINE_STRIP;
            }
        circle.type = visualization_msgs::Marker::LINE_LIST;
        door[0].type = visualization_msgs::Marker::CYLINDER;
        door[1].type = visualization_msgs::Marker::CYLINDER;

        /init of points and lines
        points.scale.x = 0.2;
        points.scale.y = 0.2;
        points.color.g = 1.0;
        points.color.a = 1.0f;

        for(int lines_num = 0;lines_num < 11;lines_num++){
            lines[lines_num].scale.x = 0.1;
            lines[lines_num].scale.y = 0.1;
            lines[lines_num].color.r = 1.0;
            lines[lines_num].color.g = 1.0;
            lines[lines_num].color.b = 1.0;
            lines[lines_num].color.a = 1.0;
            }


        circle.sc