Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

update to use separated action types #601

Merged
merged 10 commits into from
Mar 12, 2019
Merged

update to use separated action types #601

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
10 commits
Select commit Hold shift + click to select a range
e813b71
match renamed action types
dirk-thomas Feb 12, 2019
f31fa24
fix action type casting
dirk-thomas Feb 14, 2019
5a549be
rename type/field to use correct term
dirk-thomas Feb 15, 2019
29c5da9
rename custom GoalID type to avoid naming collision, update types usi…
dirk-thomas Feb 18, 2019
9e626fb
remove obsolete comments
dirk-thomas Feb 21, 2019
881501b
change signature of set_succeeded / set_canceled
dirk-thomas Feb 26, 2019
37f9095
change signature of on_terminal_state_(uuid_, result_msg);set_suc…
dirk-thomas Feb 26, 2019
f81e862
change signature of set_aborted
dirk-thomas Feb 26, 2019
d1e8c9c
change signature of publish_feedback
dirk-thomas Feb 26, 2019
0026e25
update another test
dirk-thomas Feb 26, 2019
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
78 changes: 36 additions & 42 deletions rclcpp_action/include/rclcpp_action/client.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -148,7 +148,7 @@ class ClientBase : public rclcpp::Waitable
/// \internal
RCLCPP_ACTION_PUBLIC
virtual
GoalID
GoalUUID
generate_goal_id();

/// \internal
Expand Down Expand Up @@ -260,10 +260,10 @@ class Client : public ClientBase
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,
Expand All @@ -286,26 +286,23 @@ class Client : public ClientBase
// 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));
Expand All @@ -324,7 +321,7 @@ class Client : public ClientBase
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_);
Expand Down Expand Up @@ -406,15 +403,15 @@ class Client : public ClientBase
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());
}

/// \internal
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());
}

Expand All @@ -429,39 +426,36 @@ class Client : public ClientBase
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
void
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(),
"Received feedback for unknown goal. Ignoring...");
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());
}

Expand All @@ -470,11 +464,10 @@ class Client : public ClientBase
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(),
Expand All @@ -498,21 +491,22 @@ class Client : public ClientBase
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());
});
Expand All @@ -536,7 +530,7 @@ class Client : public ClientBase
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
Expand Down
21 changes: 11 additions & 10 deletions rclcpp_action/include/rclcpp_action/client_goal_handle.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -52,30 +52,31 @@ class ClientGoalHandle
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
Expand Down Expand Up @@ -108,16 +109,16 @@ class ClientGoalHandle
set_status(int8_t status);

void
set_result(const Result & result);
set_result(const WrappedResult & wrapped_result);

void
invalidate();

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};
Expand Down
10 changes: 5 additions & 5 deletions rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -39,7 +39,7 @@ ClientGoalHandle<ActionT>::~ClientGoalHandle()
}

template<typename ActionT>
const GoalID &
const GoalUUID &
ClientGoalHandle<ActionT>::get_goal_id() const
{
// return info_.goal_id;
Expand All @@ -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_);
Expand All @@ -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>
Expand Down
Loading