ROS之rviz显示历史运动轨迹(Marker--自己看的笔记)

#include <ros/ros.h>
#include <std_msgs/String.h>

#include <geometry_msgs/Point.h>
#include <visualization_msgs/Marker.h>

visualization_msgs::Marker mark1;
visualization_msgs::Marker mark2;
visualization_msgs::Marker mark3;

void callback1(const geometry_msgs::Point::ConstPtr& msg)
{

    geometry_msgs::Point p1;

    p1.x = (msg->x)/1000;
    p1.y = (msg->y)/1000;
    p1.z = (msg->z)/1000;
    if(mark1.points.size() >= 40)
    {    //当这个ｖｅｃｔｅｒ的长度大于４０个，就删除前面的数
        mark1.points.erase(mark1.points.begin());
    }
    mark1.points.push_back(p1);            
}

void callback2(const geometry_msgs::Point::ConstPtr& msg)
{

    geometry_msgs::Point p2;

    p2.x = (msg->x)/1000;
    p2.y = (msg->y)/1000;
    p2.z = (msg->z)/1000;
    if(mark2.points.size() >= 40)
    {
        mark2.points.erase(mark2.points.begin());
    }
    mark2.points.push_back(p2);            
}

void callback3(const geometry_msgs::Point::ConstPtr& msg)
{

    geometry_msgs::Point p3;

    p3.x = (msg->x)/1000;
    p3.y = (msg->y)/1000;
    p3.z = (msg->z)/1000;
    if(mark3.points.size() >= 40)
    {
        mark3.points.erase(mark3.points.begin());
    }
    mark3.points.push_back(p3);            
}

int main(int argc, char** argv)
{
    std::string sub_topic;
    ros::init(argc, argv, "marker");
    ros::NodeHandle n;
    
    ros::Publisher pub1 = n.advertise<visualization_msgs::Marker>("/dobot_maker1",100);
    ros::Publisher pub2 = n.advertise<visualization_msgs::Marker>("/dobot_maker2",100);
    ros::Publisher pub3 = n.advertise<visualization_msgs::Marker>("/dobot_maker3",100);
    ros::Subscriber sub1 = n.subscribe("follow_target1", 100, callback1);
    ros::Subscriber sub2 = n.subscribe("follow_target2", 100, callback2);
    ros::Subscriber sub3 = n.subscribe("follow_target3", 100, callback3);
    
    ros::Rate r(10);
    
    while(ros::ok())
    {
        mark1.action = mark1.ADD;
        mark1.header.stamp = ros::Time::now();
        mark1.header.frame_id = "kinect_frame_optical";
        mark1.lifetime = ros::Duration();
        mark1.ns = "markers";
        mark1.id = 0;
        mark1.type = mark1.LINE_STRIP;
        mark1.pose.orientation.w = 1.0;
        mark1.scale.x = 0.1;
        mark1.scale.y = 0.1;
        mark1.color.r = 255.0/255.0;    
        mark1.color.g = 0.0/255.0;
        mark1.color.b = 0.0/255.0;
        mark1.color.a = 1.0;

        mark2.action = mark2.ADD;
        mark2.header.stamp = ros::Time::now();
        mark2.header.frame_id = "kinect_frame_optical";
        mark2.lifetime = ros::Duration();
        mark2.ns = "markers";
        mark2.id = 0;
        mark2.type = mark2.LINE_STRIP;
        mark2.pose.orientation.w = 1.0;
        mark2.scale.x = 0.1;
        mark2.scale.y = 0.1;
        mark2.color.r = 0.0/255.0;    
        mark2.color.g = 255.0/255.0;
        mark2.color.b = 0.0/255.0;
        mark2.color.a = 1.0;

        mark3.action = mark3.ADD;
        mark3.header.stamp = ros::Time::now();
        mark3.header.frame_id = "kinect_frame_optical";
        mark3.lifetime = ros::Duration();
        mark3.ns = "markers";
        mark3.id = 0;
        mark3.type = mark3.LINE_STRIP;
        mark3.pose.orientation.w = 1.0;
        mark3.scale.x = 0.1;
        mark3.scale.y = 0.1;
        mark3.color.r = 0.0/255.0;    
        mark3.color.g = 0.0/255.0;
        mark3.color.b = 255.0/255.0;
        mark3.color.a = 1.0;

          ros::spinOnce();
          pub1.publish(mark1);
        pub2.publish(mark2);
        pub3.publish(mark3);

        r.sleep();

    }

    return 0;

}