niklas/rclcpp
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

update to use separated action types (#601)

Browse source 
* match renamed action types

* fix action type casting

* rename type/field to use correct term

* rename custom GoalID type to avoid naming collision, update types using unique_identifier_msgs

* remove obsolete comments

* change signature of set_succeeded / set_canceled

* change signature of     on_terminal_state_(uuid_, result_msg);set_succeeded / set_canceled

* change signature of set_aborted

* change signature of publish_feedback

* update another test
This commit is contained in:
Michel Hidalgo 2019-03-11 21:12:47 -07:00 • committed by Dirk Thomas
parent d2d9ad8796
commit 718d24f942
11 changed files with 220 additions and 219 deletions
Download patch file Download diff file Expand all files Collapse all files

78
rclcpp_action/include/rclcpp_action/client.hpp
View file

@ -148,7 +148,7 @@ protected:
/// \internal /// \internal
RCLCPP_ACTION_PUBLIC RCLCPP_ACTION_PUBLIC
virtual virtual
GoalID GoalUUID
generate_goal_id(); generate_goal_id();
/// \internal /// \internal
@ -260,10 +260,10 @@ public:
using Goal = typename ActionT::Goal; using Goal = typename ActionT::Goal;
using Feedback = typename ActionT::Feedback; using Feedback = typename ActionT::Feedback;
using GoalHandle = ClientGoalHandle<ActionT>; using GoalHandle = ClientGoalHandle<ActionT>;
using Result = typename GoalHandle::Result; using WrappedResult = typename GoalHandle::WrappedResult;
using FeedbackCallback = typename ClientGoalHandle<ActionT>::FeedbackCallback; using FeedbackCallback = typename ClientGoalHandle<ActionT>::FeedbackCallback;
using CancelRequest = typename ActionT::CancelGoalService::Request; using CancelRequest = typename ActionT::Impl::CancelGoalService::Request;
using CancelResponse = typename ActionT::CancelGoalService::Response; using CancelResponse = typename ActionT::Impl::CancelGoalService::Response;
Client( Client(
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base,
@ -286,26 +286,23 @@ public:
// Put promise in the heap to move it around. // Put promise in the heap to move it around.
auto promise = std::make_shared<std::promise<typename GoalHandle::SharedPtr>>(); auto promise = std::make_shared<std::promise<typename GoalHandle::SharedPtr>>();
std::shared_future<typename GoalHandle::SharedPtr> future(promise->get_future()); std::shared_future<typename GoalHandle::SharedPtr> future(promise->get_future());
using GoalRequest = typename ActionT::GoalRequestService::Request; using GoalRequest = typename ActionT::Impl::SendGoalService::Request;
// auto goal_request = std::make_shared<GoalRequest>(); auto goal_request = std::make_shared<GoalRequest>();
// goal_request->goal_id = this->generate_goal_id(); goal_request->goal_id.uuid = this->generate_goal_id();
// goal_request->goal = goal; goal_request->goal = goal;
auto goal_request = std::make_shared<GoalRequest>(goal);
goal_request->action_goal_id.uuid = this->generate_goal_id();
this->send_goal_request( this->send_goal_request(
std::static_pointer_cast<void>(goal_request), std::static_pointer_cast<void>(goal_request),
[this, goal_request, callback, ignore_result, promise]( [this, goal_request, callback, ignore_result, promise](
std::shared_ptr<void> response) mutable std::shared_ptr<void> response) mutable
{ {
using GoalResponse = typename ActionT::GoalRequestService::Response; using GoalResponse = typename ActionT::Impl::SendGoalService::Response;
auto goal_response = std::static_pointer_cast<GoalResponse>(response); auto goal_response = std::static_pointer_cast<GoalResponse>(response);
if (!goal_response->accepted) { if (!goal_response->accepted) {
promise->set_value(nullptr); promise->set_value(nullptr);
return; return;
} }
GoalInfo goal_info; GoalInfo goal_info;
// goal_info.goal_id = goal_request->goal_id; goal_info.goal_id.uuid = goal_request->goal_id.uuid;
goal_info.goal_id.uuid = goal_request->action_goal_id.uuid;
goal_info.stamp = goal_response->stamp; goal_info.stamp = goal_response->stamp;
// Do not use std::make_shared as friendship cannot be forwarded. // Do not use std::make_shared as friendship cannot be forwarded.
std::shared_ptr<GoalHandle> goal_handle(new GoalHandle(goal_info, callback)); std::shared_ptr<GoalHandle> goal_handle(new GoalHandle(goal_info, callback));
@ -324,7 +321,7 @@ public:
return future; return future;
} }
std::shared_future<Result> std::shared_future<WrappedResult>
async_get_result(typename GoalHandle::SharedPtr goal_handle) async_get_result(typename GoalHandle::SharedPtr goal_handle)
{ {
std::lock_guard<std::mutex> lock(goal_handles_mutex_); std::lock_guard<std::mutex> lock(goal_handles_mutex_);
@ -406,7 +403,7 @@ private:
std::shared_ptr<void> std::shared_ptr<void>
create_goal_response() const override create_goal_response() const override
{ {
using GoalResponse = typename ActionT::GoalRequestService::Response; using GoalResponse = typename ActionT::Impl::SendGoalService::Response;
return std::shared_ptr<void>(new GoalResponse()); return std::shared_ptr<void>(new GoalResponse());
} }
@ -414,7 +411,7 @@ private:
std::shared_ptr<void> std::shared_ptr<void>
create_result_response() const override create_result_response() const override
{ {
using GoalResultResponse = typename ActionT::GoalResultService::Response; using GoalResultResponse = typename ActionT::Impl::GetResultService::Response;
return std::shared_ptr<void>(new GoalResultResponse()); return std::shared_ptr<void>(new GoalResultResponse());
} }
@ -429,9 +426,8 @@ private:
std::shared_ptr<void> std::shared_ptr<void>
create_feedback_message() const override create_feedback_message() const override
{ {
// using FeedbackMessage = typename ActionT::FeedbackMessage; using FeedbackMessage = typename ActionT::Impl::FeedbackMessage;
// return std::shared_ptr<void>(new FeedbackMessage()); return std::shared_ptr<void>(new FeedbackMessage());
return std::shared_ptr<void>(new Feedback());
} }
/// \internal /// \internal
@ -439,13 +435,10 @@ private:
handle_feedback_message(std::shared_ptr<void> message) override handle_feedback_message(std::shared_ptr<void> message) override
{ {
std::lock_guard<std::mutex> guard(goal_handles_mutex_); std::lock_guard<std::mutex> guard(goal_handles_mutex_);
// using FeedbackMessage = typename ActionT::FeedbackMessage; using FeedbackMessage = typename ActionT::Impl::FeedbackMessage;
// typename FeedbackMessage::SharedPtr feedback_message = typename FeedbackMessage::SharedPtr feedback_message =
// std::static_pointer_cast<FeedbackMessage>(message); std::static_pointer_cast<FeedbackMessage>(message);
typename Feedback::SharedPtr feedback_message = const GoalUUID & goal_id = feedback_message->goal_id.uuid;
std::static_pointer_cast<Feedback>(message);
// const GoalID & goal_id = feedback_message->goal_id;
const GoalID & goal_id = feedback_message->action_goal_id.uuid;
if (goal_handles_.count(goal_id) == 0) { if (goal_handles_.count(goal_id) == 0) {
RCLCPP_DEBUG( RCLCPP_DEBUG(
this->get_logger(), this->get_logger(),
@ -453,15 +446,16 @@ private:
return; return;
} }
typename GoalHandle::SharedPtr goal_handle = goal_handles_[goal_id]; typename GoalHandle::SharedPtr goal_handle = goal_handles_[goal_id];
// goal_handle->call_feedback_callback(goal_handle, feedback_message->feedback); auto feedback = std::make_shared<Feedback>();
goal_handle->call_feedback_callback(goal_handle, feedback_message); *feedback = feedback_message->feedback;
goal_handle->call_feedback_callback(goal_handle, feedback);
} }
/// \internal /// \internal
std::shared_ptr<void> std::shared_ptr<void>
create_status_message() const override create_status_message() const override
{ {
using GoalStatusMessage = typename ActionT::GoalStatusMessage; using GoalStatusMessage = typename ActionT::Impl::GoalStatusMessage;
return std::shared_ptr<void>(new GoalStatusMessage()); return std::shared_ptr<void>(new GoalStatusMessage());
} }
@ -470,11 +464,10 @@ private:
handle_status_message(std::shared_ptr<void> message) override handle_status_message(std::shared_ptr<void> message) override
{ {
std::lock_guard<std::mutex> guard(goal_handles_mutex_); std::lock_guard<std::mutex> guard(goal_handles_mutex_);
using GoalStatusMessage = typename ActionT::GoalStatusMessage; using GoalStatusMessage = typename ActionT::Impl::GoalStatusMessage;
auto status_message = std::static_pointer_cast<GoalStatusMessage>(message); auto status_message = std::static_pointer_cast<GoalStatusMessage>(message);
for (const GoalStatus & status : status_message->status_list) { for (const GoalStatus & status : status_message->status_list) {
// const GoalID & goal_id = status.goal_info.goal_id; const GoalUUID & goal_id = status.goal_info.goal_id.uuid;
const GoalID & goal_id = status.goal_info.goal_id.uuid;
if (goal_handles_.count(goal_id) == 0) { if (goal_handles_.count(goal_id) == 0) {
RCLCPP_DEBUG( RCLCPP_DEBUG(
this->get_logger(), this->get_logger(),
@ -498,21 +491,22 @@ private:
void void
make_result_aware(typename GoalHandle::SharedPtr goal_handle) make_result_aware(typename GoalHandle::SharedPtr goal_handle)
{ {
using GoalResultRequest = typename ActionT::GoalResultService::Request; using GoalResultRequest = typename ActionT::Impl::GetResultService::Request;
auto goal_result_request = std::make_shared<GoalResultRequest>(); auto goal_result_request = std::make_shared<GoalResultRequest>();
// goal_result_request.goal_id = goal_handle->get_goal_id(); goal_result_request->goal_id.uuid = goal_handle->get_goal_id();
goal_result_request->action_goal_id.uuid = goal_handle->get_goal_id();
this->send_result_request( this->send_result_request(
std::static_pointer_cast<void>(goal_result_request), std::static_pointer_cast<void>(goal_result_request),
[goal_handle, this](std::shared_ptr<void> response) mutable [goal_handle, this](std::shared_ptr<void> response) mutable
{ {
// Wrap the response in a struct with the fields a user cares about // Wrap the response in a struct with the fields a user cares about
Result result; WrappedResult wrapped_result;
using GoalResultResponse = typename ActionT::GoalResultService::Response; using GoalResultResponse = typename ActionT::Impl::GetResultService::Response;
result.response = std::static_pointer_cast<GoalResultResponse>(response); auto result_response = std::static_pointer_cast<GoalResultResponse>(response);
result.goal_id = goal_handle->get_goal_id(); wrapped_result.result = std::make_shared<typename ActionT::Result>();
result.code = static_cast<ResultCode>(result.response->action_status); *wrapped_result.result = result_response->result;
goal_handle->set_result(result); wrapped_result.goal_id = goal_handle->get_goal_id();
wrapped_result.code = static_cast<ResultCode>(result_response->status);
goal_handle->set_result(wrapped_result);
std::lock_guard<std::mutex> lock(goal_handles_mutex_); std::lock_guard<std::mutex> lock(goal_handles_mutex_);
goal_handles_.erase(goal_handle->get_goal_id()); goal_handles_.erase(goal_handle->get_goal_id());
}); });
@ -536,7 +530,7 @@ private:
return future; return future;
} }
std::map<GoalID, typename GoalHandle::SharedPtr> goal_handles_; std::map<GoalUUID, typename GoalHandle::SharedPtr> goal_handles_;
std::mutex goal_handles_mutex_; std::mutex goal_handles_mutex_;
}; };
} // namespace rclcpp_action } // namespace rclcpp_action

21
rclcpp_action/include/rclcpp_action/client_goal_handle.hpp
View file

@ -52,30 +52,31 @@ public:
RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(ClientGoalHandle) RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(ClientGoalHandle)
// A wrapper that defines the result of an action // A wrapper that defines the result of an action
typedef struct Result typedef struct WrappedResult
{ {
/// The unique identifier of the goal /// The unique identifier of the goal
GoalID goal_id; GoalUUID goal_id;
/// A status to indicate if the goal was canceled, aborted, or suceeded /// A status to indicate if the goal was canceled, aborted, or suceeded
ResultCode code; ResultCode code;
/// User defined fields sent back with an action /// User defined fields sent back with an action
typename ActionT::Result::SharedPtr response; typename ActionT::Result::SharedPtr result;
} Result; } WrappedResult;
using Feedback = typename ActionT::Feedback; using Feedback = typename ActionT::Feedback;
using FeedbackCallback = using FeedbackCallback =
std::function<void ( std::function<void (
typename ClientGoalHandle<ActionT>::SharedPtr, const std::shared_ptr<const Feedback>)>; typename ClientGoalHandle<ActionT>::SharedPtr,
const std::shared_ptr<const Feedback>)>;
virtual ~ClientGoalHandle(); virtual ~ClientGoalHandle();
const GoalID & const GoalUUID &
get_goal_id() const; get_goal_id() const;
rclcpp::Time rclcpp::Time
get_goal_stamp() const; get_goal_stamp() const;
std::shared_future<Result> std::shared_future<WrappedResult>
async_result(); async_result();
int8_t int8_t
@ -108,7 +109,7 @@ private:
set_status(int8_t status); set_status(int8_t status);
void void
set_result(const Result & result); set_result(const WrappedResult & wrapped_result);
void void
invalidate(); invalidate();
@ -116,8 +117,8 @@ private:
GoalInfo info_; GoalInfo info_;
bool is_result_aware_{false}; bool is_result_aware_{false};
std::promise<Result> result_promise_; std::promise<WrappedResult> result_promise_;
std::shared_future<Result> result_future_; std::shared_future<WrappedResult> result_future_;
FeedbackCallback feedback_callback_{nullptr}; FeedbackCallback feedback_callback_{nullptr};
int8_t status_{GoalStatus::STATUS_ACCEPTED}; int8_t status_{GoalStatus::STATUS_ACCEPTED};

10
rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp
View file

@ -39,7 +39,7 @@ ClientGoalHandle<ActionT>::~ClientGoalHandle()
} }
template<typename ActionT> template<typename ActionT>
const GoalID & const GoalUUID &
ClientGoalHandle<ActionT>::get_goal_id() const ClientGoalHandle<ActionT>::get_goal_id() const
{ {
// return info_.goal_id; // return info_.goal_id;
@ -54,7 +54,7 @@ ClientGoalHandle<ActionT>::get_goal_stamp() const
} }
template<typename ActionT> template<typename ActionT>
std::shared_future<typename ClientGoalHandle<ActionT>::Result> std::shared_future<typename ClientGoalHandle<ActionT>::WrappedResult>
ClientGoalHandle<ActionT>::async_result() ClientGoalHandle<ActionT>::async_result()
{ {
std::lock_guard<std::mutex> guard(handle_mutex_); std::lock_guard<std::mutex> guard(handle_mutex_);
@ -66,11 +66,11 @@ ClientGoalHandle<ActionT>::async_result()
template<typename ActionT> template<typename ActionT>
void void
ClientGoalHandle<ActionT>::set_result(const Result & result) ClientGoalHandle<ActionT>::set_result(const WrappedResult & wrapped_result)
{ {
std::lock_guard<std::mutex> guard(handle_mutex_); std::lock_guard<std::mutex> guard(handle_mutex_);
status_ = static_cast<int8_t>(result.code); status_ = static_cast<int8_t>(wrapped_result.code);
result_promise_.set_value(result); result_promise_.set_value(wrapped_result);
} }
template<typename ActionT> template<typename ActionT>

69
rclcpp_action/include/rclcpp_action/server.hpp
View file

@ -146,7 +146,7 @@ protected:
RCLCPP_ACTION_PUBLIC RCLCPP_ACTION_PUBLIC
virtual virtual
std::pair<GoalResponse, std::shared_ptr<void>> std::pair<GoalResponse, std::shared_ptr<void>>
call_handle_goal_callback(GoalID &, std::shared_ptr<void> request) = 0; call_handle_goal_callback(GoalUUID &, std::shared_ptr<void> request) = 0;
// ServerBase will determine which goal ids are being cancelled, and then call this function for // ServerBase will determine which goal ids are being cancelled, and then call this function for
// each goal id. // each goal id.
@ -155,13 +155,13 @@ protected:
RCLCPP_ACTION_PUBLIC RCLCPP_ACTION_PUBLIC
virtual virtual
CancelResponse CancelResponse
call_handle_cancel_callback(const GoalID & uuid) = 0; call_handle_cancel_callback(const GoalUUID & uuid) = 0;
/// Given a goal request message, return the UUID contained within. /// Given a goal request message, return the UUID contained within.
/// \internal /// \internal
RCLCPP_ACTION_PUBLIC RCLCPP_ACTION_PUBLIC
virtual virtual
GoalID GoalUUID
get_goal_id_from_goal_request(void * message) = 0; get_goal_id_from_goal_request(void * message) = 0;
/// Create an empty goal request message so it can be taken from a lower layer. /// Create an empty goal request message so it can be taken from a lower layer.
@ -178,13 +178,13 @@ protected:
void void
call_goal_accepted_callback( call_goal_accepted_callback(
std::shared_ptr<rcl_action_goal_handle_t> rcl_goal_handle, std::shared_ptr<rcl_action_goal_handle_t> rcl_goal_handle,
GoalID uuid, std::shared_ptr<void> goal_request_message) = 0; GoalUUID uuid, std::shared_ptr<void> goal_request_message) = 0;
/// Given a result request message, return the UUID contained within. /// Given a result request message, return the UUID contained within.
/// \internal /// \internal
RCLCPP_ACTION_PUBLIC RCLCPP_ACTION_PUBLIC
virtual virtual
GoalID GoalUUID
get_goal_id_from_result_request(void * message) = 0; get_goal_id_from_result_request(void * message) = 0;
/// Create an empty goal request message so it can be taken from a lower layer. /// Create an empty goal request message so it can be taken from a lower layer.
@ -214,7 +214,7 @@ protected:
/// \internal /// \internal
RCLCPP_ACTION_PUBLIC RCLCPP_ACTION_PUBLIC
void void
publish_result(const GoalID & uuid, std::shared_ptr<void> result_msg); publish_result(const GoalUUID & uuid, std::shared_ptr<void> result_msg);
/// \internal /// \internal
RCLCPP_ACTION_PUBLIC RCLCPP_ACTION_PUBLIC
@ -272,7 +272,7 @@ public:
/// Signature of a callback that accepts or rejects goal requests. /// Signature of a callback that accepts or rejects goal requests.
using GoalCallback = std::function<GoalResponse( using GoalCallback = std::function<GoalResponse(
const GoalID &, std::shared_ptr<const typename ActionT::Goal>)>; const GoalUUID &, std::shared_ptr<const typename ActionT::Goal>)>;
/// Signature of a callback that accepts or rejects requests to cancel a goal. /// Signature of a callback that accepts or rejects requests to cancel a goal.
using CancelCallback = std::function<CancelResponse(std::shared_ptr<ServerGoalHandle<ActionT>>)>; using CancelCallback = std::function<CancelResponse(std::shared_ptr<ServerGoalHandle<ActionT>>)>;
/// Signature of a callback that is used to notify when the goal has been accepted. /// Signature of a callback that is used to notify when the goal has been accepted.
@ -335,16 +335,14 @@ protected:
/// \internal /// \internal
std::pair<GoalResponse, std::shared_ptr<void>> std::pair<GoalResponse, std::shared_ptr<void>>
call_handle_goal_callback(GoalID & uuid, std::shared_ptr<void> message) override call_handle_goal_callback(GoalUUID & uuid, std::shared_ptr<void> message) override
{ {
// TODO(sloretz) update and remove assert when IDL pipeline allows nesting user's type auto request = std::static_pointer_cast<
static_assert( typename ActionT::Impl::SendGoalService::Request>(message);
std::is_same<typename ActionT::Goal, typename ActionT::GoalRequestService::Request>::value, auto goal = std::shared_ptr<typename ActionT::Goal>(request, &request->goal);
"Assuming user fields were merged with goal request fields"); GoalResponse user_response = handle_goal_(uuid, goal);
GoalResponse user_response = handle_goal_(
uuid, std::static_pointer_cast<typename ActionT::Goal>(message));
auto ros_response = std::make_shared<typename ActionT::GoalRequestService::Response>(); auto ros_response = std::make_shared<typename ActionT::Impl::SendGoalService::Response>();
ros_response->accepted = GoalResponse::ACCEPT_AND_EXECUTE == user_response || ros_response->accepted = GoalResponse::ACCEPT_AND_EXECUTE == user_response ||
GoalResponse::ACCEPT_AND_DEFER == user_response; GoalResponse::ACCEPT_AND_DEFER == user_response;
return std::make_pair(user_response, ros_response); return std::make_pair(user_response, ros_response);
@ -352,7 +350,7 @@ protected:
/// \internal /// \internal
CancelResponse CancelResponse
call_handle_cancel_callback(const GoalID & uuid) override call_handle_cancel_callback(const GoalUUID & uuid) override
{ {
std::lock_guard<std::mutex> lock(goal_handles_mutex_); std::lock_guard<std::mutex> lock(goal_handles_mutex_);
CancelResponse resp = CancelResponse::REJECT; CancelResponse resp = CancelResponse::REJECT;
@ -373,13 +371,13 @@ protected:
void void
call_goal_accepted_callback( call_goal_accepted_callback(
std::shared_ptr<rcl_action_goal_handle_t> rcl_goal_handle, std::shared_ptr<rcl_action_goal_handle_t> rcl_goal_handle,
GoalID uuid, std::shared_ptr<void> goal_request_message) override GoalUUID uuid, std::shared_ptr<void> goal_request_message) override
{ {
std::shared_ptr<ServerGoalHandle<ActionT>> goal_handle; std::shared_ptr<ServerGoalHandle<ActionT>> goal_handle;
std::weak_ptr<Server<ActionT>> weak_this = this->shared_from_this(); std::weak_ptr<Server<ActionT>> weak_this = this->shared_from_this();
std::function<void(const GoalID &, std::shared_ptr<void>)> on_terminal_state = std::function<void(const GoalUUID &, std::shared_ptr<void>)> on_terminal_state =
[weak_this](const GoalID & uuid, std::shared_ptr<void> result_message) [weak_this](const GoalUUID & uuid, std::shared_ptr<void> result_message)
{ {
std::shared_ptr<Server<ActionT>> shared_this = weak_this.lock(); std::shared_ptr<Server<ActionT>> shared_this = weak_this.lock();
if (!shared_this) { if (!shared_this) {
@ -396,8 +394,8 @@ protected:
shared_this->goal_handles_.erase(uuid); shared_this->goal_handles_.erase(uuid);
}; };
std::function<void(const GoalID &)> on_executing = std::function<void(const GoalUUID &)> on_executing =
[weak_this](const GoalID & uuid) [weak_this](const GoalUUID & uuid)
{ {
std::shared_ptr<Server<ActionT>> shared_this = weak_this.lock(); std::shared_ptr<Server<ActionT>> shared_this = weak_this.lock();
if (!shared_this) { if (!shared_this) {
@ -408,8 +406,8 @@ protected:
shared_this->publish_status(); shared_this->publish_status();
}; };
std::function<void(std::shared_ptr<typename ActionT::Feedback>)> publish_feedback = std::function<void(std::shared_ptr<typename ActionT::Impl::FeedbackMessage>)> publish_feedback =
[weak_this](std::shared_ptr<typename ActionT::Feedback> feedback_msg) [weak_this](std::shared_ptr<typename ActionT::Impl::FeedbackMessage> feedback_msg)
{ {
std::shared_ptr<Server<ActionT>> shared_this = weak_this.lock(); std::shared_ptr<Server<ActionT>> shared_this = weak_this.lock();
if (!shared_this) { if (!shared_this) {
@ -418,11 +416,12 @@ protected:
shared_this->publish_feedback(std::static_pointer_cast<void>(feedback_msg)); shared_this->publish_feedback(std::static_pointer_cast<void>(feedback_msg));
}; };
auto request = std::static_pointer_cast<
const typename ActionT::Impl::SendGoalService::Request>(goal_request_message);
auto goal = std::shared_ptr<const typename ActionT::Goal>(request, &request->goal);
goal_handle.reset( goal_handle.reset(
new ServerGoalHandle<ActionT>( new ServerGoalHandle<ActionT>(
rcl_goal_handle, uuid, rcl_goal_handle, uuid, goal, on_terminal_state, on_executing, publish_feedback));
std::static_pointer_cast<const typename ActionT::Goal>(goal_request_message),
on_terminal_state, on_executing, publish_feedback));
{ {
std::lock_guard<std::mutex> lock(goal_handles_mutex_); std::lock_guard<std::mutex> lock(goal_handles_mutex_);
goal_handles_[uuid] = goal_handle; goal_handles_[uuid] = goal_handle;
@ -431,41 +430,41 @@ protected:
} }
/// \internal /// \internal
GoalID GoalUUID
get_goal_id_from_goal_request(void * message) override get_goal_id_from_goal_request(void * message) override
{ {
return return
static_cast<typename ActionT::GoalRequestService::Request *>(message)->action_goal_id.uuid; static_cast<typename ActionT::Impl::SendGoalService::Request *>(message)->goal_id.uuid;
} }
/// \internal /// \internal
std::shared_ptr<void> std::shared_ptr<void>
create_goal_request() override create_goal_request() override
{ {
return std::shared_ptr<void>(new typename ActionT::GoalRequestService::Request()); return std::shared_ptr<void>(new typename ActionT::Impl::SendGoalService::Request());
} }
/// \internal /// \internal
GoalID GoalUUID
get_goal_id_from_result_request(void * message) override get_goal_id_from_result_request(void * message) override
{ {
return return
static_cast<typename ActionT::GoalResultService::Request *>(message)->action_goal_id.uuid; static_cast<typename ActionT::Impl::GetResultService::Request *>(message)->goal_id.uuid;
} }
/// \internal /// \internal
std::shared_ptr<void> std::shared_ptr<void>
create_result_request() override create_result_request() override
{ {
return std::shared_ptr<void>(new typename ActionT::GoalResultService::Request()); return std::shared_ptr<void>(new typename ActionT::Impl::GetResultService::Request());
} }
/// \internal /// \internal
std::shared_ptr<void> std::shared_ptr<void>
create_result_response(decltype(action_msgs::msg::GoalStatus::status) status) override create_result_response(decltype(action_msgs::msg::GoalStatus::status) status) override
{ {
auto result = std::make_shared<typename ActionT::GoalResultService::Response>(); auto result = std::make_shared<typename ActionT::Impl::GetResultService::Response>();
result->action_status = status; result->status = status;
return std::static_pointer_cast<void>(result); return std::static_pointer_cast<void>(result);
} }
@ -480,7 +479,7 @@ private:
using GoalHandleWeakPtr = std::weak_ptr<ServerGoalHandle<ActionT>>; using GoalHandleWeakPtr = std::weak_ptr<ServerGoalHandle<ActionT>>;
/// A map of goal id to goal handle weak pointers. /// A map of goal id to goal handle weak pointers.
/// This is used to provide a goal handle to handle_cancel. /// This is used to provide a goal handle to handle_cancel.
std::unordered_map<GoalID, GoalHandleWeakPtr> goal_handles_; std::unordered_map<GoalUUID, GoalHandleWeakPtr> goal_handles_;
std::mutex goal_handles_mutex_; std::mutex goal_handles_mutex_;
}; };
} // namespace rclcpp_action } // namespace rclcpp_action

50
rclcpp_action/include/rclcpp_action/server_goal_handle.hpp
View file

@ -147,8 +147,10 @@ public:
void void
publish_feedback(std::shared_ptr<typename ActionT::Feedback> feedback_msg) publish_feedback(std::shared_ptr<typename ActionT::Feedback> feedback_msg)
{ {
feedback_msg->action_goal_id.uuid = uuid_; auto feedback_message = std::make_shared<typename ActionT::Impl::FeedbackMessage>();
publish_feedback_(feedback_msg); feedback_message->goal_id.uuid = uuid_;
feedback_message->feedback = *feedback_msg;
publish_feedback_(feedback_message);
} }
// TODO(sloretz) which exception is raised? // TODO(sloretz) which exception is raised?
@ -165,8 +167,10 @@ public:
set_aborted(typename ActionT::Result::SharedPtr result_msg) set_aborted(typename ActionT::Result::SharedPtr result_msg)
{ {
_set_aborted(); _set_aborted();
result_msg->action_status = action_msgs::msg::GoalStatus::STATUS_ABORTED; auto response = std::make_shared<typename ActionT::Impl::GetResultService::Response>();
on_terminal_state_(uuid_, result_msg); response->status = action_msgs::msg::GoalStatus::STATUS_ABORTED;
response->result = *result_msg;
on_terminal_state_(uuid_, response);
} }
/// Indicate that a goal has been reached. /// Indicate that a goal has been reached.
@ -182,8 +186,10 @@ public:
set_succeeded(typename ActionT::Result::SharedPtr result_msg) set_succeeded(typename ActionT::Result::SharedPtr result_msg)
{ {
_set_succeeded(); _set_succeeded();
result_msg->action_status = action_msgs::msg::GoalStatus::STATUS_SUCCEEDED; auto response = std::make_shared<typename ActionT::Impl::GetResultService::Response>();
on_terminal_state_(uuid_, result_msg); response->status = action_msgs::msg::GoalStatus::STATUS_SUCCEEDED;
response->result = *result_msg;
on_terminal_state_(uuid_, response);
} }
/// Indicate that a goal has been canceled. /// Indicate that a goal has been canceled.
@ -199,8 +205,10 @@ public:
set_canceled(typename ActionT::Result::SharedPtr result_msg) set_canceled(typename ActionT::Result::SharedPtr result_msg)
{ {
_set_canceled(); _set_canceled();
result_msg->action_status = action_msgs::msg::GoalStatus::STATUS_CANCELED; auto response = std::make_shared<typename ActionT::Impl::GetResultService::Response>();
on_terminal_state_(uuid_, result_msg); response->status = action_msgs::msg::GoalStatus::STATUS_CANCELED;
response->result = *result_msg;
on_terminal_state_(uuid_, response);
} }
/// Indicate that the server is starting to execute a goal. /// Indicate that the server is starting to execute a goal.
@ -215,7 +223,7 @@ public:
on_executing_(uuid_); on_executing_(uuid_);
} }
/// Get the original request message describing the goal. /// Get the user provided message describing the goal.
const std::shared_ptr<const typename ActionT::Goal> const std::shared_ptr<const typename ActionT::Goal>
get_goal() const get_goal() const
{ {
@ -223,7 +231,7 @@ public:
} }
/// Get the unique identifier of the goal /// Get the unique identifier of the goal
const GoalID & const GoalUUID &
get_goal_id() const get_goal_id() const
{ {
return uuid_; return uuid_;
@ -233,8 +241,8 @@ public:
{ {
// Cancel goal if handle was allowed to destruct without reaching a terminal state // Cancel goal if handle was allowed to destruct without reaching a terminal state
if (try_canceling()) { if (try_canceling()) {
auto null_result = std::make_shared<typename ActionT::Result>(); auto null_result = std::make_shared<typename ActionT::Impl::GetResultService::Response>();
null_result->action_status = action_msgs::msg::GoalStatus::STATUS_CANCELED; null_result->status = action_msgs::msg::GoalStatus::STATUS_CANCELED;
on_terminal_state_(uuid_, null_result); on_terminal_state_(uuid_, null_result);
} }
} }
@ -243,11 +251,11 @@ protected:
/// \internal /// \internal
ServerGoalHandle( ServerGoalHandle(
std::shared_ptr<rcl_action_goal_handle_t> rcl_handle, std::shared_ptr<rcl_action_goal_handle_t> rcl_handle,
GoalID uuid, GoalUUID uuid,
std::shared_ptr<const typename ActionT::Goal> goal, std::shared_ptr<const typename ActionT::Goal> goal,
std::function<void(const GoalID &, std::shared_ptr<void>)> on_terminal_state, std::function<void(const GoalUUID &, std::shared_ptr<void>)> on_terminal_state,
std::function<void(const GoalID &)> on_executing, std::function<void(const GoalUUID &)> on_executing,
std::function<void(std::shared_ptr<typename ActionT::Feedback>)> publish_feedback std::function<void(std::shared_ptr<typename ActionT::Impl::FeedbackMessage>)> publish_feedback
) )
: ServerGoalHandleBase(rcl_handle), goal_(goal), uuid_(uuid), : ServerGoalHandleBase(rcl_handle), goal_(goal), uuid_(uuid),
on_terminal_state_(on_terminal_state), on_executing_(on_executing), on_terminal_state_(on_terminal_state), on_executing_(on_executing),
@ -255,17 +263,17 @@ protected:
{ {
} }
/// The original request message describing the goal. /// The user provided message describing the goal.
const std::shared_ptr<const typename ActionT::Goal> goal_; const std::shared_ptr<const typename ActionT::Goal> goal_;
/// A unique id for the goal request. /// A unique id for the goal request.
const GoalID uuid_; const GoalUUID uuid_;
friend Server<ActionT>; friend Server<ActionT>;
std::function<void(const GoalID &, std::shared_ptr<void>)> on_terminal_state_; std::function<void(const GoalUUID &, std::shared_ptr<void>)> on_terminal_state_;
std::function<void(const GoalID &)> on_executing_; std::function<void(const GoalUUID &)> on_executing_;
std::function<void(std::shared_ptr<typename ActionT::Feedback>)> publish_feedback_; std::function<void(std::shared_ptr<typename ActionT::Impl::FeedbackMessage>)> publish_feedback_;
}; };
} // namespace rclcpp_action } // namespace rclcpp_action

18
rclcpp_action/include/rclcpp_action/types.hpp
View file

@ -29,34 +29,34 @@
namespace rclcpp_action namespace rclcpp_action
{ {
using GoalID = std::array<uint8_t, UUID_SIZE>; using GoalUUID = std::array<uint8_t, UUID_SIZE>;
using GoalStatus = action_msgs::msg::GoalStatus; using GoalStatus = action_msgs::msg::GoalStatus;
using GoalInfo = action_msgs::msg::GoalInfo; using GoalInfo = action_msgs::msg::GoalInfo;
/// Convert a goal id to a human readable string. /// Convert a goal id to a human readable string.
RCLCPP_ACTION_PUBLIC RCLCPP_ACTION_PUBLIC
std::string std::string
to_string(const GoalID & goal_id); to_string(const GoalUUID & goal_id);
// Convert C++ GoalID to rcl_action_goal_info_t // Convert C++ GoalID to rcl_action_goal_info_t
RCLCPP_ACTION_PUBLIC RCLCPP_ACTION_PUBLIC
void void
convert(const GoalID & goal_id, rcl_action_goal_info_t * info); convert(const GoalUUID & goal_id, rcl_action_goal_info_t * info);
// Convert rcl_action_goal_info_t to C++ GoalID // Convert rcl_action_goal_info_t to C++ GoalID
RCLCPP_ACTION_PUBLIC RCLCPP_ACTION_PUBLIC
void void
convert(const rcl_action_goal_info_t & info, GoalID * goal_id); convert(const rcl_action_goal_info_t & info, GoalUUID * goal_id);
} // namespace rclcpp_action } // namespace rclcpp_action
namespace std namespace std
{ {
template<> template<>
struct less<rclcpp_action::GoalID> struct less<rclcpp_action::GoalUUID>
{ {
bool operator()( bool operator()(
const rclcpp_action::GoalID & lhs, const rclcpp_action::GoalUUID & lhs,
const rclcpp_action::GoalID & rhs) const const rclcpp_action::GoalUUID & rhs) const
{ {
return lhs < rhs; return lhs < rhs;
} }
@ -64,9 +64,9 @@ struct less<rclcpp_action::GoalID>
/// Hash a goal id so it can be used as a key in std::unordered_map /// Hash a goal id so it can be used as a key in std::unordered_map
template<> template<>
struct hash<rclcpp_action::GoalID> struct hash<rclcpp_action::GoalUUID>
{ {
size_t operator()(const rclcpp_action::GoalID & uuid) const noexcept size_t operator()(const rclcpp_action::GoalUUID & uuid) const noexcept
{ {
// TODO(sloretz) Use someone else's hash function and cite it // TODO(sloretz) Use someone else's hash function and cite it
size_t result = 0; size_t result = 0;

4
rclcpp_action/src/client.cpp
View file

@ -366,10 +366,10 @@ ClientBase::send_cancel_request(std::shared_ptr<void> request, ResponseCallback
pimpl_->pending_cancel_responses[sequence_number] = callback; pimpl_->pending_cancel_responses[sequence_number] = callback;
} }
GoalID GoalUUID
ClientBase::generate_goal_id() ClientBase::generate_goal_id()
{ {
GoalID goal_id; GoalUUID goal_id;
// TODO(hidmic): Do something better than this for UUID generation. // TODO(hidmic): Do something better than this for UUID generation.
// std::generate( // std::generate(
// goal_id.uuid.begin(), goal_id.uuid.end(), // goal_id.uuid.begin(), goal_id.uuid.end(),

18
rclcpp_action/src/server.cpp
View file

@ -28,7 +28,7 @@
#include <vector> #include <vector>
using rclcpp_action::ServerBase; using rclcpp_action::ServerBase;
using rclcpp_action::GoalID; using rclcpp_action::GoalUUID;
namespace rclcpp_action namespace rclcpp_action
{ {
@ -62,11 +62,11 @@ public:
bool goal_expired_ = false; bool goal_expired_ = false;
// Results to be kept until the goal expires after reaching a terminal state // Results to be kept until the goal expires after reaching a terminal state
std::unordered_map<GoalID, std::shared_ptr<void>> goal_results_; std::unordered_map<GoalUUID, std::shared_ptr<void>> goal_results_;
// Requests for results are kept until a result becomes available // Requests for results are kept until a result becomes available
std::unordered_map<GoalID, std::vector<rmw_request_id_t>> result_requests_; std::unordered_map<GoalUUID, std::vector<rmw_request_id_t>> result_requests_;
// rcl goal handles are kept so api to send result doesn't try to access freed memory // rcl goal handles are kept so api to send result doesn't try to access freed memory
std::unordered_map<GoalID, std::shared_ptr<rcl_action_goal_handle_t>> goal_handles_; std::unordered_map<GoalUUID, std::shared_ptr<rcl_action_goal_handle_t>> goal_handles_;
rclcpp::Logger logger_; rclcpp::Logger logger_;
}; };
@ -228,7 +228,7 @@ ServerBase::execute_goal_request_received()
rclcpp::exceptions::throw_from_rcl_error(ret); rclcpp::exceptions::throw_from_rcl_error(ret);
} }
GoalID uuid = get_goal_id_from_goal_request(message.get()); GoalUUID uuid = get_goal_id_from_goal_request(message.get());
convert(uuid, &goal_info); convert(uuid, &goal_info);
// Call user's callback, getting the user's response and a ros message to send back // Call user's callback, getting the user's response and a ros message to send back
@ -339,7 +339,7 @@ ServerBase::execute_cancel_request_received()
// For each canceled goal, call cancel callback // For each canceled goal, call cancel callback
for (size_t i = 0; i < goals.size; ++i) { for (size_t i = 0; i < goals.size; ++i) {
const rcl_action_goal_info_t & goal_info = goals.data[i]; const rcl_action_goal_info_t & goal_info = goals.data[i];
GoalID uuid; GoalUUID uuid;
convert(goal_info, &uuid); convert(goal_info, &uuid);
auto response_code = call_handle_cancel_callback(uuid); auto response_code = call_handle_cancel_callback(uuid);
if (CancelResponse::ACCEPT == response_code) { if (CancelResponse::ACCEPT == response_code) {
@ -388,7 +388,7 @@ ServerBase::execute_result_request_received()
std::shared_ptr<void> result_response; std::shared_ptr<void> result_response;
// check if the goal exists // check if the goal exists
GoalID uuid = get_goal_id_from_result_request(result_request.get()); GoalUUID uuid = get_goal_id_from_result_request(result_request.get());
rcl_action_goal_info_t goal_info; rcl_action_goal_info_t goal_info;
convert(uuid, &goal_info); convert(uuid, &goal_info);
bool goal_exists; bool goal_exists;
@ -433,7 +433,7 @@ ServerBase::execute_check_expired_goals()
rclcpp::exceptions::throw_from_rcl_error(ret); rclcpp::exceptions::throw_from_rcl_error(ret);
} else if (num_expired) { } else if (num_expired) {
// A goal expired! // A goal expired!
GoalID uuid; GoalUUID uuid;
convert(expired_goals[0], &uuid); convert(expired_goals[0], &uuid);
RCLCPP_DEBUG(pimpl_->logger_, "Expired goal %s", to_string(uuid).c_str()); RCLCPP_DEBUG(pimpl_->logger_, "Expired goal %s", to_string(uuid).c_str());
pimpl_->goal_results_.erase(uuid); pimpl_->goal_results_.erase(uuid);
@ -497,7 +497,7 @@ ServerBase::publish_status()
} }
void void
ServerBase::publish_result(const GoalID & uuid, std::shared_ptr<void> result_msg) ServerBase::publish_result(const GoalUUID & uuid, std::shared_ptr<void> result_msg)
{ {
// Check that the goal exists // Check that the goal exists
rcl_action_goal_info_t goal_info; rcl_action_goal_info_t goal_info;

6
rclcpp_action/src/types.cpp
View file

@ -20,7 +20,7 @@
namespace rclcpp_action namespace rclcpp_action
{ {
std::string std::string
to_string(const GoalID & goal_id) to_string(const GoalUUID & goal_id)
{ {
std::stringstream stream; std::stringstream stream;
stream << std::hex; stream << std::hex;
@ -31,7 +31,7 @@ to_string(const GoalID & goal_id)
} }
void void
convert(const GoalID & goal_id, rcl_action_goal_info_t * info) convert(const GoalUUID & goal_id, rcl_action_goal_info_t * info)
{ {
for (size_t i = 0; i < 16; ++i) { for (size_t i = 0; i < 16; ++i) {
info->goal_id.uuid[i] = goal_id[i]; info->goal_id.uuid[i] = goal_id[i];
@ -39,7 +39,7 @@ convert(const GoalID & goal_id, rcl_action_goal_info_t * info)
} }
void void
convert(const rcl_action_goal_info_t & info, GoalID * goal_id) convert(const rcl_action_goal_info_t & info, GoalUUID * goal_id)
{ {
for (size_t i = 0; i < 16; ++i) { for (size_t i = 0; i < 16; ++i) {
(*goal_id)[i] = info.goal_id.uuid[i]; (*goal_id)[i] = info.goal_id.uuid[i];

74
rclcpp_action/test/test_client.cpp
View file

@ -24,7 +24,6 @@
#include <rclcpp/clock.hpp> #include <rclcpp/clock.hpp>
#include <rclcpp/exceptions.hpp> #include <rclcpp/exceptions.hpp>
#include <rclcpp/executors.hpp> #include <rclcpp/executors.hpp>
#include <rclcpp/executors/multi_threaded_executor.hpp>
#include <rclcpp/node.hpp> #include <rclcpp/node.hpp>
#include <rclcpp/publisher.hpp> #include <rclcpp/publisher.hpp>
#include <rclcpp/rclcpp.hpp> #include <rclcpp/rclcpp.hpp>
@ -55,17 +54,17 @@ protected:
using ActionType = test_msgs::action::Fibonacci; using ActionType = test_msgs::action::Fibonacci;
using ActionGoal = ActionType::Goal; using ActionGoal = ActionType::Goal;
using ActionGoalHandle = rclcpp_action::ClientGoalHandle<ActionType>; using ActionGoalHandle = rclcpp_action::ClientGoalHandle<ActionType>;
using ActionGoalRequestService = ActionType::GoalRequestService; using ActionGoalRequestService = ActionType::Impl::SendGoalService;
using ActionGoalRequest = ActionGoalRequestService::Request; using ActionGoalRequest = ActionGoalRequestService::Request;
using ActionGoalResponse = ActionGoalRequestService::Response; using ActionGoalResponse = ActionGoalRequestService::Response;
using ActionGoalResultService = ActionType::GoalResultService; using ActionGoalResultService = ActionType::Impl::GetResultService;
using ActionGoalResultRequest = ActionGoalResultService::Request; using ActionGoalResultRequest = ActionGoalResultService::Request;
using ActionGoalResultResponse = ActionGoalResultService::Response; using ActionGoalResultResponse = ActionGoalResultService::Response;
using ActionCancelGoalService = ActionType::CancelGoalService; using ActionCancelGoalService = ActionType::Impl::CancelGoalService;
using ActionCancelGoalRequest = ActionType::CancelGoalService::Request; using ActionCancelGoalRequest = ActionType::Impl::CancelGoalService::Request;
using ActionCancelGoalResponse = ActionType::CancelGoalService::Response; using ActionCancelGoalResponse = ActionType::Impl::CancelGoalService::Response;
using ActionStatusMessage = ActionType::GoalStatusMessage; using ActionStatusMessage = ActionType::Impl::GoalStatusMessage;
using ActionFeedbackMessage = ActionType::Feedback; using ActionFeedbackMessage = ActionType::Impl::FeedbackMessage;
using ActionFeedback = ActionType::Feedback; using ActionFeedback = ActionType::Feedback;
static void SetUpTestCase() static void SetUpTestCase()
@ -90,9 +89,9 @@ protected:
ActionGoalResponse::SharedPtr response) ActionGoalResponse::SharedPtr response)
{ {
response->stamp = clock.now(); response->stamp = clock.now();
response->accepted = (request->order >= 0); response->accepted = (request->goal.order >= 0);
if (response->accepted) { if (response->accepted) {
goals[request->action_goal_id.uuid] = {request, response}; goals[request->goal_id.uuid] = {request, response};
} }
}); });
ASSERT_TRUE(goal_service != nullptr); ASSERT_TRUE(goal_service != nullptr);
@ -108,29 +107,30 @@ protected:
const ActionGoalResultRequest::SharedPtr request, const ActionGoalResultRequest::SharedPtr request,
ActionGoalResultResponse::SharedPtr response) ActionGoalResultResponse::SharedPtr response)
{ {
if (goals.count(request->action_goal_id.uuid) == 1) { if (goals.count(request->goal_id.uuid) == 1) {
auto goal_request = goals[request->action_goal_id.uuid].first; auto goal_request = goals[request->goal_id.uuid].first;
auto goal_response = goals[request->action_goal_id.uuid].second; auto goal_response = goals[request->goal_id.uuid].second;
ActionStatusMessage status_message; ActionStatusMessage status_message;
rclcpp_action::GoalStatus goal_status; rclcpp_action::GoalStatus goal_status;
goal_status.goal_info.goal_id.uuid = goal_request->action_goal_id.uuid; goal_status.goal_info.goal_id.uuid = goal_request->goal_id.uuid;
goal_status.goal_info.stamp = goal_response->stamp; goal_status.goal_info.stamp = goal_response->stamp;
goal_status.status = rclcpp_action::GoalStatus::STATUS_EXECUTING; goal_status.status = rclcpp_action::GoalStatus::STATUS_EXECUTING;
status_message.status_list.push_back(goal_status); status_message.status_list.push_back(goal_status);
status_publisher->publish(status_message); status_publisher->publish(status_message);
client_executor.spin_once(); client_executor.spin_once();
ActionFeedbackMessage feedback_message; ActionFeedbackMessage feedback_message;
feedback_message.action_goal_id.uuid = goal_request->action_goal_id.uuid; feedback_message.goal_id.uuid = goal_request->goal_id.uuid;
feedback_message.sequence.push_back(0); feedback_message.feedback.sequence.push_back(0);
feedback_publisher->publish(feedback_message); feedback_publisher->publish(feedback_message);
client_executor.spin_once(); client_executor.spin_once();
if (goal_request->order > 0) { if (goal_request->goal.order > 0) {
feedback_message.sequence.push_back(1); feedback_message.feedback.sequence.push_back(1);
feedback_publisher->publish(feedback_message); feedback_publisher->publish(feedback_message);
client_executor.spin_once(); client_executor.spin_once();
for (int i = 1; i < goal_request->order; ++i) { for (int i = 1; i < goal_request->goal.order; ++i) {
feedback_message.sequence.push_back( feedback_message.feedback.sequence.push_back(
feedback_message.sequence[i] + feedback_message.sequence[i - 1]); feedback_message.feedback.sequence[i] +
feedback_message.feedback.sequence[i - 1]);
feedback_publisher->publish(feedback_message); feedback_publisher->publish(feedback_message);
client_executor.spin_once(); client_executor.spin_once();
} }
@ -139,11 +139,11 @@ protected:
status_message.status_list[0] = goal_status; status_message.status_list[0] = goal_status;
status_publisher->publish(status_message); status_publisher->publish(status_message);
client_executor.spin_once(); client_executor.spin_once();
response->sequence = feedback_message.sequence; response->result.sequence = feedback_message.feedback.sequence;
response->action_status = rclcpp_action::GoalStatus::STATUS_SUCCEEDED; response->status = rclcpp_action::GoalStatus::STATUS_SUCCEEDED;
goals.erase(request->action_goal_id.uuid); goals.erase(request->goal_id.uuid);
} else { } else {
response->action_status = rclcpp_action::GoalStatus::STATUS_UNKNOWN; response->status = rclcpp_action::GoalStatus::STATUS_UNKNOWN;
} }
}); });
ASSERT_TRUE(result_service != nullptr); ASSERT_TRUE(result_service != nullptr);
@ -159,7 +159,7 @@ protected:
const ActionCancelGoalRequest::SharedPtr request, const ActionCancelGoalRequest::SharedPtr request,
ActionCancelGoalResponse::SharedPtr response) ActionCancelGoalResponse::SharedPtr response)
{ {
rclcpp_action::GoalID zero_uuid; rclcpp_action::GoalUUID zero_uuid;
std::fill(zero_uuid.begin(), zero_uuid.end(), 0u); std::fill(zero_uuid.begin(), zero_uuid.end(), 0u);
const rclcpp::Time cancel_stamp = request->goal_info.stamp; const rclcpp::Time cancel_stamp = request->goal_info.stamp;
bool cancel_all = ( bool cancel_all = (
@ -172,11 +172,11 @@ protected:
auto goal_response = it->second.second; auto goal_response = it->second.second;
const rclcpp::Time goal_stamp = goal_response->stamp; const rclcpp::Time goal_stamp = goal_response->stamp;
bool cancel_this = ( bool cancel_this = (
request->goal_info.goal_id.uuid == goal_request->action_goal_id.uuid || request->goal_info.goal_id.uuid == goal_request->goal_id.uuid ||
cancel_stamp > goal_stamp); cancel_stamp > goal_stamp);
if (cancel_all || cancel_this) { if (cancel_all || cancel_this) {
rclcpp_action::GoalStatus goal_status; rclcpp_action::GoalStatus goal_status;
goal_status.goal_info.goal_id.uuid = goal_request->action_goal_id.uuid; goal_status.goal_info.goal_id.uuid = goal_request->goal_id.uuid;
goal_status.goal_info.stamp = goal_response->stamp; goal_status.goal_info.stamp = goal_response->stamp;
goal_status.status = rclcpp_action::GoalStatus::STATUS_CANCELED; goal_status.status = rclcpp_action::GoalStatus::STATUS_CANCELED;
status_message.status_list.push_back(goal_status); status_message.status_list.push_back(goal_status);
@ -252,7 +252,7 @@ protected:
const char * const action_name{"fibonacci_test"}; const char * const action_name{"fibonacci_test"};
std::map< std::map<
rclcpp_action::GoalID, rclcpp_action::GoalUUID,
std::pair< std::pair<
typename ActionGoalRequest::SharedPtr, typename ActionGoalRequest::SharedPtr,
typename ActionGoalResponse::SharedPtr>> goals; typename ActionGoalResponse::SharedPtr>> goals;
@ -305,11 +305,11 @@ TEST_F(TestClient, async_send_goal_and_ignore_feedback_but_wait_for_result)
EXPECT_TRUE(goal_handle->is_result_aware()); EXPECT_TRUE(goal_handle->is_result_aware());
auto future_result = goal_handle->async_result(); auto future_result = goal_handle->async_result();
dual_spin_until_future_complete(future_result); dual_spin_until_future_complete(future_result);
auto result = future_result.get(); auto wrapped_result = future_result.get();
ASSERT_EQ(6ul, result.response->sequence.size()); ASSERT_EQ(6ul, wrapped_result.result->sequence.size());
EXPECT_EQ(0, result.response->sequence[0]); EXPECT_EQ(0, wrapped_result.result->sequence[0]);
EXPECT_EQ(1, result.response->sequence[1]); EXPECT_EQ(1, wrapped_result.result->sequence[1]);
EXPECT_EQ(5, result.response->sequence[5]); EXPECT_EQ(5, wrapped_result.result->sequence[5]);
} }
TEST_F(TestClient, async_send_goal_with_feedback_and_result) TEST_F(TestClient, async_send_goal_with_feedback_and_result)
@ -337,10 +337,10 @@ TEST_F(TestClient, async_send_goal_with_feedback_and_result)
EXPECT_TRUE(goal_handle->is_result_aware()); EXPECT_TRUE(goal_handle->is_result_aware());
auto future_result = goal_handle->async_result(); auto future_result = goal_handle->async_result();
dual_spin_until_future_complete(future_result); dual_spin_until_future_complete(future_result);
auto result = future_result.get(); auto wrapped_result = future_result.get();
ASSERT_EQ(5u, result.response->sequence.size()); ASSERT_EQ(5u, wrapped_result.result->sequence.size());
EXPECT_EQ(3, result.response->sequence.back()); EXPECT_EQ(3, wrapped_result.result->sequence.back());
EXPECT_EQ(5, feedback_count); EXPECT_EQ(5, feedback_count);
} }

91
rclcpp_action/test/test_server.cpp
View file

@ -26,7 +26,7 @@
#include "rclcpp_action/server.hpp" #include "rclcpp_action/server.hpp"
using Fibonacci = test_msgs::action::Fibonacci; using Fibonacci = test_msgs::action::Fibonacci;
using GoalID = rclcpp_action::GoalID; using GoalUUID = rclcpp_action::GoalUUID;
class TestServer : public ::testing::Test class TestServer : public ::testing::Test
{ {
@ -36,16 +36,16 @@ protected:
rclcpp::init(0, nullptr); rclcpp::init(0, nullptr);
} }
std::shared_ptr<Fibonacci::GoalRequestService::Request> std::shared_ptr<Fibonacci::Impl::SendGoalService::Request>
send_goal_request(rclcpp::Node::SharedPtr node, GoalID uuid) send_goal_request(rclcpp::Node::SharedPtr node, GoalUUID uuid)
{ {
auto client = node->create_client<Fibonacci::GoalRequestService>( auto client = node->create_client<Fibonacci::Impl::SendGoalService>(
"fibonacci/_action/send_goal"); "fibonacci/_action/send_goal");
if (!client->wait_for_service(std::chrono::seconds(20))) { if (!client->wait_for_service(std::chrono::seconds(20))) {
throw std::runtime_error("send goal service didn't become available"); throw std::runtime_error("send goal service didn't become available");
} }
auto request = std::make_shared<Fibonacci::GoalRequestService::Request>(); auto request = std::make_shared<Fibonacci::Impl::SendGoalService::Request>();
request->action_goal_id.uuid = uuid; request->goal_id.uuid = uuid;
auto future = client->async_send_request(request); auto future = client->async_send_request(request);
if (rclcpp::executor::FutureReturnCode::SUCCESS != if (rclcpp::executor::FutureReturnCode::SUCCESS !=
rclcpp::spin_until_future_complete(node, future)) rclcpp::spin_until_future_complete(node, future))
@ -56,14 +56,14 @@ protected:
} }
void void
send_cancel_request(rclcpp::Node::SharedPtr node, GoalID uuid) send_cancel_request(rclcpp::Node::SharedPtr node, GoalUUID uuid)
{ {
auto cancel_client = node->create_client<Fibonacci::CancelGoalService>( auto cancel_client = node->create_client<Fibonacci::Impl::CancelGoalService>(
"fibonacci/_action/cancel_goal"); "fibonacci/_action/cancel_goal");
if (!cancel_client->wait_for_service(std::chrono::seconds(20))) { if (!cancel_client->wait_for_service(std::chrono::seconds(20))) {
throw std::runtime_error("cancel goal service didn't become available"); throw std::runtime_error("cancel goal service didn't become available");
} }
auto request = std::make_shared<Fibonacci::CancelGoalService::Request>(); auto request = std::make_shared<Fibonacci::Impl::CancelGoalService::Request>();
request->goal_info.goal_id.uuid = uuid; request->goal_info.goal_id.uuid = uuid;
auto future = cancel_client->async_send_request(request); auto future = cancel_client->async_send_request(request);
if (rclcpp::executor::FutureReturnCode::SUCCESS != if (rclcpp::executor::FutureReturnCode::SUCCESS !=
@ -80,7 +80,7 @@ TEST_F(TestServer, construction_and_destruction)
using GoalHandle = rclcpp_action::ServerGoalHandle<Fibonacci>; using GoalHandle = rclcpp_action::ServerGoalHandle<Fibonacci>;
auto as = rclcpp_action::create_server<Fibonacci>(node, "fibonacci", auto as = rclcpp_action::create_server<Fibonacci>(node, "fibonacci",
[](const GoalID &, std::shared_ptr<const Fibonacci::Goal>) { [](const GoalUUID &, std::shared_ptr<const Fibonacci::Goal>) {
return rclcpp_action::GoalResponse::REJECT; return rclcpp_action::GoalResponse::REJECT;
}, },
[](std::shared_ptr<GoalHandle>) { [](std::shared_ptr<GoalHandle>) {
@ -93,10 +93,10 @@ TEST_F(TestServer, construction_and_destruction)
TEST_F(TestServer, handle_goal_called) TEST_F(TestServer, handle_goal_called)
{ {
auto node = std::make_shared<rclcpp::Node>("handle_goal_node", "/rclcpp_action/handle_goal"); auto node = std::make_shared<rclcpp::Node>("handle_goal_node", "/rclcpp_action/handle_goal");
GoalID received_uuid; GoalUUID received_uuid;
auto handle_goal = [&received_uuid]( auto handle_goal = [&received_uuid](
const GoalID & uuid, std::shared_ptr<const Fibonacci::Goal>) const GoalUUID & uuid, std::shared_ptr<const Fibonacci::Goal>)
{ {
received_uuid = uuid; received_uuid = uuid;
return rclcpp_action::GoalResponse::REJECT; return rclcpp_action::GoalResponse::REJECT;
@ -114,15 +114,15 @@ TEST_F(TestServer, handle_goal_called)
// Create a client that calls the goal request service // Create a client that calls the goal request service
// Make sure the UUID received is the same as the one sent // Make sure the UUID received is the same as the one sent
auto client = node->create_client<Fibonacci::GoalRequestService>( auto client = node->create_client<Fibonacci::Impl::SendGoalService>(
"fibonacci/_action/send_goal"); "fibonacci/_action/send_goal");
ASSERT_TRUE(client->wait_for_service(std::chrono::seconds(20))); ASSERT_TRUE(client->wait_for_service(std::chrono::seconds(20)));
auto request = std::make_shared<Fibonacci::GoalRequestService::Request>(); auto request = std::make_shared<Fibonacci::Impl::SendGoalService::Request>();
const GoalID uuid{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16}}; const GoalUUID uuid{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16}};
request->action_goal_id.uuid = uuid; request->goal_id.uuid = uuid;
auto future = client->async_send_request(request); auto future = client->async_send_request(request);
ASSERT_EQ( ASSERT_EQ(
@ -135,10 +135,10 @@ TEST_F(TestServer, handle_goal_called)
TEST_F(TestServer, handle_accepted_called) TEST_F(TestServer, handle_accepted_called)
{ {
auto node = std::make_shared<rclcpp::Node>("handle_exec_node", "/rclcpp_action/handle_accepted"); auto node = std::make_shared<rclcpp::Node>("handle_exec_node", "/rclcpp_action/handle_accepted");
const GoalID uuid{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16}}; const GoalUUID uuid{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16}};
auto handle_goal = []( auto handle_goal = [](
const GoalID &, std::shared_ptr<const Fibonacci::Goal>) const GoalUUID &, std::shared_ptr<const Fibonacci::Goal>)
{ {
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
}; };
@ -164,16 +164,16 @@ TEST_F(TestServer, handle_accepted_called)
ASSERT_TRUE(received_handle); ASSERT_TRUE(received_handle);
ASSERT_TRUE(received_handle->is_active()); ASSERT_TRUE(received_handle->is_active());
EXPECT_EQ(uuid, received_handle->get_goal_id()); EXPECT_EQ(uuid, received_handle->get_goal_id());
EXPECT_EQ(*request, *(received_handle->get_goal())); EXPECT_EQ(request->goal, *(received_handle->get_goal()));
} }
TEST_F(TestServer, handle_cancel_called) TEST_F(TestServer, handle_cancel_called)
{ {
auto node = std::make_shared<rclcpp::Node>("handle_cancel_node", "/rclcpp_action/handle_cancel"); auto node = std::make_shared<rclcpp::Node>("handle_cancel_node", "/rclcpp_action/handle_cancel");
const GoalID uuid{{10, 20, 30, 40, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16}}; const GoalUUID uuid{{10, 20, 30, 40, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16}};
auto handle_goal = []( auto handle_goal = [](
const GoalID &, std::shared_ptr<const Fibonacci::Goal>) const GoalUUID &, std::shared_ptr<const Fibonacci::Goal>)
{ {
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
}; };
@ -210,10 +210,10 @@ TEST_F(TestServer, handle_cancel_called)
TEST_F(TestServer, publish_status_accepted) TEST_F(TestServer, publish_status_accepted)
{ {
auto node = std::make_shared<rclcpp::Node>("status_accept_node", "/rclcpp_action/status_accept"); auto node = std::make_shared<rclcpp::Node>("status_accept_node", "/rclcpp_action/status_accept");
const GoalID uuid{{1, 2, 3, 4, 5, 6, 7, 8, 9, 100, 110, 120, 13, 14, 15, 16}}; const GoalUUID uuid{{1, 2, 3, 4, 5, 6, 7, 8, 9, 100, 110, 120, 13, 14, 15, 16}};
auto handle_goal = []( auto handle_goal = [](
const GoalID &, std::shared_ptr<const Fibonacci::Goal>) const GoalUUID &, std::shared_ptr<const Fibonacci::Goal>)
{ {
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
}; };
@ -271,10 +271,10 @@ TEST_F(TestServer, publish_status_accepted)
TEST_F(TestServer, publish_status_canceling) TEST_F(TestServer, publish_status_canceling)
{ {
auto node = std::make_shared<rclcpp::Node>("status_cancel_node", "/rclcpp_action/status_cancel"); auto node = std::make_shared<rclcpp::Node>("status_cancel_node", "/rclcpp_action/status_cancel");
const GoalID uuid{{1, 2, 3, 40, 5, 6, 7, 80, 9, 10, 11, 120, 13, 14, 15, 160}}; const GoalUUID uuid{{1, 2, 3, 40, 5, 6, 7, 80, 9, 10, 11, 120, 13, 14, 15, 160}};
auto handle_goal = []( auto handle_goal = [](
const GoalID &, std::shared_ptr<const Fibonacci::Goal>) const GoalUUID &, std::shared_ptr<const Fibonacci::Goal>)
{ {
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
}; };
@ -326,10 +326,10 @@ TEST_F(TestServer, publish_status_canceling)
TEST_F(TestServer, publish_status_canceled) TEST_F(TestServer, publish_status_canceled)
{ {
auto node = std::make_shared<rclcpp::Node>("status_canceled", "/rclcpp_action/status_canceled"); auto node = std::make_shared<rclcpp::Node>("status_canceled", "/rclcpp_action/status_canceled");
const GoalID uuid{{1, 2, 3, 40, 5, 6, 70, 8, 9, 1, 11, 120, 13, 140, 15, 160}}; const GoalUUID uuid{{1, 2, 3, 40, 5, 6, 70, 8, 9, 1, 11, 120, 13, 140, 15, 160}};
auto handle_goal = []( auto handle_goal = [](
const GoalID &, std::shared_ptr<const Fibonacci::Goal>) const GoalUUID &, std::shared_ptr<const Fibonacci::Goal>)
{ {
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
}; };
@ -383,10 +383,10 @@ TEST_F(TestServer, publish_status_canceled)
TEST_F(TestServer, publish_status_succeeded) TEST_F(TestServer, publish_status_succeeded)
{ {
auto node = std::make_shared<rclcpp::Node>("status_succeeded", "/rclcpp_action/status_succeeded"); auto node = std::make_shared<rclcpp::Node>("status_succeeded", "/rclcpp_action/status_succeeded");
const GoalID uuid{{1, 2, 3, 40, 5, 6, 70, 8, 9, 1, 11, 120, 13, 140, 15, 160}}; const GoalUUID uuid{{1, 2, 3, 40, 5, 6, 70, 8, 9, 1, 11, 120, 13, 140, 15, 160}};
auto handle_goal = []( auto handle_goal = [](
const GoalID &, std::shared_ptr<const Fibonacci::Goal>) const GoalUUID &, std::shared_ptr<const Fibonacci::Goal>)
{ {
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
}; };
@ -438,10 +438,10 @@ TEST_F(TestServer, publish_status_succeeded)
TEST_F(TestServer, publish_status_aborted) TEST_F(TestServer, publish_status_aborted)
{ {
auto node = std::make_shared<rclcpp::Node>("status_aborted", "/rclcpp_action/status_aborted"); auto node = std::make_shared<rclcpp::Node>("status_aborted", "/rclcpp_action/status_aborted");
const GoalID uuid{{1, 2, 3, 40, 5, 6, 70, 8, 9, 1, 11, 120, 13, 140, 15, 160}}; const GoalUUID uuid{{1, 2, 3, 40, 5, 6, 70, 8, 9, 1, 11, 120, 13, 140, 15, 160}};
auto handle_goal = []( auto handle_goal = [](
const GoalID &, std::shared_ptr<const Fibonacci::Goal>) const GoalUUID &, std::shared_ptr<const Fibonacci::Goal>)
{ {
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
}; };
@ -493,10 +493,10 @@ TEST_F(TestServer, publish_status_aborted)
TEST_F(TestServer, publish_feedback) TEST_F(TestServer, publish_feedback)
{ {
auto node = std::make_shared<rclcpp::Node>("pub_feedback", "/rclcpp_action/pub_feedback"); auto node = std::make_shared<rclcpp::Node>("pub_feedback", "/rclcpp_action/pub_feedback");
const GoalID uuid{{1, 20, 30, 4, 5, 6, 70, 8, 9, 1, 11, 120, 13, 14, 15, 160}}; const GoalUUID uuid{{1, 20, 30, 4, 5, 6, 70, 8, 9, 1, 11, 120, 13, 14, 15, 160}};
auto handle_goal = []( auto handle_goal = [](
const GoalID &, std::shared_ptr<const Fibonacci::Goal>) const GoalUUID &, std::shared_ptr<const Fibonacci::Goal>)
{ {
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
}; };
@ -521,7 +521,7 @@ TEST_F(TestServer, publish_feedback)
(void)as; (void)as;
// Subscribe to feedback messages // Subscribe to feedback messages
using FeedbackT = Fibonacci::Feedback; using FeedbackT = Fibonacci::Impl::FeedbackMessage;
std::vector<FeedbackT::SharedPtr> received_msgs; std::vector<FeedbackT::SharedPtr> received_msgs;
auto subscriber = node->create_subscription<FeedbackT>( auto subscriber = node->create_subscription<FeedbackT>(
"fibonacci/_action/feedback", [&received_msgs](FeedbackT::SharedPtr msg) "fibonacci/_action/feedback", [&received_msgs](FeedbackT::SharedPtr msg)
@ -531,7 +531,7 @@ TEST_F(TestServer, publish_feedback)
send_goal_request(node, uuid); send_goal_request(node, uuid);
auto sent_message = std::make_shared<FeedbackT>(); auto sent_message = std::make_shared<Fibonacci::Feedback>();
sent_message->sequence = {1, 1, 2, 3, 5}; sent_message->sequence = {1, 1, 2, 3, 5};
received_handle->publish_feedback(sent_message); received_handle->publish_feedback(sent_message);
@ -544,17 +544,16 @@ TEST_F(TestServer, publish_feedback)
ASSERT_EQ(1u, received_msgs.size()); ASSERT_EQ(1u, received_msgs.size());
auto & msg = received_msgs.back(); auto & msg = received_msgs.back();
ASSERT_EQ(sent_message->sequence, msg->sequence); ASSERT_EQ(sent_message->sequence, msg->feedback.sequence);
ASSERT_EQ(uuid, msg->action_goal_id.uuid);
} }
TEST_F(TestServer, get_result) TEST_F(TestServer, get_result)
{ {
auto node = std::make_shared<rclcpp::Node>("get_result", "/rclcpp_action/get_result"); auto node = std::make_shared<rclcpp::Node>("get_result", "/rclcpp_action/get_result");
const GoalID uuid{{1, 2, 3, 4, 5, 6, 7, 80, 90, 10, 11, 12, 13, 14, 15, 160}}; const GoalUUID uuid{{1, 2, 3, 4, 5, 6, 7, 80, 90, 10, 11, 12, 13, 14, 15, 160}};
auto handle_goal = []( auto handle_goal = [](
const GoalID &, std::shared_ptr<const Fibonacci::Goal>) const GoalUUID &, std::shared_ptr<const Fibonacci::Goal>)
{ {
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
}; };
@ -581,13 +580,13 @@ TEST_F(TestServer, get_result)
send_goal_request(node, uuid); send_goal_request(node, uuid);
// Send result request // Send result request
auto result_client = node->create_client<Fibonacci::GoalResultService>( auto result_client = node->create_client<Fibonacci::Impl::GetResultService>(
"fibonacci/_action/get_result"); "fibonacci/_action/get_result");
if (!result_client->wait_for_service(std::chrono::seconds(20))) { if (!result_client->wait_for_service(std::chrono::seconds(20))) {
throw std::runtime_error("get result service didn't become available"); throw std::runtime_error("get result service didn't become available");
} }
auto request = std::make_shared<Fibonacci::GoalResultService::Request>(); auto request = std::make_shared<Fibonacci::Impl::GetResultService::Request>();
request->action_goal_id.uuid = uuid; request->goal_id.uuid = uuid;
auto future = result_client->async_send_request(request); auto future = result_client->async_send_request(request);
// Send a result // Send a result
@ -600,17 +599,17 @@ TEST_F(TestServer, get_result)
rclcpp::spin_until_future_complete(node, future)); rclcpp::spin_until_future_complete(node, future));
auto response = future.get(); auto response = future.get();
EXPECT_EQ(action_msgs::msg::GoalStatus::STATUS_SUCCEEDED, response->action_status); EXPECT_EQ(action_msgs::msg::GoalStatus::STATUS_SUCCEEDED, response->status);
EXPECT_EQ(result->sequence, response->sequence); EXPECT_EQ(result->sequence, response->result.sequence);
} }
TEST_F(TestServer, deferred_execution) TEST_F(TestServer, deferred_execution)
{ {
auto node = std::make_shared<rclcpp::Node>("defer_exec", "/rclcpp_action/defer_exec"); auto node = std::make_shared<rclcpp::Node>("defer_exec", "/rclcpp_action/defer_exec");
const GoalID uuid{{1, 2, 3, 40, 5, 6, 70, 8, 9, 1, 11, 120, 13, 140, 15, 160}}; const GoalUUID uuid{{1, 2, 3, 40, 5, 6, 70, 8, 9, 1, 11, 120, 13, 140, 15, 160}};
auto handle_goal = []( auto handle_goal = [](
const GoalID &, std::shared_ptr<const Fibonacci::Goal>) const GoalUUID &, std::shared_ptr<const Fibonacci::Goal>)
{ {
return rclcpp_action::GoalResponse::ACCEPT_AND_DEFER; return rclcpp_action::GoalResponse::ACCEPT_AND_DEFER;
}; };