ros路径可视化的一种方案

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

#include <sstream> //字符串转换
#include <fstream>
#include <string>
#include <vector>
#include <iomanip>
#include <algorithm>
using namespace std;
vector<double> location_x, location_y;
void ReadPath()
{
    ifstream fin("src/using_markers/files/123.txt");
    string line;
    string s1 = "path";
    string s2="pathmode";
    
    if(fin) 
    {
        while (getline(fin, line)) //按行读取到line_info中
        {
            //把每一行所有的冒号替换成逗号
            replace(line.begin(),line.end(),':',',');
            //筛选掉开头不是path的行
            int index=line.find(s1);
            int index2=line.find(s2);
            vector<string> Waypoints;
            string info;
            if(index==0 && index2==-1)
            {
                //cout<<line<<endl;
                istringstream sin(line); //create string input object
                while (getline(sin, info, ','))  
                {
                    //replace(info.begin(),info.end(),':',',');
                    //cout << "info:" << info << endl;
                    Waypoints.push_back(info);
                }
                string x_str = Waypoints[1];
                string y_str = Waypoints[2];
                double x, y;
                stringstream sx, sy; //transform string to double
                sx << x_str;
                sy << y_str;

                sx >> x;
                sy >> y;
                location_x.push_back(x);
                location_y.push_back(y);
            }
        }
    }
    else 
    {
        cout<<"no such file"<<endl;;
    }

    // for(int j=0;j<location_x.size();j++)
    // {
    //     cout<<setprecision(10)<<"location_x[" << j << "]: " << location_x[j]<<endl;
    //     cout<<setprecision(10)<<"location_x[" << j << "]: " << location_y[j]<<endl;
    //     cout<< endl;
    // }
}




int main( int argc, char** argv )
{
    setlocale(LC_ALL,"");

    ros::init(argc, argv, "points_and_lines");
    ros::NodeHandle n;
    ros::Publisher marker_pub = n.advertise<visualization_msgs::Marker>("visualization_marker", 10);
    ros::Rate r(30);

    float f = 0.0;
    ReadPath();
    while (ros::ok())
    {

        //创建一个 visualization_msgs/Marker消息，并且初始化所有共享的数据。消息成员默认为0，仅仅设置位姿成员w。
        visualization_msgs::Marker points, line_strip, line_list;
        points.header.frame_id = line_strip.header.frame_id = line_list.header.frame_id = "/base_link";
        points.header.stamp = line_strip.header.stamp = line_list.header.stamp = ros::Time::now();
        points.ns = line_strip.ns = line_list.ns = "points_and_lines";
        points.action = line_strip.action = line_list.action = visualization_msgs::Marker::ADD;
        points.pose.orientation.w = line_strip.pose.orientation.w = line_list.pose.orientation.w = 1.0;


        //分配三个不同的id到三个markers。points_and_lines名称空间的使用确保彼此不会相互冲突。
        points.id = 0;
        line_strip.id = 1;
        //line_list.id = 2;


        //设置marker类型到 POINTS, LINE_STRIP 和 LINE_LIST
        points.type = visualization_msgs::Marker::POINTS;
        line_strip.type = visualization_msgs::Marker::LINE_STRIP;
        //line_list.type = visualization_msgs::Marker::LINE_LIST;



        // scale成员对于这些marker类型是不同的,POINTS marker分别使用x和y作为宽和高，然而LINE_STRIP和LINE_LIST marker仅仅使用x，定义为线的宽度。单位是米。
        points.scale.x = 0.2;
        points.scale.y = 0.2;

        // LINE_STRIP/LINE_LIST markers use only the x component of scale, for the line width
        line_strip.scale.x = 0.1;
    // line_list.scale.x = 0.1;

        // 点为绿色
        points.color.g = 1.0f;
        points.color.a = 1.0;

        // Line strip 是蓝色
        line_strip.color.b = 1.0;
        line_strip.color.a = 1.0;


        //使用正弦和余弦生成螺旋结构。POINTS和LINE_STRIP markers都仅仅需要1个点作为每个顶点，然而LINE_LIST marker需要2个点 。
        for (uint32_t i = 0; i < location_x.size(); ++i)
        {
        float y = 0;
        float z = 0;

        geometry_msgs::Point p;
        p.x = location_x[i];
        p.y = location_y[i];
        // p.x = location_x[i];
        // p.y = location_y[i];
        cout<<p.x<<" "<<p.y<<endl;
        p.z = 0;

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

        // The line list needs two points for each line
        line_list.points.push_back(p);
        p.z += 0.0;
        //line_list.points.push_back(p);
        }

        //发布各个markers
        marker_pub.publish(points);
        marker_pub.publish(line_strip);
        //marker_pub.publish(line_list);

        r.sleep();
        f += 0.04;
    }
    }

做导航系统时候写的，用的visualization_msgs::Marker通过点线构成。实现农机路径可视化
代码链接：链接：https://pan.baidu.com/s/15GnwAFFmpXmiPWrz_eHfWw?pwd=lgdi
提取码：lgdi