Skip to content

Commit

Permalink
match renamed action types
Browse files Browse the repository at this point in the history
  • Loading branch information
@dirk-thomas
dirk-thomas committed Feb 12, 2019
1 parent 4046563 commit da7f21e
Show file tree
Hide file tree
Showing 6 changed files with 101 additions and 108 deletions.
59 changes: 28 additions & 31 deletions rclcpp_action/include/rclcpp_action/client.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -262,8 +262,8 @@ class Client : public ClientBase
using GoalHandle = ClientGoalHandle<ActionT>;
using Result = typename GoalHandle::Result;
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,24 @@ 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 = 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;
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 Down Expand Up @@ -406,15 +404,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 +427,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 GoalID & goal_id = feedback_message->goal_id;
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,7 +465,7 @@ 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;
Expand Down Expand Up @@ -498,20 +493,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 = 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);
using GoalResultResponse = typename ActionT::Impl::GetResultService::Response;
auto result_response = std::static_pointer_cast<GoalResultResponse>(response);
result.response = std::make_shared<typename ActionT::Result>();
*result.response = result_response->result;
result.goal_id = goal_handle->get_goal_id();
result.code = static_cast<ResultCode>(result.response->action_status);
result.code = static_cast<ResultCode>(result_response->status);
goal_handle->set_result(result);
std::lock_guard<std::mutex> lock(goal_handles_mutex_);
goal_handles_.erase(goal_handle->get_goal_id());
Expand Down
3 changes: 2 additions & 1 deletion rclcpp_action/include/rclcpp_action/client_goal_handle.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -65,7 +65,8 @@ class ClientGoalHandle
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();

Expand Down
22 changes: 9 additions & 13 deletions rclcpp_action/include/rclcpp_action/server.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -337,14 +337,10 @@ class Server : public ServerBase, public std::enable_shared_from_this<Server<Act
std::pair<GoalResponse, std::shared_ptr<void>>
call_handle_goal_callback(GoalID & 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 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);
Expand Down Expand Up @@ -408,8 +404,8 @@ class Server : public ServerBase, public std::enable_shared_from_this<Server<Act
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) {
Expand All @@ -435,37 +431,37 @@ class Server : public ServerBase, public std::enable_shared_from_this<Server<Act
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;
}

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

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

Expand Down
24 changes: 12 additions & 12 deletions rclcpp_action/include/rclcpp_action/server_goal_handle.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -145,9 +145,9 @@ class ServerGoalHandle : public ServerGoalHandleBase
* \param[in] feedback_msg the message to publish to clients.
*/
void
publish_feedback(std::shared_ptr<typename ActionT::Feedback> feedback_msg)
publish_feedback(std::shared_ptr<typename ActionT::Impl::FeedbackMessage> feedback_msg)
{
feedback_msg->action_goal_id.uuid = uuid_;
feedback_msg->goal_id = uuid_;
publish_feedback_(feedback_msg);
}

Expand All @@ -162,10 +162,10 @@ class ServerGoalHandle : public ServerGoalHandleBase
* \param[in] result_msg the final result to send to clients.
*/
void
set_aborted(typename ActionT::Result::SharedPtr result_msg)
set_aborted(typename ActionT::Impl::GetResultService::Response::SharedPtr result_msg)
{
_set_aborted();
result_msg->action_status = action_msgs::msg::GoalStatus::STATUS_ABORTED;
result_msg->status = action_msgs::msg::GoalStatus::STATUS_ABORTED;
on_terminal_state_(uuid_, result_msg);
}

Expand All @@ -179,10 +179,10 @@ class ServerGoalHandle : public ServerGoalHandleBase
* \param[in] result_msg the final result to send to clients.
*/
void
set_succeeded(typename ActionT::Result::SharedPtr result_msg)
set_succeeded(typename ActionT::Impl::GetResultService::Response::SharedPtr result_msg)
{
_set_succeeded();
result_msg->action_status = action_msgs::msg::GoalStatus::STATUS_SUCCEEDED;
result_msg->status = action_msgs::msg::GoalStatus::STATUS_SUCCEEDED;
on_terminal_state_(uuid_, result_msg);
}

Expand All @@ -196,10 +196,10 @@ class ServerGoalHandle : public ServerGoalHandleBase
* \param[in] result_msg the final result to send to clients.
*/
void
set_canceled(typename ActionT::Result::SharedPtr result_msg)
set_canceled(typename ActionT::Impl::GetResultService::Response::SharedPtr result_msg)
{
_set_canceled();
result_msg->action_status = action_msgs::msg::GoalStatus::STATUS_CANCELED;
result_msg->status = action_msgs::msg::GoalStatus::STATUS_CANCELED;
on_terminal_state_(uuid_, result_msg);
}

Expand Down Expand Up @@ -233,8 +233,8 @@ class ServerGoalHandle : public ServerGoalHandleBase
{
// 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);
}
}
Expand All @@ -247,7 +247,7 @@ class ServerGoalHandle : public ServerGoalHandleBase
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(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),
Expand All @@ -265,7 +265,7 @@ class ServerGoalHandle : public ServerGoalHandleBase

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(std::shared_ptr<typename ActionT::Impl::FeedbackMessage>)> publish_feedback_;
};
} // namespace rclcpp_action

Expand Down
Loading

0 comments on commit da7f21e

Please sign in to comment.