#374 added clicking functionality

Author: manuelgitgomes

atom_rviz_plugins/image_display_with_click/scripts/test_image_display_with_click

@@ -5,6 +5,9 @@ import cv2
 import rospy
 
 from cv_bridge import CvBridge, CvBridgeError
+from pynput import keyboard
+from pynput.keyboard import KeyCode
+from scipy.spatial import distance
 from sensor_msgs.msg import Image
 from geometry_msgs.msg import PointStamped
 
@@ -32,19 +35,35 @@ def callbackImageClickReceived(point_msg):
     global points
     points.append(point)
 
+def key_pressed(key):
+    global save
+    if key == KeyCode.from_char('s'):
+        print('True')
+        save = True
 
 def main():
+    global save
+
+    save = False
+
     rospy.init_node('test_image_display_with_click', anonymous=False)
 
     topic_image = rospy.names.remap_name('image_topic')
     topic_image_labeled = topic_image + '/labeled'
     topic_image_click = topic_image_labeled + '/click'
 
+    # Defining acceptable radius
+    radius = 3
+
     global image
     subscriber_image = rospy.Subscriber(topic_image, Image, partial(callbackImageReceived))
     subscriber_image_click = rospy.Subscriber(topic_image_click, PointStamped, callbackImageClickReceived)
     publisher_image = rospy.Publisher(topic_image_labeled, Image, queue_size=10)
 
+    # Defining keyboard listener
+    keyboard_listener = keyboard.Listener(on_press=key_pressed)
+    keyboard_listener.start()
+
     cv2.namedWindow('Image', cv2.WINDOW_AUTOSIZE)
 
     rate = rospy.Rate(0.1)
@@ -67,6 +86,10 @@ def main():
         image_msg = bridge.cv2_to_imgmsg(gui_image)
         publisher_image.publish(image_msg)
 
+        if save:
+            start_to_end_distance = distance.euclidean(points[1], points[-1])
+            print(start_to_end_distance)
+
         # Show just for debug, not usually used
         cv2.imshow("Image", gui_image)
         cv2.waitKey(30)