private Vector3 GetPose(Messages.Geometry.PoseStamped message)
{
return(new Vector3(
message.pose.position.x,
message.pose.position.y,
message.pose.position.z));
}
public void PublishPoseIIWA()
{
GameObject worldFrame = GameObject.Find("IIWA");
GameObject goalFrame = GameObject.Find("BallFigure");
pose_msg pose = CreatePoseMessageIIWA(worldFrame.transform, goalFrame.transform);
rosSocket.Publish(publicationIdIIWA, pose);
}
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);
}
private void SendPose(TimeSpan currentTime, Quaternion currentRotation, Vector3 currentPosition)
{
int[] structuredTime = Timer.GetSecondsNanosecondsStructure(currentTime);
RosSharp.RosBridgeClient.Messages.Geometry.PoseStamped message = new RosSharp.RosBridgeClient.Messages.Geometry.PoseStamped();
message.header.frame_id = Config.HololensWorldFrame;
message.header.seq = frameIdx++;
message.header.stamp.secs = structuredTime[0];
message.header.stamp.nsecs = structuredTime[1];
CoordinateTransformations.ConvertPoseUnity2ROS(ref currentPosition, ref currentRotation);
message.pose.orientation.x = currentRotation.x;
message.pose.orientation.y = currentRotation.y;
message.pose.orientation.z = currentRotation.z;
message.pose.orientation.w = currentRotation.w;
message.pose.position.x = currentPosition.x;
message.pose.position.y = currentPosition.y;
message.pose.position.z = currentPosition.z;
publisher.Publish(message);
System.Diagnostics.Debug.WriteLine("POSE at time: " + ((double)structuredTime[0] + ((double)structuredTime[1]) / 1e9).ToString() +
" --- (" + currentPosition.x + ", " + currentPosition.y + ", " + currentPosition.z + ")");
}
