public void ParseAndResolveTypes()
{
GoalMessage.ParseAndResolveTypes();
GoalActionMessage.ParseAndResolveTypes();
ResultMessage.ParseAndResolveTypes();
ResultActionMessage.ParseAndResolveTypes();
FeedbackMessage.ParseAndResolveTypes();
FeedbackActionMessage.ParseAndResolveTypes();
}
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);
}
public ClientGoalHandle(
IActionClient <TGoal, TResult, TFeedback> actionClient,
GoalActionMessage <TGoal> goalAction,
Action <ClientGoalHandle <TGoal, TResult, TFeedback> > onTransitionCallback,
Action <ClientGoalHandle <TGoal, TResult, TFeedback>, FeedbackActionMessage <TFeedback> > onFeedbackCallback
)
{
this.actionClient = actionClient;
Id = goalAction.GoalId.id;
Goal = goalAction;
State = CommunicationState.WAITING_FOR_GOAL_ACK;
this.OnTransitionCallback = onTransitionCallback;
this.OnFeedbackCallback = onFeedbackCallback;
Active = true;
}
private void GoalCallback(GoalActionMessage <TGoal> goalAction)
{
if (!started)
{
return;
}
GoalID goalId = goalAction.GoalId;
ROS.Debug()($"[{ThisNode.Name}] [actionlib] The action server has received a new goal request");
ServerGoalHandle <TGoal, TResult, TFeedback> observedGoalHandle = null;
if (goalHandles.ContainsKey(goalId.id))
{
observedGoalHandle = goalHandles[goalId.id];
}
if (observedGoalHandle != null)
{
// The goal could already be in a recalling state if a cancel came in before the goal
if (observedGoalHandle.GoalStatus.status == GoalStatus.RECALLING)
{
observedGoalHandle.GoalStatus.status = GoalStatus.RECALLED;
PublishResult(observedGoalHandle.GoalStatus, null); // Empty result
}
}
else
{
// Create and register new goal handle
GoalStatus goalStatus = new GoalStatus();
goalStatus.status = GoalStatus.PENDING;
var newGoalHandle = new ServerGoalHandle <TGoal, TResult, TFeedback>(this, goalId,
goalStatus, goalAction.Goal
);
newGoalHandle.DestructionTime = ROS.GetTime(goalId.stamp);
//lock( lockGoalHandles )
//{
goalHandles[goalId.id] = newGoalHandle;
//}
goalCallback?.Invoke(newGoalHandle);
}
}
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);
}
