Source Code for Module intera_interface.head_display

  1  #!/usr/bin/env python 
  2   
  3  # Copyright (c) 2016, Rethink Robotics 
  4  # All rights reserved. 
  5  # 
  6  # Redistribution and use in source and binary forms, with or without 
  7  # modification, are permitted provided that the following conditions are met: 
  8  # 
  9  # 1. Redistributions of source code must retain the above copyright notice, 
 10  #    this list of conditions and the following disclaimer. 
 11  # 2. Redistributions in binary form must reproduce the above copyright 
 12  #    notice, this list of conditions and the following disclaimer in the 
 13  #    documentation and/or other materials provided with the distribution. 
 14  # 3. Neither the name of the Rethink Robotics nor the names of its 
 15  #    contributors may be used to endorse or promote products derived from 
 16  #    this software without specific prior written permission. 
 17  # 
 18  # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 
 19  # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 
 20  # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 
 21  # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 
 22  # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 
 23  # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 
 24  # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 
 25  # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 
 26  # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 
 27  # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 
 28  # POSSIBILITY OF SUCH DAMAGE. 
 29   
 30  import os 
 31  import cv2 
 32  import cv_bridge 
 33  import rospy 
 34   
 35  from sensor_msgs.msg import Image 
 36   
 37   
 38 -class HeadDisplay(object): 
 39      """ 
 40      Interface class for head display 
 41   
 42      Displays a given image file or multiple files on the robot's face. 
 43   
 44      Pass the relative or absolute file path to an image file on your 
 45      computer, and the example will read and convert the image using 
 46      cv_bridge, sending it to the screen as a standard ROS Image Message. 
 47   
 48      Notes: 
 49      Max screen resolution is 1024x600. 
 50      Images are always aligned to the top-left corner. 
 51      Image formats are those supported by OpenCv - LoadImage(). 
 52      """ 
 53   
 54 -    def __init__(self): 
 55          """ 
 56          Constructor 
 57          """ 
 58          self._image_pub = rospy.Publisher('/robot/head_display', Image, latch=True, queue_size=10) 
 59   
 60 -    def _setup_image(self, image_path): 
 61          """ 
 62          Load the image located at the specified path 
 63   
 64          @type image_path: str 
 65          @param image_path: the relative or absolute file path to the image file 
 66   
 67          @rtype: sensor_msgs/Image or None 
 68          @param: Returns sensor_msgs/Image if image convertable and None otherwise 
 69          """ 
 70          if not os.access(image_path, os.R_OK): 
 71              rospy.logerr("Cannot read file at '{0}'".format(image_path)) 
 72              return None 
 73   
 74          img = cv2.imread(image_path) 
 75          # Return msg 
 76          return cv_bridge.CvBridge().cv2_to_imgmsg(img, encoding="bgr8") 
 77   
 78 -    def display_image(self, image_path, display_in_loop=False, display_rate=1.0): 
 79          """ 
 80          Displays image(s) to robot's head 
 81   
 82          @type image_path: list 
 83          @param image_path: the relative or absolute file path to the image file(s) 
 84   
 85          @type display_in_loop: bool 
 86          @param display_in_loop: Set True to loop through all image files infinitely 
 87   
 88          @type display_rate: float 
 89          @param display_rate: the rate to publish image into head 
 90          """ 
 91          rospy.logdebug("Display images in loop:'{0}', frequency: '{1}'".format(display_in_loop, display_rate)) 
 92   
 93          image_msg = [] 
 94          image_list = image_path if isinstance(image_path, list) else [image_path] 
 95          for one_path in image_list: 
 96              cv_img = self._setup_image(one_path) 
 97              if cv_img: 
 98                  image_msg.append(cv_img) 
 99   
100          if not image_msg: 
101              rospy.logerr("Image message list is empty") 
102          else: 
103              r = rospy.Rate(display_rate) 
104              while not rospy.is_shutdown(): 
105                  for one_msg in image_msg: 
106                      self._image_pub.publish(one_msg) 
107                      r.sleep() 
108                  if not display_in_loop: 
109                      break 
110