cube.py
Go to the documentation of this file.
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 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         #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 


interactive_marker_tutorials
Author(s): David Gossow
autogenerated on Thu Jun 6 2019 17:45:02