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; } }