public CompoundTerm DoAction(CompoundTerm action)
{
switch (action.Name)
{
case ("Tests"): return(null);
//Liigutame konna
case ("MoveTurtle"):
GeometryTwist message = new GeometryTwist();
var speed = (float)action[0];
var angleX = (float)action[1];
if (speed <= angleX)
{
message.linear.x = speed;
}
else
{
message.linear.x = 7.0f;
message.angular.z = 7.0f;
}
rosSocket.Publish(publish_vel, message);
isCollision = false;
return(null);
//Kontrollime ette antud aja pärast kas konn on jõudnud kohale
case ("checkCollision"):
Console.WriteLine(isCollision);
var task = Task.Run(() => moveOrFail());
if (task.Wait(TimeSpan.FromSeconds(10)))
{
return(task.Result);
}
else
{
throw new Exception("Timed out");
}
default: throw new Exception("Unexpected action " + action);
}
}
public CompoundTerm DoAction(CompoundTerm action)
{
switch (action.Name)
{
case ("Tests"): return(null);
case ("StartPosition"):
xposition = 0;
TurtlesimPose messageP = new TurtlesimPose();
messageP.x = 0;
messageP.y = 0;
messageP.theta = 0;
messageP.angular_velocity = 0;
messageP.linear_velocity = 0;
rosSocket.Publish(publication_id, messageP);
return(null);
case ("MoveTurtle"):
GeometryTwist message = new GeometryTwist();
float speed = (float)action[0];
message.linear.x = speed;
rosSocket.Publish(publication_id, message);
return(null);
case ("Goal_Start"):
if (xposition > 0)
{
return(NModel.Terms.Action.Create("Goal_Finish"));
}
else
{
return(NModel.Terms.Action.Create("Goal_Failed"));
}
default: throw new Exception("Unexpected action " + action);
}
}
public GeometryTwistWithCovariance()
{
twist = new GeometryTwist();
covariance = new float[32];
}
