Subscribing from 2 cameras simultaneously in sawyer robot

asked 2019-01-03 23:43:53 -0500

homagni gravatar image

updated 2019-01-03 23:50:01 -0500

Hello, I was recently trying to follow along the code for displaying two camera images (from head and back end of the hand) from the sawyer one-handed robot. Here: http://sdk.rethinkrobotics.com/intera...

Now the camera interface in sawyer does not allow to have two cameras on, but I need it !, so I did : (after installing sawyer gazebo from here :http://sdk.rethinkrobotics.com/intera/Gazebo_Tutorial)

import argparse
import numpy as np
import cv2
from cv_bridge import CvBridge, CvBridgeError
import rospy
import intera_interface
from sensor_msgs.msg import Image
import message_filters
def img_callback(image_hand,image_head):
    bridge1 = CvBridge()
    bridge2 = CvBridge()
    try:
      cv_image1 = bridge1.imgmsg_to_cv2(image_hand, "mono8")
    except CvBridgeError as e:
      print(e)
    cv2.imshow("Hand Image window", cv_image1)
    cur_image_hand=cv2.resize(cv_image1, (80, 80))
    cv2.waitKey(3)
    try:
      cv_image2 = bridge2.imgmsg_to_cv2(image_head, "bgr8")
    except CvBridgeError as e:
      print(e)
    cv2.imshow("Head Image window", cv_image2)
    gray_image = cv2.cvtColor(cv_image2, cv2.COLOR_BGR2GRAY)
    cur_image_head=cv2.resize(gray_image, (80, 80))
    #rospy.spin()
    cv2.waitKey(3)
def main():
    rospy.init_node('image_converter', anonymous=True)
    image_sub_hand = message_filters.Subscriber("/io/internal_camera/right_hand_camera/image_rect",Image) 
    image_sub_head = message_filters.Subscriber("/io/internal_camera/head_camera/image_rect_color",Image)
    #ts = message_filters.TimeSynchronizer([self.image_sub_hand, self.image_sub_head], 10)
    ts = message_filters.TimeSynchronizer([image_sub_hand, image_sub_head], 10)
    ts.registerCallback(img_callback)
    rospy.spin()

if __name__ == '__main__':
    main()

This code starts with both the images showing nicely together in two seperate windows for a few seconds, but after that both the cv windows blacken out and images are not updated anymore . Please suggest what to do, and provide me the best solution that allows me to access these two topics at any time using python code.

edit retag flag offensive close merge delete