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