public override void UpdateModel() { if (ActionObjectMetadata.ObjectModel == null) { return; } Vector3?dimensions = null; switch (ActionObjectMetadata.ObjectModel.Type) { case ObjectModel.TypeEnum.Box: dimensions = TransformConvertor.ROSToUnityScale(new Vector3((float)ActionObjectMetadata.ObjectModel.Box.SizeX, (float)ActionObjectMetadata.ObjectModel.Box.SizeY, (float)ActionObjectMetadata.ObjectModel.Box.SizeZ)); break; case ObjectModel.TypeEnum.Sphere: dimensions = TransformConvertor.ROSToUnityScale(new Vector3((float)ActionObjectMetadata.ObjectModel.Sphere.Radius, (float)ActionObjectMetadata.ObjectModel.Sphere.Radius, (float)ActionObjectMetadata.ObjectModel.Sphere.Radius)); break; case ObjectModel.TypeEnum.Cylinder: dimensions = TransformConvertor.ROSToUnityScale(new Vector3((float)ActionObjectMetadata.ObjectModel.Cylinder.Radius, (float)ActionObjectMetadata.ObjectModel.Cylinder.Radius, (float)ActionObjectMetadata.ObjectModel.Cylinder.Height)); break; } if (dimensions != null) { Model.transform.localScale = new Vector3(dimensions.Value.x, dimensions.Value.y, dimensions.Value.z); } }
public async void CreateActionObject() { string newActionObjectName = (string)nameInput.GetValue(); if (Base.Parameter.CheckIfAllValuesValid(actionParameters)) { List <IO.Swagger.Model.Parameter> parameters = new List <IO.Swagger.Model.Parameter>(); foreach (IParameter actionParameter in actionParameters) { if (!parametersMetadata.TryGetValue(actionParameter.GetName(), out Base.ParameterMetadata actionParameterMetadata)) { Base.Notifications.Instance.ShowNotification("Failed to create new action", "Failed to get metadata for action parameter: " + actionParameter.GetName()); return; } IO.Swagger.Model.ActionParameter ap = new IO.Swagger.Model.ActionParameter(name: actionParameter.GetName(), value: JsonConvert.SerializeObject(actionParameter.GetValue()), type: actionParameterMetadata.Type); parameters.Add(DataHelper.ActionParameterToParameter(ap)); } try { IO.Swagger.Model.Pose pose = null; if (actionObjectMetadata.HasPose) { Vector3 abovePoint = SceneManager.Instance.GetCollisionFreePointAbove(SceneManager.Instance.SceneOrigin.transform, actionObjectMetadata.GetModelBB(), SceneManager.Instance.SceneOrigin.transform.localRotation); IO.Swagger.Model.Position offset = DataHelper.Vector3ToPosition(TransformConvertor.UnityToROS(abovePoint)); pose = new IO.Swagger.Model.Pose(position: offset, orientation: new IO.Swagger.Model.Orientation(1, 0, 0, 0)); } await Base.WebsocketManager.Instance.AddObjectToScene(newActionObjectName, actionObjectMetadata.Type, pose, parameters); Close(); } catch (Base.RequestFailedException e) { Base.Notifications.Instance.ShowNotification("Failed to add action", e.Message); } } }
public IO.Swagger.Model.Pose CreatePoseInTheView(float distance = 0.3f) { Ray ray = Camera.main.ScreenPointToRay(new Vector3(Screen.width / 2, Screen.height / 2, 0f)); Vector3 point = TransformConvertor.UnityToROS(GameManager.Instance.Scene.transform.InverseTransformPoint(ray.GetPoint(distance))); return(new IO.Swagger.Model.Pose(position: DataHelper.Vector3ToPosition(point), orientation: DataHelper.QuaternionToOrientation(Quaternion.identity))); }
public override Quaternion GetSceneOrientation() { if (ActionObjectMetadata.HasPose) { return(TransformConvertor.ROSToUnity(DataHelper.OrientationToQuaternion(Data.Pose.Orientation))); } else { return(PlayerPrefsHelper.LoadQuaternion("scene/" + SceneManager.Instance.SceneMeta.Id + "/action_object/" + Data.Id + "/pose/rotation", Quaternion.identity)); } }
public override Vector3 GetScenePosition() { if (ActionObjectMetadata.HasPose) { return(TransformConvertor.ROSToUnity(DataHelper.PositionToVector3(Data.Pose.Position))); } else { return(PlayerPrefsHelper.LoadVector3("scene/" + SceneManager.Instance.SceneMeta.Id + "/action_object/" + Data.Id + "/pose/position", Vector3.zero)); } }
public async void AddAP(string name) { Debug.Assert(CurrentActionPoint != null); Vector3 abovePoint = SceneManager.Instance.GetCollisionFreePointAbove(CurrentActionPoint.transform, Vector3.one * 0.025f, Quaternion.identity); IO.Swagger.Model.Position offset = DataHelper.Vector3ToPosition(TransformConvertor.UnityToROS(abovePoint)); bool result = await GameManager.Instance.AddActionPoint(name, CurrentActionPoint.Data.Id, offset); if (result) { inputDialog.Close(); } UpdateMenu(); }
public void CreateModel() { if (ActionObjectMetadata.ObjectModel == null || ActionObjectMetadata.ObjectModel.Type == IO.Swagger.Model.ObjectModel.TypeEnum.None) { Model = Instantiate(CubePrefab, Visual.transform); Visual.transform.localScale = new Vector3(0.05f, 0.01f, 0.05f); } else { switch (ActionObjectMetadata.ObjectModel.Type) { case IO.Swagger.Model.ObjectModel.TypeEnum.Box: Model = Instantiate(CubePrefab, Visual.transform); Visual.transform.localScale = TransformConvertor.ROSToUnityScale(new Vector3((float)ActionObjectMetadata.ObjectModel.Box.SizeX, (float)ActionObjectMetadata.ObjectModel.Box.SizeY, (float)ActionObjectMetadata.ObjectModel.Box.SizeZ)); break; case IO.Swagger.Model.ObjectModel.TypeEnum.Cylinder: Model = Instantiate(CylinderPrefab, Visual.transform); Visual.transform.localScale = new Vector3((float)ActionObjectMetadata.ObjectModel.Cylinder.Radius, (float)ActionObjectMetadata.ObjectModel.Cylinder.Height, (float)ActionObjectMetadata.ObjectModel.Cylinder.Radius); break; case IO.Swagger.Model.ObjectModel.TypeEnum.Sphere: Model = Instantiate(SpherePrefab, Visual.transform); Visual.transform.localScale = new Vector3((float)ActionObjectMetadata.ObjectModel.Sphere.Radius, (float)ActionObjectMetadata.ObjectModel.Sphere.Radius, (float)ActionObjectMetadata.ObjectModel.Sphere.Radius); break; default: Model = Instantiate(CubePrefab, Visual.transform); Visual.transform.localScale = new Vector3(0.05f, 0.01f, 0.05f); break; } } if (IsRobot()) { Model.tag = "Robot"; } gameObject.GetComponent <BindParentToChild>().ChildToBind = Model; Model.GetComponent <OnClickCollider>().Target = gameObject; Model.transform.localScale = new Vector3(1, 1, 1); modelRenderer = Model.GetComponent <Renderer>(); outlineOnClick = gameObject.GetComponent <OutlineOnClick>(); outlineOnClick.InitRenderers(new List <Renderer>() { modelRenderer }); }
private void Update() { if (CanvasGroup.alpha == 1) { if (gizmo != null && SceneManager.Instance.IsRobotAndEESelected()) { if (world) { gizmo.transform.rotation = GameManager.Instance.Scene.transform.rotation; } else { gizmo.transform.rotation = SceneManager.Instance.SelectedRobot.GetTransform().rotation;// * Quaternion.Inverse(GameManager.Instance.Scene.transform.rotation); } if (translate) { Vector3 position = TransformConvertor.UnityToROS(OrigPose.transform.InverseTransformPoint(SceneManager.Instance.SelectedEndEffector.transform.position)); //Coordinates.X.SetValueMeters(position.x); //Coordinates.Y.SetValueMeters(position.y); //Coordinates.Z.SetValueMeters(position.z); gizmo.SetXDelta(position.x); gizmo.SetYDelta(position.y); gizmo.SetZDelta(position.z); } else { Quaternion newrotation = TransformConvertor.UnityToROS(SceneManager.Instance.SelectedEndEffector.transform.rotation * Quaternion.Inverse(OrigPose.transform.transform.rotation)); //Coordinates.X.SetValueDegrees(newrotation.eulerAngles.x); //Coordinates.Y.SetValueDegrees(newrotation.eulerAngles.y); //Coordinates.Z.SetValueDegrees(newrotation.eulerAngles.z); gizmo.SetXDeltaRotation(newrotation.eulerAngles.x); gizmo.SetYDeltaRotation(newrotation.eulerAngles.y); gizmo.SetZDeltaRotation(newrotation.eulerAngles.z); } } } }
public void UpdateOrientationsVisuals() { foreach (Transform transform in orientations.transform) { Destroy(transform.gameObject); } if (!ProjectManager.Instance.APOrientationsVisible) { return; } if (!OrientationsVisible) { return; } foreach (IO.Swagger.Model.NamedOrientation orientation in Data.Orientations) { APOrientation apOrientation = Instantiate(ActionsManager.Instance.ActionPointOrientationPrefab, orientations.transform).GetComponent <APOrientation>(); apOrientation.transform.localRotation = TransformConvertor.ROSToUnity(DataHelper.OrientationToQuaternion(orientation.Orientation)); apOrientation.ActionPoint = this; apOrientation.OrientationId = orientation.Id; } }
public async void CreateNewAP(string name) { Debug.Assert(CurrentObject != null); /*IO.Swagger.Model.Position offset = new IO.Swagger.Model.Position(); * Vector3 aboveModel = CurrentObject.GetTopPoint(); * aboveModel.y += 0.1f; * offset = DataHelper.Vector3ToPosition(TransformConvertor.UnityToROS(CurrentObject.transform.InverseTransformPoint(aboveModel))); */ Vector3 abovePoint = SceneManager.Instance.GetCollisionFreePointAbove(CurrentObject.transform.localPosition); IO.Swagger.Model.Position offset = DataHelper.Vector3ToPosition(TransformConvertor.UnityToROS(CurrentObject.transform.InverseTransformPoint(abovePoint))); bool result = await GameManager.Instance.AddActionPoint(name, CurrentObject.Data.Id, offset); //Base.Scene.Instance.SpawnActionPoint(CurrentObject.GetComponent<Base.ActionObject>(), null); if (result) { inputDialog.Close(); } UpdateMenu(); }
public async void AddOrientation(string name, string robotName) { Debug.Assert(CurrentActionPoint != null); IRobot robot; try { robot = SceneManager.Instance.GetRobotByName(robotName); } catch (ItemNotFoundException ex) { Notifications.Instance.ShowNotification("Failed to add orientation", "Could not found robot called: " + robotName); Debug.LogError(ex); return; } if (CurrentActionPoint.OrientationNameExist(name) || CurrentActionPoint.JointsNameExist(name)) { Notifications.Instance.ShowNotification("Failed to add orientation", "There already exists orientation or joints with name " + name); return; } IO.Swagger.Model.Orientation orientation = new IO.Swagger.Model.Orientation(); if (CurrentActionPoint.Parent != null) { orientation = DataHelper.QuaternionToOrientation(TransformConvertor.UnityToROS(Quaternion.Inverse(CurrentActionPoint.Parent.GetTransform().rotation))); } preselectedOrientation = name; bool successOrientation = await Base.GameManager.Instance.AddActionPointOrientation(CurrentActionPoint, orientation, name); bool successJoints = await Base.GameManager.Instance.AddActionPointJoints(CurrentActionPoint, name, robot.GetId()); if (successOrientation && successJoints) { inputDialog.Close(); } else { preselectedOrientation = null; } }
public async void CreateActionObject() { string newActionObjectName = (string)nameInput.GetValue(); if (Base.Parameter.CheckIfAllValuesValid(actionParameters)) { List <IO.Swagger.Model.Parameter> parameters = new List <IO.Swagger.Model.Parameter>(); foreach (IParameter actionParameter in actionParameters) { if (!parametersMetadata.TryGetValue(actionParameter.GetName(), out Base.ParameterMetadata actionParameterMetadata)) { Base.Notifications.Instance.ShowNotification("Failed to create new action object", "Failed to get metadata for action object parameter: " + actionParameter.GetName()); return; } IO.Swagger.Model.ActionParameter ap = new IO.Swagger.Model.ActionParameter(name: actionParameter.GetName(), value: JsonConvert.SerializeObject(actionParameter.GetValue()), type: actionParameterMetadata.Type); parameters.Add(DataHelper.ActionParameterToParameter(ap)); } try { Ray ray = Camera.main.ScreenPointToRay(new Vector3(Screen.width / 2, Screen.height / 2, 0f)); Vector3 point = TransformConvertor.UnityToROS(GameManager.Instance.Scene.transform.InverseTransformPoint(ray.GetPoint(0.5f))); IO.Swagger.Model.Pose pose = null; if (actionObjectMetadata.HasPose) { pose = new IO.Swagger.Model.Pose(position: DataHelper.Vector3ToPosition(point), orientation: DataHelper.QuaternionToOrientation(Quaternion.identity)); } SceneManager.Instance.SelectCreatedActionObject = newActionObjectName; await Base.WebsocketManager.Instance.AddObjectToScene(newActionObjectName, actionObjectMetadata.Type, pose, parameters); callback?.Invoke(); Close(); } catch (Base.RequestFailedException e) { Base.Notifications.Instance.ShowNotification("Failed to add action", e.Message); } } }
public IO.Swagger.Model.Pose GetPose() { if (ActionObjectMetadata.HasPose) { return(new IO.Swagger.Model.Pose(position: DataHelper.Vector3ToPosition(TransformConvertor.UnityToROS(transform.localPosition)), orientation: DataHelper.QuaternionToOrientation(TransformConvertor.UnityToROS(transform.localRotation)))); } else { return(new IO.Swagger.Model.Pose(new IO.Swagger.Model.Orientation(), new IO.Swagger.Model.Position())); } }
public async Task UpdateMenu() { CalibrateBtn.gameObject.SetActive(false); if (!SceneManager.Instance.SceneStarted) { UpdatePositionBlockVO.SetActive(false); UpdatePositionBlockMesh.SetActive(false); Hide(); return; } CalibrateBtn.Button.onClick.RemoveAllListeners(); if (currentObject.IsRobot()) { CalibrateBtn.gameObject.SetActive(true); CalibrateBtn.SetDescription("Calibrate robot"); CalibrateBtn.Button.onClick.AddListener(() => ShowCalibrateRobotDialog()); if (SceneManager.Instance.GetCamerasNames().Count > 0) { CalibrateBtn.SetInteractivity(true); } else { CalibrateBtn.SetInteractivity(false, "Could not calibrate robot without camera"); } } else if (currentObject.IsCamera()) { CalibrateBtn.gameObject.SetActive(true); CalibrateBtn.SetDescription("Calibrate camera"); CalibrateBtn.Button.onClick.AddListener(() => ShowCalibrateCameraDialog()); } if (SceneManager.Instance.IsRobotAndEESelected()) { if (currentObject.ActionObjectMetadata.ObjectModel?.Type == IO.Swagger.Model.ObjectModel.TypeEnum.Mesh && currentObject.ActionObjectMetadata.ObjectModel.Mesh.FocusPoints?.Count > 0) { UpdatePositionBlockVO.SetActive(false); UpdatePositionBlockMesh.SetActive(true); int idx = 0; foreach (AimingPointSphere sphere in spheres) { if (sphere != null) { Destroy(sphere.gameObject); } } spheres.Clear(); foreach (IO.Swagger.Model.Pose point in currentObject.ActionObjectMetadata.ObjectModel.Mesh.FocusPoints) { AimingPointSphere sphere = Instantiate(Sphere, currentObject.transform).GetComponent <AimingPointSphere>(); sphere.transform.localScale = new Vector3(0.02f, 0.02f, 0.02f); sphere.transform.localPosition = TransformConvertor.ROSToUnity(DataHelper.PositionToVector3(point.Position)); sphere.transform.localRotation = TransformConvertor.ROSToUnity(DataHelper.OrientationToQuaternion(point.Orientation)); sphere.Init(idx, $"Aiming point #{idx}"); spheres.Add(sphere); ++idx; } try { List <int> finishedIndexes = await WebsocketManager.Instance.ObjectAimingAddPoint(0, true); foreach (AimingPointSphere sphere in spheres) { sphere.SetAimed(finishedIndexes.Contains(sphere.Index)); } if (!automaticPointSelection) { currentFocusPoint = 0; } StartObjectFocusingButton.SetInteractivity(false, "Already started"); SavePositionButton.SetInteractivity(true); CancelAimingButton.SetInteractivity(true); await CheckDoneBtn(); AimingInProgress = true; if (currentObject is ActionObject3D actionObject3D) { actionObject3D.UnHighlight(); } UpdateCurrentPointLabel(); if (!automaticPointSelection && currentObject.ActionObjectMetadata.ObjectModel.Mesh.FocusPoints.Count > 1) { NextButton.SetInteractivity(true); PreviousButton.SetInteractivity(true); PreviousPoint(); } } catch (RequestFailedException ex) { StartObjectFocusingButton.SetInteractivity(true); FocusObjectDoneButton.SetInteractivity(false, "No aiming in progress"); NextButton.SetInteractivity(false, "No aiming in progress"); PreviousButton.SetInteractivity(false, "No aiming in progress"); SavePositionButton.SetInteractivity(false, "No aiming in progress"); CancelAimingButton.SetInteractivity(false, "No aiming in progress"); AimingInProgress = false; if (currentObject is ActionObject3D actionObject3D) { actionObject3D.Highlight(); } } } else if (!currentObject.IsRobot() && !currentObject.IsCamera() && currentObject.ActionObjectMetadata.ObjectModel != null) { UpdatePositionBlockVO.SetActive(true); UpdatePositionBlockMesh.SetActive(false); ShowModelSwitch.Interactable = SceneManager.Instance.RobotsEEVisible; if (ShowModelSwitch.Interactable && ShowModelSwitch.Switch.isOn) { ShowModelOnEE(); } } else { UpdatePositionBlockVO.SetActive(false); UpdatePositionBlockMesh.SetActive(false); } } else { UpdatePositionBlockVO.SetActive(false); UpdatePositionBlockMesh.SetActive(false); } }
public override void SetScenePosition(Vector3 position) { Data.Position = DataHelper.Vector3ToPosition(TransformConvertor.UnityToROS(position)); }
public override Vector3 GetScenePosition() { return(TransformConvertor.ROSToUnity(DataHelper.PositionToVector3(Data.Position))); }
public void SetOrientation(IO.Swagger.Model.Orientation orientation) { transform.localRotation = TransformConvertor.ROSToUnity(DataHelper.OrientationToQuaternion(orientation)); }
public async override void CopyObjectClick() { if (selectedObject is ActionObject actionObject) { List <IO.Swagger.Model.Parameter> parameters = new List <IO.Swagger.Model.Parameter>(); foreach (Base.Parameter p in actionObject.ObjectParameters.Values) { parameters.Add(DataHelper.ActionParameterToParameter(p)); } string newName = SceneManager.Instance.GetFreeAOName(actionObject.GetName()); SceneManager.Instance.SelectCreatedActionObject = newName; SceneManager.Instance.OpenTransformMenuOnCreatedObject = true; await WebsocketManager.Instance.AddObjectToScene(newName, actionObject.ActionObjectMetadata.Type, new IO.Swagger.Model.Pose( orientation: DataHelper.QuaternionToOrientation(TransformConvertor.UnityToROS(actionObject.transform.localRotation)), position: DataHelper.Vector3ToPosition(TransformConvertor.UnityToROS(actionObject.transform.localPosition))), parameters); } }
public override void SetSceneOrientation(Quaternion orientation) { Data.Pose.Orientation = DataHelper.QuaternionToOrientation(TransformConvertor.UnityToROS(orientation)); }
public override Quaternion GetSceneOrientation() { return(TransformConvertor.ROSToUnity(DataHelper.OrientationToQuaternion(Data.Pose.Orientation))); }
public override void CreateModel(CollisionModels customCollisionModels = null) { if (ActionObjectMetadata.ObjectModel == null || ActionObjectMetadata.ObjectModel.Type == IO.Swagger.Model.ObjectModel.TypeEnum.None) { Model = Instantiate(CubePrefab, Visual.transform); Model.transform.localScale = new Vector3(0.05f, 0.01f, 0.05f); } else { switch (ActionObjectMetadata.ObjectModel.Type) { case IO.Swagger.Model.ObjectModel.TypeEnum.Box: Model = Instantiate(CubePrefab, Visual.transform); if (customCollisionModels == null) { Model.transform.localScale = TransformConvertor.ROSToUnityScale(new Vector3((float)ActionObjectMetadata.ObjectModel.Box.SizeX, (float)ActionObjectMetadata.ObjectModel.Box.SizeY, (float)ActionObjectMetadata.ObjectModel.Box.SizeZ)); } else { foreach (IO.Swagger.Model.Box box in customCollisionModels.Boxes) { if (box.Id == ActionObjectMetadata.Type) { Model.transform.localScale = TransformConvertor.ROSToUnityScale(new Vector3((float)box.SizeX, (float)box.SizeY, (float)box.SizeZ)); break; } } } break; case IO.Swagger.Model.ObjectModel.TypeEnum.Cylinder: Model = Instantiate(CylinderPrefab, Visual.transform); if (customCollisionModels == null) { Model.transform.localScale = new Vector3((float)ActionObjectMetadata.ObjectModel.Cylinder.Radius, (float)ActionObjectMetadata.ObjectModel.Cylinder.Height / 2, (float)ActionObjectMetadata.ObjectModel.Cylinder.Radius); } else { foreach (IO.Swagger.Model.Cylinder cylinder in customCollisionModels.Cylinders) { if (cylinder.Id == ActionObjectMetadata.Type) { Model.transform.localScale = new Vector3((float)cylinder.Radius, (float)cylinder.Height, (float)cylinder.Radius); break; } } } break; case IO.Swagger.Model.ObjectModel.TypeEnum.Sphere: Model = Instantiate(SpherePrefab, Visual.transform); if (customCollisionModels == null) { Model.transform.localScale = new Vector3((float)ActionObjectMetadata.ObjectModel.Sphere.Radius, (float)ActionObjectMetadata.ObjectModel.Sphere.Radius, (float)ActionObjectMetadata.ObjectModel.Sphere.Radius); } else { foreach (IO.Swagger.Model.Sphere sphere in customCollisionModels.Spheres) { if (sphere.Id == ActionObjectMetadata.Type) { Model.transform.localScale = new Vector3((float)sphere.Radius, (float)sphere.Radius, (float)sphere.Radius); break; } } } break; default: Model = Instantiate(CubePrefab, Visual.transform); Model.transform.localScale = new Vector3(0.05f, 0.01f, 0.05f); break; } } if (IsRobot()) { Model.tag = "Robot"; } gameObject.GetComponent <BindParentToChild>().ChildToBind = Model; Collider = Model.GetComponent <Collider>(); Model.GetComponent <OnClickCollider>().Target = gameObject; modelRenderer = Model.GetComponent <Renderer>(); modelMaterial = modelRenderer.material; outlineOnClick = gameObject.GetComponent <OutlineOnClick>(); outlineOnClick.InitRenderers(new List <Renderer>() { modelRenderer }); Model.AddComponent <GizmoOutlineHandler>().OutlineOnClick = outlineOnClick; }
public IO.Swagger.Model.Pose GetPose() { return(new IO.Swagger.Model.Pose(position: DataHelper.Vector3ToPosition(TransformConvertor.UnityToROS(transform.position)), orientation: DataHelper.QuaternionToOrientation(TransformConvertor.UnityToROS(transform.rotation)))); }
public override void CreateModel(CollisionModels customCollisionModels = null) { if (ActionObjectMetadata.ObjectModel == null || ActionObjectMetadata.ObjectModel.Type == IO.Swagger.Model.ObjectModel.TypeEnum.None) { Model = Instantiate(CubePrefab, Visual.transform); Model.transform.localScale = new Vector3(0.05f, 0.01f, 0.05f); } else { switch (ActionObjectMetadata.ObjectModel.Type) { case IO.Swagger.Model.ObjectModel.TypeEnum.Box: Model = Instantiate(CubePrefab, Visual.transform); if (customCollisionModels == null) { Model.transform.localScale = TransformConvertor.ROSToUnityScale(new Vector3((float)ActionObjectMetadata.ObjectModel.Box.SizeX, (float)ActionObjectMetadata.ObjectModel.Box.SizeY, (float)ActionObjectMetadata.ObjectModel.Box.SizeZ)); } else { foreach (IO.Swagger.Model.Box box in customCollisionModels.Boxes) { if (box.Id == ActionObjectMetadata.Type) { Model.transform.localScale = TransformConvertor.ROSToUnityScale(new Vector3((float)box.SizeX, (float)box.SizeY, (float)box.SizeZ)); break; } } } break; case IO.Swagger.Model.ObjectModel.TypeEnum.Cylinder: Model = Instantiate(CylinderPrefab, Visual.transform); if (customCollisionModels == null) { Model.transform.localScale = new Vector3((float)ActionObjectMetadata.ObjectModel.Cylinder.Radius, (float)ActionObjectMetadata.ObjectModel.Cylinder.Height / 2, (float)ActionObjectMetadata.ObjectModel.Cylinder.Radius); } else { foreach (IO.Swagger.Model.Cylinder cylinder in customCollisionModels.Cylinders) { if (cylinder.Id == ActionObjectMetadata.Type) { Model.transform.localScale = new Vector3((float)cylinder.Radius, (float)cylinder.Height, (float)cylinder.Radius); break; } } } break; case IO.Swagger.Model.ObjectModel.TypeEnum.Sphere: Model = Instantiate(SpherePrefab, Visual.transform); if (customCollisionModels == null) { Model.transform.localScale = new Vector3((float)ActionObjectMetadata.ObjectModel.Sphere.Radius, (float)ActionObjectMetadata.ObjectModel.Sphere.Radius, (float)ActionObjectMetadata.ObjectModel.Sphere.Radius); } else { foreach (IO.Swagger.Model.Sphere sphere in customCollisionModels.Spheres) { if (sphere.Id == ActionObjectMetadata.Type) { Model.transform.localScale = new Vector3((float)sphere.Radius, (float)sphere.Radius, (float)sphere.Radius); break; } } } break; case ObjectModel.TypeEnum.Mesh: MeshImporter.Instance.OnMeshImported += OnModelLoaded; MeshImporter.Instance.LoadModel(ActionObjectMetadata.ObjectModel.Mesh, GetId()); Model = Instantiate(CubePrefab, Visual.transform); Model.transform.localScale = new Vector3(0.05f, 0.01f, 0.05f); break; default: Model = Instantiate(CubePrefab, Visual.transform); Model.transform.localScale = new Vector3(0.05f, 0.01f, 0.05f); break; } } //if (IsRobot()) { // Model.tag = "Robot"; //} gameObject.GetComponent <BindParentToChild>().ChildToBind = Model; Collider = Model.GetComponent <Collider>(); Colliders.Add(Collider); Model.GetComponent <OnClickCollider>().Target = gameObject; outlineOnClick = gameObject.GetComponent <OutlineOnClick>(); aoRenderers.Clear(); aoRenderers.AddRange(Model.GetComponentsInChildren <Renderer>(true)); outlineOnClick.InitRenderers(aoRenderers); }
public async void GetMarkerPosition(CameraParameters cameraParams, string image, bool autoCalibrate = false, bool force = false, bool showNotification = false) { try { // receive cameraPose from server IO.Swagger.Model.EstimatedPose markerEstimatedPose = await WebsocketManager.Instance.GetCameraPose(cameraParams, image, inverse : true); if ((float)markerEstimatedPose.Quality > anchorQuality || force) { anchorQuality = (float)markerEstimatedPose.Quality; //Notifications.Instance.ShowNotification("Marker quality", markerEstimatedPose.Quality.ToString()); IO.Swagger.Model.Pose markerPose = markerEstimatedPose.Pose; Vector3 markerPositionReceived = new Vector3((float)markerPose.Position.X, (float)markerPose.Position.Y, (float)markerPose.Position.Z); Quaternion markerRotationReceived = new Quaternion((float)markerPose.Orientation.X, (float)markerPose.Orientation.Y, (float)markerPose.Orientation.Z, (float)markerPose.Orientation.W); Matrix4x4 markerMatrix = AdjustMatrixByScreenOrientation(Matrix4x4.TRS(markerPositionReceived, markerRotationReceived, Vector3.one)); Vector3 markerPosition = TransformConvertor.OpenCVToUnity(TransformConvertor.GetPositionFromMatrix(markerMatrix)); Quaternion markerRotation = TransformConvertor.OpenCVToUnity(TransformConvertor.GetQuaternionFromMatrix(markerMatrix)); // Marker Position GameObject marker = Instantiate(MarkerPositionGameObject); marker.transform.localPosition = ARCameraTransformMatrix.MultiplyPoint3x4(markerPosition); //ARCamera.TransformPoint(markerPosition); marker.transform.localRotation = TransformConvertor.GetQuaternionFromMatrix(ARCameraTransformMatrix) * markerRotation; //ARCamera.transform.rotation * markerRotation; marker.transform.localScale = new Vector3(1f, 1f, 1f); CreateLocalAnchor(marker); //System.IO.File.WriteAllBytes(Application.persistentDataPath + "/image" + imageNum + ".jpg", m_Texture.EncodeToJPG()); //imageNum++; // Transformation Inversion to get Camera Position //markerMatrix = Matrix4x4.TRS(markerPosition, markerRotation, Vector3.one); // create translation, rotation and scaling matrix //Matrix4x4 cameraMatrix = markerMatrix.inverse; // inverse to get marker rotation matrix //cameraMatrix.SetColumn(3, Vector4.zero); // set translation column to zeros //Vector3 cameraPos = cameraMatrix.MultiplyPoint3x4(markerPosition); // transform cameraPosition by marker matrix //cameraPos = -1 * cameraPos; //if (WorldAnchorLocal != null) { // // Camera Position // GameObject camera = Instantiate(MarkerPositionGameObject, WorldAnchorLocal.transform); // camera.transform.localPosition = cameraPos; // camera.transform.localRotation = Quaternion.LookRotation(cameraMatrix.GetColumn(2), cameraMatrix.GetColumn(1)); // camera.transform.localScale = new Vector3(0.05f, 0.05f, 0.05f); //} //Notifications.Instance.ShowNotification("Marker position", "GetCameraPose inverse true"); markerDetectionState = MarkerDetectionState.Success; if (showNotification) { Notifications.Instance.ShowNotification("Calibration successful", ""); } } else { markerDetectionState = MarkerDetectionState.Failure; if (showNotification) { Notifications.Instance.ShowNotification("No markers visible", "Find some markers and try again."); } } } catch (RequestFailedException ex) { markerDetectionState = MarkerDetectionState.Failure; Debug.Log("No markers visible"); if (showNotification) { Notifications.Instance.ShowNotification("No markers visible", "Find some markers and try again."); } } }
public async void MoveHereModel(bool avoid_collision = true) { List <IO.Swagger.Model.Joint> modelJoints; //joints to move the model to string robotId; if (isOrientationDetail) { try { IO.Swagger.Model.Pose pose = new IO.Swagger.Model.Pose(orientation.Orientation, DataHelper.Vector3ToPosition(TransformConvertor.UnityToROS(CurrentActionPoint.transform.position))); List <IO.Swagger.Model.Joint> startJoints = SceneManager.Instance.SelectedRobot.GetJoints(); modelJoints = await WebsocketManager.Instance.InverseKinematics(SceneManager.Instance.SelectedRobot.GetId(), SceneManager.Instance.SelectedEndEffector.GetName(), true, pose, startJoints); await PrepareRobotModel(SceneManager.Instance.SelectedRobot.GetId(), false); if (!avoid_collision) { Notifications.Instance.ShowNotification("The model is in a collision with other object!", ""); } } catch (ItemNotFoundException ex) { Notifications.Instance.ShowNotification("Unable to move here model", ex.Message); return; } catch (RequestFailedException ex) { if (avoid_collision) //if this is first call, try it again without avoiding collisions { MoveHereModel(false); } else { Notifications.Instance.ShowNotification("Unable to move here model", ex.Message); } return; } } else //joints menu { modelJoints = this.joints.Joints; robotId = this.joints.RobotId; } foreach (IO.Swagger.Model.Joint joint in modelJoints) { SceneManager.Instance.SelectedRobot.SetJointValue(joint.Name, (float)joint.Value); } }