Single Camera Servo Server

How to use the Single Camera Servo Server robot skill

This is a servo & camera video server which allows a remote client to move servos and receive video stream from a camera device. This is specifically useful for those creating Unity apps that operate as a client to ARC. where the camera video stream can be received and servo positions can be sent. The camera must be streaming for the servo positions to transmit.

Demo #1
This is an overview of how this robot skill can integrate with a unity scene using the Unity animation tool.



Demo #2
This example uses a block in Unity that controls an EZ-Robot JD shoulder. It's a very simple example of how powerful and easy it is to control any servo in ARC. The blocks have gravity and a hinge, so as it swings the angle is read and pushed into ARC to move the respective servo.


Demo #3
This is Mickey's demonstration of controlling the servos through unity and joysticks.


How It Works
Code from the client (ie unity in this case) will connect to the camera servo skill over tcp. It streams the servo positions and receives the camera view. 




Example Client App Source Code
Here is an example test app src that connects to localhost (127.0.0.1), moves a servo on port D2 and displays the camera video stream. The sample app is C# .Net source-code and can be downloaded.

Download C# .Net Example Source code: Test App.zip (2020/12/17)


Test Unity Project
I have included a test Unity project for example reference. The example rotates a cube on the screen using the ARROW keys. The cube projects the texture from the camera stream onto it. The arrow keys will also move the servos connected to port D0 and D1 relative to the rotation of the cube.

You can download the project here: Unity Test.zip (2020/12/17)


Use In Unity
The stream client files in the "Test App" can be included in a Unity project to receive the video and move servos. The test app demonstrates how to move the servos using the methods, and how to display the video on Windows. To display the video in Unity, follow the steps below. The video works by becoming a Texture2D that can be applied to any material.

To use this in your Unity App, copy the files from the Test App\ServoServerClient\*.cs into your Unity project.




Examine Test Project
The Unity project displays the ARC camera stream on a rotating cube. While, allowing the 2d sprite to control servos D0 and D1 by the X and Y position, respectively. Clicking on the scene will move the sprite and also move the servos.




Any components within the group can have their position or rotation, etc. extracted and sent to ARC. If you have a 3d model of a robot, each joint position/rotation can be sent to ARC. 

The most interesting thing to look at is the Scene object -> ServoCameraGroup. Notice it has child GameObjects. Those child GameObjects can be queried for their rotation or position or whatever is desired and sent to ARC as servo positions. Also, the camera image can be rendered to any material as a 2d texture.



Look at the ServoCameraGroup to see the script



