Skip to content

Commit

Permalink
rename custom GoalID type to avoid naming collision, update types usi…
Browse files Browse the repository at this point in the history
…ng unique_identifier_msgs
  • Loading branch information
dirk-thomas committed Feb 18, 2019
1 parent 9b5cd43 commit 78eb836
Show file tree
Hide file tree
Showing 11 changed files with 102 additions and 103 deletions.
17 changes: 8 additions & 9 deletions rclcpp_action/include/rclcpp_action/client.hpp
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 @@ -288,7 +288,7 @@ class Client : public ClientBase
std::shared_future<typename GoalHandle::SharedPtr> future(promise->get_future());
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_id.uuid = this->generate_goal_id();
goal_request->goal = goal;
this->send_goal_request(
std::static_pointer_cast<void>(goal_request),
Expand All @@ -302,8 +302,7 @@ class Client : public ClientBase
return;
}
GoalInfo goal_info;
// goal_info.goal_id = goal_request->goal_id;
goal_info.goal_id.uuid = goal_request->goal_id;
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 Down Expand Up @@ -439,7 +438,7 @@ class Client : public ClientBase
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;
const GoalUUID & goal_id = feedback_message->goal_id.uuid;
if (goal_handles_.count(goal_id) == 0) {
RCLCPP_DEBUG(
this->get_logger(),
Expand Down Expand Up @@ -468,8 +467,8 @@ class Client : public ClientBase
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;
const GoalUUID & goal_id = status.goal_info.goal_id.uuid;
if (goal_handles_.count(goal_id) == 0) {
RCLCPP_DEBUG(
this->get_logger(),
Expand All @@ -496,7 +495,7 @@ class Client : public ClientBase
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->goal_id = 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
Expand Down Expand Up @@ -533,7 +532,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
4 changes: 2 additions & 2 deletions rclcpp_action/include/rclcpp_action/client_goal_handle.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -55,7 +55,7 @@ class ClientGoalHandle
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
Expand All @@ -70,7 +70,7 @@ class ClientGoalHandle

virtual ~ClientGoalHandle();

const GoalID &
const GoalUUID &
get_goal_id() const;

rclcpp::Time
Expand Down
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 Down
38 changes: 19 additions & 19 deletions rclcpp_action/include/rclcpp_action/server.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -146,7 +146,7 @@ class ServerBase : public rclcpp::Waitable
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.
Expand All @@ -155,13 +155,13 @@ class ServerBase : public rclcpp::Waitable
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.
Expand All @@ -178,13 +178,13 @@ class ServerBase : public rclcpp::Waitable
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.
Expand Down Expand Up @@ -214,7 +214,7 @@ class ServerBase : public rclcpp::Waitable
/// \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
Expand Down Expand Up @@ -272,7 +272,7 @@ class Server : public ServerBase, public std::enable_shared_from_this<Server<Act

/// 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.
Expand Down Expand Up @@ -335,7 +335,7 @@ class Server : public ServerBase, public std::enable_shared_from_this<Server<Act

/// \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
{
auto request = std::static_pointer_cast<
typename ActionT::Impl::SendGoalService::Request>(message);
Expand All @@ -350,7 +350,7 @@ class Server : public ServerBase, public std::enable_shared_from_this<Server<Act

/// \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;
Expand All @@ -371,13 +371,13 @@ class Server : public ServerBase, public std::enable_shared_from_this<Server<Act
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) {
Expand All @@ -394,8 +394,8 @@ class Server : public ServerBase, public std::enable_shared_from_this<Server<Act
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) {
Expand Down Expand Up @@ -430,11 +430,11 @@ class Server : public ServerBase, public std::enable_shared_from_this<Server<Act
}

/// \internal
GoalID
GoalUUID
get_goal_id_from_goal_request(void * message) override
{
return
static_cast<typename ActionT::Impl::SendGoalService::Request *>(message)->goal_id;
static_cast<typename ActionT::Impl::SendGoalService::Request *>(message)->goal_id.uuid;
}

/// \internal
Expand All @@ -445,11 +445,11 @@ class Server : public ServerBase, public std::enable_shared_from_this<Server<Act
}

/// \internal
GoalID
GoalUUID
get_goal_id_from_result_request(void * message) override
{
return
static_cast<typename ActionT::Impl::GetResultService::Request *>(message)->goal_id;
static_cast<typename ActionT::Impl::GetResultService::Request *>(message)->goal_id.uuid;
}

/// \internal
Expand Down Expand Up @@ -479,7 +479,7 @@ class Server : public ServerBase, public std::enable_shared_from_this<Server<Act
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
Expand Down
16 changes: 8 additions & 8 deletions rclcpp_action/include/rclcpp_action/server_goal_handle.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -147,7 +147,7 @@ class ServerGoalHandle : public ServerGoalHandleBase
void
publish_feedback(std::shared_ptr<typename ActionT::Impl::FeedbackMessage> feedback_msg)
{
feedback_msg->goal_id = uuid_;
feedback_msg->goal_id.uuid = uuid_;
publish_feedback_(feedback_msg);
}

Expand Down Expand Up @@ -223,7 +223,7 @@ class ServerGoalHandle : public ServerGoalHandleBase
}

/// Get the unique identifier of the goal
const GoalID &
const GoalUUID &
get_goal_id() const
{
return uuid_;
Expand All @@ -243,10 +243,10 @@ class ServerGoalHandle : public ServerGoalHandleBase
/// \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(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),
Expand All @@ -259,12 +259,12 @@ class ServerGoalHandle : public ServerGoalHandleBase
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(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
Expand Down
18 changes: 9 additions & 9 deletions rclcpp_action/include/rclcpp_action/types.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -29,44 +29,44 @@
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;
}
};

/// 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;
Expand Down
4 changes: 2 additions & 2 deletions rclcpp_action/src/client.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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(),
Expand Down
Loading

0 comments on commit 78eb836

Please sign in to comment.