Dual Camera Servo Server

Name: Dual Camera Servo Server
Author: DJ Sures

by Synthiam Inc.

A server for DIY usage that allows a client to control servos and sends two stereoscopic camera streams to the client.

Requires ARC v1 (Updated 6/14/2022)

How to add the Dual Camera Servo Server robot skill

Load the most recent release of ARC (Get ARC).
Press the Project tab from the top menu bar in ARC.
Press Add Robot Skill from the button ribbon bar in ARC.
Choose the Virtual Reality category tab.
Press the Dual Camera Servo Server icon to add the robot skill to your project.

Don't have a robot yet?

Follow the Getting Started Guide to build a robot and use the Dual Camera Servo Server robot skill.

How to use the Dual Camera Servo Server robot skill

A server for DIY usage that allows a client to control servos and sends two camera streams to the client for stereoscopic view. This allows a custom VR app to use two cameras for stereoscopic view.

C# .Net Win Forms Test App

We have provided an example C# .Net test app demonstrating how a client can connect to this robot skill server. The app can be downloaded here: Dual Camera servo Test App.zip

Unity VR App

The ideal custom VR app can be created in Unity. If using an Oculus Quest 2, we have a Unity example to demonstrate how to bind the hand, controller, and headset positions to servos. In addition, the left and right camera images can be assigned to a Raw Image game object.

Download the C# .Net Win Forms Test App from the link above and copy the Comm folder to your Unity asset project. This folder contains the StreamClient that will be used to connect to this robot skill, receive both camera images, and transmit servo positions.


using Assets.Comm;
using Synthiam_Unity_Common;
using UnityEngine;
using UnityEngine.UI;

namespace SceneRobotView {

  public class RobotView : MonoBehaviour {

    StreamClient _streamClient;
    bool _initialized = false;
    bool _isClosing = false;

    volatile byte [] _toDisplayLeft = new byte[]{ };
    volatile byte [] _toDisplayRight = new byte[]{ };

    RawImage _leftImage;
    RawImage _rightImage;

    void Start() {

      _leftImage = GameObject.Find("Left Eye Image").GetComponent();
      _leftImage.texture = new Texture2D(1280, 720, TextureFormat.RGB24, false);

      _rightImage = GameObject.Find("Right Eye Image").GetComponent();
      _rightImage.texture = new Texture2D(1280, 720, TextureFormat.RGB24, false);

      _streamClient = new StreamClient();
      _streamClient.OnLeftImageDataReady += _streamClient_OnLeftImageDataReady;
      _streamClient.OnRightImageDataReady += _streamClient_OnRightImageDataReady;
      _streamClient.OnStop += _streamClient_OnStop;
      _streamClient.OnStart += _streamClient_OnStart;
      _streamClient.OnError += _streamClient_OnError;
      _streamClient.Start("127.0.0.1", 8082);
    }

    private void _streamClient_OnError(System.DateTime time, string logTxt) {

    }

    private void _streamClient_OnStart() {

    }

    private void _streamClient_OnStop() {

      if (_isClosing)
        return;
    }

    private void _streamClient_OnLeftImageDataReady(byte[] imageData) {

      if (!_initialized || _isClosing)
        return;

      lock (_toDisplayLeft)
        _toDisplayLeft = imageData;
    }

    private void _streamClient_OnRightImageDataReady(byte[] imageData) {

      if (!_initialized || _isClosing)
        return;

      lock (_toDisplayRight)
        _toDisplayRight = imageData;
    }

    private void OnDestroy() {

      _isClosing = true;

      _streamClient.Stop();
      _streamClient.Dispose();
    }

