add_pole.py
Go to the documentation of this file.
00001 #! /usr/bin/env python
00002 
00003 PKG = 'planning_environment'
00004 
00005 import roslib; roslib.load_manifest(PKG)
00006 import rospy
00007 import arm_navigation_msgs.srv
00008 import sys
00009 import unittest
00010 
00011 
00012 import sensor_msgs.msg
00013 import arm_navigation_msgs.msg
00014 from arm_navigation_msgs.msg import CollisionObject
00015 from arm_navigation_msgs.msg import AttachedCollisionObject
00016 from arm_navigation_msgs.msg import CollisionOperation
00017 from arm_navigation_msgs.msg import Shape
00018 from geometry_msgs.msg import Pose
00019 
00020 get_collision_objects_service_name = "get_collision_objects"
00021 
00022 def test_add_convert_objects():
00023 
00024     obj_pub = rospy.Publisher('collision_object',CollisionObject)
00025     att_pub = rospy.Publisher('attached_collision_object',AttachedCollisionObject)
00026 
00027     rospy.init_node('test_collision_objects')
00028 
00029     #let everything settle down
00030     rospy.sleep(5.)
00031       
00032     obj1 = CollisionObject()
00033     
00034     obj1.header.stamp = rospy.Time.now()
00035     obj1.header.frame_id = "base_link"
00036     obj1.id = "obj1";
00037     obj1.operation.operation = arm_navigation_msgs.msg.CollisionObjectOperation.ADD
00038     obj1.shapes = [Shape() for _ in range(1)]
00039     obj1.shapes[0].type = Shape.BOX
00040     obj1.shapes[0].dimensions = [float() for _ in range(3)]
00041     obj1.shapes[0].dimensions[0] = .1
00042     obj1.shapes[0].dimensions[1] = .1
00043     obj1.shapes[0].dimensions[2] = .75
00044     obj1.poses = [Pose() for _ in range(1)]
00045     obj1.poses[0].position.x = .6
00046     obj1.poses[0].position.y = -.6
00047     obj1.poses[0].position.z = .375
00048     obj1.poses[0].orientation.x = 0
00049     obj1.poses[0].orientation.y = 0
00050     obj1.poses[0].orientation.z = 0
00051     obj1.poses[0].orientation.w = 1
00052 
00053     att_obj = AttachedCollisionObject()
00054     att_obj.link_name = "r_gripper_palm_link"
00055     att_obj.touch_links = ['r_gripper_palm_link', 'r_gripper_r_finger_link', 'r_gripper_l_finger_link',
00056                            'r_gripper_r_finger_tip_link', 'r_gripper_l_finger_tip_link', 'r_wrist_roll_link', 'r_wrist_flex_link', 'r_forearm_link', 'r_gripper_motor_accelerometer_link']
00057     obj2 = CollisionObject()
00058     
00059     obj2.header.stamp = rospy.Time.now()
00060     obj2.header.frame_id = "r_gripper_palm_link"
00061     obj2.id = "obj2";
00062     obj2.operation.operation = arm_navigation_msgs.msg.CollisionObjectOperation.ADD
00063     obj2.shapes = [Shape() for _ in range(1)]
00064     obj2.shapes[0].type = Shape.CYLINDER
00065     obj2.shapes[0].dimensions = [float() for _ in range(2)]
00066     obj2.shapes[0].dimensions[0] = .025
00067     obj2.shapes[0].dimensions[1] = .5
00068     obj2.poses = [Pose() for _ in range(1)]
00069     obj2.poses[0].position.x = .12
00070     obj2.poses[0].position.y = 0
00071     obj2.poses[0].position.z = 0
00072     obj2.poses[0].orientation.x = 0
00073     obj2.poses[0].orientation.y = 0
00074     obj2.poses[0].orientation.z = 0
00075     obj2.poses[0].orientation.w = 1
00076     att_obj.object = obj2
00077     r = rospy.Rate(.1)
00078 
00079     while(True):
00080         obj1.header.stamp = rospy.Time.now()
00081         obj_pub.publish(obj1)
00082         att_obj.object.header.stamp = rospy.Time.now()
00083         att_pub.publish(att_obj)
00084         r.sleep()
00085 
00086 if __name__ == '__main__':
00087 
00088     test_add_convert_objects()
00089     
00090 
00091     


planning_environment
Author(s): Ioan Sucan
autogenerated on Thu Dec 12 2013 11:09:24