Go to the documentation of this file.00001
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 rospy
00033
00034 from interactive_markers.interactive_marker_server import *
00035 from visualization_msgs.msg import *
00036 from math import sqrt
00037
00038 positions = list()
00039
00040 def processFeedback( feedback ):
00041 if feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE:
00042
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
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