Access to Sawyer's Cameras Simultaneously


senaa

Recommended Posts

I'm trying to access both of Sawyer's cameras at the same time, from separate nodes, but experiencing an issue where it appears only one of the cameras is producing data.

If I run the nodes individually, the cameras work fine, but if both are accessed simultaneously one of them becomes unresponsive.

Also, I've noticed in RViz, if I set up one camera view, video streams through okay, but if I then change the topic to the other camera nothing comes through. If you try to have two camera views, again one of the cameras becomes unresponsive.

Overall, running both cameras together, or attempting to switch between the cameras individually results in one of them becoming unresponsive (sometimes the right hand camera, sometimes the head camera).

Is there a topic or service I should be calling to 'activate' the camera I would like? Is there a way to stream from both cameras simultaneously?

Link to comment
Share on other sites

I've since answered my own question, thought it might be useful to share. If I've missed an easier way to do this please let me know (or if there's some important reason as to why you shouldn't have both streaming at once).

Commands to activate/deactivate the cameras are sent to the cameras via a service as a JSON. The snippet below shows how to deactivate/activate the camera streams, to get both streaming simultaneously just set the status to true and call this for each camera.

I'm wondering if there's other commands for controlling focus, exposure, the flash etc... :D

#!/usr/bin/env python
# Aran Sena 2018
#
# Code example only, provided without guarantees
#
# Example for how to get both cameras streaming together
#
####

import rospy
from intera_core_msgs.srv._IOComponentCommandSrv import IOComponentCommandSrv
from intera_core_msgs.msg._IOComponentCommand import IOComponentCommand


def camera_command_client(camera, status, timeout=0.0):
    rospy.wait_for_service('/io/internal_camera/' + camera + '/command')
    try:
        head_cam_control = rospy.ServiceProxy('/io/internal_camera/' + camera + '/command', IOComponentCommandSrv)
        cmd = IOComponentCommand()
        cmd.time = rospy.Time.now()
        cmd.op = 'set'
        if status:
            cmd.args = '{"signals": {"camera_streaming": {"data": [true], "format": {"type": "bool"}}}}'
        else:
            cmd.args = '{"signals": {"camera_streaming": {"data": [false], "format": {"type": "bool"}}}}'

        resp = head_cam_control(cmd, timeout)
        print resp

    except rospy.ServiceException, e:
        print "Service call failed: %s"%e


if __name__ == '__main__':
    rospy.init_node('camera_command_client')
    camera_command_client(camera='head_camera', status=True)
    camera_command_client(camera='right_hand_camera', status=True)

 

 

Link to comment
Share on other sites

Archived

This topic is now archived and is closed to further replies.