Old data keeps being visualized in rviz
I have a callback lineVisualization
which publishes a MarkerArray containing lines. Inside rviz I visualize the lines. Now the problem I am facing is that some old lines which should not be detected anymore are showing up in rviz.
- Sometimes they disapear after a while but somethines they don't.
- If I then un-select the namespace from the MarkerArray and then disapear.
From the two above facts I observe I conclude that rviz for some reason stores my visualization data and keeps showing it? However since I never worked with Markers I wonder if I am not doing something wrong in my code.
The relevant code piece is displayed below.
void lineVisualization(const std_msgs::Float32MultiArray::ConstPtr& msgLines)
{
// Create marker and markerarray
visualization_msgs::MarkerArray lines;
cmd_pub.publish(lines);
int offset = 0;
// For each line
for(int i = 0; i < (int)(msgLines->data.size() / 4); i++, offset = 4*i)
{
visualization_msgs::Marker line;
// Init
line.header.frame_id = "/pico/base";
line.header.stamp = ros::Time::now();
line.lifetime = ros::Duration();
line.id = i;
line.ns = "HoughLines";
line.type = visualization_msgs::Marker::LINE_STRIP;
line.action = visualization_msgs::Marker::ADD;
line.pose.orientation.w = 1.0;
line.scale.x = 0.05;
line.color.r = 0.0f;
line.color.g = 0.0f;
line.color.b = 1.0f;
line.color.a = 1.0f;
// Create points
geometry_msgs::Point p1;
geometry_msgs::Point p2;
p1.x = (float)(msgLines->data[offset ] / 100);
p1.y = -(float)(msgLines->data[offset + 1] / 100);
p1.z = 0;
p2.x = (float)(msgLines->data[offset + 2] / 100);
p2.y = -(float)(msgLines->data[offset + 3] / 100);
p2.z = 0;
// Create line segment
line.points.push_back(p1);
line.points.push_back(p2);
// Add line to lines
lines.markers.push_back(line);
// Clear line point
line.points.clear();
}
// Publish
cmd_pub.publish(lines);
// Clear lines
lines.markers.clear();
}