interactive_marker.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 # Copyright 2011 Shadow Robot Company Ltd.
4 #
5 # This program is free software: you can redistribute it and/or modify it
6 # under the terms of the GNU General Public License as published by the Free
7 # Software Foundation version 2 of the License.
8 #
9 # This program is distributed in the hope that it will be useful, but WITHOUT
10 # ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
11 # FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
12 # more details.
13 #
14 # You should have received a copy of the GNU General Public License along
15 # with this program. If not, see <http://www.gnu.org/licenses/>.
16 
17 import rospy
18 
19 from interactive_markers.interactive_marker_server import InteractiveMarker, InteractiveMarkerFeedback
20 from visualization_msgs.msg import Marker, InteractiveMarkerControl
21 
22 
24  def __init__(self, object_names, callback_fct, interactive_server):
25  # create an interactive marker server on the topic namespace simple_marker
26  self.server = interactive_server
27 
28  self.callback_function = callback_fct
29  self.object_names = object_names
30 
31  self.int_markers = {}
32  self.object_markers = {}
33  self.object_controls = {}
34  self.select_controls = {}
35 
36  for object_name in self.object_names:
37  self.create_marker(object_name)
38 
39  def create_marker(self, object_name):
40  # create an interactive marker for our server
41  int_marker = InteractiveMarker()
42  int_marker.header.frame_id = "/"+object_name
43  int_marker.name = object_name
44  int_marker.description = "Select this object"
45  int_marker.pose.position.z = -0.3
46 
47  self.int_markers[object_name] = int_marker
48 
49  # create a marker over the object
50  object_marker = Marker()
51  object_marker.type = Marker.SPHERE
52  object_marker.pose.position.z = 0.6
53 
54  object_marker.scale.x = 0.1
55  object_marker.scale.y = 0.1
56  object_marker.scale.z = 0.1
57  object_marker.color.r = 0.6
58  object_marker.color.g = 0.02
59  object_marker.color.b = 1.0
60  object_marker.color.a = 1.0
61  self.object_markers[object_name] = object_marker
62 
63  # create a non-interactive control which contains the box
64  object_control = InteractiveMarkerControl()
65  object_control.interaction_mode = InteractiveMarkerControl.BUTTON
66  object_control.always_visible = True
67  object_control.markers.append(object_marker)
68 
69  self.object_controls[object_name] = object_control
70 
71  # add the control to the interactive marker
72  self.int_markers[object_name].controls.append(object_control)
73 
74  # add the interactive marker to our collection &
75  # tell the server to call processFeedback() when feedback arrives for it
76  self.server.insert(self.int_markers[object_name], self.processFeedback)
77 
78  # 'commit' changes and send to all clients
79  self.server.applyChanges()
80 
81  def processFeedback(self, feedback):
82  # this was not a click on the sphere
83  if feedback.event_type != InteractiveMarkerFeedback.BUTTON_CLICK:
84  return
85  # One of the object has been selected.
86  # We'll update all the dynamic markers
87  # to make the selected one prominent.
88 
89  # this is the name of the selected object.
90  selected_name = feedback.marker_name
91 
92  # we loop through all our interactive markers.
93  for index, name in enumerate(self.object_controls):
94  self.object_controls[name].markers.remove(self.object_markers[name])
95 
96  if name == selected_name:
97  # the selected object marker is set
98  # higher, bigger and in green
99  self.object_markers[name].pose.position.z = 0.65
100 
101  self.object_markers[name].scale.x = 0.15
102  self.object_markers[name].scale.y = 0.15
103  self.object_markers[name].scale.z = 0.15
104  self.object_markers[name].color.r = 0.32
105  self.object_markers[name].color.g = 0.83
106  self.object_markers[name].color.b = 0.15
107  else:
108  # the other objects are slightly lower,
109  # smaller and in purple
110  self.object_markers[name].pose.position.z = 0.6
111 
112  self.object_markers[name].scale.x = 0.1
113  self.object_markers[name].scale.y = 0.1
114  self.object_markers[name].scale.z = 0.1
115  self.object_markers[name].color.r = 0.6
116  self.object_markers[name].color.g = 0.02
117  self.object_markers[name].color.b = 1.0
118 
119  self.object_controls[name].markers.append(self.object_markers[name])
120 
121  self.server.insert(self.int_markers[name])
122 
123  # we update the interactive marker server
124  self.server.applyChanges()
125  self.callback_function(selected_name)
126 
127 
128 # this is just used for debug
129 if __name__ == "__main__":
130  rospy.init_node("simple_marker")
131 
132  int_mark = InteractiveConnectorSelector(["srh/position/palm"], None)
133  rospy.spin()
def __init__(self, object_names, callback_fct, interactive_server)


sr_utilities
Author(s): Ugo Cupcic
autogenerated on Mon Feb 28 2022 23:52:19