interface_interactive_marker.py

Go to the documentation of this file.

#!/usr/bin/env python
 
import rospy
 
from frame_editor.objects import *
from frame_editor.commands import *
from frame_editor.interface import Interface
 
from frame_editor.constructors_geometry import *
from frame_editor.constructors_std import *
 
from geometry_msgs.msg import Pose
 
from interactive_markers.interactive_marker_server import *
from visualization_msgs.msg import InteractiveMarkerControl
 
 
class FrameEditor_InteractiveMarker(Interface):
 
    def __init__(self, frame_editor):
        self.editor = frame_editor
        self.editor.observers.append(self)
 
        self.server = InteractiveMarkerServer("frame_editor_interactive")
 
        self.set_marker_settings(["x", "y", "z", "a", "b", "c"])
 
        self.old_frame = None
 
 
    def update(self, editor, level, elements):
 
        if level & 2:
            
            if self.editor.active_frame is not self.old_frame:
                self.make_interactive(self.editor.active_frame)
 
        if level & 4:
            if self.editor.active_frame is not None:
                self.int_marker.name = self.editor.active_frame.name
                self.int_marker.header.frame_id = self.editor.active_frame.parent
                self.int_marker.pose = self.editor.active_frame.pose
                self.server.insert(self.int_marker, self.callback_marker)
                self.server.applyChanges()
 
 
    def make_interactive(self, frame):
 
        
        if self.old_frame is not None:
            self.server.erase(self.old_frame.name)
            self.server.applyChanges()
 
        self.old_frame = frame
 
        if frame is not None:
            self.set_marker_settings(["x", "y", "z", "a", "b", "c"], frame)
 
            self.int_marker.name = frame.name
            self.int_marker.header.frame_id = frame.parent
            self.int_marker.pose = frame.pose
 
            self.server.insert(self.int_marker, self.callback_marker)
            self.server.applyChanges()
 
 
    def callback_marker(self, feedback):
        self.editor.command(Command_SetPose(self.editor, self.editor.active_frame, FromPoint(feedback.pose.position), FromQuaternion(feedback.pose.orientation)))
 
 
    def set_marker_settings(self, arrows, frame=None, scale=0.25):
        '''arrows is a list with any number of the following strings: ["x", "y", "z", "a", "b", "c"].'''
 
        if frame:
            style = frame.style
        else:
            style = "none"
 
        
        int_marker = InteractiveMarker()
        int_marker.header.frame_id = ""
        int_marker.name = ""
        int_marker.description = "Frame Editor"
        int_marker.pose = Pose()
        int_marker.scale = scale
 
        if "x" in arrows:
            control = InteractiveMarkerControl()
            control.name = "move_x"
            control.orientation = NewQuaternion(1, 0, 0, 1)
            control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
            int_marker.controls.append(control);
 
        if "y" in arrows:
            control = InteractiveMarkerControl()
            control.name = "move_y"
            control.orientation = NewQuaternion(0, 1, 0, 1)
            control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
            int_marker.controls.append(control);
 
        if "z" in arrows:
            control = InteractiveMarkerControl()
            control.name = "move_z"
            control.orientation = NewQuaternion(0, 0, 1, 1)
            control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
            int_marker.controls.append(control);
 
        if "a" in arrows:
            control = InteractiveMarkerControl()
            control.name = "rotate_x"
            control.orientation = NewQuaternion(1, 0, 0, 1)
            control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
            int_marker.controls.append(control);
 
        if "b" in arrows:
            control = InteractiveMarkerControl()
            control.name = "rotate_y"
            control.orientation = NewQuaternion(0, 1, 0, 1)
            control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
            int_marker.controls.append(control);
 
 
        if "c" in arrows:
            control = InteractiveMarkerControl()
            control.name = "rotate_z"
            control.orientation = NewQuaternion(0, 0, 1, 1)
            control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
            int_marker.controls.append(control);
 
 
        
 
        #if style != "none":
        #    style_marker = frame.marker
 
        #style_control = InteractiveMarkerControl()
        #style_control.always_visible = True
        #style_control.markers.append(style_marker)
 
        #if style != "none":
        #    int_marker.controls.append(style_control)
 
 
        self.int_marker = int_marker
 
# eof