async static Task UpdateProgress(
ISteppedMotionClient client,
RosRoboChatActionClient roboChatclient,
string channelName,
string messageId,
string disposition,
string moveGroupName,
CancellationToken cancel
)
{
await Task.Yield();
double lastProgress = 0.0;
while (!client.GoalTask.IsCanceled && !client.GoalTask.IsFaulted && !client.GoalTask.IsCompleted)
{
var state = client.MotionState; // read immutable state snapshot once
if (state.Progress != lastProgress)
{
lastProgress = state.Progress;
var messageBody = new SteppedMotionMessage {
GoalId = client.GoalId, Progress = lastProgress, MoveGroupName = moveGroupName
};
roboChatclient.CallMessageCommand(channelName, "update", messageBody.ToString(), messageId, new string[] { disposition });
}
await Task.Delay(100, cancel);
}
}
private async static Task HandleStepwiseMotions(ISteppedMotionClient client, IMoveGroup group)
{
string channelName = "MotionDialog";
string topic = "SteppedMotions";
string disposition = "SteppedMotion";
using (var RosRoboChatClient = new RosRoboChatActionClient(rosClient.GlobalNodeHandle))
{
RosRoboChatClient.CreateChat(channelName, topic);
var messageBody = new SteppedMotionMessage {
GoalId = client.GoalId, Progress = 0, MoveGroupName = group.Name
}.ToString();
string messageId = RosRoboChatClient.CallMessageCommand(channelName, "add", messageBody, null, new string[] { disposition });
try
{
var cancelReport = new CancellationTokenSource();
xamlamoveit.StepwiseMoveJResult result;
var updateProgressTask = UpdateProgress(client, RosRoboChatClient, channelName, messageId, disposition, group.Name, cancelReport.Token);
try
{
result = await client.GoalTask;
}
finally
{
cancelReport.Cancel();
await updateProgressTask.WhenCompleted();
}
if (result.result < 0)
{
throw new Exception($"SteppedMotion was not finished: { result.result }");
}
}
finally
{
RosRoboChatClient.CallMessageCommand(channelName, "remove", null, messageId, new string[] { disposition });
}
}
}
