private void Update()
{
string linkName = (m_IsLeft) ? "left_hand" : "right_hand";
List <string> linkNames = new List <string>();
linkNames.Add(linkName);
var xDic = new Dictionary <string, float>();
var yDic = new Dictionary <string, float>();
var zDic = new Dictionary <string, float>();
var qxDic = new Dictionary <string, float>();
var qyDic = new Dictionary <string, float>();
var qzDic = new Dictionary <string, float>();
var qwDic = new Dictionary <string, float>();
Vector3 gazeboPosition = GazeboUtility.UnityPositionToGazebo(transform.position);
Quaternion gazeboRotation = GazeboUtility.UnityRotationToGazebo(transform.rotation);
//Vector3 gazeboPosition = transform.position;
//Quaternion gazeboRotation = transform.rotation;
xDic.Add(linkName, gazeboPosition.x);
yDic.Add(linkName, gazeboPosition.y);
zDic.Add(linkName, gazeboPosition.z);
qxDic.Add(linkName, gazeboRotation.x);
qyDic.Add(linkName, gazeboRotation.y);
qzDic.Add(linkName, gazeboRotation.z);
qwDic.Add(linkName, gazeboRotation.w);
ROSBridgeLib.custom_msgs.RoboyPoseMsg msg = new ROSBridgeLib.custom_msgs.RoboyPoseMsg("hands", linkNames, xDic, yDic, zDic, qxDic, qyDic, qzDic, qwDic);
ROSBridge.Instance.Publish(RoboyHandsPublisher.GetMessageTopic(), msg);
}
/// <summary>
/// Turn Roboy with the movement of the HMD.
/// </summary>
private void translateRoboy()
{
// References to roboy parts we need for rotation/ translation
Transform head_parent = transform.GetChild(0).Find("head");
Transform head_pivot = head_parent.GetChild(0);
Transform torso_pivot = transform.GetChild(0).Find("torso_pivot");
// Check whether the user has rotated the headset in x direction or not
if (m_CurrentAngleX != m_Cam.transform.eulerAngles.x)
{
// If the headset was rotated, rotate roboy
head_parent.RotateAround(head_pivot.position, Vector3.right, m_Cam.transform.eulerAngles.x - m_CurrentAngleX);
}
m_CurrentAngleX = m_Cam.transform.eulerAngles.x;
// Check whether the user has rotated the headset in y direction or not
if (m_CurrentAngleY != m_Cam.transform.eulerAngles.y)
{
// If the headset was rotated, rotate roboy
head_parent.RotateAround(head_pivot.position, Vector3.up, m_Cam.transform.eulerAngles.y - m_CurrentAngleY);
}
m_CurrentAngleY = m_Cam.transform.eulerAngles.y;
// Move roboy accordingly to headset movement
Quaternion headRotation = InputTracking.GetLocalRotation(VRNode.Head);
transform.position = m_Cam.transform.position + (headRotation * Vector3.forward) * (-0.3f);
// Publish position to gazebo
ReceivePosition(GazeboUtility.UnityPositionToGazebo(transform.position));
// The torso of roboy will be rotated towards the (right)controller position
torso_pivot.transform.LookAt(new Vector3(m_Controller.transform.position.x, torso_pivot.transform.position.y, m_Controller.transform.position.z));
// Send rotation data via ROS
// Convert the headset rotation from unity coordinate spaze to gazebo coordinates
Quaternion rot = GazeboUtility.UnityRotationToGazebo(headRotation);
float x_angle = 0.0f;
float y_angle = 0.0f;
// Angle for torso rotation
float t_angle = 0.0f;
// Convert head rotation
if (rot.eulerAngles.x > 180)
{
y_angle = (rot.eulerAngles.x - 360) * Mathf.Deg2Rad;
}
else
{
y_angle = rot.eulerAngles.x * Mathf.Deg2Rad;
}
x_angle = rot.eulerAngles.z * Mathf.Deg2Rad;
// Now convert torso rotation
if (torso_pivot.eulerAngles.y > 180)
{
t_angle = (torso_pivot.eulerAngles.y - 360) * Mathf.Deg2Rad;
}
else
{
t_angle = torso_pivot.eulerAngles.y * Mathf.Deg2Rad;
}
// Determine which joints should me modified
List <string> joints = new List <string>();
// X rotation
joints.Add("neck_3");
// Y rotation
joints.Add("neck_4");
// Body rotation
joints.Add("spine_1");
// Determine the angle for the joints
List <float> angles = new List <float>();
// Add the x-angle of the headset after conversion from unity to ros
angles.Add(x_angle);
// Add the y-angle of the headset after conversion from unity to ros
angles.Add(y_angle);
// Add the torso rotation angle after conversion from unity to ros
angles.Add(t_angle * (-1.0f));
// Start sending the actual message
ReceiveExternalJoint(joints, angles);
}