void IStepper.Reset()
{
TurtlesimPose message = new TurtlesimPose();
message.x = 0;
message.y = 0;
rosSocket.Publish(publication_id, message);
}
private static void subscriptionHandler(Message message)
{
TurtlesimPose geo = (TurtlesimPose)message;
Console.WriteLine("Väljast2:");
Console.WriteLine(geo.x);
xposition = geo.x;
yposition = geo.y;
}
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);
}
}
