// ---- Add Objects ----- public void AddObjectToSceneAtPos(PlaceableObject newObj, float x, float y, float phi, Color color) { newObj.objectID = totalObjects++; newObj.transform.position = new Vector3(x / Eyesim.Scale, newObj.defaultVerticalOffset, y / Eyesim.Scale); newObj.transform.rotation = Quaternion.Euler(new Vector3(0f, Eyesim.UnityToEyeSimAngle(phi), 0f)); newObj.isInit = true; if (newObj is Robot) { SimManager.instance.AddRobotToScene(newObj as Robot); } else if (newObj is WorldObject) { SimManager.instance.AddWorldObjectToScene(newObj as WorldObject); } else if (newObj is Marker) { SimManager.instance.AddMarkerToScene(newObj as Marker); newObj.transform.rotation = Quaternion.Euler(new Vector3(90f, 0f, 0f)); (newObj as Marker).SetColor(color); } else { Debug.Log("Error adding objects: Unknown type"); Destroy(newObj.gameObject); } }
// Save a State public void SaveState() { defaultState = new List <ObjectState>(); worldChanged = false; stateID = 0; foreach (Robot robot in allRobots) { ObjectState state = new ObjectState(); state.id = robot.objectID; state.type = robot.type; state.pos = Eyesim.GeneratePositionStringFromTransform(robot.transform); stateID = robot.objectID > stateID ? robot.objectID : stateID; defaultState.Add(state); } foreach (WorldObject wObj in allWorldObjects) { ObjectState state = new ObjectState(); state.id = wObj.objectID; state.type = wObj.type; state.pos = Eyesim.GeneratePositionStringFromTransform(wObj.transform); stateID = wObj.objectID > stateID ? wObj.objectID : stateID; defaultState.Add(state); } foreach (Marker mark in allMarkers) { ObjectState state = new ObjectState(); state.type = "marker"; state.pos = Eyesim.GeneratePositionString(mark.transform.position.x, mark.transform.position.z, mark.mColor.r) + ":" + mark.mColor.g + ":" + mark.mColor.b + ":" + mark.mColor.a; state.color = mark.mColor; defaultState.Add(state); } }
// Set the speed of a single motor public void SetMotorSpeed(int motor, int speed) { int factor = Eyesim.ClampInt(speed, -100, 100); float vSpeed = factor / 100f * maxMotorTorque; if (motor > driveWheels.Count || motor < 0) { Debug.Log("SetMotorSpeed: Bad motor input"); return; } driveWheels[motor].SetSpeed(vSpeed); }
// Update is called once per frame void Update() { if (robot is IPSDSensors) { psdLeftValue.text = psdController.GetPSDValue(0).ToString(); psdFrontValue.text = psdController.GetPSDValue(1).ToString(); psdRightValue.text = psdController.GetPSDValue(2).ToString(); } if (!SimManager.instance.isPaused) { robotXValue.text = (Eyesim.Scale * robot.transform.position.x).ToString("N2"); robotZValue.text = (Eyesim.Scale * robot.transform.position.z).ToString("N2"); robotPhiValue.text = Eyesim.UnityToEyeSimAngle(robot.transform.rotation.eulerAngles.y).ToString("N2"); } if (robot is IVWDrive) { Int16[] pos = (robot as IVWDrive).GetPose(); vwXtext.text = pos[0].ToString("N2"); vwYtext.text = pos[1].ToString("N2"); vwPhiText.text = pos[2].ToString("N2"); } }
// Write scene to a Sim file public void SaveSim(string filepath) { PauseSimulation(); string worldPath; if (worldFilepath == null) { worldPath = Path.Combine(Path.GetDirectoryName(filepath), Path.GetFileNameWithoutExtension(filepath) + "_world.wld"); SaveWorld(worldPath); } else { worldPath = worldFilepath; } FileStream fs = File.Open(filepath, FileMode.Create); using (StreamWriter writer = new StreamWriter(fs, System.Text.Encoding.ASCII)) { // Save world, and link to default save location writer.WriteLine("# Sim file created " + DateTime.Now.ToString(@"MM\/dd\/yyyy h\:mm tt") + Environment.NewLine); writer.WriteLine("# World File " + Environment.NewLine + "world " + "\"" + worldPath + "\""); writer.Write(Environment.NewLine + Environment.NewLine); // Save robot locations writer.WriteLine("# Robots"); foreach (Robot rob in allRobots) { writer.WriteLine(rob.type + " " + (int)Math.Floor(rob.transform.position.x * Eyesim.Scale) + " " + (int)Math.Floor(rob.transform.position.z * Eyesim.Scale) + " " + (int)Math.Floor(Eyesim.UnityToEyeSimAngle(rob.transform.eulerAngles.y))); } writer.WriteLine(); // Save object locations writer.WriteLine("# Objects"); foreach (WorldObject wObj in allWorldObjects) { writer.WriteLine(wObj.type + " " + (int)Math.Floor(wObj.transform.position.x * Eyesim.Scale) + " " + (int)Math.Floor(wObj.transform.position.z * Eyesim.Scale) + " " + (int)Math.Floor(Eyesim.UnityToEyeSimAngle(wObj.transform.eulerAngles.y))); } // Save markers writer.WriteLine("# Markers"); foreach (Marker mark in allMarkers) { writer.WriteLine("marker " + (int)Math.Floor(mark.transform.position.x * Eyesim.Scale) + " " + (int)Math.Floor(mark.transform.position.z * Eyesim.Scale) + " " + (int)Math.Round(mark.mColor.r * 255) + " " + (int)Math.Round(mark.mColor.g * 255) + " " + (int)Math.Round(mark.mColor.b * 255) + " " + (int)Math.Round(mark.mColor.a * 255)); } ResumeSimulation(); } EyesimLogger.instance.Log("Saved sim to file " + filepath); }