segment_array_sample.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 import numpy as np
4 from scipy.spatial import ConvexHull
5 
6 import rospy
7 from jsk_recognition_msgs.msg import Segment
8 from jsk_recognition_msgs.msg import SegmentArray
9 from geometry_msgs.msg import Point
10 
11 
12 def main():
13  rospy.init_node('segment_array_sample')
14 
15  pub = rospy.Publisher('~output', SegmentArray, queue_size=1)
16  r = rospy.Rate(rospy.get_param('~rate', 1.0))
17 
18  segment_array = SegmentArray()
19  segment_array.header.frame_id = rospy.get_param('~frame_id', 'map')
20 
21  N = 30
22  while not rospy.is_shutdown():
23  points = np.random.rand(N, 3)
24  hull = ConvexHull(points)
25  segment_array.header.stamp = rospy.Time.now()
26  segment_array.segments = []
27  for i in hull.simplices:
28  a, b, c = points[i]
29  segment_array.segments.extend(
30  [Segment(start_point=Point(a[0], a[1], a[2]),
31  end_point=Point(b[0], b[1], b[2])),
32  Segment(start_point=Point(b[0], b[1], b[2]),
33  end_point=Point(c[0], c[1], c[2])),
34  Segment(start_point=Point(a[0], a[1], a[2]),
35  end_point=Point(c[0], c[1], c[2])),
36  ])
37  pub.publish(segment_array)
38  r.sleep()
39 
40 
41 if __name__ == '__main__':
42  main()


jsk_rviz_plugins
Author(s): Kei Okada , Yohei Kakiuchi , Shohei Fujii , Ryohei Ueda
autogenerated on Sat Mar 20 2021 03:03:18