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