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;
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue