segment_array_sample.py

Go to the documentation of this file.

#!/usr/bin/env python
 
import numpy as np
from scipy.spatial import ConvexHull
 
import rospy
from jsk_recognition_msgs.msg import Segment
from jsk_recognition_msgs.msg import SegmentArray
from geometry_msgs.msg import Point
 
 
def main():
    rospy.init_node('segment_array_sample')
 
    pub = rospy.Publisher('~output', SegmentArray, queue_size=1)
    r = rospy.Rate(rospy.get_param('~rate', 1.0))
 
    segment_array = SegmentArray()
    segment_array.header.frame_id = rospy.get_param('~frame_id', 'map')
 
    N = 30
    while not rospy.is_shutdown():
        points = np.random.rand(N, 3)
        hull = ConvexHull(points)
        segment_array.header.stamp = rospy.Time.now()
        segment_array.segments = []
        for i in hull.simplices:
            a, b, c = points[i]
            segment_array.segments.extend(
                [Segment(start_point=Point(a[0], a[1], a[2]),
                         end_point=Point(b[0], b[1], b[2])),
                 Segment(start_point=Point(b[0], b[1], b[2]),
                         end_point=Point(c[0], c[1], c[2])),
                 Segment(start_point=Point(a[0], a[1], a[2]),
                         end_point=Point(c[0], c[1], c[2])),
                ])
        pub.publish(segment_array)
        r.sleep()
 
 
if __name__ == '__main__':
    main()