Marker display points Overwrite
I would like to display Marker points with rviz. However I wish to have the points overwrite (persistent) in every iteration and not plot new points in each iteration.
I am plotting a set of 50 points per iteration. I want the points to accumulate, ie. at 2nd iteration I have 100 points on display on 3rd iteration 150 points and so on.
I noticed the documentation link:rviz/DisplayType/Marker have a parameter lifetime. I tried setting it to zero as in the code but still cannot get that effect. How can I achieve it?
#include <ros/ros.h>
#include <visualization_msgs/Marker.h>
#include <geometry_msgs/Point.h>
int main( int argc, char ** argv )
{
ros::init(argc, argv, "viz3d");
ros::NodeHandle nh;
ros::Publisher pub = nh.advertise<visualization_msgs::Marker>("viz_marker", 100 );
ros::Rate rate(2);
cv::Mat frame, dframe;
ros::Time stamp = ros::Time::now();
while( ros::ok() )
{
ROS_INFO_STREAM( "Publishing point " );
visualization_msgs::Marker msg;
msg.header.frame_id = "/my_frame";
msg.header.stamp = stamp;
msg.ns = "xuv";
msg.id = 0;
msg.action = visualization_msgs::Marker::ADD;
ros::Duration du(0.0);
msg.lifetime = du;
msg.type = visualization_msgs::Marker::POINTS;
msg.scale.x = 0.02;
msg.scale.y = 0.02;
for( int i=0; i<50 ; i++)
{
geometry_msgs::Point pt;
pt.x = drand48();
pt.y = 10*drand48();
pt.z = drand48();
std_msgs::ColorRGBA col;
col.r = 1.0;
col.g = 0.0;
col.b = 0.0;
col.a = 1.0;
msg.colors.push_back(col);
msg.points.push_back(pt);
}
msg.color.r = 0.0;
msg.color.g = 0.0;
msg.color.b = 0.0;
msg.color.a = 1.0;
pub.publish(msg);
rate.sleep();
}
}