public void OnPickUp() { Vector3 invPos = objectSelectionManager.CurrentSelectedObject.transform.InverseTransformPoint(cursor.transform.position); previousPlacePos = objectSelectionManager.CurrentSelectedObject.transform.position; lastPlacePos = objectSelectionManager.CurrentSelectedObject.transform.position; //object1, object2, object3 //this.transform.position = new Vector3(objectSelectionManager.CurrentSelectedObject.transform.position.x, cursor.transform.position.y, objectSelectionManager.CurrentSelectedObject.transform.position.z); Vector3 cursor_pos = cursor.transform.position; targetMarker.position = cursor_pos; //Vector3 target_pos = objectSelectionManager.CurrentSelectedObject.transform.position; //Vector3 invPos = objectSelectionManager.CurrentSelectedObject.transform.InverseTransformPoint(cursor.transform.position); //Vector3 invPos = cursor_pos - target_pos; //float[] data = { invPos.x, -invPos.z, invPos.y }; RosMessages_old.geometry_msgs.PointStamped_old ps = new RosMessages_old.geometry_msgs.PointStamped_old(); RosMessages_old.geometry_msgs.Point_old p = new RosMessages_old.geometry_msgs.Point_old(); ps.point = p; ps.point.x = invPos.z; ps.point.y = -invPos.x; ps.point.z = invPos.y; Header_old h = new Header_old(); ps.header = h; ps.header.frame_id = objectSelectionManager.CurrentSelectedObject.transform.GetChild(0).name; objectWithTargetMarker = objectSelectionManager.CurrentSelectedObject; rosManager.RosBridge.EnqueRosCommand(new RosPublish_old("/hololens/plan_pick", ps)); }
public void OnPlace() { Vector3 invPos = tableTop.transform.InverseTransformPoint(cursor.transform.position); //targetMarker.position = cursor.transform.position; // TODO this.previousPlacePos = this.lastPlacePos; this.lastPlacePos = cursor.transform.position; lastPlacePos.y += 0.129f; objectWithTargetMarker.transform.position = lastPlacePos; //Vector3 invPos = cursor.transform.position - tableTop.position; RosMessages_old.geometry_msgs.PointStamped_old ps = new RosMessages_old.geometry_msgs.PointStamped_old(); RosMessages_old.geometry_msgs.Point_old p = new RosMessages_old.geometry_msgs.Point_old(); ps.point = p; ps.point.x = invPos.z; ps.point.y = -invPos.x; //TODO check the order of the coordinates!!! ps.point.z = invPos.y; Header_old h = new Header_old(); ps.header = h; rosManager.RosBridge.EnqueRosCommand(new RosPublish_old("/hololens/plan_place", ps)); }