update to use separated action types (#601)
* 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
Dirk Thomas
parent
d2d9ad8796
commit
718d24f942
11 changed files with 220 additions and 219 deletions
|
|
@ -148,7 +148,7 @@ protected:
|
|||
/// \internal
|
||||
RCLCPP_ACTION_PUBLIC
|
||||
virtual
|
||||
GoalID
|
||||
GoalUUID
|
||||
generate_goal_id();
|
||||
|
||||
/// \internal
|
||||
|
|
@ -260,10 +260,10 @@ public:
|
|||
using Goal = typename ActionT::Goal;
|
||||
using Feedback = typename ActionT::Feedback;
|
||||
using GoalHandle = ClientGoalHandle<ActionT>;
|
||||
using Result = typename GoalHandle::Result;
|
||||
using WrappedResult = typename GoalHandle::WrappedResult;
|
||||
using FeedbackCallback = typename ClientGoalHandle<ActionT>::FeedbackCallback;
|
||||
using CancelRequest = typename ActionT::CancelGoalService::Request;
|
||||
using CancelResponse = typename ActionT::CancelGoalService::Response;
|
||||
using CancelRequest = typename ActionT::Impl::CancelGoalService::Request;
|
||||
using CancelResponse = typename ActionT::Impl::CancelGoalService::Response;
|
||||
|
||||
Client(
|
||||
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base,
|
||||
|
|
@ -286,26 +286,23 @@ public:
|
|||
// Put promise in the heap to move it around.
|
||||
auto promise = std::make_shared<std::promise<typename GoalHandle::SharedPtr>>();
|
||||
std::shared_future<typename GoalHandle::SharedPtr> future(promise->get_future());
|
||||
using GoalRequest = typename ActionT::GoalRequestService::Request;
|
||||
// auto goal_request = std::make_shared<GoalRequest>();
|
||||
// goal_request->goal_id = this->generate_goal_id();
|
||||
// goal_request->goal = goal;
|
||||
auto goal_request = std::make_shared<GoalRequest>(goal);
|
||||
goal_request->action_goal_id.uuid = this->generate_goal_id();
|
||||
using GoalRequest = typename ActionT::Impl::SendGoalService::Request;
|
||||
auto goal_request = std::make_shared<GoalRequest>();
|
||||
goal_request->goal_id.uuid = this->generate_goal_id();
|
||||
goal_request->goal = goal;
|
||||
this->send_goal_request(
|
||||
std::static_pointer_cast<void>(goal_request),
|
||||
[this, goal_request, callback, ignore_result, promise](
|
||||
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);
|
||||
if (!goal_response->accepted) {
|
||||
promise->set_value(nullptr);
|
||||
return;
|
||||
}
|
||||
GoalInfo goal_info;
|
||||
// goal_info.goal_id = goal_request->goal_id;
|
||||
goal_info.goal_id.uuid = goal_request->action_goal_id.uuid;
|
||||
goal_info.goal_id.uuid = goal_request->goal_id.uuid;
|
||||
goal_info.stamp = goal_response->stamp;
|
||||
// Do not use std::make_shared as friendship cannot be forwarded.
|
||||
std::shared_ptr<GoalHandle> goal_handle(new GoalHandle(goal_info, callback));
|
||||
|
|
@ -324,7 +321,7 @@ public:
|
|||
return future;
|
||||
}
|
||||
|
||||
std::shared_future<Result>
|
||||
std::shared_future<WrappedResult>
|
||||
async_get_result(typename GoalHandle::SharedPtr goal_handle)
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(goal_handles_mutex_);
|
||||
|
|
@ -406,7 +403,7 @@ private:
|
|||
std::shared_ptr<void>
|
||||
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());
|
||||
}
|
||||
|
||||
|
|
@ -414,7 +411,7 @@ private:
|
|||
std::shared_ptr<void>
|
||||
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());
|
||||
}
|
||||
|
||||
|
|
@ -429,9 +426,8 @@ private:
|
|||
std::shared_ptr<void>
|
||||
create_feedback_message() const override
|
||||
{
|
||||
// using FeedbackMessage = typename ActionT::FeedbackMessage;
|
||||
// return std::shared_ptr<void>(new FeedbackMessage());
|
||||
return std::shared_ptr<void>(new Feedback());
|
||||
using FeedbackMessage = typename ActionT::Impl::FeedbackMessage;
|
||||
return std::shared_ptr<void>(new FeedbackMessage());
|
||||
}
|
||||
|
||||
/// \internal
|
||||
|
|
@ -439,13 +435,10 @@ private:
|
|||
handle_feedback_message(std::shared_ptr<void> message) override
|
||||
{
|
||||
std::lock_guard<std::mutex> guard(goal_handles_mutex_);
|
||||
// using FeedbackMessage = typename ActionT::FeedbackMessage;
|
||||
// typename FeedbackMessage::SharedPtr feedback_message =
|
||||
// std::static_pointer_cast<FeedbackMessage>(message);
|
||||
typename Feedback::SharedPtr feedback_message =
|
||||
std::static_pointer_cast<Feedback>(message);
|
||||
// const GoalID & goal_id = feedback_message->goal_id;
|
||||
const GoalID & goal_id = feedback_message->action_goal_id.uuid;
|
||||
using FeedbackMessage = typename ActionT::Impl::FeedbackMessage;
|
||||
typename FeedbackMessage::SharedPtr feedback_message =
|
||||
std::static_pointer_cast<FeedbackMessage>(message);
|
||||
const GoalUUID & goal_id = feedback_message->goal_id.uuid;
|
||||
if (goal_handles_.count(goal_id) == 0) {
|
||||
RCLCPP_DEBUG(
|
||||
this->get_logger(),
|
||||
|
|
@ -453,15 +446,16 @@ private:
|
|||
return;
|
||||
}
|
||||
typename GoalHandle::SharedPtr goal_handle = goal_handles_[goal_id];
|
||||
// goal_handle->call_feedback_callback(goal_handle, feedback_message->feedback);
|
||||
goal_handle->call_feedback_callback(goal_handle, feedback_message);
|
||||
auto feedback = std::make_shared<Feedback>();
|
||||
*feedback = feedback_message->feedback;
|
||||
goal_handle->call_feedback_callback(goal_handle, feedback);
|
||||
}
|
||||
|
||||
/// \internal
|
||||
std::shared_ptr<void>
|
||||
create_status_message() const override
|
||||
{
|
||||
using GoalStatusMessage = typename ActionT::GoalStatusMessage;
|
||||
using GoalStatusMessage = typename ActionT::Impl::GoalStatusMessage;
|
||||
return std::shared_ptr<void>(new GoalStatusMessage());
|
||||
}
|
||||
|
||||
|
|
@ -470,11 +464,10 @@ private:
|
|||
handle_status_message(std::shared_ptr<void> message) override
|
||||
{
|
||||
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);
|
||||
for (const GoalStatus & status : status_message->status_list) {
|
||||
// const GoalID & goal_id = status.goal_info.goal_id;
|
||||
const GoalID & goal_id = status.goal_info.goal_id.uuid;
|
||||
const GoalUUID & goal_id = status.goal_info.goal_id.uuid;
|
||||
if (goal_handles_.count(goal_id) == 0) {
|
||||
RCLCPP_DEBUG(
|
||||
this->get_logger(),
|
||||
|
|
@ -498,21 +491,22 @@ private:
|
|||
void
|
||||
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>();
|
||||
// goal_result_request.goal_id = goal_handle->get_goal_id();
|
||||
goal_result_request->action_goal_id.uuid = goal_handle->get_goal_id();
|
||||
goal_result_request->goal_id.uuid = goal_handle->get_goal_id();
|
||||
this->send_result_request(
|
||||
std::static_pointer_cast<void>(goal_result_request),
|
||||
[goal_handle, this](std::shared_ptr<void> response) mutable
|
||||
{
|
||||
// Wrap the response in a struct with the fields a user cares about
|
||||
Result result;
|
||||
using GoalResultResponse = typename ActionT::GoalResultService::Response;
|
||||
result.response = std::static_pointer_cast<GoalResultResponse>(response);
|
||||
result.goal_id = goal_handle->get_goal_id();
|
||||
result.code = static_cast<ResultCode>(result.response->action_status);
|
||||
goal_handle->set_result(result);
|
||||
WrappedResult wrapped_result;
|
||||
using GoalResultResponse = typename ActionT::Impl::GetResultService::Response;
|
||||
auto result_response = std::static_pointer_cast<GoalResultResponse>(response);
|
||||
wrapped_result.result = std::make_shared<typename ActionT::Result>();
|
||||
*wrapped_result.result = result_response->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_);
|
||||
goal_handles_.erase(goal_handle->get_goal_id());
|
||||
});
|
||||
|
|
@ -536,7 +530,7 @@ private:
|
|||
return future;
|
||||
}
|
||||
|
||||
std::map<GoalID, typename GoalHandle::SharedPtr> goal_handles_;
|
||||
std::map<GoalUUID, typename GoalHandle::SharedPtr> goal_handles_;
|
||||
std::mutex goal_handles_mutex_;
|
||||
};
|
||||
} // namespace rclcpp_action
|
||||
|
|
|
|||
|
|
@ -52,30 +52,31 @@ public:
|
|||
RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(ClientGoalHandle)
|
||||
|
||||
// A wrapper that defines the result of an action
|
||||
typedef struct Result
|
||||
typedef struct WrappedResult
|
||||
{
|
||||
/// The unique identifier of the goal
|
||||
GoalID goal_id;
|
||||
GoalUUID goal_id;
|
||||
/// A status to indicate if the goal was canceled, aborted, or suceeded
|
||||
ResultCode code;
|
||||
/// User defined fields sent back with an action
|
||||
typename ActionT::Result::SharedPtr response;
|
||||
} Result;
|
||||
typename ActionT::Result::SharedPtr result;
|
||||
} WrappedResult;
|
||||
|
||||
using Feedback = typename ActionT::Feedback;
|
||||
using FeedbackCallback =
|
||||
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();
|
||||
|
||||
const GoalID &
|
||||
const GoalUUID &
|
||||
get_goal_id() const;
|
||||
|
||||
rclcpp::Time
|
||||
get_goal_stamp() const;
|
||||
|
||||
std::shared_future<Result>
|
||||
std::shared_future<WrappedResult>
|
||||
async_result();
|
||||
|
||||
int8_t
|
||||
|
|
@ -108,7 +109,7 @@ private:
|
|||
set_status(int8_t status);
|
||||
|
||||
void
|
||||
set_result(const Result & result);
|
||||
set_result(const WrappedResult & wrapped_result);
|
||||
|
||||
void
|
||||
invalidate();
|
||||
|
|
@ -116,8 +117,8 @@ private:
|
|||
GoalInfo info_;
|
||||
|
||||
bool is_result_aware_{false};
|
||||
std::promise<Result> result_promise_;
|
||||
std::shared_future<Result> result_future_;
|
||||
std::promise<WrappedResult> result_promise_;
|
||||
std::shared_future<WrappedResult> result_future_;
|
||||
|
||||
FeedbackCallback feedback_callback_{nullptr};
|
||||
int8_t status_{GoalStatus::STATUS_ACCEPTED};
|
||||
|
|
|
|||
|
|
@ -39,7 +39,7 @@ ClientGoalHandle<ActionT>::~ClientGoalHandle()
|
|||
}
|
||||
|
||||
template<typename ActionT>
|
||||
const GoalID &
|
||||
const GoalUUID &
|
||||
ClientGoalHandle<ActionT>::get_goal_id() const
|
||||
{
|
||||
// return info_.goal_id;
|
||||
|
|
@ -54,7 +54,7 @@ ClientGoalHandle<ActionT>::get_goal_stamp() const
|
|||
}
|
||||
|
||||
template<typename ActionT>
|
||||
std::shared_future<typename ClientGoalHandle<ActionT>::Result>
|
||||
std::shared_future<typename ClientGoalHandle<ActionT>::WrappedResult>
|
||||
ClientGoalHandle<ActionT>::async_result()
|
||||
{
|
||||
std::lock_guard<std::mutex> guard(handle_mutex_);
|
||||
|
|
@ -66,11 +66,11 @@ ClientGoalHandle<ActionT>::async_result()
|
|||
|
||||
template<typename ActionT>
|
||||
void
|
||||
ClientGoalHandle<ActionT>::set_result(const Result & result)
|
||||
ClientGoalHandle<ActionT>::set_result(const WrappedResult & wrapped_result)
|
||||
{
|
||||
std::lock_guard<std::mutex> guard(handle_mutex_);
|
||||
status_ = static_cast<int8_t>(result.code);
|
||||
result_promise_.set_value(result);
|
||||
status_ = static_cast<int8_t>(wrapped_result.code);
|
||||
result_promise_.set_value(wrapped_result);
|
||||
}
|
||||
|
||||
template<typename ActionT>
|
||||
|
|
|
|||
|
|
@ -146,7 +146,7 @@ protected:
|
|||
RCLCPP_ACTION_PUBLIC
|
||||
virtual
|
||||
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
|
||||
// each goal id.
|
||||
|
|
@ -155,13 +155,13 @@ protected:
|
|||
RCLCPP_ACTION_PUBLIC
|
||||
virtual
|
||||
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.
|
||||
/// \internal
|
||||
RCLCPP_ACTION_PUBLIC
|
||||
virtual
|
||||
GoalID
|
||||
GoalUUID
|
||||
get_goal_id_from_goal_request(void * message) = 0;
|
||||
|
||||
/// Create an empty goal request message so it can be taken from a lower layer.
|
||||
|
|
@ -178,13 +178,13 @@ protected:
|
|||
void
|
||||
call_goal_accepted_callback(
|
||||
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.
|
||||
/// \internal
|
||||
RCLCPP_ACTION_PUBLIC
|
||||
virtual
|
||||
GoalID
|
||||
GoalUUID
|
||||
get_goal_id_from_result_request(void * message) = 0;
|
||||
|
||||
/// Create an empty goal request message so it can be taken from a lower layer.
|
||||
|
|
@ -214,7 +214,7 @@ protected:
|
|||
/// \internal
|
||||
RCLCPP_ACTION_PUBLIC
|
||||
void
|
||||
publish_result(const GoalID & uuid, std::shared_ptr<void> result_msg);
|
||||
publish_result(const GoalUUID & uuid, std::shared_ptr<void> result_msg);
|
||||
|
||||
/// \internal
|
||||
RCLCPP_ACTION_PUBLIC
|
||||
|
|
@ -272,7 +272,7 @@ public:
|
|||
|
||||
/// Signature of a callback that accepts or rejects goal requests.
|
||||
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.
|
||||
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.
|
||||
|
|
@ -335,16 +335,14 @@ protected:
|
|||
|
||||
/// \internal
|
||||
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
|
||||
static_assert(
|
||||
std::is_same<typename ActionT::Goal, typename ActionT::GoalRequestService::Request>::value,
|
||||
"Assuming user fields were merged with goal request fields");
|
||||
GoalResponse user_response = handle_goal_(
|
||||
uuid, std::static_pointer_cast<typename ActionT::Goal>(message));
|
||||
auto request = std::static_pointer_cast<
|
||||
typename ActionT::Impl::SendGoalService::Request>(message);
|
||||
auto goal = std::shared_ptr<typename ActionT::Goal>(request, &request->goal);
|
||||
GoalResponse user_response = handle_goal_(uuid, goal);
|
||||
|
||||
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 ||
|
||||
GoalResponse::ACCEPT_AND_DEFER == user_response;
|
||||
return std::make_pair(user_response, ros_response);
|
||||
|
|
@ -352,7 +350,7 @@ protected:
|
|||
|
||||
/// \internal
|
||||
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_);
|
||||
CancelResponse resp = CancelResponse::REJECT;
|
||||
|
|
@ -373,13 +371,13 @@ protected:
|
|||
void
|
||||
call_goal_accepted_callback(
|
||||
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::weak_ptr<Server<ActionT>> weak_this = this->shared_from_this();
|
||||
|
||||
std::function<void(const GoalID &, std::shared_ptr<void>)> on_terminal_state =
|
||||
[weak_this](const GoalID & uuid, std::shared_ptr<void> result_message)
|
||||
std::function<void(const GoalUUID &, std::shared_ptr<void>)> on_terminal_state =
|
||||
[weak_this](const GoalUUID & uuid, std::shared_ptr<void> result_message)
|
||||
{
|
||||
std::shared_ptr<Server<ActionT>> shared_this = weak_this.lock();
|
||||
if (!shared_this) {
|
||||
|
|
@ -396,8 +394,8 @@ protected:
|
|||
shared_this->goal_handles_.erase(uuid);
|
||||
};
|
||||
|
||||
std::function<void(const GoalID &)> on_executing =
|
||||
[weak_this](const GoalID & uuid)
|
||||
std::function<void(const GoalUUID &)> on_executing =
|
||||
[weak_this](const GoalUUID & uuid)
|
||||
{
|
||||
std::shared_ptr<Server<ActionT>> shared_this = weak_this.lock();
|
||||
if (!shared_this) {
|
||||
|
|
@ -408,8 +406,8 @@ protected:
|
|||
shared_this->publish_status();
|
||||
};
|
||||
|
||||
std::function<void(std::shared_ptr<typename ActionT::Feedback>)> publish_feedback =
|
||||
[weak_this](std::shared_ptr<typename ActionT::Feedback> feedback_msg)
|
||||
std::function<void(std::shared_ptr<typename ActionT::Impl::FeedbackMessage>)> publish_feedback =
|
||||
[weak_this](std::shared_ptr<typename ActionT::Impl::FeedbackMessage> feedback_msg)
|
||||
{
|
||||
std::shared_ptr<Server<ActionT>> shared_this = weak_this.lock();
|
||||
if (!shared_this) {
|
||||
|
|
@ -418,11 +416,12 @@ protected:
|
|||
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(
|
||||
new ServerGoalHandle<ActionT>(
|
||||
rcl_goal_handle, uuid,
|
||||
std::static_pointer_cast<const typename ActionT::Goal>(goal_request_message),
|
||||
on_terminal_state, on_executing, publish_feedback));
|
||||
rcl_goal_handle, uuid, goal, on_terminal_state, on_executing, publish_feedback));
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(goal_handles_mutex_);
|
||||
goal_handles_[uuid] = goal_handle;
|
||||
|
|
@ -431,41 +430,41 @@ protected:
|
|||
}
|
||||
|
||||
/// \internal
|
||||
GoalID
|
||||
GoalUUID
|
||||
get_goal_id_from_goal_request(void * message) override
|
||||
{
|
||||
return
|
||||
static_cast<typename ActionT::GoalRequestService::Request *>(message)->action_goal_id.uuid;
|
||||
static_cast<typename ActionT::Impl::SendGoalService::Request *>(message)->goal_id.uuid;
|
||||
}
|
||||
|
||||
/// \internal
|
||||
std::shared_ptr<void>
|
||||
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
|
||||
GoalID
|
||||
GoalUUID
|
||||
get_goal_id_from_result_request(void * message) override
|
||||
{
|
||||
return
|
||||
static_cast<typename ActionT::GoalResultService::Request *>(message)->action_goal_id.uuid;
|
||||
static_cast<typename ActionT::Impl::GetResultService::Request *>(message)->goal_id.uuid;
|
||||
}
|
||||
|
||||
/// \internal
|
||||
std::shared_ptr<void>
|
||||
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
|
||||
std::shared_ptr<void>
|
||||
create_result_response(decltype(action_msgs::msg::GoalStatus::status) status) override
|
||||
{
|
||||
auto result = std::make_shared<typename ActionT::GoalResultService::Response>();
|
||||
result->action_status = status;
|
||||
auto result = std::make_shared<typename ActionT::Impl::GetResultService::Response>();
|
||||
result->status = status;
|
||||
return std::static_pointer_cast<void>(result);
|
||||
}
|
||||
|
||||
|
|
@ -480,7 +479,7 @@ private:
|
|||
using GoalHandleWeakPtr = std::weak_ptr<ServerGoalHandle<ActionT>>;
|
||||
/// A map of goal id to goal handle weak pointers.
|
||||
/// 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_;
|
||||
};
|
||||
} // namespace rclcpp_action
|
||||
|
|
|
|||
|
|
@ -147,8 +147,10 @@ public:
|
|||
void
|
||||
publish_feedback(std::shared_ptr<typename ActionT::Feedback> feedback_msg)
|
||||
{
|
||||
feedback_msg->action_goal_id.uuid = uuid_;
|
||||
publish_feedback_(feedback_msg);
|
||||
auto feedback_message = std::make_shared<typename ActionT::Impl::FeedbackMessage>();
|
||||
feedback_message->goal_id.uuid = uuid_;
|
||||
feedback_message->feedback = *feedback_msg;
|
||||
publish_feedback_(feedback_message);
|
||||
}
|
||||
|
||||
// TODO(sloretz) which exception is raised?
|
||||
|
|
@ -165,8 +167,10 @@ public:
|
|||
set_aborted(typename ActionT::Result::SharedPtr result_msg)
|
||||
{
|
||||
_set_aborted();
|
||||
result_msg->action_status = action_msgs::msg::GoalStatus::STATUS_ABORTED;
|
||||
on_terminal_state_(uuid_, result_msg);
|
||||
auto response = std::make_shared<typename ActionT::Impl::GetResultService::Response>();
|
||||
response->status = action_msgs::msg::GoalStatus::STATUS_ABORTED;
|
||||
response->result = *result_msg;
|
||||
on_terminal_state_(uuid_, response);
|
||||
}
|
||||
|
||||
/// Indicate that a goal has been reached.
|
||||
|
|
@ -182,8 +186,10 @@ public:
|
|||
set_succeeded(typename ActionT::Result::SharedPtr result_msg)
|
||||
{
|
||||
_set_succeeded();
|
||||
result_msg->action_status = action_msgs::msg::GoalStatus::STATUS_SUCCEEDED;
|
||||
on_terminal_state_(uuid_, result_msg);
|
||||
auto response = std::make_shared<typename ActionT::Impl::GetResultService::Response>();
|
||||
response->status = action_msgs::msg::GoalStatus::STATUS_SUCCEEDED;
|
||||
response->result = *result_msg;
|
||||
on_terminal_state_(uuid_, response);
|
||||
}
|
||||
|
||||
/// Indicate that a goal has been canceled.
|
||||
|
|
@ -199,8 +205,10 @@ public:
|
|||
set_canceled(typename ActionT::Result::SharedPtr result_msg)
|
||||
{
|
||||
_set_canceled();
|
||||
result_msg->action_status = action_msgs::msg::GoalStatus::STATUS_CANCELED;
|
||||
on_terminal_state_(uuid_, result_msg);
|
||||
auto response = std::make_shared<typename ActionT::Impl::GetResultService::Response>();
|
||||
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.
|
||||
|
|
@ -215,7 +223,7 @@ public:
|
|||
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>
|
||||
get_goal() const
|
||||
{
|
||||
|
|
@ -223,7 +231,7 @@ public:
|
|||
}
|
||||
|
||||
/// Get the unique identifier of the goal
|
||||
const GoalID &
|
||||
const GoalUUID &
|
||||
get_goal_id() const
|
||||
{
|
||||
return uuid_;
|
||||
|
|
@ -233,8 +241,8 @@ public:
|
|||
{
|
||||
// Cancel goal if handle was allowed to destruct without reaching a terminal state
|
||||
if (try_canceling()) {
|
||||
auto null_result = std::make_shared<typename ActionT::Result>();
|
||||
null_result->action_status = action_msgs::msg::GoalStatus::STATUS_CANCELED;
|
||||
auto null_result = std::make_shared<typename ActionT::Impl::GetResultService::Response>();
|
||||
null_result->status = action_msgs::msg::GoalStatus::STATUS_CANCELED;
|
||||
on_terminal_state_(uuid_, null_result);
|
||||
}
|
||||
}
|
||||
|
|
@ -243,11 +251,11 @@ protected:
|
|||
/// \internal
|
||||
ServerGoalHandle(
|
||||
std::shared_ptr<rcl_action_goal_handle_t> rcl_handle,
|
||||
GoalID uuid,
|
||||
GoalUUID uuid,
|
||||
std::shared_ptr<const typename ActionT::Goal> goal,
|
||||
std::function<void(const GoalID &, std::shared_ptr<void>)> on_terminal_state,
|
||||
std::function<void(const GoalID &)> on_executing,
|
||||
std::function<void(std::shared_ptr<typename ActionT::Feedback>)> publish_feedback
|
||||
std::function<void(const GoalUUID &, std::shared_ptr<void>)> on_terminal_state,
|
||||
std::function<void(const GoalUUID &)> on_executing,
|
||||
std::function<void(std::shared_ptr<typename ActionT::Impl::FeedbackMessage>)> publish_feedback
|
||||
)
|
||||
: ServerGoalHandleBase(rcl_handle), goal_(goal), uuid_(uuid),
|
||||
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_;
|
||||
|
||||
/// A unique id for the goal request.
|
||||
const GoalID uuid_;
|
||||
const GoalUUID uuid_;
|
||||
|
||||
friend Server<ActionT>;
|
||||
|
||||
std::function<void(const GoalID &, std::shared_ptr<void>)> on_terminal_state_;
|
||||
std::function<void(const GoalID &)> on_executing_;
|
||||
std::function<void(std::shared_ptr<typename ActionT::Feedback>)> publish_feedback_;
|
||||
std::function<void(const GoalUUID &, std::shared_ptr<void>)> on_terminal_state_;
|
||||
std::function<void(const GoalUUID &)> on_executing_;
|
||||
std::function<void(std::shared_ptr<typename ActionT::Impl::FeedbackMessage>)> publish_feedback_;
|
||||
};
|
||||
} // namespace rclcpp_action
|
||||
|
||||
|
|
|
|||
|
|
@ -29,34 +29,34 @@
|
|||
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 GoalInfo = action_msgs::msg::GoalInfo;
|
||||
|
||||
/// Convert a goal id to a human readable string.
|
||||
RCLCPP_ACTION_PUBLIC
|
||||
std::string
|
||||
to_string(const GoalID & goal_id);
|
||||
to_string(const GoalUUID & goal_id);
|
||||
|
||||
// Convert C++ GoalID to rcl_action_goal_info_t
|
||||
RCLCPP_ACTION_PUBLIC
|
||||
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
|
||||
RCLCPP_ACTION_PUBLIC
|
||||
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 std
|
||||
{
|
||||
template<>
|
||||
struct less<rclcpp_action::GoalID>
|
||||
struct less<rclcpp_action::GoalUUID>
|
||||
{
|
||||
bool operator()(
|
||||
const rclcpp_action::GoalID & lhs,
|
||||
const rclcpp_action::GoalID & rhs) const
|
||||
const rclcpp_action::GoalUUID & lhs,
|
||||
const rclcpp_action::GoalUUID & rhs) const
|
||||
{
|
||||
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
|
||||
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
|
||||
size_t result = 0;
|
||||
|
|
|
|||
|
|
@ -366,10 +366,10 @@ ClientBase::send_cancel_request(std::shared_ptr<void> request, ResponseCallback
|
|||
pimpl_->pending_cancel_responses[sequence_number] = callback;
|
||||
}
|
||||
|
||||
GoalID
|
||||
GoalUUID
|
||||
ClientBase::generate_goal_id()
|
||||
{
|
||||
GoalID goal_id;
|
||||
GoalUUID goal_id;
|
||||
// TODO(hidmic): Do something better than this for UUID generation.
|
||||
// std::generate(
|
||||
// goal_id.uuid.begin(), goal_id.uuid.end(),
|
||||
|
|
|
|||
|
|
@ -28,7 +28,7 @@
|
|||
#include <vector>
|
||||
|
||||
using rclcpp_action::ServerBase;
|
||||
using rclcpp_action::GoalID;
|
||||
using rclcpp_action::GoalUUID;
|
||||
|
||||
namespace rclcpp_action
|
||||
{
|
||||
|
|
@ -62,11 +62,11 @@ public:
|
|||
bool goal_expired_ = false;
|
||||
|
||||
// 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
|
||||
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
|
||||
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_;
|
||||
};
|
||||
|
|
@ -228,7 +228,7 @@ ServerBase::execute_goal_request_received()
|
|||
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);
|
||||
|
||||
// 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 (size_t i = 0; i < goals.size; ++i) {
|
||||
const rcl_action_goal_info_t & goal_info = goals.data[i];
|
||||
GoalID uuid;
|
||||
GoalUUID uuid;
|
||||
convert(goal_info, &uuid);
|
||||
auto response_code = call_handle_cancel_callback(uuid);
|
||||
if (CancelResponse::ACCEPT == response_code) {
|
||||
|
|
@ -388,7 +388,7 @@ ServerBase::execute_result_request_received()
|
|||
std::shared_ptr<void> result_response;
|
||||
|
||||
// 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;
|
||||
convert(uuid, &goal_info);
|
||||
bool goal_exists;
|
||||
|
|
@ -433,7 +433,7 @@ ServerBase::execute_check_expired_goals()
|
|||
rclcpp::exceptions::throw_from_rcl_error(ret);
|
||||
} else if (num_expired) {
|
||||
// A goal expired!
|
||||
GoalID uuid;
|
||||
GoalUUID uuid;
|
||||
convert(expired_goals[0], &uuid);
|
||||
RCLCPP_DEBUG(pimpl_->logger_, "Expired goal %s", to_string(uuid).c_str());
|
||||
pimpl_->goal_results_.erase(uuid);
|
||||
|
|
@ -497,7 +497,7 @@ ServerBase::publish_status()
|
|||
}
|
||||
|
||||
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
|
||||
rcl_action_goal_info_t goal_info;
|
||||
|
|
|
|||
|
|
@ -20,7 +20,7 @@
|
|||
namespace rclcpp_action
|
||||
{
|
||||
std::string
|
||||
to_string(const GoalID & goal_id)
|
||||
to_string(const GoalUUID & goal_id)
|
||||
{
|
||||
std::stringstream stream;
|
||||
stream << std::hex;
|
||||
|
|
@ -31,7 +31,7 @@ to_string(const GoalID & goal_id)
|
|||
}
|
||||
|
||||
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) {
|
||||
info->goal_id.uuid[i] = goal_id[i];
|
||||
|
|
@ -39,7 +39,7 @@ convert(const GoalID & goal_id, rcl_action_goal_info_t * info)
|
|||
}
|
||||
|
||||
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) {
|
||||
(*goal_id)[i] = info.goal_id.uuid[i];
|
||||
|
|
|
|||
|
|
@ -24,7 +24,6 @@
|
|||
#include <rclcpp/clock.hpp>
|
||||
#include <rclcpp/exceptions.hpp>
|
||||
#include <rclcpp/executors.hpp>
|
||||
#include <rclcpp/executors/multi_threaded_executor.hpp>
|
||||
#include <rclcpp/node.hpp>
|
||||
#include <rclcpp/publisher.hpp>
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
|
|
@ -55,17 +54,17 @@ protected:
|
|||
using ActionType = test_msgs::action::Fibonacci;
|
||||
using ActionGoal = ActionType::Goal;
|
||||
using ActionGoalHandle = rclcpp_action::ClientGoalHandle<ActionType>;
|
||||
using ActionGoalRequestService = ActionType::GoalRequestService;
|
||||
using ActionGoalRequestService = ActionType::Impl::SendGoalService;
|
||||
using ActionGoalRequest = ActionGoalRequestService::Request;
|
||||
using ActionGoalResponse = ActionGoalRequestService::Response;
|
||||
using ActionGoalResultService = ActionType::GoalResultService;
|
||||
using ActionGoalResultService = ActionType::Impl::GetResultService;
|
||||
using ActionGoalResultRequest = ActionGoalResultService::Request;
|
||||
using ActionGoalResultResponse = ActionGoalResultService::Response;
|
||||
using ActionCancelGoalService = ActionType::CancelGoalService;
|
||||
using ActionCancelGoalRequest = ActionType::CancelGoalService::Request;
|
||||
using ActionCancelGoalResponse = ActionType::CancelGoalService::Response;
|
||||
using ActionStatusMessage = ActionType::GoalStatusMessage;
|
||||
using ActionFeedbackMessage = ActionType::Feedback;
|
||||
using ActionCancelGoalService = ActionType::Impl::CancelGoalService;
|
||||
using ActionCancelGoalRequest = ActionType::Impl::CancelGoalService::Request;
|
||||
using ActionCancelGoalResponse = ActionType::Impl::CancelGoalService::Response;
|
||||
using ActionStatusMessage = ActionType::Impl::GoalStatusMessage;
|
||||
using ActionFeedbackMessage = ActionType::Impl::FeedbackMessage;
|
||||
using ActionFeedback = ActionType::Feedback;
|
||||
|
||||
static void SetUpTestCase()
|
||||
|
|
@ -90,9 +89,9 @@ protected:
|
|||
ActionGoalResponse::SharedPtr response)
|
||||
{
|
||||
response->stamp = clock.now();
|
||||
response->accepted = (request->order >= 0);
|
||||
response->accepted = (request->goal.order >= 0);
|
||||
if (response->accepted) {
|
||||
goals[request->action_goal_id.uuid] = {request, response};
|
||||
goals[request->goal_id.uuid] = {request, response};
|
||||
}
|
||||
});
|
||||
ASSERT_TRUE(goal_service != nullptr);
|
||||
|
|
@ -108,29 +107,30 @@ protected:
|
|||
const ActionGoalResultRequest::SharedPtr request,
|
||||
ActionGoalResultResponse::SharedPtr response)
|
||||
{
|
||||
if (goals.count(request->action_goal_id.uuid) == 1) {
|
||||
auto goal_request = goals[request->action_goal_id.uuid].first;
|
||||
auto goal_response = goals[request->action_goal_id.uuid].second;
|
||||
if (goals.count(request->goal_id.uuid) == 1) {
|
||||
auto goal_request = goals[request->goal_id.uuid].first;
|
||||
auto goal_response = goals[request->goal_id.uuid].second;
|
||||
ActionStatusMessage status_message;
|
||||
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.status = rclcpp_action::GoalStatus::STATUS_EXECUTING;
|
||||
status_message.status_list.push_back(goal_status);
|
||||
status_publisher->publish(status_message);
|
||||
client_executor.spin_once();
|
||||
ActionFeedbackMessage feedback_message;
|
||||
feedback_message.action_goal_id.uuid = goal_request->action_goal_id.uuid;
|
||||
feedback_message.sequence.push_back(0);
|
||||
feedback_message.goal_id.uuid = goal_request->goal_id.uuid;
|
||||
feedback_message.feedback.sequence.push_back(0);
|
||||
feedback_publisher->publish(feedback_message);
|
||||
client_executor.spin_once();
|
||||
if (goal_request->order > 0) {
|
||||
feedback_message.sequence.push_back(1);
|
||||
if (goal_request->goal.order > 0) {
|
||||
feedback_message.feedback.sequence.push_back(1);
|
||||
feedback_publisher->publish(feedback_message);
|
||||
client_executor.spin_once();
|
||||
for (int i = 1; i < goal_request->order; ++i) {
|
||||
feedback_message.sequence.push_back(
|
||||
feedback_message.sequence[i] + feedback_message.sequence[i - 1]);
|
||||
for (int i = 1; i < goal_request->goal.order; ++i) {
|
||||
feedback_message.feedback.sequence.push_back(
|
||||
feedback_message.feedback.sequence[i] +
|
||||
feedback_message.feedback.sequence[i - 1]);
|
||||
feedback_publisher->publish(feedback_message);
|
||||
client_executor.spin_once();
|
||||
}
|
||||
|
|
@ -139,11 +139,11 @@ protected:
|
|||
status_message.status_list[0] = goal_status;
|
||||
status_publisher->publish(status_message);
|
||||
client_executor.spin_once();
|
||||
response->sequence = feedback_message.sequence;
|
||||
response->action_status = rclcpp_action::GoalStatus::STATUS_SUCCEEDED;
|
||||
goals.erase(request->action_goal_id.uuid);
|
||||
response->result.sequence = feedback_message.feedback.sequence;
|
||||
response->status = rclcpp_action::GoalStatus::STATUS_SUCCEEDED;
|
||||
goals.erase(request->goal_id.uuid);
|
||||
} else {
|
||||
response->action_status = rclcpp_action::GoalStatus::STATUS_UNKNOWN;
|
||||
response->status = rclcpp_action::GoalStatus::STATUS_UNKNOWN;
|
||||
}
|
||||
});
|
||||
ASSERT_TRUE(result_service != nullptr);
|
||||
|
|
@ -159,7 +159,7 @@ protected:
|
|||
const ActionCancelGoalRequest::SharedPtr request,
|
||||
ActionCancelGoalResponse::SharedPtr response)
|
||||
{
|
||||
rclcpp_action::GoalID zero_uuid;
|
||||
rclcpp_action::GoalUUID zero_uuid;
|
||||
std::fill(zero_uuid.begin(), zero_uuid.end(), 0u);
|
||||
const rclcpp::Time cancel_stamp = request->goal_info.stamp;
|
||||
bool cancel_all = (
|
||||
|
|
@ -172,11 +172,11 @@ protected:
|
|||
auto goal_response = it->second.second;
|
||||
const rclcpp::Time goal_stamp = goal_response->stamp;
|
||||
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);
|
||||
if (cancel_all || cancel_this) {
|
||||
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.status = rclcpp_action::GoalStatus::STATUS_CANCELED;
|
||||
status_message.status_list.push_back(goal_status);
|
||||
|
|
@ -252,7 +252,7 @@ protected:
|
|||
const char * const action_name{"fibonacci_test"};
|
||||
|
||||
std::map<
|
||||
rclcpp_action::GoalID,
|
||||
rclcpp_action::GoalUUID,
|
||||
std::pair<
|
||||
typename ActionGoalRequest::SharedPtr,
|
||||
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());
|
||||
auto future_result = goal_handle->async_result();
|
||||
dual_spin_until_future_complete(future_result);
|
||||
auto result = future_result.get();
|
||||
ASSERT_EQ(6ul, result.response->sequence.size());
|
||||
EXPECT_EQ(0, result.response->sequence[0]);
|
||||
EXPECT_EQ(1, result.response->sequence[1]);
|
||||
EXPECT_EQ(5, result.response->sequence[5]);
|
||||
auto wrapped_result = future_result.get();
|
||||
ASSERT_EQ(6ul, wrapped_result.result->sequence.size());
|
||||
EXPECT_EQ(0, wrapped_result.result->sequence[0]);
|
||||
EXPECT_EQ(1, wrapped_result.result->sequence[1]);
|
||||
EXPECT_EQ(5, wrapped_result.result->sequence[5]);
|
||||
}
|
||||
|
||||
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());
|
||||
auto future_result = goal_handle->async_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());
|
||||
EXPECT_EQ(3, result.response->sequence.back());
|
||||
ASSERT_EQ(5u, wrapped_result.result->sequence.size());
|
||||
EXPECT_EQ(3, wrapped_result.result->sequence.back());
|
||||
EXPECT_EQ(5, feedback_count);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -26,7 +26,7 @@
|
|||
#include "rclcpp_action/server.hpp"
|
||||
|
||||
using Fibonacci = test_msgs::action::Fibonacci;
|
||||
using GoalID = rclcpp_action::GoalID;
|
||||
using GoalUUID = rclcpp_action::GoalUUID;
|
||||
|
||||
class TestServer : public ::testing::Test
|
||||
{
|
||||
|
|
@ -36,16 +36,16 @@ protected:
|
|||
rclcpp::init(0, nullptr);
|
||||
}
|
||||
|
||||
std::shared_ptr<Fibonacci::GoalRequestService::Request>
|
||||
send_goal_request(rclcpp::Node::SharedPtr node, GoalID uuid)
|
||||
std::shared_ptr<Fibonacci::Impl::SendGoalService::Request>
|
||||
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");
|
||||
if (!client->wait_for_service(std::chrono::seconds(20))) {
|
||||
throw std::runtime_error("send goal service didn't become available");
|
||||
}
|
||||
auto request = std::make_shared<Fibonacci::GoalRequestService::Request>();
|
||||
request->action_goal_id.uuid = uuid;
|
||||
auto request = std::make_shared<Fibonacci::Impl::SendGoalService::Request>();
|
||||
request->goal_id.uuid = uuid;
|
||||
auto future = client->async_send_request(request);
|
||||
if (rclcpp::executor::FutureReturnCode::SUCCESS !=
|
||||
rclcpp::spin_until_future_complete(node, future))
|
||||
|
|
@ -56,14 +56,14 @@ protected:
|
|||
}
|
||||
|
||||
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");
|
||||
if (!cancel_client->wait_for_service(std::chrono::seconds(20))) {
|
||||
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;
|
||||
auto future = cancel_client->async_send_request(request);
|
||||
if (rclcpp::executor::FutureReturnCode::SUCCESS !=
|
||||
|
|
@ -80,7 +80,7 @@ TEST_F(TestServer, construction_and_destruction)
|
|||
|
||||
using GoalHandle = rclcpp_action::ServerGoalHandle<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;
|
||||
},
|
||||
[](std::shared_ptr<GoalHandle>) {
|
||||
|
|
@ -93,10 +93,10 @@ TEST_F(TestServer, construction_and_destruction)
|
|||
TEST_F(TestServer, handle_goal_called)
|
||||
{
|
||||
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](
|
||||
const GoalID & uuid, std::shared_ptr<const Fibonacci::Goal>)
|
||||
const GoalUUID & uuid, std::shared_ptr<const Fibonacci::Goal>)
|
||||
{
|
||||
received_uuid = uuid;
|
||||
return rclcpp_action::GoalResponse::REJECT;
|
||||
|
|
@ -114,15 +114,15 @@ TEST_F(TestServer, handle_goal_called)
|
|||
// Create a client that calls the goal request service
|
||||
// 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");
|
||||
|
||||
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}};
|
||||
request->action_goal_id.uuid = uuid;
|
||||
const GoalUUID uuid{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16}};
|
||||
request->goal_id.uuid = uuid;
|
||||
|
||||
auto future = client->async_send_request(request);
|
||||
ASSERT_EQ(
|
||||
|
|
@ -135,10 +135,10 @@ TEST_F(TestServer, handle_goal_called)
|
|||
TEST_F(TestServer, handle_accepted_called)
|
||||
{
|
||||
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 = [](
|
||||
const GoalID &, std::shared_ptr<const Fibonacci::Goal>)
|
||||
const GoalUUID &, std::shared_ptr<const Fibonacci::Goal>)
|
||||
{
|
||||
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->is_active());
|
||||
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)
|
||||
{
|
||||
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 = [](
|
||||
const GoalID &, std::shared_ptr<const Fibonacci::Goal>)
|
||||
const GoalUUID &, std::shared_ptr<const Fibonacci::Goal>)
|
||||
{
|
||||
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
|
||||
};
|
||||
|
|
@ -210,10 +210,10 @@ TEST_F(TestServer, handle_cancel_called)
|
|||
TEST_F(TestServer, publish_status_accepted)
|
||||
{
|
||||
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 = [](
|
||||
const GoalID &, std::shared_ptr<const Fibonacci::Goal>)
|
||||
const GoalUUID &, std::shared_ptr<const Fibonacci::Goal>)
|
||||
{
|
||||
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
|
||||
};
|
||||
|
|
@ -271,10 +271,10 @@ TEST_F(TestServer, publish_status_accepted)
|
|||
TEST_F(TestServer, publish_status_canceling)
|
||||
{
|
||||
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 = [](
|
||||
const GoalID &, std::shared_ptr<const Fibonacci::Goal>)
|
||||
const GoalUUID &, std::shared_ptr<const Fibonacci::Goal>)
|
||||
{
|
||||
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
|
||||
};
|
||||
|
|
@ -326,10 +326,10 @@ TEST_F(TestServer, publish_status_canceling)
|
|||
TEST_F(TestServer, publish_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 = [](
|
||||
const GoalID &, std::shared_ptr<const Fibonacci::Goal>)
|
||||
const GoalUUID &, std::shared_ptr<const Fibonacci::Goal>)
|
||||
{
|
||||
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
|
||||
};
|
||||
|
|
@ -383,10 +383,10 @@ TEST_F(TestServer, publish_status_canceled)
|
|||
TEST_F(TestServer, publish_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 = [](
|
||||
const GoalID &, std::shared_ptr<const Fibonacci::Goal>)
|
||||
const GoalUUID &, std::shared_ptr<const Fibonacci::Goal>)
|
||||
{
|
||||
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
|
||||
};
|
||||
|
|
@ -438,10 +438,10 @@ TEST_F(TestServer, publish_status_succeeded)
|
|||
TEST_F(TestServer, publish_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 = [](
|
||||
const GoalID &, std::shared_ptr<const Fibonacci::Goal>)
|
||||
const GoalUUID &, std::shared_ptr<const Fibonacci::Goal>)
|
||||
{
|
||||
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
|
||||
};
|
||||
|
|
@ -493,10 +493,10 @@ TEST_F(TestServer, publish_status_aborted)
|
|||
TEST_F(TestServer, publish_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 = [](
|
||||
const GoalID &, std::shared_ptr<const Fibonacci::Goal>)
|
||||
const GoalUUID &, std::shared_ptr<const Fibonacci::Goal>)
|
||||
{
|
||||
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
|
||||
};
|
||||
|
|
@ -521,7 +521,7 @@ TEST_F(TestServer, publish_feedback)
|
|||
(void)as;
|
||||
|
||||
// Subscribe to feedback messages
|
||||
using FeedbackT = Fibonacci::Feedback;
|
||||
using FeedbackT = Fibonacci::Impl::FeedbackMessage;
|
||||
std::vector<FeedbackT::SharedPtr> received_msgs;
|
||||
auto subscriber = node->create_subscription<FeedbackT>(
|
||||
"fibonacci/_action/feedback", [&received_msgs](FeedbackT::SharedPtr msg)
|
||||
|
|
@ -531,7 +531,7 @@ TEST_F(TestServer, publish_feedback)
|
|||
|
||||
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};
|
||||
received_handle->publish_feedback(sent_message);
|
||||
|
||||
|
|
@ -544,17 +544,16 @@ TEST_F(TestServer, publish_feedback)
|
|||
|
||||
ASSERT_EQ(1u, received_msgs.size());
|
||||
auto & msg = received_msgs.back();
|
||||
ASSERT_EQ(sent_message->sequence, msg->sequence);
|
||||
ASSERT_EQ(uuid, msg->action_goal_id.uuid);
|
||||
ASSERT_EQ(sent_message->sequence, msg->feedback.sequence);
|
||||
}
|
||||
|
||||
TEST_F(TestServer, 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 = [](
|
||||
const GoalID &, std::shared_ptr<const Fibonacci::Goal>)
|
||||
const GoalUUID &, std::shared_ptr<const Fibonacci::Goal>)
|
||||
{
|
||||
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
|
||||
};
|
||||
|
|
@ -581,13 +580,13 @@ TEST_F(TestServer, get_result)
|
|||
send_goal_request(node, uuid);
|
||||
|
||||
// Send result request
|
||||
auto result_client = node->create_client<Fibonacci::GoalResultService>(
|
||||
auto result_client = node->create_client<Fibonacci::Impl::GetResultService>(
|
||||
"fibonacci/_action/get_result");
|
||||
if (!result_client->wait_for_service(std::chrono::seconds(20))) {
|
||||
throw std::runtime_error("get result service didn't become available");
|
||||
}
|
||||
auto request = std::make_shared<Fibonacci::GoalResultService::Request>();
|
||||
request->action_goal_id.uuid = uuid;
|
||||
auto request = std::make_shared<Fibonacci::Impl::GetResultService::Request>();
|
||||
request->goal_id.uuid = uuid;
|
||||
auto future = result_client->async_send_request(request);
|
||||
|
||||
// Send a result
|
||||
|
|
@ -600,17 +599,17 @@ TEST_F(TestServer, get_result)
|
|||
rclcpp::spin_until_future_complete(node, future));
|
||||
|
||||
auto response = future.get();
|
||||
EXPECT_EQ(action_msgs::msg::GoalStatus::STATUS_SUCCEEDED, response->action_status);
|
||||
EXPECT_EQ(result->sequence, response->sequence);
|
||||
EXPECT_EQ(action_msgs::msg::GoalStatus::STATUS_SUCCEEDED, response->status);
|
||||
EXPECT_EQ(result->sequence, response->result.sequence);
|
||||
}
|
||||
|
||||
TEST_F(TestServer, deferred_execution)
|
||||
{
|
||||
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 = [](
|
||||
const GoalID &, std::shared_ptr<const Fibonacci::Goal>)
|
||||
const GoalUUID &, std::shared_ptr<const Fibonacci::Goal>)
|
||||
{
|
||||
return rclcpp_action::GoalResponse::ACCEPT_AND_DEFER;
|
||||
};
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue