Source Code for Module intera_interface.head_display

 1  # Copyright (c) 2013-2017, Rethink Robotics Inc. 
 2  # 
 3  # Licensed under the Apache License, Version 2.0 (the "License"); 
 4  # you may not use this file except in compliance with the License. 
 5  # You may obtain a copy of the License at 
 6  # 
 7  #     http://www.apache.org/licenses/LICENSE-2.0 
 8  # 
 9  # Unless required by applicable law or agreed to in writing, software 
10  # distributed under the License is distributed on an "AS IS" BASIS, 
11  # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 
12  # See the License for the specific language governing permissions and 
13  # limitations under the License. 
14   
15  import os 
16  import cv2 
17  import cv_bridge 
18  import rospy 
19   
20  from sensor_msgs.msg import Image 
21   
22   
23 -class HeadDisplay(object): 
24      """ 
25      Interface class for head display 
26   
27      Displays a given image file or multiple files on the robot's face. 
28   
29      Pass the relative or absolute file path to an image file on your 
30      computer, and the example will read and convert the image using 
31      cv_bridge, sending it to the screen as a standard ROS Image Message. 
32   
33      Notes: 
34      Max screen resolution is 1024x600. 
35      Images are always aligned to the top-left corner. 
36      Image formats are those supported by OpenCv - LoadImage(). 
37      """ 
38   
39 -    def __init__(self): 
40          """ 
41          Constructor 
42          """ 
43          self._image_pub = rospy.Publisher('/robot/head_display', Image, latch=True, queue_size=10) 
44   
45 -    def _setup_image(self, image_path): 
46          """ 
47          Load the image located at the specified path 
48   
49          @type image_path: str 
50          @param image_path: the relative or absolute file path to the image file 
51   
52          @rtype: sensor_msgs/Image or None 
53          @param: Returns sensor_msgs/Image if image convertable and None otherwise 
54          """ 
55          if not os.access(image_path, os.R_OK): 
56              rospy.logerr("Cannot read file at '{0}'".format(image_path)) 
57              return None 
58   
59          img = cv2.imread(image_path) 
60          # Return msg 
61          return cv_bridge.CvBridge().cv2_to_imgmsg(img, encoding="bgr8") 
62   
63 -    def display_image(self, image_path, display_in_loop=False, display_rate=1.0): 
64          """ 
65          Displays image(s) to robot's head 
66   
67          @type image_path: list 
68          @param image_path: the relative or absolute file path to the image file(s) 
69   
70          @type display_in_loop: bool 
71          @param display_in_loop: Set True to loop through all image files infinitely 
72   
73          @type display_rate: float 
74          @param display_rate: the rate to publish image into head 
75          """ 
76          rospy.logdebug("Display images in loop:'{0}', frequency: '{1}'".format(display_in_loop, display_rate)) 
77   
78          image_msg = [] 
79          image_list = image_path if isinstance(image_path, list) else [image_path] 
80          for one_path in image_list: 
81              cv_img = self._setup_image(one_path) 
82              if cv_img: 
83                  image_msg.append(cv_img) 
84   
85          if not image_msg: 
86              rospy.logerr("Image message list is empty") 
87          else: 
88              r = rospy.Rate(display_rate) 
89              while not rospy.is_shutdown(): 
90                  for one_msg in image_msg: 
91                      self._image_pub.publish(one_msg) 
92                      r.sleep() 
93                  if not display_in_loop: 
94                      break 
95