// Publisher.cs
// copyright 2022- https://robot-creation-diary.com/
using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using Unity.Robotics.ROSTCPConnector;
using OculusMsgMsg = RosMessageTypes.CppPubsub.OculusMsgMsg;
using UnityEngine.XR;
using Node = UnityEngine.XR.XRNode;
public class Publisher : MonoBehaviour
{
ROSConnection ros;
public string topicName = "oculus_topic";
public float publishMessageFrequency = 0.015f;
public float[] head_euler;
public float[] head_position;
public float[] hmd_position;
public float[] l_controller_rotation;
public float[] r_controller_rotation;
public float[] r_controller_position;
public float[] l_controller_position;
public sbyte a_button;
public sbyte b_button;
public sbyte x_button;
public sbyte y_button;
public float l_triger;
public float r_triger;
public float[] l_stick;
public float[] r_stick;
private float timeElapsed;
// Start is called before the first frame update
void Start()
{
this.head_euler = new float[3];
this.head_position = new float[3];
this.hmd_position = new float[3];
this.l_controller_rotation = new float[4];
this.r_controller_rotation = new float[4];
this.r_controller_position = new float[3];
this.l_controller_position = new float[3];
this.a_button = 0;
this.b_button = 0;
this.x_button = 0;
this.y_button = 0;
this.l_triger = 0.0f;
this.r_triger = 0.0f;
this.l_stick = new float[2];
this.r_stick = new float[2];
ros = ROSConnection.GetOrCreateInstance();
ros.RegisterPublisher<OculusMsgMsg>(topicName);
}
// Update is called once per frame
void Update()
{
timeElapsed += Time.deltaTime;
if (timeElapsed > publishMessageFrequency)
{
List<XRNodeState> DevStat = new List<XRNodeState>();
InputTracking.GetNodeStates(DevStat);
Quaternion hmdRotation;
Quaternion lControllerRotation;
Quaternion rControllerRotation;
Vector3 hmdPosition;
Vector3 rControllerPosition;
Vector3 lControllerPosition;
sbyte aButton = 0;
sbyte bButton = 0;
sbyte xButton = 0;
sbyte yButton = 0;
float lTriger;
float rTriger;
Vector2 lStick;
Vector2 rStick;
foreach (XRNodeState s in DevStat)
{
if (s.nodeType == XRNode.Head)
{
s.TryGetRotation(out hmdRotation);
this.SetHeadEuler(hmdRotation.eulerAngles);
s.TryGetPosition(out hmdPosition);
this.SetHmdPosition(hmdPosition);
GameObject head = GameObject.Find("HeadPosition");
this.SetHeadPosition(head.transform.position);
}
if (s.nodeType == XRNode.LeftHand)
{
s.TryGetRotation(out lControllerRotation);
this.SetLControllerRotation(lControllerRotation);
s.TryGetPosition(out lControllerPosition);
this.SetLControllerPosition(lControllerPosition);
}
if (s.nodeType == XRNode.RightHand)
{
s.TryGetRotation(out rControllerRotation);
this.SetRControllerRotation(rControllerRotation);
var xa = new Vector3(1, 0, 0);
xa = rControllerRotation * xa;
var ya = new Vector3(0, 1, 0);
ya = rControllerRotation * ya;
var za = new Vector3(0, 0, 1);
za = rControllerRotation * za;
s.TryGetPosition(out rControllerPosition);
this.SetRControllerPosition(rControllerPosition);
}
}
if (OVRInput.GetDown(OVRInput.Button.One,
OVRInput.Controller.RTouch) || OVRInput.Get(OVRInput.Button.One,
OVRInput.Controller.RTouch))
{
aButton = 1;
}
else {
aButton = 0;
}
this.SetAButton(aButton);
if (OVRInput.GetDown(OVRInput.Button.Two,
OVRInput.Controller.RTouch) || OVRInput.Get(OVRInput.Button.Two,
OVRInput.Controller.RTouch))
{
bButton = 1;
}
else
{
bButton = 0;
}
this.SetBButton(bButton);
if (OVRInput.GetDown(OVRInput.Button.One,
OVRInput.Controller.LTouch) || OVRInput.Get(OVRInput.Button.One,
OVRInput.Controller.LTouch))
{
xButton = 1;
}
else {
xButton = 0;
}
this.SetXButton(xButton);
if (OVRInput.GetDown(OVRInput.Button.Two,
OVRInput.Controller.LTouch) || OVRInput.Get(OVRInput.Button.Two,
OVRInput.Controller.LTouch))
{
yButton = 1;
}
else
{
yButton = 0;
}
this.SetYButton(yButton);
lTriger = OVRInput.Get(OVRInput.Axis1D.PrimaryIndexTrigger, OVRInput.Controller.LTouch);
rTriger = OVRInput.Get(OVRInput.Axis1D.PrimaryIndexTrigger, OVRInput.Controller.RTouch);
this.SetLTriger(lTriger);
this.SetRTriger(rTriger);
lStick = OVRInput.Get(OVRInput.Axis2D.PrimaryThumbstick, OVRInput.Controller.LTouch);
rStick = OVRInput.Get(OVRInput.Axis2D.PrimaryThumbstick, OVRInput.Controller.LTouch);
this.SetLStick(lStick);
this.SetRStick(rStick);
// Finally send the message to server_endpoint.py running in ROS
OculusMsgMsg outData = new OculusMsgMsg(this.head_euler,
this.head_position,
this.hmd_position,
this.l_controller_rotation,
this.r_controller_rotation,
this.l_controller_position,
this.r_controller_position,
this.a_button,
this.b_button,
this.x_button,
this.y_button,
this.l_triger,
this.r_triger,
this.l_stick,
this.r_stick
);
ros.Publish(topicName, outData);
timeElapsed = 0;
}
}
public void SetHeadEuler(Vector3 head_euler)
{
this.head_euler[0] = head_euler.x;
this.head_euler[1] = head_euler.y;
this.head_euler[2] = head_euler.z;
}
public void SetHeadPosition(Vector3 head_position)
{
this.head_position[0] = head_position.x;
this.head_position[1] = head_position.y;
this.head_position[2] = head_position.z;
}
public void SetHmdPosition(Vector3 head_position)
{
this.hmd_position[0] = head_position.x;
this.hmd_position[1] = head_position.y;
this.hmd_position[2] = head_position.z;
}
public void SetLControllerRotation(Quaternion l_controller_rotation)
{
this.l_controller_rotation[0] = l_controller_rotation.w;
this.l_controller_rotation[1] = l_controller_rotation.x;
this.l_controller_rotation[2] = l_controller_rotation.y;
this.l_controller_rotation[3] = l_controller_rotation.z;
}
public void SetRControllerRotation(Quaternion r_controller_rotation)
{
this.r_controller_rotation[0] = r_controller_rotation.w;
this.r_controller_rotation[1] = r_controller_rotation.x;
this.r_controller_rotation[2] = r_controller_rotation.y;
this.r_controller_rotation[3] = r_controller_rotation.z;
}
public void SetRControllerPosition(Vector3 r_controller_position)
{
this.r_controller_position[0] = r_controller_position.x;
this.r_controller_position[1] = r_controller_position.y;
this.r_controller_position[2] = r_controller_position.z;
}
public void SetLControllerPosition(Vector3 l_controller_position)
{
this.l_controller_position[0] = l_controller_position.x;
this.l_controller_position[1] = l_controller_position.y;
this.l_controller_position[2] = l_controller_position.z;
}
public void SetAButton(sbyte a_button)
{
this.a_button = a_button;
}
public void SetBButton(sbyte b_button)
{
this.b_button = b_button;
}
public void SetXButton(sbyte x_button)
{
this.x_button = x_button;
}
public void SetYButton(sbyte y_button)
{
this.y_button = y_button;
}
public void SetLTriger(float l_triger)
{
this.l_triger = l_triger;
}
public void SetRTriger(float r_triger)
{
this.r_triger = r_triger;
}
public void SetLStick(Vector2 l_stick)
{
this.l_stick[0] = l_stick.x;
this.l_stick[1] = l_stick.y;
}
public void SetRStick(Vector2 r_stick)
{
this.r_stick[0] = r_stick.x;
this.r_stick[1] = r_stick.y;
}
}
