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