    void Update() {

      _initialized = true;

      if (_isClosing)
        return;

      if (Camera.current == null)
        return;

      lock (_toDisplayLeft)
        if (_toDisplayLeft.Length > 0)
          (_leftImage.texture as Texture2D).LoadImage(_toDisplayLeft);

      lock (_toDisplayRight)
        if (_toDisplayRight.Length > 0)
          (_rightImage.texture as Texture2D).LoadImage(_toDisplayRight);

      if (_streamClient.IsRunning && OVRManager.hasInputFocus) {

        // https://developer.oculus.com/documentation/unity/unity-handtracking/
        // https://github.com/handzlikchris/Unity.QuestRemoteHandTracking/blob/master/HandsDataSender.cs
        // Hand_WristRoot = 0 // root frame of the hand, where the wrist is located
        // Hand_ForearmStub = 1 // frame for user's forearm
        // Hand_Thumb0 = 2 // thumb trapezium bone
        // Hand_Thumb1 = 3 // thumb metacarpal bone
        // Hand_Thumb2 = 4 // thumb proximal phalange bone
        // Hand_Thumb3 = 5 // thumb distal phalange bone
        // Hand_Index1 = 6 // index proximal phalange bone
        // Hand_Index2 = 7 // index intermediate phalange bone
        // Hand_Index3 = 8 // index distal phalange bone
        // Hand_Middle1 = 9 // middle proximal phalange bone
        // Hand_Middle2 = 10 // middle intermediate phalange bone
        // Hand_Middle3 = 11 // middle distal phalange bone
        // Hand_Ring1 = 12 // ring proximal phalange bone
        // Hand_Ring2 = 13 // ring intermediate phalange bone
        // Hand_Ring3 = 14 // ring distal phalange bone
        // Hand_Pinky0 = 15 // pinky metacarpal bone
        // Hand_Pinky1 = 16 // pinky proximal phalange bone
        // Hand_Pinky2 = 17 // pinky intermediate phalange bone
        // Hand_Pinky3 = 18 // pinky distal phalange bone

        OVRPlugin.HandState handState = default(OVRPlugin.HandState);

        if (OVRPlugin.GetHandState(OVRPlugin.Step.Render, OVRPlugin.Hand.HandLeft, ref handState)) {

          _streamClient.SendPacketLeftHand(
            Utilities.MapToByteHalf(handState.BoneRotations[5].x),
            Utilities.MapToByteHalf(handState.BoneRotations[5].y),
            Utilities.MapToByteHalf(handState.BoneRotations[5].z),

            Utilities.MapToByteHalf(handState.BoneRotations[8].x),
            Utilities.MapToByteHalf(handState.BoneRotations[8].y),
            Utilities.MapToByteHalf(handState.BoneRotations[8].z),

            Utilities.MapToByteHalf(handState.BoneRotations[11].x),
            Utilities.MapToByteHalf(handState.BoneRotations[11].y),
            Utilities.MapToByteHalf(handState.BoneRotations[11].z),

            Utilities.MapToByteHalf(handState.BoneRotations[14].x),
            Utilities.MapToByteHalf(handState.BoneRotations[14].y),
            Utilities.MapToByteHalf(handState.BoneRotations[14].z),

            Utilities.MapToByteHalf(handState.BoneRotations[18].x),
            Utilities.MapToByteHalf(handState.BoneRotations[18].y),
            Utilities.MapToByteHalf(handState.BoneRotations[18].z),

            Utilities.MapToByte(handState.RootPose.Position.x),
            Utilities.MapToByte(handState.RootPose.Position.y),
            Utilities.MapToByte(handState.RootPose.Position.z));
        } else if (OVRInput.GetControllerPositionTracked(OVRInput.Controller.LTouch)) {

          var controller = OVRInput.GetLocalControllerPosition(OVRInput.Controller.LTouch);

          var thumbStick = OVRInput.Get(OVRInput.Axis2D.PrimaryThumbstick, OVRInput.Controller.LTouch);

          var indexTrigger = OVRInput.Get(OVRInput.Axis1D.PrimaryIndexTrigger, OVRInput.Controller.LTouch);

          var handTrigger = OVRInput.Get(OVRInput.Axis1D.PrimaryHandTrigger, OVRInput.Controller.LTouch);

          var buttonA = OVRInput.Get(OVRInput.Button.One, OVRInput.Controller.LTouch);

          var buttonB = OVRInput.Get(OVRInput.Button.Two, OVRInput.Controller.LTouch);

          _streamClient.SendPacketLeftController(
            Utilities.MapToByte(controller.x),
            Utilities.MapToByte(controller.y),
            Utilities.MapToByte(controller.z),
            Utilities.MapToByte(thumbStick.x),
            Utilities.MapToByte(thumbStick.y),
            Utilities.MapToByteHalf(indexTrigger),
            Utilities.MapToByteHalf(handTrigger),
            (byte)(buttonA ? 1 : 0),
            (byte)(buttonB ? 1 : 0));
        }

        if (OVRPlugin.GetHandState(OVRPlugin.Step.Render, OVRPlugin.Hand.HandRight, ref handState)) {

          _streamClient.SendPacketRightHand(
            Utilities.MapToByteHalf(handState.BoneRotations[5].x),
            Utilities.MapToByteHalf(handState.BoneRotations[5].y),
            Utilities.MapToByteHalf(handState.BoneRotations[5].z),

            Utilities.MapToByteHalf(handState.BoneRotations[8].x),
            Utilities.MapToByteHalf(handState.BoneRotations[8].y),
            Utilities.MapToByteHalf(handState.BoneRotations[8].z),

            Utilities.MapToByteHalf(handState.BoneRotations[11].x),
            Utilities.MapToByteHalf(handState.BoneRotations[11].y),
            Utilities.MapToByteHalf(handState.BoneRotations[11].z),

            Utilities.MapToByteHalf(handState.BoneRotations[14].x),
            Utilities.MapToByteHalf(handState.BoneRotations[14].y),
            Utilities.MapToByteHalf(handState.BoneRotations[14].z),

            Utilities.MapToByteHalf(handState.BoneRotations[18].x),
            Utilities.MapToByteHalf(handState.BoneRotations[18].y),
            Utilities.MapToByteHalf(handState.BoneRotations[18].z),

            Utilities.MapToByte(handState.RootPose.Position.x),
            Utilities.MapToByte(handState.RootPose.Position.y),
            Utilities.MapToByte(handState.RootPose.Position.z));
        } else if (OVRInput.GetControllerPositionTracked(OVRInput.Controller.RTouch)) {

          var controller = OVRInput.GetLocalControllerPosition(OVRInput.Controller.RTouch);

          var thumbStick = OVRInput.Get(OVRInput.Axis2D.PrimaryThumbstick, OVRInput.Controller.RTouch);

          var indexTrigger = OVRInput.Get(OVRInput.Axis1D.PrimaryIndexTrigger, OVRInput.Controller.RTouch);

          var handTrigger = OVRInput.Get(OVRInput.Axis1D.PrimaryHandTrigger, OVRInput.Controller.RTouch);

          var buttonA = OVRInput.Get(OVRInput.Button.One, OVRInput.Controller.RTouch);

          var buttonB = OVRInput.Get(OVRInput.Button.Two, OVRInput.Controller.RTouch);

          _streamClient.SendPacketRightController(
            Utilities.MapToByte(controller.x),
            Utilities.MapToByte(controller.y),
            Utilities.MapToByte(controller.z),
            Utilities.MapToByte(thumbStick.x),
            Utilities.MapToByte(thumbStick.y),
            Utilities.MapToByteHalf(indexTrigger),
            Utilities.MapToByteHalf(handTrigger),
            (byte)(buttonA ? 1 : 0),
            (byte)(buttonB ? 1 : 0));
        }

          _streamClient.SendPacketHeadset(
            Utilities.MapToByte(Camera.current.transform.localRotation.x),
            Utilities.MapToByte(Camera.current.transform.localRotation.y),
            Utilities.MapToByte(Camera.current.transform.localRotation.z));
      }
    }
  }
}

