Skip to content

[Actionlib] Fix implementation to the latest implementation on rospy #3

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
wants to merge 2 commits into
base: master
Choose a base branch
from
Open

[Actionlib] Fix implementation to the latest implementation on rospy #3

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
332e649
[Actionlib] Update ActionServer implementation with correct SpinCallb…
wijanarko-sukma Jun 21, 2018
104dd6d
[Actionlib] Change UpdateStatus method to the correct implementation …
wijanarko-sukma Jun 21, 2018
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
15 changes: 9 additions & 6 deletions Uml.Robotics.Ros/ActionLib/ActionClient.cs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -197,7 +197,7 @@ public bool WaitForActionServerToStart(TimeSpan? timeout = null)
return false;
}
}
Thread.Sleep(1);
Thread.Sleep(10);
}

return false;
Expand Down Expand Up @@ -236,7 +236,7 @@ public bool WaitForActionServerToStart(TimeSpan? timeout = null)
return false;
}

await Task.Delay(1);
await Task.Delay(10);
}

return false;
Expand Down Expand Up @@ -267,7 +267,7 @@ public bool WaitForActionServerToStartSpinning(TimeSpan? timeout, SingleThreadSp
}

spinner.SpinOnce();
Thread.Sleep(1);
Thread.Sleep(10);
}