The script ServoCamera.cs will be responsible for
Start
- create and instance of the StreamClient object
- have the StreamClient connect to ARC at an IP address (this case it's using local machine 127.0.0.1)
- assign child gameobjects to local variables that we will be using in Update (this makes cpu happy)
- connecte to the ARC server

Update
- obtaining rotation/position/whatever data from children and add to the servo position cache (in this example a sprite position)
- sending the cache of servo positions
- displaying the incoming image on a material as a texture

Let's take a look at the code for ServoCamera.cs and read the comments of how it is working

Code:

using EZ_Robot_Unity_DLL;
using UnityEngine;

public class ServoCamera : MonoBehaviour {

  ServoServerClient _streamClient;
  bool _initialized = false;
  Texture2D _texture;

  volatile byte [] _toDisplay = new byte[]{ };

  Transform _cube;
  Transform _sprite;

  /// 
  /// We have this script added to a parent that has children. 
  /// Because we can access the children's transformation or position from here to set servo positions
  /// In the update, we'll just grab the children and use thier data to send to ARC
  /// 
  void Start() {

    // This is the texture that will hold the camera image from ARC
    // We apply this texture to a cube
    _texture = new Texture2D(640, 480, TextureFormat.RGB24, false);

    // assign a local variable to the children so we don't have to search for them on each frame (makes cpu happy)
    _cube = gameObject.transform.Find("Cube");
    _sprite = gameObject.transform.Find("MoveableSprite");

    //Create a client that will connect to ARC at the specified IP address
    // Once connected, any available video data from the ARC camera will raise the OnImageDataReady event
    _streamClient = new ServoServerClient();
    _streamClient.OnImageDataReady += _streamClient_OnImageDataReady;
    _streamClient.Start("127.0.0.1", 8282);
  }

  /// 
  /// This event is raised for every camera image that is received from the connected ARC server.
  /// We assign the image data to a volatile array that will be used in the update the texture with the latest image
  /// 
  private void _streamClient_OnImageDataReady(byte[] imageData) {

    if (!_initialized)
      return;

    _toDisplay = imageData;
  }

  void OnDisable() {

    _streamClient.Stop();
  }

  /// 
  /// Unity runs this on every render frame
  /// We get the keyboard input to move the camera around
  /// And we map the cube's X and Y rotation values to D0 and D1 servo positions in ARC, respectively
  /// 
  void Update() {

    _initialized = true;

    if (Input.GetKey(KeyCode.Escape))
      Application.Quit();

    // add the position of the servos to the cache based on the location of the sprice
    // We set the positions to cache in this loop rather than trying to send a position each time
    // That way we can send a bulk change which is much faster on bandwidth
    // So, add your servo positions to the cache and then send them all after
    _streamClient.SetCachedServoPosition(ServoServerClient.ServoPortEnum.D0, _streamClient.MapToByte(_sprite.transform.position.x));
    _streamClient.SetCachedServoPosition(ServoServerClient.ServoPortEnum.D1, _streamClient.MapToByte(_sprite.transform.position.y));

    // Send all the servo positions that have been cached
    _streamClient.SendCachedServoPositions();

    // Display the latest camera image by rendering it to the texture and applying to the cube's material
    if (_toDisplay.Length > 0) {

      _texture.LoadImage(_toDisplay);

      var material = _cube.GetComponent().material;
      material.mainTexture = _texture;
      material.mainTextureScale = new Vector2(1, -1);
    }
  }
}

The thing is, you will need a camera connected for it to work. The Unity servoServerClient assumes you have a camera connected and streaming for the servopositions to send.

_streamClient.SetCachedServoPosition(ServoServerClient.ServoPortEnum.D0, (byte)SomeInt);

using EZ_Robot_Unity_DLL;
using BioIK;
using System.Collections;
using UnityEngine;
public class CameraCubeRotate : MonoBehaviour {
    ServoServerClient _streamClient;
    bool _initialized = false;
    public float _speed = 0.5f;
    Texture2D _texture;
    float positionAbs;
    public GameObject D0;
    public GameObject D1;
    public GameObject D2;
    volatile byte [] _toDisplay = new byte[]{ };
    void Start() {
        _texture = new Texture2D(640, 480, TextureFormat.RGB24, false);
        _streamClient = new ServoServerClient();
        _streamClient.OnImageDataReady += _streamClient_OnImageDataReady;
        _streamClient.Start("127.0.0.1", 8282);
    }
    private void _streamClient_OnImageDataReady(byte[] imageData) {
        if (!_initialized)
          return;
        _toDisplay = imageData;
      }
      void OnDisable() {
        _streamClient.Stop();
      }
      void Update() {
        _initialized = true;
        if (Input.GetKey(KeyCode.Escape))
          Application.Quit();
    
        if (Input.GetKey(KeyCode.RightArrow)) {
          transform.Rotate(new Vector3(0, -_speed, 0));
          _streamClient.SetCachedServoPosition(ServoServerClient.ServoPortEnum.D1, _streamClient.MapToByte(transform.rotation.x));
        }
        if (Input.GetKey(KeyCode.LeftArrow)) {
          transform.Rotate(new Vector3(0, _speed, 0));
          _streamClient.SetCachedServoPosition(ServoServerClient.ServoPortEnum.D1, _streamClient.MapToByte(transform.rotation.x));
        }
        if (Input.GetKey(KeyCode.DownArrow)) {
          transform.Rotate(new Vector3(_speed, 0, 0));
          _streamClient.SetCachedServoPosition(ServoServerClient.ServoPortEnum.D0, _streamClient.MapToByte(transform.rotation.y));
        }
        if (Input.GetKey(KeyCode.UpArrow)) {
          transform.Rotate(new Vector3(-_speed, 0, 0));
          _streamClient.SetCachedServoPosition(ServoServerClient.ServoPortEnum.D0, _streamClient.MapToByte(transform.rotation.y));
        }
        BioJoint joint_D0 = D0.GetComponent<BioJoint>();
        double value_D0 = joint_D0.Z.GetTargetValue();
        int position_D0= Mathf.RoundToInt((float)value_D0);
        int positionAbs_D0 = Mathf.Abs(position_D0 - 90);
        print(positionAbs_D0);
        BioJoint joint_D1 = D1.GetComponent<BioJoint>();
        double value_D1 = joint_D1.Y.GetTargetValue();
        int position_D1 = Mathf.RoundToInt((float)value_D1);
        int positionAbs_D1 = Mathf.Abs(180 - (position_D1 - 90));
        print(positionAbs_D1);
        BioJoint joint_D2 = D2.GetComponent<BioJoint>();
        double value_D2 = joint_D2.Y.GetTargetValue();
        int position_D2 = Mathf.RoundToInt((float)value_D2);
        int positionAbs_D2 = Mathf.Abs(position_D2 - 90);
        
        _streamClient.SetCachedServoPosition(ServoServerClient.ServoPortEnum.D0, (byte)positionAbs_D0);
        _streamClient.SetCachedServoPosition(ServoServerClient.ServoPortEnum.D1, (byte)positionAbs_D1);
        _streamClient.SetCachedServoPosition(ServoServerClient.ServoPortEnum.D2, (byte)positionAbs_D2);
        // Send all the servo positions if there's been a change
        if (_streamClient.HasServoChanged)
        _streamClient.SendCachedServoPositions();
        if (_toDisplay.Length > 0) {
          _texture.LoadImage(_toDisplay);
          var material = GetComponent<Renderer>().material;
          material.mainTexture = _texture;
          material.mainTextureScale = new Vector2(1, -1);
        }
      }
    }

using EZ_Robot_Unity_DLL;
using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using UnityEngine.Networking;
using Debug = UnityEngine.Debug;
namespace BioIK
{
    public class ServoDefinitionsAndSend : MonoBehaviour
    {
        public int positionAbs;
        ServoServerClient _streamClient;
        bool _initialized = false;
        Texture2D _texture;
        volatile byte[] _toDisplay = new byte[] { };
        private static readonly ServoDefinition[] ServoDefinitions = new ServoDefinition[]
        {
            new ServoDefinition("JD_Rig_v01:Right_Rotate_Shoulder", AxisType.Z, ServoPortEnum.D0, false, 1, 180, 90),
            //new ServoDefinition("JD_Rig_v01:Right_Upper_Arm", AxisType.Y, ServoPortEnum.D1, false, 1, 180, 90),
            //new ServoDefinition("JD_Rig_v01:Right_Lower_arm", AxisType.Y, ServoPortEnum.D2, false, 1, 180, 90),
        };
        private class ServoDefinition
        {
            public readonly string JointName;
            public readonly AxisType Axis;
            public readonly ServoPortEnum Port;
            public readonly bool inverted;
            public readonly int minValue;
            public readonly int maxValue;
            public readonly int middleValue;
            public ServoDefinition(string jointName, AxisType axis, ServoPortEnum port, bool inverted = false, int minValue = 1, int maxValue = 180, int middleValue = 90)
            {
                this.JointName = jointName;
                this.Axis = axis;
                this.Port = port;
                this.inverted = inverted;
                this.minValue = minValue;
                this.maxValue = maxValue;
                this.middleValue = middleValue;
            }
            private int ClampValue(int degrees)
            {
                return degrees < this.minValue ? this.minValue : degrees > this.maxValue ? this.maxValue : degrees;
            }
            public int AdjustValue(int degrees)
            {
                var servoValue = FixRotation(degrees, this.inverted, this.middleValue);
                var clampedValue = this.ClampValue(servoValue);
                return clampedValue;
            }
        }
        private static int FixRotation(int deg, bool inverted, int middleAngle = 90)
        {
            return !inverted ? middleAngle + deg : middleAngle * 2 - (middleAngle + deg);
        }
        public enum AxisType
        {
            X = 0,
            Y = 1,
            Z = 2
        }
        public enum ServoPortEnum
        {
            D0 = 0,
            D1 = 1,
            D2 = 2
        }
        void Start()
        {
            _texture = new Texture2D(640, 480, TextureFormat.RGB24, false);
            _streamClient = new ServoServerClient();
            _streamClient.OnImageDataReady += _streamClient_OnImageDataReady;
            _streamClient.Start("127.0.0.1", 8282);
        }
        private void _streamClient_OnImageDataReady(byte[] imageData)
        {
            if (!_initialized)
                return;
            _toDisplay = imageData;
        }
        void OnDisable()
        {
            _streamClient.Stop();
        }
        private void Update()
        {
            _initialized = true;
            int ServoNumber = ServoDefinitions.Length;
            for (int i = 0; i < ServoNumber; i++)
            {
                string Name = (ServoDefinitions[i].JointName);
                var ServoJoint = GameObject.Find(Name);
                BioJoint joint = ServoJoint.GetComponent<BioJoint>();
                print(ServoJoint);
                bool invert = (ServoDefinitions[i].inverted);
                var PortNumber = (ServoDefinitions[i].Port);
                print(PortNumber);
                var UpVector = ((int)ServoDefinitions[i].Axis);
                if (UpVector == 0)
                {
                    print("X");
                    var Value = Mathf.RoundToInt((float)joint.X.GetTargetValue());
                    if (invert)
                    { positionAbs = 180 - (Mathf.Abs(Value - 90)); }
                    else
                    { positionAbs = Mathf.Abs(Value - 90); }
                    //print(positionAbs);
                    _streamClient.SetServoPositionAndSend(ServoServerClient.ServoPortEnum.D0, (byte)positionAbs);
                };
                if (UpVector == 1)
                {
                    print("Y");
                    var Value = Mathf.RoundToInt((float)joint.Y.GetTargetValue());
                    if (invert)
                    { positionAbs = 180 - (Mathf.Abs(Value - 90)); }
                    else
                    { positionAbs = Mathf.Abs(Value - 90); }
                    //print(positionAbs);
                    _streamClient.SetServoPositionAndSend(ServoServerClient.ServoPortEnum.D0, (byte)positionAbs);
                };
                if (UpVector == 2)
                {
                    print("Z");
                    var Value = Mathf.RoundToInt((float)joint.Z.GetTargetValue());
                    if (invert)
                    { positionAbs = 180 - (Mathf.Abs(Value - 90)); }
                    else
                    { positionAbs = Mathf.Abs(Value - 90); }
                    //print(positionAbs);
                    _streamClient.SetServoPositionAndSend(ServoServerClient.ServoPortEnum.D0, (byte)positionAbs);
                };
            }
            if (_streamClient.HasServoChanged)
                _streamClient.SendCachedServoPositions();
            if (_toDisplay.Length > 0)
            {
                _texture.LoadImage(_toDisplay);
                var material = GetComponent<Renderer>().material;
                material.mainTexture = _texture;
                material.mainTextureScale = new Vector2(1, -1);
            }
        }
    }
}

using EZ_Robot_Unity_DLL;
using UnityEngine;

public class ServoCamera : MonoBehaviour {

  ServoServerClient _streamClient;
  bool _initialized = false;
  Texture2D _texture;

  volatile byte [] _toDisplay = new byte[]{ };

  Transform _cube;
  Transform _sprite;

  /// 
  /// We have this script added to a parent that has children. 
  /// Because we can access the children's transformation or position from here to set servo positions
  /// In the update, we'll just grab the children and use thier data to send to ARC
  /// 
  void Start() {

    // This is the texture that will hold the camera image from ARC
    // We apply this texture to a cube
    _texture = new Texture2D(640, 480, TextureFormat.RGB24, false);

    // assign a local variable to the children so we don't have to search for them on each frame (makes cpu happy)
    _cube = gameObject.transform.Find("Cube");
    _sprite = gameObject.transform.Find("MoveableSprite");

    //Create a client that will connect to ARC at the specified IP address
    // Once connected, any available video data from the ARC camera will raise the OnImageDataReady event
    _streamClient = new ServoServerClient();
    _streamClient.OnImageDataReady += _streamClient_OnImageDataReady;
    _streamClient.Start("127.0.0.1", 8282);
  }

  /// 
  /// This event is raised for every camera image that is received from the connected ARC server.
  /// We assign the image data to a volatile array that will be used in the update the texture with the latest image
  /// 
  private void _streamClient_OnImageDataReady(byte[] imageData) {

    if (!_initialized)
      return;

    _toDisplay = imageData;
  }

  void OnDisable() {

    _streamClient.Stop();
  }

  /// 
  /// Unity runs this on every render frame
  /// We get the keyboard input to move the camera around
  /// And we map the cube's X and Y rotation values to D0 and D1 servo positions in ARC, respectively
  /// 
  void Update() {

    _initialized = true;

    if (Input.GetKey(KeyCode.Escape))
      Application.Quit();

    // add the position of the servos to the cache based on the location of the sprice
    // We set the positions to cache in this loop rather than trying to send a position each time
    // That way we can send a bulk change which is much faster on bandwidth
    // So, add your servo positions to the cache and then send them all after
    _streamClient.SetCachedServoPosition(ServoServerClient.ServoPortEnum.D0, _streamClient.MapToByte(_sprite.transform.position.x));
    _streamClient.SetCachedServoPosition(ServoServerClient.ServoPortEnum.D1, _streamClient.MapToByte(_sprite.transform.position.y));

    // Send all the servo positions that have been cached
    _streamClient.SendCachedServoPositions();

    // Display the latest camera image by rendering it to the texture and applying to the cube's material
    if (_toDisplay.Length > 0) {

      _texture.LoadImage(_toDisplay);

      var material = _cube.GetComponent().material;
      material.mainTexture = _texture;
      material.mainTextureScale = new Vector2(1, -1);
    }
  }
}