public static void Ros2Unity(ref UnityQuaternion dataOut, RosQuaternion dataIn)
{
dataOut.x = dataIn.y;
dataOut.y = -dataIn.z;
dataOut.z = -dataIn.x;
dataOut.w = dataIn.w;
}
pose_m CreatePoseMessage(Transform base_T, Transform goal_T)
{
pose_m pose = new pose_m();
Vector3 pos = goal_T.position - base_T.position;
Quaternion ori = Quaternion.Inverse(base_T.rotation) * goal_T.rotation;
ori = TransformExtensions.Unity2Ros(ori);
pos = TransformExtensions.Unity2Ros(pos);
Point_msg point = new Point_msg
{
x = -pos.y,
y = pos.x,
z = pos.z
};
float d = Mathf.Sqrt(ori.x * ori.x + ori.y * ori.y + ori.z * ori.z + ori.w * ori.w);
Quaternion_msg quaternion = new Quaternion_msg
{
x = ori.x / d,
y = ori.y / d,
z = ori.z / d,
w = ori.w / d
};
pose.position = point;
pose.orientation = quaternion;
return(pose);
}
/// <summary>
/// Publishes some mock data for testing purposes and updates the hand of roboy.
/// </summary>
/// <returns>The publisher.</returns>
/// <param name="waitTime">Wait time.</param>
private IEnumerator StartPublisher(float waitTime)
{
while (true)
{
yield return(new WaitForSeconds(waitTime));
base.Start();
// TEST MESSAGE FOR RIGHT HAND UPDATE
RosSharp.RosBridgeClient.Messages.Roboy.Pose message = new RosSharp.RosBridgeClient.Messages.Roboy.Pose();
message.id = 2;
// Conversion between ROS Quaternion and Unity Quaternion
RosSharp.RosBridgeClient.Messages.Geometry.Quaternion or = new RosSharp.RosBridgeClient.Messages.Geometry.Quaternion();
Quaternion currentOr = Quaternion.Euler((float)-3.353, (float)-148.248, (float)-12.106);
or.x = currentOr.x;
or.y = currentOr.y;
or.z = currentOr.z;
or.w = currentOr.w;
message.orientation = or;
// Conversion between ROS Point and Unity Vector3
RosSharp.RosBridgeClient.Messages.Geometry.Point po = new RosSharp.RosBridgeClient.Messages.Geometry.Point();
po.x = (float)-0.514;
po.y = (float)-0.05;
po.z = (float)-0.552;
message.position = po;
// Update position after a couple of seconds
yield return(new WaitForSeconds(waitTime));
PublishMessage(message);
break;
}
}
public static void Unity2Ros(RosQuaternion dataOut, ref UnityQuaternion dataIn)
{
dataOut.x = -dataIn.z;
dataOut.y = dataIn.x;
dataOut.z = -dataIn.y;
dataOut.w = dataIn.w;
}
private Messages.Geometry.Quaternion GetGeometryQuaternion(Quaternion quaternion)
{
Messages.Geometry.Quaternion geometryQuaternion = new Messages.Geometry.Quaternion();
geometryQuaternion.x = quaternion.x;
geometryQuaternion.y = quaternion.y;
geometryQuaternion.z = quaternion.z;
geometryQuaternion.w = quaternion.w;
return(geometryQuaternion);
}
private Messages.Geometry.Quaternion GetGeometryQuaternion(Quaternion quaternion)
{
Messages.Geometry.Quaternion geometryQuaternion = new Messages.Geometry.Quaternion
{
x = quaternion.x,
y = quaternion.y,
z = quaternion.z,
w = quaternion.w
};
return(geometryQuaternion);
}
pose_msg CreatePoseMessageIIWA(Transform base_T, Transform goal_T)
{
pose_msg pose = new pose_msg();
Vector3 pos = goal_T.position - base_T.position;
Quaternion ori = Quaternion.Inverse(base_T.rotation) * goal_T.rotation;
ori = TransformExtensions.Unity2Ros(ori);
pos = TransformExtensions.Unity2Ros(pos);
Point_msg point = new Point_msg
{
x = -pos.x,
y = -pos.y,
z = pos.z
};
float d = Mathf.Sqrt(ori.x * ori.x + ori.y * ori.y + ori.z * ori.z + ori.w * ori.w);
Quaternion_msg quaternion = new Quaternion_msg
{
x = ori.x / d,
y = ori.y / d,
z = ori.z / d,
w = ori.w / d
};
//DateTime.UtcNow.Millisecond;
HeaderExtensions.Update(pose.header);
pose.header.stamp.secs = DateTime.UtcNow.Second;;
pose.header.stamp.nsecs = DateTime.UtcNow.Millisecond;
pose.header.frame_id = "map";
pose.pose.position = point;
pose.pose.orientation = quaternion;
return(pose);
}
public override void update()
{
RosSharp.RosBridgeClient.Messages.NavSatFix positionMes = positionSubs.position;
rotation = compassSubs.orientation;
imuMes = imuSubs.imuData;
if (imuMes != null && positionMes != null) //tady by to chtelo spis overovat,zda je pripojeno
{
RosSharp.RosBridgeClient.Messages.Geometry.Quaternion imuQuat = imuMes.orientation;
//pitchRoll = RosSharp.TransformExtensions.Ros2Unity(imuQuat); z nejakho duvodu nefunguje, tak vezmu primo kod metody
pitchRoll = new Quaternion(imuQuat.y, -imuQuat.z, -imuQuat.x, imuQuat.w);
if (positionMes.latitude == 0 && positionMes.longitude == 0 && positionMes.altitude == 0) // no GPS fix
{
//position = new Vector3(0, 0, 0);
Debug.Log("no GPS fix");
}
else
{
Debug.Log(offset);
position = Map.GeoToWorldPosition(new Mapbox.Utils.Vector2d(positionMes.latitude, positionMes.longitude), false);
//set new AltitudeOffset
if (Input.GetKeyUp("o") || offset)
{
float altitude = (float)positionMes.altitude;
PlayerPrefs.SetFloat("AltitudeOffset", (getGroundAltitude() - altitude));
offset = false;
}
float altitudeOffset = PlayerPrefs.GetFloat("AltitudeOffset");
// position.y = (float)positionMes.altitude + altitudeOffset; //doplnim vysku
position.y = (float)positionMes.altitude + altitudeOffset;
}
// Debug.Log("DroneRosData: lat: " + positionMes.latitude + ", lon: " + positionMes.longitude + ", alt: "+ positionMes.altitude + ", compass: "******", UMU eulerAngles: " + pitchRoll.eulerAngles);
}
}
/// <summary>
/// Gets the init parameters from the XML File.
/// </summary>
/// <param name="poseCode">Pose code.</param>
public void GetInitParameters(int poseCode)
{
XmlDocument xmlDoc = new XmlDocument();
xmlDoc.LoadXml(XML_FILE_WAVE1.text);
switch (poseCode)
{
case 0:
xmlDoc.LoadXml(XML_FILE.text);
break;
case 1:
xmlDoc.LoadXml(XML_FILE_WAVE1.text);
break;
case 2:
xmlDoc.LoadXml(XML_FILE_WAVE2.text);
break;
default:
break;
}
foreach (Transform t in Roboy)
{
if (t != null & t.CompareTag("RoboyPart"))
{
XmlNode node = xmlDoc.SelectSingleNode("/sdf/model/link[@name='" + t.name + "']/pose");
string[] poseString = node.InnerText.Split(null);
float x = float.Parse(poseString[0]);
float y = float.Parse(poseString[1]);
float z = float.Parse(poseString[2]);
float alpha = float.Parse(poseString[3]);
float beta = float.Parse(poseString[4]);
float gamma = float.Parse(poseString[5]);
Vector3 pos = new Vector3(x, y, z);
Quaternion q = Quaternion.Euler(new Vector3(alpha, beta, gamma));
// Conversion between ROS Quaternions/Point and Unity Quaternion/Point
RosSharp.RosBridgeClient.Messages.Geometry.Quaternion or = new RosSharp.RosBridgeClient.Messages.Geometry.Quaternion();
or.x = q.x;
or.y = q.y;
or.z = q.z;
or.w = q.w;
RosSharp.RosBridgeClient.Messages.Geometry.Point po = new RosSharp.RosBridgeClient.Messages.Geometry.Point();
po.x = pos.x;
po.y = pos.y;
po.z = pos.z;
//Create Message
RosSharp.RosBridgeClient.Messages.Roboy.Pose message = new RosSharp.RosBridgeClient.Messages.Roboy.Pose();
message.orientation = or;
message.position = po;
switch (t.name)
{
// Mapping of IDs to parts is fixed here but is "random". This mapping is used for encoding later in the Pose Manager.
case "upper_arm_right":
message.id = 0;
break;
case "forearm_right":
message.id = 1;
break;
case "hand_right":
message.id = 2;
break;
case "elbow_right":
message.id = 3;
break;
case "upper_arm_left":
message.id = 4;
break;
case "forearm_left":
message.id = 5;
break;
case "hand_left":
message.id = 6;
break;
case "elbow_left":
message.id = 7;
break;
case "head":
message.id = 8;
break;
default:
continue;
}
gameObject.GetComponent <MockPosePublisher>().PublishMessage(message);
}
}
}
public static void QuaternionToQuaternion(ref UnityQuaternion dataOut, RosQuaternion dataIn)
{
Ros2Unity(ref dataOut, dataIn);
}
public static void QuaternionToQuaternion(RosQuaternion dataOut, ref UnityQuaternion dataIn)
{
Unity2Ros(dataOut, ref dataIn);
}
