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 + ")"); }