interface_markers.py

Go to the documentation of this file.

#!/usr/bin/env python
 
import copy
import rospy
 
from frame_editor.interface import Interface
 
from visualization_msgs.msg import Marker
 
 
class FrameEditor_Markers(Interface):
 
    def __init__(self, frame_editor):
        self.editor = frame_editor
        self.editor.observers.append(self)
 
        self.publisher = rospy.Publisher("frame_editor_marker", Marker, queue_size=10, latch=False)
        self.last_publish_time = rospy.Time.now()
        self.publish_period = rospy.Duration(2.0)
 
 
    def update(self, editor, level, elements):
 
        
 
        for element in elements:
            if not element:
                continue
            if element.marker:
                self.publish_marker(element)
 
 
    def publish_marker(self, element):
 
        element.update_marker() 
 
        marker = copy.deepcopy(element.marker) # copy
 
        marker.header.frame_id = element.name
        marker.header.stamp = rospy.Time() # zero time
        marker.ns = "frame_editor_markers"
        marker.frame_locked = True # Tells rviz to retransform the marker into the current location of the specified frame every update cycle.
 
        if element.hidden:
            marker.action = Marker.DELETE
        else:
            marker.action = Marker.ADD
 
        if element.style == "mesh":
            if element.path == "" or element.path is None:
                marker.action = Marker.DELETE
 
        self.publisher.publish(marker)
 
 
    def broadcast(self, editor):
        
        if (rospy.Time.now() - self.last_publish_time) >= self.publish_period:
 
            
            self.update(editor, 0, editor.frames.values())
 
            self.last_publish_time = rospy.Time.now()
 
# eof