Go to the documentation of this file.00001
00002 import roslib; roslib.load_manifest('arm_navigation_tutorials')
00003 import sys
00004 import rospy
00005
00006 from mapping_msgs.msg import CollisionObject
00007 from mapping_msgs.msg import CollisionObjectOperation
00008 from geometric_shapes_msgs.msg import Shape
00009 from geometry_msgs.msg import Pose
00010
00011
00012 def add_cylinder(id, bottom, radius, height, frameid, pub):
00013 cylinder_object = CollisionObject()
00014 cylinder_object.id = id
00015 cylinder_object.operation.operation = CollisionObjectOperation.ADD
00016 cylinder_object.header.frame_id = frameid
00017 cylinder_object.header.stamp = rospy.Time.now()
00018
00019 shp = Shape()
00020 shp.type = Shape.CYLINDER
00021 shp.dimensions = [0, 0]
00022 shp.dimensions[0] = radius
00023 shp.dimensions[1] = height
00024
00025 pose = Pose()
00026 pose.position.x = bottom[0]
00027 pose.position.y = bottom[1]
00028 pose.position.z = bottom[2] + height/2.
00029 pose.orientation.x = 0
00030 pose.orientation.y = 0
00031 pose.orientation.z = 0
00032 pose.orientation.w = 1
00033 cylinder_object.shapes.append(shp)
00034 cylinder_object.poses.append(pose)
00035
00036 pub.publish(cylinder_object)
00037
00038
00039 if __name__ == '__main__':
00040 rospy.init_node('add_cylinder_python')
00041
00042 pub = rospy.Publisher('collision_object', CollisionObject)
00043 rospy.sleep(2.)
00044
00045 add_cylinder('pole', (0.6, -0.6, 0.), 0.1, 0.75, 'base_link', pub)
00046 rospy.loginfo('Should have published')
00047 rospy.sleep(2.)
00048
00049
00050