/// <summary> /// Simple function to update the size of the cylinder which visualizes the current separation distance. /// </summary> /// <param name="robot"></param>The robot for which the update should be made. private void UpdateVisualizationDistance(IRobot robot) { if (robot != null && robot.IsValid && app.World.FindComponent("SeparationVisualization_" + robot.Name) != null) { ISimComponent comp = app.World.FindComponent("SeparationVisualization_" + robot.Name); ISimNode node = robot.Component.FindNode("mountplate"); Matrix matrix = comp.TransformationInReference; matrix.SetP(new Vector3(node.TransformationInWorld.Px, node.TransformationInWorld.Py, 201)); comp.TransformationInReference = matrix; ICylinderFeature cylinder = (ICylinderFeature)app.World.FindComponent("SeparationVisualization_" + robot.Name).FindFeature("SeparationVisualization"); if (robotList[robot].currentSeperationDistance <= 100) { cylinder.GetProperty("Radius").Value = "100"; comp.GetProperty("SeparationDistance").Value = "100"; } else { cylinder.GetProperty("Radius").Value = robotList[robot].currentSeperationDistance.ToString(); comp.GetProperty("SeparationDistance").Value = robotList[robot].currentSeperationDistance; } cylinder.Rebuild(); } else { ms.AppendMessage("UpdateVisualizationDistance: Failed to find robot component!", MessageLevel.Warning); } }
/// <summary> /// Initialization at Simulation startup. Each time the play button is pressed, this is triggered. /// </summary> /// <param name="sender"></param>The source of the simulation start event. /// <param name="e"></param>The simulation start event. public void SimulationStarted(object sender, EventArgs e) { ms.AppendMessage("Simulation Started", MessageLevel.Warning); timer = statisticsManager.CreateTimer(RegularTick, TICK_INTERVAL); timer.StartStopTimer(true); if (app.World.FindComponent("WorksHuman") != null) { human = app.World.FindComponent("WorksHuman"); if (human.GetProperty("AngleIndicatorZRotation") != null) { humanAngleIndicatorZRotation = human.GetProperty("AngleIndicatorZRotation"); } } }
/// <summary> /// Sets value of OPCUA node property to corresponding VC property /// </summary> private void UpdateValueToVcProperty(NodeState node) { Debug.Assert(System.Threading.Thread.CurrentThread == System.Windows.Application.Current.Dispatcher.Thread); if (!UaBrowseName2VcComponentName.ContainsKey(node.BrowseName.Name)) { _vcUtils.VcWriteWarningMsg(String.Format("Component with property {0} not found", node.BrowseName.Name)); return; } // Cast property OPCUA node to BaseDataVariableState type BaseDataVariableState uaProperty = (BaseDataVariableState)node; // Get property component as VC object ISimComponent vcComponent = _vcUtils.GetComponent(UaBrowseName2VcComponentName[node.BrowseName.Name]); IProperty vcProperty = vcComponent.GetProperty(node.DisplayName.ToString()); if (uaProperty.DataType == new NodeId(DataTypeIds.String)) { if ((string)uaProperty.Value == (string)vcProperty.Value) { return; } vcProperty.Value = (string)uaProperty.Value; } else if (uaProperty.DataType == new NodeId(DataTypeIds.Boolean)) { if ((bool)uaProperty.Value == (bool)vcProperty.Value) { return; } vcProperty.Value = (bool)uaProperty.Value; } else if (uaProperty.DataType == new NodeId(DataTypeIds.Double)) { if ((double)uaProperty.Value == (double)vcProperty.Value) { return; } vcProperty.Value = (double)uaProperty.Value; } else if (uaProperty.DataType == new NodeId(DataTypeIds.Integer)) { if ((int)uaProperty.Value == (int)vcProperty.Value) { return; } vcProperty.Value = (int)uaProperty.Value; } else { _vcUtils.VcWriteWarningMsg("OPCUA property type not supported" + uaProperty.DataType.ToString()); return; } }
/// <summary> /// Sets value of OPCUA node property to corresponding VC property /// </summary> private void ua_PropertyChanged(ISystemContext context, NodeState node, NodeStateChangeMasks changes) { if (!UaBrowseName2VcComponentName.ContainsKey(node.BrowseName.Name)) { _vcUtils.VcWriteWarningMsg(String.Format("Component with property {0} not found", node.BrowseName.Name)); return; } // Cast property OPCUA node to BaseDataVariableState type BaseDataVariableState uaProperty = (BaseDataVariableState)node; // Get property component as VC object ISimComponent vcComponent = _vcUtils.GetComponent(UaBrowseName2VcComponentName[node.BrowseName.Name]); IProperty vcProperty = vcComponent.GetProperty(node.DisplayName.ToString()); if (uaProperty.DataType == new NodeId(DataTypeIds.String)) { if ((string)uaProperty.Value == (string)vcProperty.Value) { return; } vcProperty.Value = (string)uaProperty.Value; } else if (uaProperty.DataType == new NodeId(DataTypeIds.Boolean)) { if ((bool)uaProperty.Value == (bool)vcProperty.Value) { return; } vcProperty.Value = (bool)uaProperty.Value; } else if (uaProperty.DataType == new NodeId(DataTypeIds.Double)) { if ((double)uaProperty.Value == (double)vcProperty.Value) { return; } vcProperty.Value = (double)uaProperty.Value; } else if (uaProperty.DataType == new NodeId(DataTypeIds.Integer)) { if ((int)uaProperty.Value == (int)vcProperty.Value) { return; } vcProperty.Value = (int)uaProperty.Value; } else { _vcUtils.VcWriteWarningMsg("OPCUA property type not supported" + uaProperty.DataType.ToString()); return; } }
private void stapleConfig(String stapleComponentName) { StapleSection parameterStaple = ConfigReader.readStapleConfig(); ISimComponent stapleComponent = app.Value.World.FindComponent(stapleComponentName); if (stapleComponentName != "" && stapleComponent == null) { ms.AppendMessage("Failed to find staple component with name\"" + stapleComponentName + "\"! Planning of motion aborted...", MessageLevel.Warning); return; } String stapleFileFolder = parameterStaple.staplePath.Path; if (stapleComponent.GetProperty("stlID") == null) { ms.AppendMessage("Component with name \"" + stapleComponentName + "\" has no property \"stlID\"! Planning of motion aborted...", MessageLevel.Warning); return; } String stlID = (String)stapleComponent.GetProperty("stlID").Value; String obstacleFilePath = stapleFileFolder + stlID + ".stl"; if (!File.Exists(obstacleFilePath)) { ms.AppendMessage("A path planning request requested stlID with \"" + stlID + "\" failed, because file \"" + obstacleFilePath + "\" does not exist...", MessageLevel.Warning); return; } ; if (stapleComponent.GetProperty("StackHeight") == null) { ms.AppendMessage("Failed to find StackHeight property in component with name \"" + stapleComponentName + "\"! Planning of motion aborted!", MessageLevel.Warning); return; } Vector3 staplePosition = stapleComponent.TransformationInWorld.GetP(); IDoubleProperty stackwidth = (IDoubleProperty)stapleComponent.GetProperty("StackWidth"); IDoubleProperty stacklength = (IDoubleProperty)stapleComponent.GetProperty("StackLength"); float translate_x = (float)staplePosition.X - (float)(stacklength.Value / 2.0); float translate_y = (float)staplePosition.Y - (float)(stackwidth.Value / 2.0); IDoubleProperty stackheight = (IDoubleProperty)stapleComponent.GetProperty("StackHeight"); float translate_z = (float)stackheight.Value; // setup staple obstacle int obstacleId = motionPlan.addObstacle(obstacleFilePath, translate_x / 1000.0f, translate_y / 1000.0f, translate_z / 1000.0f, 0.0f, 0.0f, 0.0f); }