interface_interactive_marker.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 import rospy
4 
5 from frame_editor.objects import *
6 from frame_editor.commands import *
7 from frame_editor.interface import Interface
8 
11 
12 from geometry_msgs.msg import Pose
13 
15 from visualization_msgs.msg import InteractiveMarkerControl
16 
17 
19 
20  def __init__(self, frame_editor):
21  self.editor = frame_editor
22  self.editor.observers.append(self)
23 
24  self.server = InteractiveMarkerServer("frame_editor_interactive")
25 
26  self.set_marker_settings(["x", "y", "z", "a", "b", "c"])
27 
28  self.old_frame = None
29 
30 
31  def update(self, editor, level, elements):
32 
33  if level & 2:
34  ## Check for change
35  if self.editor.active_frame is not self.old_frame:
36  self.make_interactive(self.editor.active_frame)
37 
38  if level & 4:
39  if self.editor.active_frame is not None:
40  self.int_marker.name = self.editor.active_frame.name
41  self.int_marker.header.frame_id = self.editor.active_frame.parent
42  self.int_marker.pose = self.editor.active_frame.pose
43  self.server.insert(self.int_marker, self.callback_marker)
44  self.server.applyChanges()
45 
46 
47  def make_interactive(self, frame):
48 
49  ## Stop currently active frame
50  if self.old_frame is not None:
51  self.server.erase(self.old_frame.name)
52  self.server.applyChanges()
53 
54  self.old_frame = frame
55 
56  if frame is not None:
57  self.set_marker_settings(["x", "y", "z", "a", "b", "c"], frame)
58 
59  self.int_marker.name = frame.name
60  self.int_marker.header.frame_id = frame.parent
61  self.int_marker.pose = frame.pose
62 
63  self.server.insert(self.int_marker, self.callback_marker)
64  self.server.applyChanges()
65 
66 
67  def callback_marker(self, feedback):
68  self.editor.command(Command_SetPose(self.editor, self.editor.active_frame, FromPoint(feedback.pose.position), FromQuaternion(feedback.pose.orientation)))
69 
70 
71  def set_marker_settings(self, arrows, frame=None, scale=0.25):
72  '''arrows is a list with any number of the following strings: ["x", "y", "z", "a", "b", "c"].'''
73 
74  if frame:
75  style = frame.style
76  else:
77  style = "none"
78 
79  ## Marker
80  int_marker = InteractiveMarker()
81  int_marker.header.frame_id = ""
82  int_marker.name = ""
83  int_marker.description = "Frame Editor"
84  int_marker.pose = Pose()
85  int_marker.scale = scale
86 
87  if "x" in arrows:
88  control = InteractiveMarkerControl()
89  control.name = "move_x"
90  control.orientation = NewQuaternion(1, 0, 0, 1)
91  control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
92  int_marker.controls.append(control);
93 
94  if "y" in arrows:
95  control = InteractiveMarkerControl()
96  control.name = "move_y"
97  control.orientation = NewQuaternion(0, 1, 0, 1)
98  control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
99  int_marker.controls.append(control);
100 
101  if "z" in arrows:
102  control = InteractiveMarkerControl()
103  control.name = "move_z"
104  control.orientation = NewQuaternion(0, 0, 1, 1)
105  control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
106  int_marker.controls.append(control);
107 
108  if "a" in arrows:
109  control = InteractiveMarkerControl()
110  control.name = "rotate_x"
111  control.orientation = NewQuaternion(1, 0, 0, 1)
112  control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
113  int_marker.controls.append(control);
114 
115  if "b" in arrows:
116  control = InteractiveMarkerControl()
117  control.name = "rotate_y"
118  control.orientation = NewQuaternion(0, 1, 0, 1)
119  control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
120  int_marker.controls.append(control);
121 
122 
123  if "c" in arrows:
124  control = InteractiveMarkerControl()
125  control.name = "rotate_z"
126  control.orientation = NewQuaternion(0, 0, 1, 1)
127  control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
128  int_marker.controls.append(control);
129 
130 
131  ## Style ##
132 
136 
137  #if style != "none":
138  # style_marker = frame.marker
139 
140  #style_control = InteractiveMarkerControl()
141  #style_control.always_visible = True
142  #style_control.markers.append(style_marker)
143 
144  #if style != "none":
145  # int_marker.controls.append(style_control)
146 
147 
148  self.int_marker = int_marker
149 
150 # eof
def NewQuaternion(x, y, z, w)
Orientation ##.
def set_marker_settings(self, arrows, frame=None, scale=0.25)
style_marker = Marker() style_marker.scale = NewVector3(0.75*scale, 0.75*scale, 0.75*scale) style_marker.color = NewColor(0.0, 0.5, 0.5, 0.75)


frame_editor
Author(s): ipa-lth , ipa-frn
autogenerated on Wed Oct 21 2020 03:36:00