MarkerArray publisher

[yifan96]

Hi, i am a bit confused with publishing MarkerArray in rospy. I can echo the /vis_maker topic in terminal but cannot visualize it in Rviz. If I put AddMarker() in while loop, the topic is published recursively and markers can be seen in Rviz. But why it cannot be visualized if AddMarker() function is outside the while loop? Thanks a lot for your help.

#! /usr/bin/env python3
import rospy
from geometry_msgs.msg import PointStamped, PoseWithCovarianceStamped, PoseStamped
from visualization_msgs.msg import Marker
from visualization_msgs.msg import MarkerArray
import tf
import random
import numpy as np
from nav_msgs.msg import Odometry, Path

global marker_pub
marker_pub = rospy.Publisher("/vis_marker", MarkerArray, queue_size=10)

def AddMarker():
    global  markerArray, marker_pub
    markerArray = MarkerArray()
    for i in range(5):
        marker = Marker()
        marker.header.frame_id = "map"
        marker.header.stamp = rospy.Time.now()

        # set shape, Arrow: 0; Cube: 1 ; Sphere: 2 ; Cylinder: 3
        marker.type = 1
        marker.action = marker.ADD
        marker.lifetime = rospy.Duration(0)

        # Set the scale of the marker
        marker.scale.x = 0.5
        marker.scale.y = 0.5
        marker.scale.z = 0.01

        # Set the color
        marker.color.r = 1.0
        marker.color.g = 0.0
        marker.color.b = 0.0
        marker.color.a = 1.0

        marker.id = i
       #the pose of the marke
        marker.pose.position.x = 2*i
        marker.pose.position.y = 2*i
        marker.pose.position.z = 0.0
        marker.pose.orientation.x = 0.0
        marker.pose.orientation.y = 0.0
        marker.pose.orientation.z = 0.0
        marker.pose.orientation.w = 1.0
        markerArray.markers.append(marker)
        marker_pub.publish(markerArray)

if __name__ == '__main__':

    rospy.init_node('tsp', anonymous=True)
    rate = rospy.Rate(10)
    rospy.loginfo("Starting...")  
    AddMarker()

    while not rospy.is_shutdown():

        rate.sleep()

[rhaschke]

Losing the first message is a known issue for ROS publishers. Just use a latched publisher:

#! /usr/bin/env python3
import rospy
from geometry_msgs.msg import PointStamped, PoseWithCovarianceStamped, PoseStamped
from visualization_msgs.msg import Marker
from visualization_msgs.msg import MarkerArray
import tf
import random
import numpy as np
from nav_msgs.msg import Odometry, Path

def AddMarker():
    markerArray = MarkerArray()
    for i in range(5):
        marker = Marker()
        marker.header.frame_id = "map"
        marker.header.stamp = rospy.Time.now()

        # set shape, Arrow: 0; Cube: 1 ; Sphere: 2 ; Cylinder: 3
        marker.type = 1
        marker.action = marker.ADD
        marker.lifetime = rospy.Duration(0)

        # Set the scale of the marker
        marker.scale.x = 0.5
        marker.scale.y = 0.5
        marker.scale.z = 0.01

        # Set the color
        marker.color.r = 1.0
        marker.color.g = 0.0
        marker.color.b = 0.0
        marker.color.a = 1.0

        marker.id = i
       #the pose of the marke
        marker.pose.position.x = 2*i
        marker.pose.position.y = 2*i
        marker.pose.position.z = 0.0
        marker.pose.orientation.x = 0.0
        marker.pose.orientation.y = 0.0
        marker.pose.orientation.z = 0.0
        marker.pose.orientation.w = 1.0
        markerArray.markers.append(marker)

    # publish() shouldn't be part of the for loop
    marker_pub.publish(markerArray)

if __name__ == '__main__':
    rospy.init_node('tsp', anonymous=True)
    rospy.loginfo("Starting...")
    # Use a latched publisher
    marker_pub = rospy.Publisher("/vis_marker", MarkerArray, queue_size=10, latch=True)
    AddMarker()
    rospy.spin()

