1
- #!/usr/bin/python
2
- # -*- coding: utf-8 -*-
1
+ #!/usr/bin/python3
2
+ # -*- coding: utf-8 -*-
3
3
4
4
# ============================================
5
5
__author__ = "ShigemichiMatsuzaki"
14
14
from cv_bridge import CvBridge
15
15
import copy
16
16
17
+
17
18
class Visualizer :
18
19
""" """
20
+
19
21
def __init__ (self ):
20
22
""" Constructor of Visualizer class
21
23
@@ -24,14 +26,18 @@ def __init__(self):
24
26
"""
25
27
# Subscribers
26
28
self .image_sub = message_filters .Subscriber ('image' , Image )
27
- self .start_point_sub = message_filters .Subscriber ('start_point' , PointStamped )
28
- self .end_point_sub = message_filters .Subscriber ('end_point' , PointStamped )
29
+ self .start_point_sub = message_filters .Subscriber (
30
+ 'start_point' , PointStamped )
31
+ self .end_point_sub = message_filters .Subscriber (
32
+ 'end_point' , PointStamped )
29
33
# Time synchronizer
30
- self .ts = message_filters .TimeSynchronizer ([self .image_sub , self .start_point_sub , self .end_point_sub ], 100 )
34
+ self .ts = message_filters .TimeSynchronizer (
35
+ [self .image_sub , self .start_point_sub , self .end_point_sub ], 100 )
31
36
self .ts .registerCallback (self .image_points_callback )
32
37
33
38
# Publisher
34
- self .image_pub = rospy .Publisher ('image_and_path' , Image , queue_size = 100 )
39
+ self .image_pub = rospy .Publisher (
40
+ 'image_and_path' , Image , queue_size = 100 )
35
41
36
42
self .bridge = CvBridge ()
37
43
@@ -43,12 +49,14 @@ def image_points_callback(self, img_msg, start_point_msg, end_point_msg):
43
49
start_point_msg(geometry_msgs/PointStamped)
44
50
end_point_msg(geometry_msgs/PointStamped)
45
51
"""
46
- # Convert the image message to
47
- cv_image = self .bridge .imgmsg_to_cv2 (img_msg , desired_encoding = 'passthrough' )
52
+ # Convert the image message to
53
+ cv_image = self .bridge .imgmsg_to_cv2 (
54
+ img_msg , desired_encoding = 'passthrough' )
48
55
49
56
cv_image_with_line = self .draw_line (cv_image ,
50
- (int (start_point_msg .point .x ), int (start_point_msg .point .y )),
51
- (int (end_point_msg .point .x ), int (end_point_msg .point .y )))
57
+ (int (start_point_msg .point .x ),
58
+ int (start_point_msg .point .y )),
59
+ (int (end_point_msg .point .x ), int (end_point_msg .point .y )))
52
60
53
61
vis_img_msg = self .bridge .cv2_to_imgmsg (cv_image_with_line )
54
62
@@ -61,26 +69,28 @@ def draw_line(self, cv_image, start_point, end_point):
61
69
cv_image(OpenCV image)
62
70
start_point_msg(geometry_msgs/PointStamped)
63
71
end_point_msg(geometry_msgs/PointStamped)
64
-
72
+
65
73
Return:
66
74
OpenCV image with a line drawn
67
75
"""
68
76
ret_image = copy .deepcopy (cv_image )
69
77
70
- cv2 .line (ret_image , start_point , end_point , color = (0 , 0 , 255 ), thickness = 10 )
78
+ cv2 .line (ret_image , start_point , end_point ,
79
+ color = (0 , 0 , 255 ), thickness = 10 )
71
80
72
81
return ret_image
73
82
83
+
74
84
def main ():
75
85
"""Main function to initialize the ROS node"""
76
86
rospy .init_node ("visualizer" )
77
87
78
88
visualizer = Visualizer ()
79
89
80
90
rospy .loginfo ('visualizer is initialized' )
81
-
82
- rospy .spin ()
91
+
92
+ rospy .spin ()
83
93
84
94
85
- if __name__ == '__main__' :
95
+ if __name__ == '__main__' :
86
96
main ()
0 commit comments