private String generateProgramStringFromProgram(Program program) { String programString = ""; foreach (Statement statement in program.statements) { if (statement.GetType().IsSubclassOf(typeof(MoveStatement))) { RosSharp.RosBridgeClient.MessageTypes.Dobot.SetPTPCmdRequest request; MoveStatement moveStatement = (MoveStatement)statement; byte ptpMode_byte; byte.TryParse("" + moveStatement.movementValueForCanvas, out ptpMode_byte); float x = moveStatement.target.x; float y = moveStatement.target.y; float z = moveStatement.target.z; float r = 0f; request = new RosSharp.RosBridgeClient.MessageTypes.Dobot.SetPTPCmdRequest(ptpMode_byte, x, y, z, r, true); programString += $"Move({request.x} {request.y} {request.z});\n"; } else { if (statement is ToggleSuction) { ToggleSuction toggleSuction = (ToggleSuction)statement; programString += $"ToggleSuction({toggleSuction.isSuctionEnabled});\n"; } } } return(programString); }
//design this blocking untill the statement is actually completed private void executeStatement(Statement currentStatement) { semaphor_dobot_is_ready = false; if (connector == null) { throw new ArgumentException("The Connector instance has to be set"); } RosSocket rosSocket = connector.RosSocket; if (currentStatement.GetType().IsSubclassOf(typeof(MoveStatement))) { MoveStatement moveStatement = (MoveStatement)currentStatement; //sphereDrawer.drawSphereAtROSCoords(moveStatement.target); RosSharp.RosBridgeClient.MessageTypes.Dobot.SetPTPCmdRequest req = moveStatement.SendCommandToRos(); String x = rosSocket.CallService <RosSharp.RosBridgeClient.MessageTypes.Dobot.SetPTPCmdRequest, RosSharp.RosBridgeClient.MessageTypes.Dobot.SetPTPCmdResponse> ("/SetPTPCmds", ResponseHandler, req); } else { if (currentStatement is ToggleSuction) { ToggleSuction toggleSuction = (ToggleSuction)currentStatement; setEndeffectorSuctionCup.setEndEffectorSuctionCup(toggleSuction.isSuctionEnabled); } } semaphor_dobot_is_ready = false; }
public RosSharp.RosBridgeClient.MessageTypes.Dobot.SetPTPCmdRequest SendCommandToRos1() { byte ptpMode_byte; byte.TryParse("" + movementValueForCanvas, out ptpMode_byte); RosSharp.RosBridgeClient.MessageTypes.Dobot.SetPTPCmdRequest request = new RosSharp.RosBridgeClient.MessageTypes.Dobot. SetPTPCmdRequest(ptpMode_byte, target.x, target.y, target.z, 0, true); return(request); }
IEnumerator generateTestSpheres() { Vector3 dobotCoords1 = new Vector3(215, 4, 145); Vector3 dobotCoords2 = new Vector3(0, 200, 0); Vector3 dobotCoords3 = new Vector3(176, 24, 79); Vector3 dobotCoords4 = new Vector3(78, 200, 241); Vector3 dobotCoords5 = new Vector3(215, 24, 145); Vector3 dobotCoords6 = new Vector3(215, 200, 145); Vector3 dobotCoords7 = new Vector3(176, 200, 145); Vector3 dobotCoords8 = new Vector3(0, 200, 145); Vector3 dobotCoords9 = new Vector3(0, 200, 79); Vector3 dobotCoords10 = new Vector3(0, 200, 241); Vector3 dobotCoords11 = new Vector3(215, 24, 241); Vector3 dobotCoords12 = new Vector3(215, 24, 0); Vector3 dobotCoords13 = new Vector3(78, 200, 145); List <Vector3> dobotCoords = new List <Vector3>(); //dobotCoords = testSpheres(); RosSharp.RosBridgeClient.MessageTypes.Dobot.SetPTPCmdRequest request = createPTPRequest(1, dobotCoords1); dobotCoords.Add(dobotCoords1); dobotCoords.Add(dobotCoords2); dobotCoords.Add(dobotCoords3); dobotCoords.Add(dobotCoords4); dobotCoords.Add(dobotCoords5); dobotCoords.Add(dobotCoords6); dobotCoords.Add(dobotCoords7); dobotCoords.Add(dobotCoords8); dobotCoords.Add(dobotCoords9); dobotCoords.Add(dobotCoords10); dobotCoords.Add(dobotCoords11); dobotCoords.Add(dobotCoords12); dobotCoords.Add(dobotCoords13); Vector3 oldTransform = targetTransform.position; foreach (Vector3 dobotCoord in dobotCoords) { oldTransform = targetTransform.position; request = createPTPRequest(1, dobotCoord); client.sendPTPCommand(request); yield return(new WaitForSeconds(3.5f)); if (Vector3.Distance(oldTransform, targetTransform.position) > 0.00005f) { dobotCoordsReachable.Add(dobotCoord); //addSphere(dobotCoord); addSphere(convertedTransformUnity.localPosition); } } coordinateTargetSelector.writeRosNodesToFile(); }
private String generateProgramStringFromProgram(Program program) { String programString = ""; foreach (Statement statement in program.statements) { //debug Debug.Log("generateProgramStringFromProgram statement:" + statement); if (statement.GetType().IsSubclassOf(typeof(MoveStatement1))) { RosSharp.RosBridgeClient.MessageTypes.Dobot.SetPTPCmdRequest request; MoveStatement1 moveStatement = (MoveStatement1)statement; byte ptpMode_byte; byte.TryParse("" + moveStatement.movementValueForCanvas, out ptpMode_byte); float x = moveStatement.target.x; float y = moveStatement.target.y; float z = moveStatement.target.z; float r = 0f; request = new RosSharp.RosBridgeClient.MessageTypes.Dobot.SetPTPCmdRequest(ptpMode_byte, x, y, z, r, true); programString += $"Move({request.x} {request.y} {request.z});\n"; } //below is to generate Program string from Program for BruteMove. The string generated on UI will read 'Brute' else if (statement.GetType().IsSubclassOf(typeof(BruteMove))) { RosSharp.RosBridgeClient.MessageTypes.Dobot.SetPTPCmdRequest request; BruteMove bruteStatement = (BruteMove)statement; byte ptpMode_byte; byte.TryParse("" + bruteStatement.movementValueForCanvas, out ptpMode_byte); float x = bruteStatement.target.x; float y = bruteStatement.target.y; float z = bruteStatement.target.z; float r = 0f; request = new RosSharp.RosBridgeClient.MessageTypes.Dobot.SetPTPCmdRequest(ptpMode_byte, x, y, z, r, true); programString += $"Brute({request.x} {request.y} {request.z});\n"; } else { if (statement is ClawUp) { ClawUp clawUp = (ClawUp)statement; // programString += $"Claw({clawUp.isClawUp});\n"; if (clawUp.isClawUp == true) { programString += $"Claw({"up"});\n"; } else { programString += $"Claw({"down"});\n"; } } /* * if (statement is ToggleSuction) * { * // ToggleSuction toggleSuction = (ToggleSuction)statement; * //programString += $"ToggleSuction({toggleSuction.isSuctionEnabled});\n"; * programString += $"Move({0} {0} {0});\n"; * } */ } } //debug Debug.Log("generateProgramStringFromProgram with co-ordinates :" + programString); return(programString); }