plane_publisher.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 import roslib
4 import rospy
5 import os
6 if os.environ["ROS_DISTRO"] == "groovy":
7  roslib.load_manifest("jsk_footstep_planner")
8 
9 from jsk_recognition_msgs.msg import PolygonArray
10 from geometry_msgs.msg import PolygonStamped, Point32
11 
13  global_frame_id = rospy.myargv()[1]
14  ret = PolygonArray()
15  ret.header.stamp = rospy.Time.now()
16  ret.header.frame_id = global_frame_id
17 
18  for i in range(-1, 5):
19  # i = 0, 1, 2, 3, 4, 5
20  x1 = i * 3 - 0.5
21  x2 = i * 3 + 1 - 0.5
22  x3 = i * 3 + 2 - 0.5
23  x4 = i * 3 + 3 - 0.5
24  points_array = [[[x2, 2, 0], [x1, 2, 0], [x1, -2, 0], [x2, -2, 0]],
25  [[x3, 2, 0.3], [x2, 2, 0.2], [x2, -2, 0.2], [x3, -2, 0.3]],
26  [[x4, 2, 0.1], [x3, 2, 0.1], [x3, -2, 0.1], [x4, -2, 0.1]]]
27  for points in points_array:
28  polygon = PolygonStamped()
29  polygon.header.frame_id = global_frame_id
30  polygon.header.stamp = rospy.Time.now()
31  for p in points:
32  polygon.polygon.points.append(Point32(*p))
33  ret.polygons.append(polygon)
34  return ret
35 
36 
37 def main():
38  rospy.init_node("plane_publisher")
39 
40  pub = rospy.Publisher("planes", PolygonArray)
41  while not rospy.is_shutdown():
42  msg = makePolygonArray()
43  pub.publish(msg)
44  rospy.sleep(1)
45 
46 if __name__ == "__main__":
47  main()


jsk_footstep_planner
Author(s): Ryohei Ueda
autogenerated on Sun May 28 2023 03:03:19