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);
}
}
public async static Task HandleStepwiseMotions(this ISteppedMotionClient client, IMoveGroup group, bool disposeClient = false)
{
try
{
const string channelName = "MotionDialog";
const string topic = "SteppedMotions";
const string disposition = "SteppedMotion";
using (var RosRoboChatClient = new RosRoboChatActionClient(group.MotionService.NodeHandle))
{
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();
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 });
}
}
}
finally
{
if (disposeClient)
{
client.Dispose();
}
}
}