Similar Skills

Upgrade to ARC Pro

Get access to the latest features and updates before they're released. You'll have everything that's needed to unleash your robot's potential!

Compare Pro Features View Subscription Plans

UweP

PRO

Germany

#1 Jun 2022 — Edited Jun 2022

Hello DJ,

great, of course I tested it right away with 2 MS LiveCams HD-3000

First, there seems to be an error:

have changed it:

Works now, both cameras are transferred and displayed on the two RawImages.

Now about the stereoscopic display, have you tested it with the Oculus?

What I did, first set Per Eye Cameras for the OVR Camera Rig (Thanks DJ, I forgot to mention it).

created 2 new layers 1x left image, 1x right image

assign the respective layer to the two RawImages

Set the two RawImages to the same position, camera aligned.

Camera "LeftEyeAnchor" culling mask changed, layer rightimage switched off (correspondingly switched off on the rightEyeAnchor leftimage)

That's how I did it in my previous tests, with the difference that I didn't use a RawImage but a GameObject (Plane).

And this is exactly where I got problems, both RawImages are displayed on both cameras (leftEyeAnchor rightEyeAnchor). I've tried everything, it doesn't seem to work with a canvas element.

So changed a bit:

Instead of the 2 RawImages I created 2 GameObjects (Plane) and adjusted the code

