add_attached_box.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 import sensor_msgs.msg
00012 import mapping_msgs.msg
00013 from mapping_msgs.msg import CollisionObject
00014 from mapping_msgs.msg import AttachedCollisionObject
00015 from arm_navigation_msgs.msg import CollisionOperation
00016 from arm_navigation_msgs.msg import Shape
00017 from geometry_msgs.msg import Pose
00018 
00019 get_collision_objects_service_name = "get_collision_objects"
00020 
00021 def test_add_convert_objects():
00022 
00023     att_pub = rospy.Publisher('attached_collision_object',AttachedCollisionObject)
00024 
00025     rospy.init_node('test_collision_objects')
00026 
00027     att_obj = AttachedCollisionObject()
00028     att_obj.link_name = "r_gripper_palm_link"
00029     att_obj.touch_links = ['r_gripper_palm_link', 'r_gripper_r_finger_link', 'r_gripper_l_finger_link',
00030                            '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']
00031     obj2 = CollisionObject()
00032     
00033     obj2.header.stamp = rospy.Time.now()
00034     obj2.header.frame_id = "r_gripper_palm_link"
00035     obj2.id = "obj2";
00036     obj2.operation.operation = mapping_msgs.msg.CollisionObjectOperation.ADD
00037     obj2.shapes = [Shape() for _ in range(1)]
00038     obj2.shapes[0].type = Shape.BOX
00039     obj2.shapes[0].dimensions = [float() for _ in range(3)]
00040     obj2.shapes[0].dimensions[0] = .2
00041     obj2.shapes[0].dimensions[1] = .4
00042     obj2.shapes[0].dimensions[2] = .2
00043     obj2.poses = [Pose() for _ in range(1)]
00044     obj2.poses[0].position.x = .12
00045     obj2.poses[0].position.y = 0
00046     obj2.poses[0].position.z = 0
00047     obj2.poses[0].orientation.x = 0
00048     obj2.poses[0].orientation.y = 0
00049     obj2.poses[0].orientation.z = 0
00050     obj2.poses[0].orientation.w = 1
00051     att_obj.object = obj2
00052     r = rospy.Rate(6)
00053 
00054     sent_twice = 0
00055 
00056     while(True):
00057         sent_twice += 1
00058         if sent_twice >= 4 and sent_twice%2 == 0:
00059             att_obj.object.operation.operation = mapping_msgs.msg.CollisionObjectOperation.DETACH_AND_ADD_AS_OBJECT
00060         elif sent_twice > 4 and sent_twice%2 == 1:
00061             att_obj.object.operation.operation = mapping_msgs.msg.CollisionObjectOperation.ATTACH_AND_REMOVE_AS_OBJECT
00062         print 'sending'    
00063         att_obj.object.header.stamp = rospy.Time.now()
00064         att_pub.publish(att_obj)     
00065         
00066         r.sleep()
00067 
00068 if __name__ == '__main__':
00069 
00070     test_add_convert_objects()
00071     
00072 
00073     


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