jsk_pcl_ros: attention_pose_set.py Source File

Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 import rospy
00004 import math
00005 from geometry_msgs.msg import PoseStamped
00006 from jsk_pcl_ros.msg import BoundingBox, BoundingBoxArray
00007 rospy.init_node("attention_pose_set")
00008 pub = rospy.Publisher("/attention_clipper/input/box_array", BoundingBoxArray)
00009 r = rospy.Rate(10)
00010 theta = 0
00011 while not rospy.is_shutdown():
00012     boxes = BoundingBoxArray()
00013     theta = math.fmod(theta + 0.1, math.pi * 2)
00014     box = BoundingBox()
00015     box.header.stamp = rospy.Time.now()
00016     box.header.frame_id = "camera_link"
00017     box.pose.orientation.w = 1
00018     box.pose.position.x = 0.8 * math.cos(theta)
00019     box.pose.position.y = 0.8 * math.sin(theta)
00020     box.pose.position.z = 0.1
00021     box.dimensions.x = 0.1
00022     box.dimensions.y = 0.1
00023     box.dimensions.z = 0.1
00024     box2 = BoundingBox()
00025     box2.header.stamp = rospy.Time.now()
00026     box2.header.frame_id = "camera_link"
00027     box2.pose.orientation.w = 1
00028     box2.pose.position.x = 0.8 * math.cos(theta)
00029     box2.pose.position.y = 0.8 * math.sin(theta)
00030     box2.pose.position.z = -0.1
00031     box2.dimensions.x = 0.1
00032     box2.dimensions.y = 0.1
00033     box2.dimensions.z = 0.1
00034     boxes.boxes = [box, box2]
00035     boxes.header.frame_id = "camera_link"
00036     boxes.header.stamp = rospy.Time.now()
00037     pub.publish(boxes)
00038     r.sleep()