cube.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 """
4 Copyright (c) 2011, Willow Garage, Inc.
5 All rights reserved.
6 
7 Redistribution and use in source and binary forms, with or without
8 modification, are permitted provided that the following conditions are met:
9 
10  * Redistributions of source code must retain the above copyright
11  notice, this list of conditions and the following disclaimer.
12  * Redistributions in binary form must reproduce the above copyright
13  notice, this list of conditions and the following disclaimer in the
14  documentation and/or other materials provided with the distribution.
15  * Neither the name of the Willow Garage, Inc. nor the names of its
16  contributors may be used to endorse or promote products derived from
17  this software without specific prior written permission.
18 
19 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
20 AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
21 IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
22 ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
23 LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
24 CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
25 SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
26 INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
27 CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
28 ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
29 POSSIBILITY OF SUCH DAMAGE.
30 """
31 
32 import rospy
33 
35 from visualization_msgs.msg import *
36 from math import sqrt
37 
38 positions = list()
39 
40 def processFeedback( feedback ):
41  if feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE:
42  #compute difference vector for this cube
43  x = feedback.pose.position.x
44  y = feedback.pose.position.y
45  z = feedback.pose.position.z
46  index = int(feedback.marker_name)
47 
48  if index > len(positions):
49  return
50 
51  dx = x - positions[index][0]
52  dy = y - positions[index][1]
53  dz = z - positions[index][2]
54 
55  # move all markers in that direction
56  for i in range(len(positions)):
57  (mx, my, mz) = positions[i]
58  d = sqrt(sqrt((x - mx)**2 + (y - my)**2)**2 + (z-mz)**2)
59  t = 1 / (d*5.0+1.0) - 0.2
60  if t < 0.0:
61  t=0.0
62  positions[i][0] += t*dx
63  positions[i][1] += t*dy
64  positions[i][2] += t*dz
65 
66  if i == index:
67  rospy.loginfo( d )
68  positions[i][0] = x
69  positions[i][1] = y
70  positions[i][2] = z
71 
72  pose = geometry_msgs.msg.Pose()
73  pose.position.x = positions[i][0]
74  pose.position.y = positions[i][1]
75  pose.position.z = positions[i][2]
76 
77  server.setPose( str(i), pose )
78  server.applyChanges()
79 
80 def makeBoxControl( msg ):
81  control = InteractiveMarkerControl()
82  control.always_visible = True
83  control.orientation_mode = InteractiveMarkerControl.VIEW_FACING
84  control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE
85  control.independent_marker_orientation = True
86 
87  marker = Marker()
88 
89  marker.type = Marker.CUBE
90  marker.scale.x = msg.scale
91  marker.scale.y = msg.scale
92  marker.scale.z = msg.scale
93  marker.color.r = 0.65+0.7*msg.pose.position.x
94  marker.color.g = 0.65+0.7*msg.pose.position.y
95  marker.color.b = 0.65+0.7*msg.pose.position.z
96  marker.color.a = 1.0
97 
98  control.markers.append( marker )
99  msg.controls.append( control )
100  return control
101 
102 def makeCube():
103  side_length = 10
104  step = 1.0/ side_length
105  count = 0
106 
107  for i in range(side_length):
108  x = -0.5 + step * i
109  for j in range(side_length):
110  y = -0.5 + step * j
111  for k in range(side_length):
112  z = step * k
113  marker = InteractiveMarker()
114  marker.header.frame_id = "base_link"
115  marker.scale = step
116 
117  marker.pose.position.x = x
118  marker.pose.position.y = y
119  marker.pose.position.z = z
120 
121  positions.append( [x,y,z] )
122 
123  marker.name = str(count)
124  makeBoxControl(marker)
125 
126  server.insert( marker, processFeedback )
127  count += 1
128 
129 if __name__=="__main__":
130  rospy.init_node("cube")
131 
132  server = InteractiveMarkerServer("cube")
133 
134  rospy.loginfo("initializing..")
135  makeCube()
136  server.applyChanges()
137 
138  rospy.spin()
139 
def makeCube()
Definition: cube.py:102
def processFeedback(feedback)
Definition: cube.py:40
def makeBoxControl(msg)
Definition: cube.py:80


interactive_marker_tutorials
Author(s): David Gossow
autogenerated on Sat May 16 2020 03:49:16