Action Server Example
Please follow the instructions in Setup and run UE Project to setup the UE project and open ROS2ActionExample.umap.
C++ Action Server
This example send next action goal when received action result.
Code
//ROS2ActionServerNode.h
UCLASS()
class TURTLEBOT3_API AROS2ActionServerNode : public AActor
{
GENERATED_BODY()
public:
AROS2ActionServerNode();
virtual void BeginPlay() override;
UPROPERTY(EditAnywhere, BlueprintReadWrite)
UROS2NodeComponent* Node = nullptr;
UPROPERTY(EditAnywhere, BlueprintReadWrite)
UROS2ActionServer* FibonacciActionServer = nullptr;
UPROPERTY(EditAnywhere, BlueprintReadWrite)
FString ActionName = TEXT("fibonacci_action");
UPROPERTY()
FTimerHandle ActionTimerHandle;
UFUNCTION()
void Execute();
UFUNCTION()
void GoalCallback(UROS2GenericAction* InAction);
UFUNCTION()
void CancelCallback();
UFUNCTION()
void ResultCallback();
private:
FROSFibonacciFB FeedbackMsg;
FROSFibonacciSGReq GoalRequest;
int Count = 0;
};
//AROS2ActionServerNode.cpp
AROS2ActionServerNode::AROS2ActionServerNode()
{
Node = CreateDefaultSubobject<UROS2NodeComponent>(TEXT("ROS2NodeComponent"));
// these parameters can be change from BP
Node->Name = TEXT("action_server_node");
Node->Namespace = TEXT("cpp");
}
void AROS2ActionServerNode::BeginPlay()
{
Super::BeginPlay();
Node->Init();
ROS2_CREATE_ACTION_SERVER(Node,
this,
ActionName,
UROS2FibonacciAction::StaticClass(),
&AROS2ActionServerNode::GoalCallback,
&AROS2ActionServerNode::ResultCallback,
&AROS2ActionServerNode::CancelCallback,
FibonacciActionServer);
}
void AROS2ActionServerNode::Execute()
{
UROS2FibonacciAction* FibonacciAction = Cast<UROS2FibonacciAction>(FibonacciActionServer->Action);
// send feedback
if (Count++ <= GoalRequest.Order)
{
FeedbackMsg.Sequence.Add(FeedbackMsg.Sequence[Count] + FeedbackMsg.Sequence[Count - 1]);
FibonacciAction->SetFeedback(FeedbackMsg);
// Log request and response
UE_LOG_WITH_INFO_NAMED(
LogTurtlebot3, Log, TEXT("[%s][C++][update feedback] added %d"), *ActionName, FeedbackMsg.Sequence.Last(0));
FibonacciActionServer->SendFeedback();
}
// send result when finish by UpdateAndSendResult
else
{
// for log
FString resultString;
// set result
FROSFibonacciGRRes ResultResponse;
ResultResponse.GRResStatus = GOAL_STATE_SUCCEEDED;
for (auto s : FeedbackMsg.Sequence)
{
ResultResponse.Sequence.Add(s);
resultString += FString::FromInt(s) + ", ";
}
FibonacciAction->SetResultResponse(ResultResponse);
FibonacciActionServer->SendResultResponse();
// stop timer
GetWorld()->GetTimerManager().ClearTimer(ActionTimerHandle);
// Log request and response
UE_LOG_WITH_INFO_NAMED(LogTurtlebot3, Log, TEXT("[%s][C++][send result] result is: %s"), *ActionName, *resultString);
;
}
}
void AROS2ActionServerNode::GoalCallback(UROS2GenericAction* InAction)
{
// retrieve goal request value
UROS2FibonacciAction* FibonacciAction = Cast<UROS2FibonacciAction>(InAction);
FibonacciAction->GetGoalRequest(GoalRequest);
// set and send goal response
FROSFibonacciSGRes goalResponse;
goalResponse.bAccepted = true; // always accept goal
goalResponse.Stamp = UGameplayStatics::GetTimeSeconds(reinterpret_cast<UObject*>(GetWorld()));
Cast<UROS2FibonacciAction>(FibonacciActionServer->Action)->SetGoalResponse(goalResponse);
FibonacciActionServer->SendGoalResponse();
// Log request and response
UE_LOG_WITH_INFO_NAMED(LogTurtlebot3, Log, TEXT("[%s][C++][goal callback]"), *ActionName);
}
void AROS2ActionServerNode::CancelCallback()
{
// stop execution timer
GetWorld()->GetTimerManager().ClearTimer(ActionTimerHandle);
// send cancel response. always success
FibonacciActionServer->ProcessAndSendCancelResponse(FROSCancelGoalRes::ERROR_NONE);
// Log request and response
UE_LOG_WITH_INFO_NAMED(LogTurtlebot3, Log, TEXT("[%s][C++][cancle callback]"), *ActionName);
}
void AROS2ActionServerNode::ResultCallback()
{
// initialize feedback msg
Cast<UROS2FibonacciAction>(FibonacciActionServer->Action)->SetGoalIdToFeedback(FeedbackMsg);
FeedbackMsg.Sequence.Empty();
FeedbackMsg.Sequence.Add(0);
FeedbackMsg.Sequence.Add(1);
Count = 0;
// set timer to execute action.
GetWorld()->GetTimerManager().SetTimer(ActionTimerHandle, this, &AROS2ActionServerNode::Execute, 1.f, true);
// Log request and response
UE_LOG_WITH_INFO_NAMED(LogTurtlebot3, Log, TEXT("[%s][C++][result callback] Start fibonacci calculation"), *ActionName);
}
Examin the code
On an AROS2ActionServerNode Actor, similar to the AROS2PublisherrNode, NodeComponent is created and initialized in the constructor but ROS2 Node is not created here. Please check Code explanations for the reason.
AROS2ActionServerNode::AROS2ActionServerNode()
{
Node = CreateDefaultSubobject<UROS2NodeComponent>(TEXT("ROS2NodeComponent"));
// these parameters can be change from BP
Node->Name = TEXT("action_server_node");
Node->Namespace = TEXT("cpp");
}
When the simulation starts, BeginPlay is called. In BeginPlay, firstly create and initialize the ROS2 Node by calling UROS2NodeComponent::Init .
void AROS2ActionServerNode::BeginPlay()
{
Super::BeginPlay();
Node->Init();
You can create a action server by using the ROS2_CREATE_ACTION_SERVER macro, which creates a action server and adds it to the node. This macro bound Goal, Result and Cancel callback functions to the action server.
// Create Action server
ROS2_CREATE_ACTION_SERVER(Node,
this,
ActionName,
UROS2FibonacciAction::StaticClass(),
&AROS2ActionServerNode::GoalCallback,
&AROS2ActionServerNode::ResultCallback,
&AROS2ActionServerNode::CancelCallback,
FibonacciActionServer);
The implementation of ROS2_CREATE_ACTION_SERVER is as follows. It uses Unreal Engine’s dynamic delegate to call the bound function when the node receives the message. You can find more information about Unreal Engine’s dynamic delegate . here.
DECLARE_DYNAMIC_DELEGATE_OneParam(FActionCallback, UROS2GenericAction*, InAction /*Action*/);
#define ROS2_CREATE_ACTION_SERVER( \
InROS2Node, InUserObject, InActionName, InActionClass, InGoalDelegate, InResultDelegate, InCancelDelegate, OutServer) \
if (ensure(IsValid(InROS2Node))) \
{ \
FActionCallback Goal; \
FSimpleCallback Result, Cancel; \
Goal.BindDynamic(InUserObject, InGoalDelegate); \
Result.BindDynamic(InUserObject, InResultDelegate); \
Cancel.BindDynamic(InUserObject, InCancelDelegate); \
OutServer = InROS2Node->CreateActionServer(InActionName, InActionClass, Goal, Result, Cancel); \
}
AROS2ActionServerNode has GoalRequest and FeedbackMsg variables use data in Execute method.
private:
FROSFibonacciFB FeedbackMsg;
FROSFibonacciSGReq GoalRequest;
int Count = 0;
When the node receives a action Result/Goal/Cancel, corresponding callback function is called. for the corresponding action (UROS2FibonacciAction) and retrieve the goal by calling GetGoalRequest.
After setting response, send goal response by calling SendGoalResponse. In this example goal is always accepted.
void AROS2ActionServerNode::GoalCallback(UROS2GenericAction* InAction)
{
// retrieve goal request value
UROS2FibonacciAction* FibonacciAction = Cast<UROS2FibonacciAction>(InAction);
FibonacciAction->GetGoalRequest(GoalRequest);
// set and send goal response
FROSFibonacciSGRes goalResponse;
goalResponse.bAccepted = true; // always accept goal
goalResponse.Stamp = UGameplayStatics::GetTimeSeconds(reinterpret_cast<UObject*>(GetWorld()));
Cast<UROS2FibonacciAction>(FibonacciActionServer->Action)->SetGoalResponse(goalResponse);
FibonacciActionServer->SendGoalResponse();
// Log request and response
UE_LOG_WITH_INFO_NAMED(LogTurtlebot3, Log, TEXT("[%s][C++][goal callback]"), *ActionName);
}
ResultCallback intialize Feedback and Count and set timer to execute action.
void AROS2ActionServerNode::ResultCallback()
{
// initialize feedback msg
Cast<UROS2FibonacciAction>(FibonacciActionServer->Action)->SetGoalIdToFeedback(FeedbackMsg);
FeedbackMsg.Sequence.Empty();
FeedbackMsg.Sequence.Add(0);
FeedbackMsg.Sequence.Add(1);
Count = 0;
// set timer to execute action.
GetWorld()->GetTimerManager().SetTimer(ActionTimerHandle, this, &AROS2ActionServerNode::Execute, 1.f, true);
// Log request and response
UE_LOG_WITH_INFO_NAMED(LogTurtlebot3, Log, TEXT("[%s][C++][result callback] Start fibonacci calculation"), *ActionName);
}
Execute is periodically called by timer started by ResultCallback. In Execute, If Count is less than Order, execute fibonacci calculation, send feedback and increment Count. If Count reach Order, send result response by calling SendResultResponse and stop timer.
void AROS2ActionServerNode::Execute()
{
UROS2FibonacciAction* FibonacciAction = Cast<UROS2FibonacciAction>(FibonacciActionServer->Action);
// send feedback
if (Count++ <= GoalRequest.Order)
{
FeedbackMsg.Sequence.Add(FeedbackMsg.Sequence[Count] + FeedbackMsg.Sequence[Count - 1]);
FibonacciAction->SetFeedback(FeedbackMsg);
// Log request and response
UE_LOG_WITH_INFO_NAMED(
LogTurtlebot3, Log, TEXT("[%s][C++][update feedback] added %d"), *ActionName, FeedbackMsg.Sequence.Last(0));
FibonacciActionServer->SendFeedback();
}
// send result when finish by UpdateAndSendResult
else
{
// for log
FString resultString;
// set result
FROSFibonacciGRRes ResultResponse;
ResultResponse.GRResStatus = GOAL_STATE_SUCCEEDED;
for (auto s : FeedbackMsg.Sequence)
{
ResultResponse.Sequence.Add(s);
resultString += FString::FromInt(s) + ", ";
}
FibonacciAction->SetResultResponse(ResultResponse);
FibonacciActionServer->SendResultResponse();
// stop timer
GetWorld()->GetTimerManager().ClearTimer(ActionTimerHandle);
// Log request and response
UE_LOG_WITH_INFO_NAMED(LogTurtlebot3, Log, TEXT("[%s][C++][send result] result is: %s"), *ActionName, *resultString);
;
}
}
When cancel request is received, stop timer and send cancel response.
void AROS2ActionServerNode::CancelCallback()
{
// stop execution timer
GetWorld()->GetTimerManager().ClearTimer(ActionTimerHandle);
// send cancel response. always success
FibonacciActionServer->ProcessAndSendCancelResponse(FROSCancelGoalRes::ERROR_NONE);
// Log request and response
UE_LOG_WITH_INFO_NAMED(LogTurtlebot3, Log, TEXT("[%s][C++][cancle callback]"), *ActionName);
}
BP Action Server
Blueprint implementation of a action server is very similar to a C++ implementation. Blueprints allow you to set logic/processes, parameters, and other details from the editor.
You can add component such as UROS2Publisher from Components panel in the editor(left side in the fig below) and set each component parameters in Details panel in the editor(right side in the fig below).
The main difference from the C++ implementation is that it uses UROS2ActionServerComponent instead of UROS2ActionServer. As UROS2ActionServerComponent is a child class of UActorComponent and has UROS2ActionServer as a member variable, you can easily add it to the Actor and set parameters from the editor.
The Action server component is attached to an Actor, which is displayed in the Components panel on the left.
Initialize the ROS2 Node using the BeginPlay event. You can set the ROSNode parameters, such as Name and Namespace, from the Details panel on the right.
Compared to C++, which uses ROS2_CREATE_ACTION_SERVER, in Blueprint, the action server is already generated as a Component before BeginPlay. Therefore, we use UROS2NodeComponent::AddActionServer to initialize the Subscriber and UROS2ActionServer::SetDelegates to bind callback methods instead. The ROS2_CREATE_ACTION_SERVER macro in C++ internally calls CreateActionServer which calls AddActionServer and SetDelegates.
GoalCallback send goal response by calling SendGoalResponse. In this example goal is always accepted.
Result Callback method initialize the feedback and start timer to periodically calls Execute function.
Execute function update fibonacci sequence and send feedback or result. Top white block, update fibonacci sequence,
Center SendFeedback block is executed if current count is less than order.
Bottom SendResult is executed if current count is equal to order.
CancelCallback clear the timer and send cancel response.