publish_img_folder.py

Go to the documentation of this file.

#!/usr/bin/env python
import roslib; roslib.load_manifest('iri_stereo_visual_odometry')
import rospy
import os
import sys
import cv
import rosbag
from cv import *
from sensor_msgs.msg import Image
from std_msgs.msg import String
from cv_bridge import CvBridge, CvBridgeError

# Definir el node
def spit():

  rospy.loginfo(" Publish Images from Folder ")
  rospy.loginfo(" ")

  # Agafar parametre folder si fa falta
  # Aqui s'ha de canviar "user" pel nom d'usuari que toca
  folder = "/home/user/iri-lab/labrobotica/algorithms/stereo_visual_odometry/trunk/images"
  folder = "/home/gmoreno/iri-lab/ros/iri-ros-pkg/iri_odometry_publisher/iri_stereo_visual_odometry/data"
  
  if(folder[6:10]=="user"):
    rospy.loginfo(" How To: 1.   donar permisos: chmod +x publish_img_folder.py ")
    rospy.loginfo("         2.a. executar: ./publish_img_folder.py (canviar user i path a linia 21)")
    rospy.loginfo(" ")

  else:
  
    # crear publishers per a left i per a right
    publeft = rospy.Publisher('stereo/left/image', Image)
    pubright = rospy.Publisher('stereo/right/image', Image)

    # iniciar el node
    rospy.init_node('spit')

    # crear objecte cv bridge
    cvb = CvBridge()

    # crear una imatge per escalfar els publishers
    font = cv.InitFont(cv.CV_FONT_HERSHEY_COMPLEX,3, 3, 0.0, 5, 8)
    warming_image = cv.CreateImage((1280,960), cv.IPL_DEPTH_8U, 1)
    cv.PutText(warming_image,"Warming", (600,450),font, 255)
    ros_image = cvb.cv_to_imgmsg(warming_image, encoding="passthrough")
    ros_image.header.stamp = rospy.Time.now()
    publeft.publish(ros_image)
    pubright.publish(ros_image)
    rospy.loginfo("  Warm images publicades")
    rospy.loginfo(" ")
    rospy.loginfo(" ---------- ")
    rospy.loginfo(" ")
    
    # llegir carpeta contenidora d'imatges
    for root, dirs, files in os.walk(folder):
      
      # ordenar els arxius per ordre alfabetic
      files.sort()
      
      # tractar cada imatge
      for name in files: 
        
        # Extreure Nom      
        filename = os.path.join(root, name)
        camera = int(filename[len(filename)-5])
      
        try:
        
          # Carregar Imatge
          cv_image = LoadImage (filename, 1)
          camera_maco = "left" if camera==1 else "right"
          rospy.loginfo("  Imatge %s carregada: ...%s",camera_maco,filename[-35:])
        
          # Mostrar imatge
          # NamedWindow ('img'); ShowImage ('img', cv_image); WaitKey ()
        
          # Convertir imatge a ROS msg
          ros_image = cvb.cv_to_imgmsg(cv_image, encoding="passthrough")
          rospy.loginfo("    1 mida: %d %d",ros_image.width,ros_image.height)
          ros_image.header.stamp = rospy.Time.now()
          ros_image.header.frame_id = "/stereo/left" if camera==1 else "/stereo/right"
          rospy.loginfo("    2 header escrit, frame_id: %s stamp: %s",ros_image.header.frame_id,ros_image.header.stamp)
          
          # Publicar imatge
          if(camera==1):
            publeft.publish(ros_image)
          else:
            pubright.publish(ros_image)
          rospy.loginfo("    3 imatge publicada")
          rospy.loginfo(" ")

        except:
          # Exception
          rospy.loginfo(" FAIL")
        
        # delay
        if(camera!=1):
          rospy.sleep(0.2)  
          rospy.loginfo(" ---------- ")
          rospy.loginfo(" ")

    rospy.loginfo(" THE END")


if __name__ == '__main__':
    try:
        spit()
    except rospy.ROSInterruptException:
        pass