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;
}
}
private Messages.Geometry.Point GetGeometryPoint(Vector3 position)
{
Messages.Geometry.Point geometryPoint = new Messages.Geometry.Point();
geometryPoint.x = position.x;
geometryPoint.y = position.y;
geometryPoint.z = position.z;
return(geometryPoint);
}
private Messages.Geometry.Point GetGeometryPoint(Vector3 position)
{
Messages.Geometry.Point geometryPoint = new Messages.Geometry.Point();
geometryPoint.x = hitNormal.x;
geometryPoint.y = hitNormal.y;
geometryPoint.z = hitNormal.z;
return(geometryPoint);
}
private Messages.Geometry.Point GetGeometryPoint()
{
Messages.Geometry.Point geometryPoint = new Messages.Geometry.Point();
geometryPoint.x = CollisionPoint2.hitPos2.z;
geometryPoint.y = -CollisionPoint2.hitPos2.x;
geometryPoint.z = CollisionPoint2.hitPos2.y;
return(geometryPoint);
}
private Messages.Geometry.Point GetGeometryPoint(Vector3 position)
{
Messages.Geometry.Point geometryPoint = new Messages.Geometry.Point
{
x = position.x,
y = position.y,
z = position.z
};
return(geometryPoint);
}
private void CreatePedestrians()
{
for (int i = 0; i < numPedestrians; i++)
{
tempPedestrian = Instantiate(pedestrianPrefab);
tempPos = poseArray.poses[i].position; // position returned by car, relative to its coordinate system (should be the same as our origin)
tempPedestrian.transform.position = new Vector3(tempPos.x, 0, tempPos.y); // localPosition should set position relative to parent
pedestrians.Add(tempPedestrian);
}
Debug.Log(numPedestrians + " pedestrians created!");
}
private void UpdatePedestrians()
{
if (pedestrians != null && pedestrians.Count > 0)
{
for (int i = 0; i < numPedestrians; i++)
{
tempPos = poseArray.poses[i].position; // In Unity y vector is pointing up, while in ROS, z is up.
pedestrians[i].transform.position = new Vector3(originCubePos.x + tempPos.x, originCubePos.y, originCubePos.z + tempPos.y); // might need position interpolation in frames which do not have a new pose. This would mean switching to a velocity model instead.
pedestrians[i].transform.localRotation = new Quaternion(0, poseArray.poses[i].orientation.z, 0, poseArray.poses[i].orientation.w); // In Unity z axis is switched with y and needs to be made negative
}
}
else
{
// To fix this from happening, you must queue every event in the HandlePosesUpdated function to the main queue so poseStatus doesn't get changed midway through Update(), this is a ductape fix
CreatePedestrians(); // Unfortunately Unity is not Thread Safe
}
}
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);
}
protected Vector3 ToVector3(Messages.Geometry.Point p)
{
return(new Vector3(p.x, p.y, p.z));
}
/// <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);
}
}
}