Otherwise the same as described above.

Is working!

You just have to align the two cameras very precisely (distance approx. 7cm), horizontal is particularly important, otherwise the brain will not be able to put the two images together.....

The PC must also be powerful enough, with my Rock Pi you have a mega delay with only one cam and the two cams are not syncronous, but that's another topic....

DJ Sures

PRO

Synthiam

#2 Jun 2022

That's fantastic feedback - thanks; I'll update the details.

As for synchronizing, the camera images are only updated in the Update() method - so that should have them synchronized.

I wonder why there would be a significant delay? Does a lower resolution help with the delay? I'm wondering if the delay is bandwidth limited or CPU limited.

DJ Sures

PRO

Synthiam

#3 Jun 2022 — Edited Jun 2022

Oh, also using GAmeObject.Find() might be too slow to use in the Update() method. Perhaps use GameObject.Find() in the constructor and assign the result to a global variable. That way, the global variable holds a reference to the gameobject, so Find() is not needed in Update.

UweP

PRO

Germany

#4 Jun 2022

I still have to test more with the Rock Pi, I only tested one cam with it and it had a very large delay. I just got it last week and set it up over the weekend, still have to test it a little...

Yes, I already thought about the GameObject.Find(), you had a corresponding comment in the other code. I just didn't get it, Unity and especially c# is new to me, I'm a bad copy paster (uhhhhh)....

My best thanks for this super skill, it brings me a good deal closer to my ideas!

DJ Sures

PRO

Synthiam

#5 Jun 2022

If you want to force a synchronization heavily, it can be done by modifying these two event methods...

First, add two global boolean variables that will keep the state change of the update.


bool _leftCameraUpdated = false;
bool _rightCameraUpdated = false;

Next, modify the event methods...


    private void _streamClient_OnLeftImageDataReady(byte[] imageData) {

      if (!_initialized || _isClosing)
        return;

      lock (_toDisplayLeft)
        _toDisplayLeft = imageData;

_leftCameraUpdated = true;
    }

    private void _streamClient_OnRightImageDataReady(byte[] imageData) {

      if (!_initialized || _isClosing)
        return;

      lock (_toDisplayRight)
        _toDisplayRight = imageData;

_rightCameraUpdated = true;
    }

Lastly, in the Update() method, change only to update the textures when both image data have been updated. Your changes to using a Plane will need to be updated in my example below...


if (_rightCameraUpdated && _leftCameraUpdated) {

      lock (_toDisplayLeft)
        if (_toDisplayLeft.Length > 0)
          (_leftImage.texture as Texture2D).LoadImage(_toDisplayLeft);

      lock (_toDisplayRight)
        if (_toDisplayRight.Length > 0)
          (_rightImage.texture as Texture2D).LoadImage(_toDisplayRight);

  _rightCameraUpdated = false;
  _leftCameraUpdated = false;
}

This will ensure the textures are only updated once the camera image has been updated

UweP

PRO

Germany

#6 Jun 2022

understand, very nice, will insert it

UweP

PRO

Germany

#7 Jun 2022

one more question, should the display in ARC of the two cameras already be displayed synchronously?

I don't need it, but I noticed the delay of the two cameras with the Rock Pi, so I immediately continued with my gaming PC and put the Rock Pi aside...

DJ Sures

PRO

Synthiam

#8 Jun 2022

Arc receives the image data as it is streamed from the usb device. Because computers run linear, even threading isn’t synchronized at the hardware level. ARC will display the incoming video frame as fast as it arrives and send it over the wire to the oculus.

You can limit the video output frame rate in the camera device settings. That will limit how often the video screen is updated, but will not affect the send to oculus. It is under the advanced tab in the camera device settings.

As for synchronizing on the oculus, the method we provided with global Boolean variables will work, as they wait for both cameras to be updated.

We can go a bit deeper and use stop watches in c# to expire old frame data. But that will have a cost of lowering the framerate.

