plane_publisher.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 import roslib
00004 import rospy
00005 import os
00006 if os.environ["ROS_DISTRO"] == "groovy":
00007   roslib.load_manifest("jsk_footstep_planner")
00008 
00009 from jsk_pcl_ros.msg import PolygonArray
00010 from geometry_msgs.msg import PolygonStamped, Point32
00011 
00012 def makePolygonArray():
00013   global_frame_id = rospy.myargv()[1]
00014   ret = PolygonArray()
00015   ret.header.stamp = rospy.Time.now()
00016   ret.header.frame_id = global_frame_id
00017 
00018   for i in range(-1, 5):
00019     # i = 0,  1, 2, 3, 4, 5
00020     x1 = i * 3 - 0.5
00021     x2 = i * 3 + 1 - 0.5
00022     x3 = i * 3 + 2 - 0.5
00023     x4 = i * 3 + 3 - 0.5
00024     points_array = [[[x2, 2, 0],   [x1, 2, 0],   [x1, -2, 0],   [x2, -2, 0]],
00025                     [[x3, 2, 0.3], [x2, 2, 0.2], [x2, -2, 0.2], [x3, -2, 0.3]],
00026                     [[x4, 2, 0.1], [x3, 2, 0.1], [x3, -2, 0.1], [x4, -2, 0.1]]]
00027     for points in points_array:
00028       polygon = PolygonStamped()
00029       polygon.header.frame_id = global_frame_id
00030       polygon.header.stamp = rospy.Time.now()
00031       for p in points:
00032         polygon.polygon.points.append(Point32(*p))
00033       ret.polygons.append(polygon)
00034   return ret
00035             
00036 
00037 def main():
00038   rospy.init_node("plane_publisher")
00039   
00040   pub = rospy.Publisher("planes", PolygonArray)
00041   while not rospy.is_shutdown():
00042     msg = makePolygonArray()
00043     pub.publish(msg)
00044     rospy.sleep(1)
00045 
00046 if __name__ == "__main__":
00047   main()


jsk_footstep_planner
Author(s): Ryohei Ueda
autogenerated on Mon Oct 6 2014 01:11:47