[yifan96]

 def vis_finished(self, i):
        print('vis_finished called')
        self.num_finished_task += 1
        self.finished_task_vis.header.frame_id = "map"
        self.finished_task_vis.header.seq = 2
        #self.finished_task_vis.header.stamp = rospy.Time.now()
        self.finished_task_vis.id = self.num_finished_task
        #shape, Arrow: 0; Cube: 1 ; Sphere: 2 ; Cylinder: 3
        self.finished_task_vis.type = 1
        self.finished_task_vis.action = Marker.ADD
        #the scale of the marker

        self.finished_task_vis.scale.x = 0.5
        self.finished_task_vis.scale.y = 0.5
        self.finished_task_vis.scale.z = 0.0

        self.finished_task_vis.color.r = 0.0
        self.finished_task_vis.color.g = 1.0
        self.finished_task_vis.color.b = 0.0
        self.finished_task_vis.color.a = 1.0      

        #self.finished_task_vis  # t the pose of the marke
        #if eval('self.next_wp{}'.format(i)) > 0:
        exec('self.finished_task_vis.pose.position.x = self.rut_sorted{}.poses[self.next_wp{}].pose.position.x'.format(i,i))
        exec('self.finished_task_vis.pose.position.y = self.rut_sorted{}.poses[self.next_wp{}].pose.position.y'.format(i,i))
        self.finished_task_vis.pose.position.z = 0.0
        self.finished_task_vis.pose.orientation.x = 0.0
        self.finished_task_vis.pose.orientation.y = 0.0
        self.finished_task_vis.pose.orientation.z = 0.0
        self.finished_task_vis.pose.orientation.w = 1.0
        self.finished_markerArray.markers.append(self.finished_task_vis)
        self.finished_pub.publish(self.finished_markerArray)

    def clickCallback(self, msg):

        self.num_unfinished_tasks += 1
        x = msg.point.x
        y = msg.point.y

        self.unfinished_task_vis.header.frame_id = "map"
        self.unfinished_task_vis.header.seq = 1
        self.unfinished_task_vis.type = 1
        self.unfinished_task_vis.scale.x = 0.5
        self.unfinished_task_vis.scale.y = 0.5
        self.unfinished_task_vis.scale.z = 0.0
        self.unfinished_task_vis.color.r = 1.0
        self.unfinished_task_vis.color.g = 0.0
        self.unfinished_task_vis.color.b = 0.0
        self.unfinished_task_vis.color.a = 1.0
        self.unfinished_task_vis.id = self.num_unfinished_tasks

        self.unfinished_task_vis.action = Marker.ADD
        # t the pose of the marke
        self.unfinished_task_vis.pose.position.x = x
        self.unfinished_task_vis.pose.position.y = y
        self.unfinished_task_vis.pose.position.z = 0.0
        self.unfinished_task_vis.pose.orientation.x = 0.0
        self.unfinished_task_vis.pose.orientation.y = 0.0
        self.unfinished_task_vis.pose.orientation.z = 0.0
        self.unfinished_task_vis.pose.orientation.w = 1.0
        self.unfinished_markerArray.markers.append(self.unfinished_task_vis)
        self.unfinished_pub.publish(self.unfinished_markerArray

Hello again, i am trying to visualize two MarkerArrays, where finished_task_vis has markers with seq = 2 and unfinished_task_vis are markers with seq = 1, but why sometimes markers in finished_task_vis are covered by thos in unfinished_task_vis and sometimes vice versa.

Thanks in advance for you kind help.

[rhaschke]

I'm not sure I get your question. If you have multiple markers that are exactly on top of each other, the (random) order of their arrival at rviz determines the rendering order and thus, which marker becomes visible - if that was your question...

[yifan96]

I'm not sure I get your question. If you have multiple markers that are exactly on top of each other, the (random) order of their arrival at rviz determines the rendering order and thus, which marker becomes visible - if that was your question...

I remember somebody told me marker.header.seq determines the priority of the marker, the marker with higher seq will be visible if multiple markers are at the same position? Is it correct? If so, does it also apply to MarkerArray? For instance, Markers in one MarkerArray have higher seq, so they will cover the other MarkerArray if markers overlap.

[rhaschke]

marker.header.seq determines the priority of the marker

No, that's wrong. The display order cannot be controlled. The seq field in the generic Header message is used to indicate a message count from a particular sender. This is useful, e.g. for control. rviz doesn't use that field at all.

[yifan96]

marker.header.seq determines the priority of the marker

No, that's wrong. The display order cannot be controlled. The seq field in the generic Header message is used to indicate a message count from a particular sender. This is useful, e.g. for control. rviz doesn't use that field at all.

Okay. So neither marker.header.seq nor marker.header.stamp affects which marker is visible and which is hidden, the rendering is just random, right? Then how can I control the visibility priority if multiple markers are at same position, assuming their scales are the same as well.

[yifan96]

marker.header.seq determines the priority of the marker

No, that's wrong. The display order cannot be controlled. The seq field in the generic Header message is used to indicate a message count from a particular sender. This is useful, e.g. for control. rviz doesn't use that field at all.

Okay. So neither marker.header.seq nor marker.header.stamp affects which marker is visible and which is hidden, the rendering is just random, right? Then how can I control the visibility priority if multiple markers are at same position, assuming their scales are the same as well.

Sorry, did not see that you replied "The display order cannot be controlled". Since it is somehow hard to track the id of shown markers and delete them in my code, I assume the simplest way is just to have those markers that I want to show in larger scales. Correct me if i am wrong.

Thank you again for you help.

[rhaschke]

Since it is somehow hard to track the id of shown markers and delete them in my code ...

Actually, that's the way to go: delete unused markers. Why would you draw multiple identical markers at the same position in first place?

[yifan96]

Since it is somehow hard to track the id of shown markers and delete them in my code ...

Actually, that's the way to go: delete unused markers. Why would you draw multiple identical markers at the same position in first place?

I am implementing robots to visit clicked points. And I want to click points in a map as tasks to be finished (in red), and the color of the visited markers should be shown in green. I tried to delete visited markers by setting id of the marker as the order of clicked point, which is unique for deletion. But since visited tasks are deleted and new tasks are coming continously, it is tricky to track id of the markers in my code.

[rhaschke]

Just change the color of the existing marker by reusing the same id in the marker message. That's the purpose of the id field: identifying existing markers :wink: