cuzk_tools/nodes/marker_pub at master · ctu-vras/cuzk_tools · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
#!/usr/bin/env python

from visualization_msgs.msg import Marker
from geometry_msgs.msg import Point
import rospy

from cuzk_tools.srv import MarkerGet, MarkerGetResponse

class MarkerGenerator():

    def __init__(self):
        rospy.init_node('marker_pub')
        rospy.Service('marker', MarkerGet, self.handle_marker)
        self.marker_pub = rospy.Publisher("/marker", Marker, queue_size = 10, latch=True)
        self.id = 0

    def get_id(self):
        self.id += 1
        return self.id-1

    def handle_marker(self,req):
        x=req.point.x
        y=req.point.y
        z=req.point.z
        frame=req.frame.data
        size = req.size.data
        marker = Marker()

        marker.header.frame_id = frame
        marker.header.stamp = rospy.Time.now()
        # set shape, Arrow: 0; Cube: 1 ; Sphere: 2 ; Cylinder: 3
        marker.type = 2
        marker.id = self.get_id()

        # Set the scale of the marker
        marker.scale.x = size
        marker.scale.y = size
        marker.scale.z = size

        # Set the color
        marker.color.r = 1.0
        marker.color.g = 0.0
        marker.color.b = 0.0
        marker.color.a = 1.0

        # Set the pose of the marker
        marker.pose.position.x = x
        marker.pose.position.y = y
        marker.pose.position.z = z
        marker.pose.orientation.x = 0.0
        marker.pose.orientation.y = 0.0
        marker.pose.orientation.z = 0.0
        marker.pose.orientation.w = 1.0

        self.marker_pub.publish(marker)

        # Return empty-ish response.
        response = MarkerGetResponse()
        return response

if __name__ == "__main__":
    node = MarkerGenerator()
    rospy.spin()