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);
}
public void GetWaypoints()
{
numOfWaypoints = waypointObjects.Count;
Initialize();
for (int i = 0; i < numOfWaypoints; i++)
{
UnityEngine.Vector3 temp_pose = (UnityEngine.Quaternion.Inverse(refGameObject.transform.rotation) * (waypointObjects[i].transform.position - refGameObject.transform.position)).Unity2Ros();
UnityEngine.Quaternion temp_q = UnityEngine.Quaternion.Inverse(refGameObject.transform.rotation) * waypointObjects[i].transform.rotation;
temp_q *= UnityEngine.Quaternion.Euler(0.0f, 0.0f, 180.0f);
temp_q = temp_q.Unity2Ros();
waypoints[i] = new Messages.Geometry.Pose
{
position = new Point {
x = temp_pose.x, y = temp_pose.y, z = temp_pose.z
},
orientation = new Messages.Geometry.Quaternion {
x = temp_q.x, y = temp_q.y, z = temp_q.z, w = temp_q.w
}
};
}
}