return false;
Expand Down Expand Up @@ -609,11 +609,12 @@ public async Task<TResult> SendGoalAsync(
private void UpdateStatus(ClientGoalHandle<TGoal, TResult, TFeedback> goalHandle, GoalStatus goalStatus)
{
// Check if ping action is correctly reflected by the status message
if (goalStatus != null)
if (goalHandle.State == CommunicationState.DONE)
{
goalHandle.LatestGoalStatus = goalStatus;
return;
}
else

if (goalStatus == null)
{
if ((goalHandle.State != CommunicationState.WAITING_FOR_GOAL_ACK) &&
(goalHandle.State != CommunicationState.WAITING_FOR_RESULT) &&
Expand All @@ -630,6 +631,8 @@ private void UpdateStatus(ClientGoalHandle<TGoal, TResult, TFeedback> goalHandle
}
}

goalHandle.LatestGoalStatus = goalStatus;

if (goalHandle.State == CommunicationState.WAITING_FOR_GOAL_ACK)
{
if (goalStatus.status == GoalStatus.PENDING)
Expand Down
71 changes: 39 additions & 32 deletions Uml.Robotics.Ros/ActionLib/ActionServer.cs
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,6 +1,7 @@
using System;
using System.Collections.Generic;
using System.Text;
using System.Threading;

using Messages.std_msgs;
using Uml.Robotics.Ros.ActionLib.Interfaces;
Expand All @@ -16,7 +17,7 @@ public class ActionServer<TGoal, TResult, TFeedback> : IActionServer<TGoal, TRes
{
public int QueueSize { get; set; } = 50;
public TimeSpan StatusListTimeout { get; private set; } = new TimeSpan(0, 0, 5);
public double StatusFrequencySeconds { get; private set; } = 5;
public double StatusFrequencyHz { get; private set; } = 5;

private const string ACTIONLIB_STATUS_FREQUENCY = "actionlib_status_frequency";
private const string STATUS_LIST_TIMEOUT = "status_list_timeout";
Expand All @@ -31,17 +32,17 @@ public class ActionServer<TGoal, TResult, TFeedback> : IActionServer<TGoal, TRes
private Publisher<GoalStatusArray> goalStatusPublisher;
private Subscriber goalSubscriber;
private Subscriber cancelSubscriber;
private TimeSpan statusInterval;
private DateTime nextStatusPublishTime;
private long spinCallbackId = 0;

private Timer timer;
private object lockGoalHandles;

public ActionServer(NodeHandle nodeHandle, string actionName)
{
this.goalHandles = new Dictionary<string, ServerGoalHandle<TGoal, TResult, TFeedback>>();
this.nodeHandle = new NodeHandle(nodeHandle, actionName);
this.lastCancel = DateTime.UtcNow;
this.started = false;
this.lockGoalHandles = new object();
}


Expand Down Expand Up @@ -101,29 +102,22 @@ public void Start()
// Read the frequency with which to publish status from the parameter server
// If not specified locally explicitly, use search param to find actionlib_status_frequency
double statusFrequency;
bool success = Param.Get(ACTIONLIB_STATUS_FREQUENCY, out statusFrequency);
bool success = Param.Get(ACTIONLIB_STATUS_FREQUENCY, out statusFrequency, 5.0);
if (success)
{
StatusFrequencySeconds = statusFrequency;
StatusFrequencyHz = statusFrequency;
}

double statusListTimeout;
success = Param.Get(STATUS_LIST_TIMEOUT, out statusListTimeout);
success = Param.Get(STATUS_LIST_TIMEOUT, out statusListTimeout, 5.0);
if (success)
{
var split = SplitSeconds(statusListTimeout);
StatusListTimeout = new TimeSpan(0, 0, split.seconds, split.milliseconds);
}

if (StatusFrequencySeconds > 0)
{
var split = SplitSeconds(StatusFrequencySeconds);
statusInterval = new TimeSpan(0, 0, split.seconds, split.milliseconds);
nextStatusPublishTime = DateTime.UtcNow + statusInterval;
var cb = new SpinCallbackImplementation(SpinCallback);
spinCallbackId = cb.Uid;
ROS.GlobalCallbackQueue.AddCallback(cb);
}
double statusFrequencySeconds = 1.0 / StatusFrequencyHz;
timer = new Timer(SpinCallback, null, 0, (int)(statusFrequencySeconds * 1000));

// Message consumers
goalSubscriber = nodeHandle.subscribe<GoalActionMessage<TGoal>>("goal", this.QueueSize, GoalCallback);
Expand Down Expand Up @@ -175,20 +169,27 @@ public void PublishStatus()
var goalStatuses = new List<GoalStatus>();

var idsToBeRemoved = new List<string>();
foreach (var pair in goalHandles)
{
goalStatuses.Add(pair.Value.GoalStatus);

if ((pair.Value.DestructionTime != null) && (pair.Value.DestructionTime + StatusListTimeout < now))
lock(lockGoalHandles)
{
foreach (var pair in goalHandles)
{
ROS.Debug()("actionlib", $"Removing server goal handle for goal id: {pair.Value.GoalId.id}");
idsToBeRemoved.Add(pair.Value.GoalId.id);
goalStatuses.Add(pair.Value.GoalStatus);

if ((pair.Value.DestructionTime != null) && (pair.Value.DestructionTime + StatusListTimeout < now))
{
ROS.Debug()("actionlib", $"Removing server goal handle for goal id: {pair.Value.GoalId.id}");
idsToBeRemoved.Add(pair.Value.GoalId.id);
}
}
}

foreach (string id in idsToBeRemoved)
{
goalHandles.Remove(id);
statusArray.status_list = goalStatuses.ToArray();
goalStatusPublisher.publish(statusArray);

foreach (string id in idsToBeRemoved)
{
goalHandles.Remove(id);
}
}
}

Expand Down Expand Up @@ -234,7 +235,10 @@ private void CancelCallback(GoalID goalId)
goalStatus.status = GoalStatus.RECALLING;
goalHandle = new ServerGoalHandle<TGoal, TResult, TFeedback>(this, goalId, goalStatus, null);
goalHandle.DestructionTime = ROS.GetTime(goalId.stamp);
goalHandles[goalId.id] = goalHandle;
lock(lockGoalHandles)
{
goalHandles[goalId.id] = goalHandle;
}
}

}
Expand Down Expand Up @@ -279,18 +283,21 @@ private void GoalCallback(GoalActionMessage<TGoal> goalAction)
var newGoalHandle = new ServerGoalHandle<TGoal, TResult, TFeedback>(this, goalId,
goalStatus, goalAction.Goal
);
goalHandles[goalId.id] = newGoalHandle;
newGoalHandle.DestructionTime = ROS.GetTime(goalId.stamp);
lock(lockGoalHandles)
{
goalHandles[goalId.id] = newGoalHandle;
}
goalCallback?.Invoke(newGoalHandle);
}
}


private void SpinCallback()
private void SpinCallback(object state)
{
if (DateTime.UtcNow > nextStatusPublishTime)
{
if (started)
{
PublishStatus();
nextStatusPublishTime = DateTime.UtcNow + statusInterval;
}
}

Expand Down