private void SendRobotTargetPose(TimeSpan currentTime, Quaternion currentRotation, Vector3 currentPosition)
{
int[] structuredTime = Timer.GetSecondsNanosecondsStructure(currentTime);
PoseStamped message = new PoseStamped();
message.header.frame_id = Config.HololensWorldFrame;
message.header.seq = robotTargetPoseFrameIdx++;
message.header.stamp.secs = structuredTime[0];
message.header.stamp.nsecs = structuredTime[1];
CoordinateTransformations.ConvertPoseUnity2ROS(ref currentPosition, ref currentRotation);
message.pose.orientation.x = Quaternion.identity.x;
message.pose.orientation.y = Quaternion.identity.y;
message.pose.orientation.z = Quaternion.identity.z;
message.pose.orientation.w = Quaternion.identity.w;
message.pose.position.x = currentPosition.x;
message.pose.position.y = currentPosition.y;
message.pose.position.z = currentPosition.z;
robotTargetPosePublisher.Publish(message);
System.Diagnostics.Debug.WriteLine("ROBOT TARGET POSE at time: " + ((double)structuredTime[0] + ((double)structuredTime[1]) / 1e9).ToString() +
" --- (" + currentPosition.x + ", " + currentPosition.y + ", " + currentPosition.z + ")");
}
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 + ")");
}
