public void ParseAndResolveTypes()
{
GoalMessage.ParseAndResolveTypes();
GoalActionMessage.ParseAndResolveTypes();
ResultMessage.ParseAndResolveTypes();
ResultActionMessage.ParseAndResolveTypes();
FeedbackMessage.ParseAndResolveTypes();
FeedbackActionMessage.ParseAndResolveTypes();
}
public void PublishResult(GoalStatus goalStatus, TResult result)
{
var newResult = new ResultActionMessage <TResult>();
newResult.Header = new Messages.std_msgs.Header();
newResult.Header.stamp = ROS.GetTime();
newResult.GoalStatus = goalStatus;
if (result != null)
{
newResult.Result = result;
}
ROS.Debug()($"[{ThisNode.Name}] [actionlib] Publishing result for goal with id: {goalStatus.goal_id.id} and stamp: {new DateTimeOffset( ROS.GetTime( goalStatus.goal_id.stamp ) ).ToUnixTimeSeconds()}");
resultPublisher.publish(newResult);
PublishStatus();
}
private void OnResultMessage(ResultActionMessage <TResult> result)
{
string goalId = result.GoalStatus.goal_id.id;
ROS.Debug()("OnResultMessage (goal_id: {0}, status: {1})", goalId, result.GoalStatus.status);
ClientGoalHandle <TGoal, TResult, TFeedback> goalHandle;
bool goalExists;
lock (gate)
{
goalExists = goalHandles.TryGetValue(result.GoalStatus.goal_id.id, out goalHandle);
}
if (goalExists)
{
ROS.Debug()("Processing result for known goal handle. (goal_id: {0})", goalId);
goalHandle.LatestGoalStatus = result.GoalStatus;
goalHandle.LatestResultAction = result;
if (goalHandle.State == CommunicationState.DONE)
{
ROS.Error()("Got a result when we were already in the DONE state (goal_id: {0})", goalId);
}
else if (goalHandle.State == CommunicationState.WAITING_FOR_GOAL_ACK ||
goalHandle.State == CommunicationState.WAITING_FOR_GOAL_ACK ||
goalHandle.State == CommunicationState.PENDING ||
goalHandle.State == CommunicationState.ACTIVE ||
goalHandle.State == CommunicationState.WAITING_FOR_RESULT ||
goalHandle.State == CommunicationState.WAITING_FOR_CANCEL_ACK ||
goalHandle.State == CommunicationState.RECALLING ||
goalHandle.State == CommunicationState.PREEMPTING)
{
UpdateStatus(goalHandle, result.GoalStatus);
TransitionToState(goalHandle, CommunicationState.DONE);
}
else
{
ROS.Error()($"Invalid comm for result message state: {goalHandle.State}.");
}
}
else
{
ROS.Debug()("Ignoring result for unknown goal (goal_id: {0})", goalId);
}
}
public void Should_ProduceCorrectMd5SumsForActionMessageClasses()
{
var inner = new Messages.control_msgs.FollowJointTrajectoryGoal();
var outer = new GoalActionMessage <Messages.control_msgs.FollowJointTrajectoryGoal> ();
var innerMd5 = inner.MD5Sum();
var outerMd5 = outer.MD5Sum();
var definition = outer.MessageDefinition();
Assert.Equal("69636787b6ecbde4d61d711979bc7ecb", innerMd5);
Assert.Equal("cff5c1d533bf2f82dd0138d57f4304bb", outerMd5);
var inner2 = new Messages.control_msgs.FollowJointTrajectoryResult();
var outer2 = new ResultActionMessage <Messages.control_msgs.FollowJointTrajectoryResult>();
var innerMd52 = inner2.MD5Sum();
var outerMd52 = outer2.MD5Sum();
var definition2 = outer2.MessageDefinition();
Assert.Equal("493383b18409bfb604b4e26c676401d2", innerMd52);
Assert.Equal("c4fb3b000dc9da4fd99699380efcc5d9", outerMd52);
var inner3 = new Messages.control_msgs.FollowJointTrajectoryFeedback();
var outer3 = new FeedbackActionMessage <Messages.control_msgs.FollowJointTrajectoryFeedback>();
var innerMd53 = inner3.MD5Sum();
var outerMd53 = outer3.MD5Sum();
var definition3 = outer3.MessageDefinition();
Assert.Equal("10817c60c2486ef6b33e97dcd87f4474", innerMd53);
Assert.Equal("d8920dc4eae9fc107e00999cce4be641", outerMd53);
var inner4 = new Messages.control_msgs.SingleJointPositionResult();
var outer4 = new ResultActionMessage <Messages.control_msgs.SingleJointPositionResult>();
var innerMd54 = inner4.MD5Sum();
var outerMd54 = outer4.MD5Sum();
var definition4 = outer4.MessageDefinition();
Assert.Equal("d41d8cd98f00b204e9800998ecf8427e", innerMd54);
Assert.Equal("1eb06eeff08fa7ea874431638cb52332", outerMd54);
}
private void OnResultMessage(ResultActionMessage <TResult> result)
{
ClientGoalHandle <TGoal, TResult, TFeedback> goalHandle;
bool goalExists;
lock (lockGoalHandles)
{
goalExists = goalHandles.TryGetValue(result.GoalStatus.goal_id.id, out goalHandle);
}
if (goalExists)
{
goalHandle.LatestGoalStatus = result.GoalStatus;
goalHandle.LatestResultAction = result;
if (goalHandle.State == CommunicationState.DONE)
{
ROS.Error()(
"Got a result when we were already in the DONE state (goal_id:" +
$" {result.GoalStatus.goal_id.id})"
);
}
else if ((goalHandle.State == CommunicationState.WAITING_FOR_GOAL_ACK) ||
(goalHandle.State == CommunicationState.WAITING_FOR_GOAL_ACK) ||
(goalHandle.State == CommunicationState.PENDING) ||
(goalHandle.State == CommunicationState.ACTIVE) ||
(goalHandle.State == CommunicationState.WAITING_FOR_RESULT) ||
(goalHandle.State == CommunicationState.WAITING_FOR_CANCEL_ACK) ||
(goalHandle.State == CommunicationState.RECALLING) ||
(goalHandle.State == CommunicationState.PREEMPTING))
{
UpdateStatus(goalHandle, result.GoalStatus);
TransitionToState(goalHandle, CommunicationState.DONE);
}
else
{
ROS.Error()($"Invalid comm for result message state: {goalHandle.State}.");
}
}
}
