rviz中使用MarkerArray绘制地图线

参考内容：

1. ROS Tutorials

2. rviz - ROS Wiki

3. ROS在rviz中实时显示轨迹（nav_msgs/Path消息的使用）

4. ROS在rviz中的不同位置显示文字(visualization_msgs::MarkerArray的使用)

文章内容：

1. 读取导航坐标csv文件数据

2. 使用rviz - ros显示数据，得到MarkerArray的地图线条，即ROS数据可视化

 

一. 分析准备

1. rviz/DisplayTypes - Markers - visualization_msgs/MarkerArray的使用。

    因为依靠地图上的点连线来绘制地图，如果只使用visualization_msgs/Marker的LINE_STRIP=4，会出现地图线交叉，出现错误。所以使用visualization_msgs/MarkerArray将地图各个路段存入数组，分别进行绘制。

2. 使用MakerArray中的Line Strip (LINE_STRIP=4)来绘制线段，使用Points (POINTS=8)来绘制点

 

二. 地图线绘制

1. 新建工程

$ mkdir -p MapLine/src
$ cd MapLine
$ catkin_make
$ source devel/setup.bash

$ cd src
$ catkin_create_pkg using_markerarray roscpp visualization_msgs

2. 编辑node文件MapLine.cpp

$ roscd using_markerarray
$ mkdir -p src

   创建MapLine.cpp，如下：

/*
 * Show Line of CARLA 0.9.1 Town1
 */
#include 
#include 
#include 
#include 
#include 

#include 
#include 
#include 

#include 

using namespace std;

int main(int argc, char** argv)
{
    vector location_x, location_y, location_z;
    /* Read data from point_right.csv */
    ifstream fin("Topology.csv");
    string line;
    while (getline(fin, line)) 
    {
        istringstream sin(line);
        vector Waypoints;
        string info;
        while (getline(sin, info, ',')) {
            Waypoints.push_back(info);
        }
        // Get x,y,z of points and transform to double
        string x_str = Waypoints[3];
        string y_str = Waypoints[4];
        string z_str = Waypoints[5];
        double x, y, z;
        stringstream sx, sy, sz;
        sx << x_str;
        sy << y_str;
        sz << z_str;
        sx >> x;
        sy >> y;
        sz >> z;
        //cout << "x= " << x << "  " << "y= " << y << "  " << "z= " << z << endl;
        cout << "Read data done!" << endl;

        location_x.push_back(x);
        location_y.push_back(y);
        location_z.push_back(z);
    }

    /* Define Marker Line */
    ros::init(argc, argv, "MapLine");
    ros::NodeHandle n;
    ros::Publisher markerArray_pub = n.advertise("MarkerArray", 10);
    ros::Rate r(1);

    
    while (ros::ok())
    {
        visualization_msgs::MarkerArray lines, MapPoints;
        for (int i = 1; i < location_x.size(); i=i+2)
        {
            // Create lines and points
            visualization_msgs::Marker line_strip, points;
            line_strip.header.frame_id = points.header.frame_id = "/my_frame";
            line_strip.header.stamp = points.header.stamp = ros::Time::now();
            line_strip.ns = points.ns = "points_and_lines";
            line_strip.action = points.action = visualization_msgs::Marker::ADD;
            //line_strip.pose.orientation.w = 1.0;
            line_strip.id = i;
            line_strip.type = visualization_msgs::Marker::LINE_STRIP;
            line_strip.scale.x = 0.1;
            line_strip.color.g = 1.0;
            line_strip.color.a = 1.0;

            points.id = 0;
            points.type = visualization_msgs::Marker::POINTS;
            points.scale.x = 2;
            points.scale.y = 2;

            points.color.r = 1.0f;
            points.color.a = 1.0;

            // line_strip obtains points
            geometry_msgs::Point p, q;
            p.x = location_x[i];
            p.y = location_y[i];
            p.z = location_z[i];
            q.x = location_x[i+1];
            q.y = location_y[i+1];
            q.z = location_z[i+1];
            cout << i << "x= " << location_x[i] << "   " << "y= " << location_y[i] << endl; 
            cout << i+1 << "x= " << location_x[i+1] << "   " << "y= " << location_y[i+1] << endl;
            cout << endl << endl;

            line_strip.points.push_back(p);        
            line_strip.points.push_back(q);
            points.points.push_back(p);
            points.points.push_back(q);

            lines.markers.push_back(line_strip);
            MapPoints.markers.push_back(points);         
        }

        cout << "MarkerArray lines size: " << lines.markers.size() << endl;
        markerArray_pub.publish(lines);
        markerArray_pub.publish(MapPoints);
        r.sleep();
    }

    cout << "Right Line Done!";
    return 0;
}

 代码命令含义可以查看ROS官方rviz的实例，讲解的很清楚。可以对比本文中MarkerArray的使用了解。

3. CMakeLists.txt添加内容

    在最后加入：

add_executable(MapLine src/MapLine.cpp)
target_link_libraries(MapLine ${catkin_LIBRARIES})

4. 编译并运行：

$ cd ~/MapLine
$ catkin_make

$ rosrun using_markerarray MapLine

    查看/trajectory 信息

rostopic echo /trajectory

三. rviz中查看结果

    运行rviz

$ rosrun rviz rviz

    查看MarkerArray类型