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 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();
}
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 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 override void SetScenePosition(Vector3 position)
{
Data.Position = DataHelper.Vector3ToPosition(TransformConvertor.UnityToROS(position));
}
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 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 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);
}
}