UweP

PRO

Germany

#9 Jun 2022 — Edited Jun 2022

we have to test that now, it always depends on who is looking at the stereoscopic image, because every person is different. some are very sensitive and quickly get problems if the pictures differ too much.

I have fewer problems with it. I taught myself to look at side by side pictures and videos using the "Crossview" technique. Once you get the hang of it, it's very entertaining, I just need to look closely at a side-by-side picture or video and the stereoscopic picture appears in front of my eyes in the middle, it's no joke, here e.g. information about it:

https://en.wikipedia.org/wiki/Stereoscopy#Freeviewing https://www.flickr.com/photos/28458566@N08/16661278842/

to practice it you can cover each of the pictures, over time you won't need it anymore....

https://www.deviantart.com/aporev/art/Cross-view-Stereo-Tutorial-111098009

DJ Sures

PRO

Synthiam

#10 Jun 2022

Wow that’s neat and comes in handy I bet. I’m ordering a second camera like yours to test more with. It will be here in a few days

DJ Sures

PRO

Synthiam

#11 Jun 2022

When using the separate left and right cameras, can you get a menu canvas to display in the center camera for both eyes?

I have Per Eye Cameras set for the OVR Camera Rig.

My canvas menu sits under the Center camera.

The per-eye cameras render their game objects correctly - but the center camera does not display the UI menu. Any ideas?

UweP

PRO

Germany

#12 Jun 2022

Thanks, I forgot to include the "Per Eye Cameras" setting, I've now added it above.

I tested a little, I can display a canvas on both cameras left/right.

blue cube on layer right green cube on layer left red canvas element on Layer UI or Default

I switched the canvas, Render Mode to World Space and positioned the canvas.

Does this help?

The center camera is only displayed in the editor, has no effect on the display in the Oculus with the "Per Eye Cameras" setting, that's how I understand it.

I also found something about the problem that you cannot hide a canvas via the layers:

https://answers.unity.com/questions/1507202/camera-culling-mask-for-ui-elements-not-working.html

UweP

PRO

Germany

#13 Jun 2022 — Edited Jun 2022

I quickly created a camera rig for 2 x Microsoft LifeCam HD-3000 Web Cams

Instructions for disassembling the cameras can be found here (InMoov users should know this ;-) ):

https://astrophotovideo.wordpress.com/adapting-a-webcam-to-a-telescope/

You can find the STL file here: Stereo_LiveCams_HD-3000_mount.stl

One more thing, tripod adapter GoPro/EZ for easier testing...

STL File: Stativ_auf_EZ.stl

DJ Sures

PRO

Synthiam

#14 Jun 2022 — Edited Jun 2022

Awesome! I’ll be using that when my second cam arrives

UweP

PRO

Germany

#15 Jun 2022

did you see my post above to your question about the canvas, might that help?

DJ Sures

PRO

Synthiam

#16 Jun 2022

It sure does. Don’t think I can have a popup menu so I might have to make the menu a separate scene

UweP

PRO

Germany

#17 Jun 2022

I've tested a lot now, unfortunately a video doesn't make sense to show you because you only get a mono image rendered in the video from the VR glasses.

It works with 2 USB cameras, the only small problem is that the USB cameras sometimes do not deliver a synchronous image.

This doesn't have to be a problem, it depends a lot on the PC (performance, USB ports). The processing of the videos on the PC and VR side is synchronous, but the USB cams deliver their images as they want (timings and delays), to avoid this, the two cameras would have to be synchronized on the hardware side. But this is not so easy to do, only very expensive USB cameras have connections for this. In our case, however, it is definitely enough to see only a stereoscopic image, we do not want to use it as a sensor.

Information on the topic: https://docs.baslerweb.com/synchronous-free-run https://robotics.stackexchange.com/questions/16454/trigger-two-usb-cameras-at-the-same-time https://www.arducam.com/industrial-grade-hardware-timestamping-usb-3-camera-modules-synchronization/ https://www.robotshop.com/community/blog/show/how-to-synchronise-two-cmos-camera-modules-for-stereo-vision

But as written, just to display a stereoscopic image, this skill works perfectly well!

I tested with this little guy (more on that soon), my attempts with the Rock Pi weren't so good:

CHUWI LarkBox Pro