jsk_pcl_ros: sample_boundingbox_occlusion

Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 import rospy
00004 from jsk_recognition_msgs.msg import BoundingBox, BoundingBoxArray
00005 from sensor_msgs.msg import CameraInfo
00006 from geometry_msgs.msg import PoseStamped
00007 
00008 candidate_pose = None
00009 
00010 def candidatePoseCallback(msg):
00011     global candidate_pose
00012     candidate_pose = msg.pose
00013     
00014 def callback(msg):
00015     if not candidate_pose:
00016         return
00017     target_array = BoundingBoxArray()
00018     target_array.header.stamp = msg.header.stamp
00019     target_array.header.frame_id = "world"
00020     target = BoundingBox()
00021     target.header.stamp = msg.header.stamp
00022     target.header.frame_id = "world"
00023     target.pose = candidate_pose
00024     target.dimensions.x = 0.2
00025     target.dimensions.y = 0.2
00026     target.dimensions.z = 0.2
00027     target_array.boxes = [target]
00028     pub_target.publish(target_array)
00029     candidate_array = BoundingBoxArray()
00030     candidate_array.header.stamp = msg.header.stamp
00031     candidate_array.header.frame_id = "world"
00032     for x in [-0.2, -0.1, 0.0, 0.1, 0.2]:
00033         for y in [-0.2, -0.1, 0.0, 0.1, 0.2]:
00034             for z in [-0.2, -0.1, 0.0, 0.1, 0.2]:
00035                 candidate = BoundingBox()
00036                 candidate.header.stamp = msg.header.stamp
00037                 candidate.header.frame_id = "world"
00038                 candidate.pose.position.z = 2 + z
00039                 candidate.pose.position.x = x
00040                 candidate.pose.position.y = y
00041                 candidate.pose.orientation.w = 1.0
00042                 candidate.dimensions.x = 0.1
00043                 candidate.dimensions.y = 0.1
00044                 candidate.dimensions.z = 0.1
00045                 candidate_array.boxes.append(candidate)
00046     pub_candidate.publish(candidate_array)
00047 
00048 
00049 if __name__ == "__main__":
00050     rospy.init_node("sample_boundingbox_occlusion_rejector")
00051     pub_target = rospy.Publisher("~output/target_boxes", BoundingBoxArray)
00052     pub_candidate = rospy.Publisher("~output/candidate_boxes", BoundingBoxArray)
00053     sub = rospy.Subscriber("~input", CameraInfo, callback)
00054     sub2 = rospy.Subscriber("~input/candidate_pose", PoseStamped, candidatePoseCallback)
00055     rospy.spin()