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); }
/// <summary> /// Publishes some mock data for testing purposes and updates the hand of roboy. /// </summary> /// <returns>The publisher.</returns> /// <param name="waitTime">Wait time.</param> private IEnumerator StartPublisher(float waitTime) { while (true) { yield return(new WaitForSeconds(waitTime)); base.Start(); // TEST MESSAGE FOR RIGHT HAND UPDATE RosSharp.RosBridgeClient.Messages.Roboy.Pose message = new RosSharp.RosBridgeClient.Messages.Roboy.Pose(); message.id = 2; // Conversion between ROS Quaternion and Unity Quaternion RosSharp.RosBridgeClient.Messages.Geometry.Quaternion or = new RosSharp.RosBridgeClient.Messages.Geometry.Quaternion(); Quaternion currentOr = Quaternion.Euler((float)-3.353, (float)-148.248, (float)-12.106); or.x = currentOr.x; or.y = currentOr.y; or.z = currentOr.z; or.w = currentOr.w; message.orientation = or; // Conversion between ROS Point and Unity Vector3 RosSharp.RosBridgeClient.Messages.Geometry.Point po = new RosSharp.RosBridgeClient.Messages.Geometry.Point(); po.x = (float)-0.514; po.y = (float)-0.05; po.z = (float)-0.552; message.position = po; // Update position after a couple of seconds yield return(new WaitForSeconds(waitTime)); PublishMessage(message); break; } }
private Messages.Geometry.Point GetGeometryPoint(Vector3 position) { Messages.Geometry.Point geometryPoint = new Messages.Geometry.Point(); geometryPoint.x = position.x; geometryPoint.y = position.y; geometryPoint.z = position.z; return(geometryPoint); }
private Messages.Geometry.Point GetGeometryPoint(Vector3 position) { Messages.Geometry.Point geometryPoint = new Messages.Geometry.Point(); geometryPoint.x = hitNormal.x; geometryPoint.y = hitNormal.y; geometryPoint.z = hitNormal.z; return(geometryPoint); }
private Messages.Geometry.Point GetGeometryPoint() { Messages.Geometry.Point geometryPoint = new Messages.Geometry.Point(); geometryPoint.x = CollisionPoint2.hitPos2.z; geometryPoint.y = -CollisionPoint2.hitPos2.x; geometryPoint.z = CollisionPoint2.hitPos2.y; return(geometryPoint); }
private Messages.Geometry.Point GetGeometryPoint(Vector3 position) { Messages.Geometry.Point geometryPoint = new Messages.Geometry.Point { x = position.x, y = position.y, z = position.z }; return(geometryPoint); }
private void CreatePedestrians() { for (int i = 0; i < numPedestrians; i++) { tempPedestrian = Instantiate(pedestrianPrefab); tempPos = poseArray.poses[i].position; // position returned by car, relative to its coordinate system (should be the same as our origin) tempPedestrian.transform.position = new Vector3(tempPos.x, 0, tempPos.y); // localPosition should set position relative to parent pedestrians.Add(tempPedestrian); } Debug.Log(numPedestrians + " pedestrians created!"); }
private void UpdatePedestrians() { if (pedestrians != null && pedestrians.Count > 0) { for (int i = 0; i < numPedestrians; i++) { tempPos = poseArray.poses[i].position; // In Unity y vector is pointing up, while in ROS, z is up. pedestrians[i].transform.position = new Vector3(originCubePos.x + tempPos.x, originCubePos.y, originCubePos.z + tempPos.y); // might need position interpolation in frames which do not have a new pose. This would mean switching to a velocity model instead. pedestrians[i].transform.localRotation = new Quaternion(0, poseArray.poses[i].orientation.z, 0, poseArray.poses[i].orientation.w); // In Unity z axis is switched with y and needs to be made negative } } else { // To fix this from happening, you must queue every event in the HandlePosesUpdated function to the main queue so poseStatus doesn't get changed midway through Update(), this is a ductape fix CreatePedestrians(); // Unfortunately Unity is not Thread Safe } }
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); }
protected Vector3 ToVector3(Messages.Geometry.Point p) { return(new Vector3(p.x, p.y, p.z)); }
/// <summary> /// Gets the init parameters from the XML File. /// </summary> /// <param name="poseCode">Pose code.</param> public void GetInitParameters(int poseCode) { XmlDocument xmlDoc = new XmlDocument(); xmlDoc.LoadXml(XML_FILE_WAVE1.text); switch (poseCode) { case 0: xmlDoc.LoadXml(XML_FILE.text); break; case 1: xmlDoc.LoadXml(XML_FILE_WAVE1.text); break; case 2: xmlDoc.LoadXml(XML_FILE_WAVE2.text); break; default: break; } foreach (Transform t in Roboy) { if (t != null & t.CompareTag("RoboyPart")) { XmlNode node = xmlDoc.SelectSingleNode("/sdf/model/link[@name='" + t.name + "']/pose"); string[] poseString = node.InnerText.Split(null); float x = float.Parse(poseString[0]); float y = float.Parse(poseString[1]); float z = float.Parse(poseString[2]); float alpha = float.Parse(poseString[3]); float beta = float.Parse(poseString[4]); float gamma = float.Parse(poseString[5]); Vector3 pos = new Vector3(x, y, z); Quaternion q = Quaternion.Euler(new Vector3(alpha, beta, gamma)); // Conversion between ROS Quaternions/Point and Unity Quaternion/Point RosSharp.RosBridgeClient.Messages.Geometry.Quaternion or = new RosSharp.RosBridgeClient.Messages.Geometry.Quaternion(); or.x = q.x; or.y = q.y; or.z = q.z; or.w = q.w; RosSharp.RosBridgeClient.Messages.Geometry.Point po = new RosSharp.RosBridgeClient.Messages.Geometry.Point(); po.x = pos.x; po.y = pos.y; po.z = pos.z; //Create Message RosSharp.RosBridgeClient.Messages.Roboy.Pose message = new RosSharp.RosBridgeClient.Messages.Roboy.Pose(); message.orientation = or; message.position = po; switch (t.name) { // Mapping of IDs to parts is fixed here but is "random". This mapping is used for encoding later in the Pose Manager. case "upper_arm_right": message.id = 0; break; case "forearm_right": message.id = 1; break; case "hand_right": message.id = 2; break; case "elbow_right": message.id = 3; break; case "upper_arm_left": message.id = 4; break; case "forearm_left": message.id = 5; break; case "hand_left": message.id = 6; break; case "elbow_left": message.id = 7; break; case "head": message.id = 8; break; default: continue; } gameObject.GetComponent <MockPosePublisher>().PublishMessage(message); } } }