Skip to content

Commit

Permalink
update to use separated action types (#601)
Browse files Browse the repository at this point in the history 
* 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
  • Loading branch information
@hidmic @dirk-thomas
hidmic authored and dirk-thomas committed Mar 12, 2019
1 parent d2d9ad8 commit 718d24f
Show file tree
Hide file tree
Showing 11 changed files with 221 additions and 220 deletions.
78 changes: 36 additions & 42 deletions rclcpp_action/include/rclcpp_action/client.hpp
View file
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
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
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

0 comments on commit 718d24f

Please sign in to comment.