$search
00001 #!/usr/bin/env python 00002 00003 """ 00004 Copyright (c) 2011, Willow Garage, Inc. 00005 All rights reserved. 00006 00007 Redistribution and use in source and binary forms, with or without 00008 modification, are permitted provided that the following conditions are met: 00009 00010 * Redistributions of source code must retain the above copyright 00011 notice, this list of conditions and the following disclaimer. 00012 * Redistributions in binary form must reproduce the above copyright 00013 notice, this list of conditions and the following disclaimer in the 00014 documentation and/or other materials provided with the distribution. 00015 * Neither the name of the Willow Garage, Inc. nor the names of its 00016 contributors may be used to endorse or promote products derived from 00017 this software without specific prior written permission. 00018 00019 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 00020 AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 00021 IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 00022 ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 00023 LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 00024 CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 00025 SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 00026 INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 00027 CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 00028 ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00029 POSSIBILITY OF SUCH DAMAGE. 00030 """ 00031 00032 import roslib; roslib.load_manifest("interactive_markers") 00033 import rospy 00034 00035 from interactive_markers.interactive_marker_server import * 00036 from math import sqrt 00037 00038 positions = list() 00039 00040 def processFeedback( feedback ): 00041 if feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE: 00042 #compute difference vector for this cube 00043 x = feedback.pose.position.x 00044 y = feedback.pose.position.y 00045 z = feedback.pose.position.z 00046 index = int(feedback.marker_name) 00047 00048 if index > len(positions): 00049 return 00050 00051 dx = x - positions[index][0] 00052 dy = y - positions[index][1] 00053 dz = z - positions[index][2] 00054 00055 # move all markers in that direction 00056 for i in range(len(positions)): 00057 (mx, my, mz) = positions[i] 00058 d = sqrt(sqrt((x - mx)**2 + (y - my)**2)**2 + (z-mz)**2) 00059 t = 1 / (d*5.0+1.0) - 0.2 00060 if t < 0.0: 00061 t=0.0 00062 positions[i][0] += t*dx 00063 positions[i][1] += t*dy 00064 positions[i][2] += t*dz 00065 00066 if i == index: 00067 rospy.loginfo( d ) 00068 positions[i][0] = x 00069 positions[i][1] = y 00070 positions[i][2] = z 00071 00072 pose = geometry_msgs.msg.Pose() 00073 pose.position.x = positions[i][0] 00074 pose.position.y = positions[i][1] 00075 pose.position.z = positions[i][2] 00076 00077 server.setPose( str(i), pose ) 00078 server.applyChanges() 00079 00080 def makeBoxControl( msg ): 00081 control = InteractiveMarkerControl() 00082 control.always_visible = True 00083 control.orientation_mode = InteractiveMarkerControl.VIEW_FACING 00084 control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE 00085 control.independent_marker_orientation = True 00086 00087 marker = Marker() 00088 00089 marker.type = Marker.CUBE 00090 marker.scale.x = msg.scale 00091 marker.scale.y = msg.scale 00092 marker.scale.z = msg.scale 00093 marker.color.r = 0.65+0.7*msg.pose.position.x 00094 marker.color.g = 0.65+0.7*msg.pose.position.y 00095 marker.color.b = 0.65+0.7*msg.pose.position.z 00096 marker.color.a = 1.0 00097 00098 control.markers.append( marker ) 00099 msg.controls.append( control ) 00100 return control 00101 00102 def makeCube(): 00103 side_length = 10 00104 step = 1.0/ side_length 00105 count = 0 00106 00107 for i in range(side_length): 00108 x = -0.5 + step * i 00109 for j in range(side_length): 00110 y = -0.5 + step * j 00111 for k in range(side_length): 00112 z = step * k 00113 marker = InteractiveMarker() 00114 marker.header.frame_id = "/base_link" 00115 marker.scale = step 00116 00117 marker.pose.position.x = x 00118 marker.pose.position.y = y 00119 marker.pose.position.z = z 00120 00121 positions.append( [x,y,z] ) 00122 00123 marker.name = str(count) 00124 makeBoxControl(marker) 00125 00126 server.insert( marker, processFeedback ) 00127 count += 1 00128 00129 if __name__=="__main__": 00130 rospy.init_node("cube") 00131 00132 server = InteractiveMarkerServer("cube") 00133 00134 rospy.loginfo("initializing..") 00135 makeCube() 00136 server.applyChanges() 00137 00138 rospy.spin() 00139