public void Shutdown()
{
statusSubscriber.shutdown();
feedbackSubscriber.shutdown();
resultSubscriber.shutdown();
GoalPublisher.shutdown();
CancelPublisher.shutdown();
nodeHandle.shutdown();
}
public void Shutdown()
{
ROS.RosShuttingDown -= ROS_RosShuttingDown;
statusSubscriber.Dispose();
feedbackSubscriber.Dispose();
resultSubscriber.Dispose();
GoalPublisher.Dispose();
CancelPublisher.Dispose();
nodeHandle.Dispose();
}
public ClientGoalHandle <TGoal, TResult, TFeedback> SendGoal(
TGoal goal,
Action <ClientGoalHandle <TGoal, TResult, TFeedback> > OnTransistionCallback = null,
Action <ClientGoalHandle <TGoal, TResult, TFeedback>, FeedbackActionMessage <TFeedback> > OnFeedbackCallback = null
)
{
// Create Goal Message;
var goalId = new GoalID();
lock (lockId)
{
var now = ROS.GetTime();
// Create sortable goal id
goalId.id = $"{ThisNode.Name}-{nextGoalId:x08}-{now.data.sec:x08}.{now.data.nsec:x08}";
goalId.stamp = now;
nextGoalId = nextGoalId + 1;
}
// Prepare Goal Message
var goalAction = new GoalActionMessage <TGoal>
{
Header = new Messages.std_msgs.Header
{
stamp = ROS.GetTime()
},
GoalId = goalId,
Goal = goal
};
// Register goal message
var goalHandle = new ClientGoalHandle <TGoal, TResult, TFeedback>(
this,
goalAction,
OnTransistionCallback,
OnFeedbackCallback
);
lock (gate)
{
goalHandles[goalAction.GoalId.id] = goalHandle;
}
// Publish goal message
GoalPublisher.Publish(goalAction);
ROS.Debug()("Goal published: {0}", goalHandle.Id);
return(goalHandle);
}
private void OnGoalConnectCallback(SingleSubscriberPublisher publisher)
{
List <ClientGoalHandle <TGoal, TResult, TFeedback> > unacknowledgedGoalHandles;
lock (gate)
{
bool keyExists = goalSubscriberCount.TryGetValue(publisher.SubscriberName, out int subscriberCount);
goalSubscriberCount[publisher.SubscriberName] = (keyExists ? subscriberCount : 0) + 1;
ROS.Debug()($"goalConnectCallback: Adding {publisher.SubscriberName} to goalSubscribers");
// check if we have unacknowledged goals (the action server might have missed the goal message when it was disconnected)
unacknowledgedGoalHandles = goalHandles.Values
.Where(x => x.State == CommunicationState.WAITING_FOR_GOAL_ACK)
.ToList();
}
foreach (var gh in unacknowledgedGoalHandles)
{
ROS.Debug()("Republishing unacknowledged goal: {0}", gh.Id);
GoalPublisher.Publish(gh.Goal);
gh.statusMissing = 0;
}
}
