[rclcpp_action] Action client implementation (#594)
* WIP * Removed async_cancel from action ClintGoalHandle API * Added status handler to action client goal handler * Added result handler to action client goal handler * Identation fix * Added get/set for action client goal handler * Changed action client goal handler attrs from rcl to cpp versions * Added check methods to action client goal handler * Removed rcl_client pointer from action client goal handler * Added basic waitable interface to action client * Updated waitable execute from action client * Added throw for rcl calls in action client * Removed duplicated ready flags from action client * Minor fix * Added header to action ClientBaseImpl execute * Mich's update to action client interface * Added trailing suffix to client pimpl attrs * Towards a consistent action client * Misc fixes for the action client * Yet more misc fixes for the action client * Few more fixes and shortcuts to deal with missing type support. * Fixed lint errors in action headers and client * Fixes to action client internal workflow. * Misc fixes to get client example to build * More misck client fixes * Remove debug print * replace logging with throw_from_rcl_error * Wrap result object given by client to user * Fix a couple bugs trying to cancel goals * Use unique_indentifier_msgs * create_client accepts group and removes waitable * Uncrustify fixes * [rclcpp_action] Adds tests for action client. * [WIP] Failing action client tests. * [rclcpp_action] Action client tests passing. * Spin both executors to make tests pass on my machine * Feedback callback uses shared pointer * comment about why make_result_aware is called * Client documentation * Execute one thing at a time * Return nullptr instead of throwing RejectedGoalError * ClientGoalHandle worries about feedback awareness * cpplint + uncrustify * Use node logging interface * ACTION -> ActionT * Make ClientBase constructor protected * Return types on different line * Avoid passing const reference to temporary * Child logger rclcpp_action * Child logger rclcpp_action * possible windows fixes * remove excess space * swap argument order * Misc test additions * Windows independent_bits_engine can't do uint8_t * Windows link issues
This commit is contained in:
Michel Hidalgo
Shane Loretz
parent
33f1e1776c
commit
91167393ea
8 changed files with 1510 additions and 91 deletions
|
|
@ -15,17 +15,26 @@
|
|||
#ifndef RCLCPP_ACTION__CLIENT_HPP_
|
||||
#define RCLCPP_ACTION__CLIENT_HPP_
|
||||
|
||||
#include <rclcpp/macros.hpp>
|
||||
#include <rclcpp/node_interfaces/node_base_interface.hpp>
|
||||
#include <rclcpp/logger.hpp>
|
||||
#include <rclcpp/time.hpp>
|
||||
#include <rclcpp/waitable.hpp>
|
||||
|
||||
#include <rosidl_generator_c/action_type_support_struct.h>
|
||||
#include <rclcpp/node_interfaces/node_base_interface.hpp>
|
||||
#include <rclcpp/time.hpp>
|
||||
#include <rosidl_typesupport_cpp/action_type_support.hpp>
|
||||
|
||||
#include <algorithm>
|
||||
#include <functional>
|
||||
#include <future>
|
||||
#include <map>
|
||||
#include <memory>
|
||||
#include <mutex>
|
||||
#include <string>
|
||||
#include <utility>
|
||||
|
||||
#include "rclcpp_action/client_goal_handle.hpp"
|
||||
#include "rclcpp_action/types.hpp"
|
||||
#include "rclcpp_action/visibility_control.hpp"
|
||||
|
||||
|
||||
|
|
@ -35,65 +44,475 @@ namespace rclcpp_action
|
|||
class ClientBaseImpl;
|
||||
|
||||
/// Base Action Client implementation
|
||||
/// It is responsible for interfacing with the C action client API.
|
||||
class ClientBase
|
||||
/// \internal
|
||||
/**
|
||||
* This class should not be used directly by users wanting to create an aciton client.
|
||||
* Instead users should use `rclcpp_action::Client<>`.
|
||||
*
|
||||
* Internally, this class is responsible for interfacing with the `rcl_action` API.
|
||||
*/
|
||||
class ClientBase : public rclcpp::Waitable
|
||||
{
|
||||
public:
|
||||
// TODO(sloretz) NodeLoggingInterface when it can be gotten off a node
|
||||
RCLCPP_ACTION_PUBLIC
|
||||
ClientBase(
|
||||
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base,
|
||||
const std::string & name,
|
||||
const rosidl_action_type_support_t * type_support);
|
||||
|
||||
RCLCPP_ACTION_PUBLIC
|
||||
virtual ~ClientBase();
|
||||
|
||||
// TODO(sloretz) add a link between this class and callbacks in the templated
|
||||
// -------------
|
||||
// Waitables API
|
||||
|
||||
/// \internal
|
||||
RCLCPP_ACTION_PUBLIC
|
||||
size_t
|
||||
get_number_of_ready_subscriptions() override;
|
||||
|
||||
/// \internal
|
||||
RCLCPP_ACTION_PUBLIC
|
||||
size_t
|
||||
get_number_of_ready_timers() override;
|
||||
|
||||
/// \internal
|
||||
RCLCPP_ACTION_PUBLIC
|
||||
size_t
|
||||
get_number_of_ready_clients() override;
|
||||
|
||||
/// \internal
|
||||
RCLCPP_ACTION_PUBLIC
|
||||
size_t
|
||||
get_number_of_ready_services() override;
|
||||
|
||||
/// \internal
|
||||
RCLCPP_ACTION_PUBLIC
|
||||
size_t
|
||||
get_number_of_ready_guard_conditions() override;
|
||||
|
||||
/// \internal
|
||||
RCLCPP_ACTION_PUBLIC
|
||||
bool
|
||||
add_to_wait_set(rcl_wait_set_t * wait_set) override;
|
||||
|
||||
/// \internal
|
||||
RCLCPP_ACTION_PUBLIC
|
||||
bool
|
||||
is_ready(rcl_wait_set_t * wait_set) override;
|
||||
|
||||
/// \internal
|
||||
RCLCPP_ACTION_PUBLIC
|
||||
void
|
||||
execute() override;
|
||||
|
||||
// End Waitables API
|
||||
// -----------------
|
||||
|
||||
protected:
|
||||
RCLCPP_ACTION_PUBLIC
|
||||
ClientBase(
|
||||
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base,
|
||||
rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging,
|
||||
const std::string & action_name,
|
||||
const rosidl_action_type_support_t * type_support,
|
||||
const rcl_action_client_options_t & options);
|
||||
|
||||
// -----------------------------------------------------
|
||||
// API for communication between ClientBase and Client<>
|
||||
using ResponseCallback = std::function<void (std::shared_ptr<void> response)>;
|
||||
|
||||
/// \internal
|
||||
RCLCPP_ACTION_PUBLIC
|
||||
rclcpp::Logger get_logger();
|
||||
|
||||
/// \internal
|
||||
RCLCPP_ACTION_PUBLIC
|
||||
virtual
|
||||
GoalID
|
||||
generate_goal_id();
|
||||
|
||||
/// \internal
|
||||
RCLCPP_ACTION_PUBLIC
|
||||
virtual
|
||||
void
|
||||
send_goal_request(
|
||||
std::shared_ptr<void> request,
|
||||
ResponseCallback callback);
|
||||
|
||||
/// \internal
|
||||
RCLCPP_ACTION_PUBLIC
|
||||
virtual
|
||||
void
|
||||
send_result_request(
|
||||
std::shared_ptr<void> request,
|
||||
ResponseCallback callback);
|
||||
|
||||
/// \internal
|
||||
RCLCPP_ACTION_PUBLIC
|
||||
virtual
|
||||
void
|
||||
send_cancel_request(
|
||||
std::shared_ptr<void> request,
|
||||
ResponseCallback callback);
|
||||
|
||||
/// \internal
|
||||
virtual
|
||||
std::shared_ptr<void>
|
||||
create_goal_response() const = 0;
|
||||
|
||||
/// \internal
|
||||
RCLCPP_ACTION_PUBLIC
|
||||
virtual
|
||||
void
|
||||
handle_goal_response(
|
||||
const rmw_request_id_t & response_header,
|
||||
std::shared_ptr<void> goal_response);
|
||||
|
||||
/// \internal
|
||||
virtual
|
||||
std::shared_ptr<void>
|
||||
create_result_response() const = 0;
|
||||
|
||||
/// \internal
|
||||
RCLCPP_ACTION_PUBLIC
|
||||
virtual
|
||||
void
|
||||
handle_result_response(
|
||||
const rmw_request_id_t & response_header,
|
||||
std::shared_ptr<void> result_response);
|
||||
|
||||
/// \internal
|
||||
virtual
|
||||
std::shared_ptr<void>
|
||||
create_cancel_response() const = 0;
|
||||
|
||||
/// \internal
|
||||
RCLCPP_ACTION_PUBLIC
|
||||
virtual
|
||||
void
|
||||
handle_cancel_response(
|
||||
const rmw_request_id_t & response_header,
|
||||
std::shared_ptr<void> cancel_response);
|
||||
|
||||
/// \internal
|
||||
virtual
|
||||
std::shared_ptr<void>
|
||||
create_feedback_message() const = 0;
|
||||
|
||||
/// \internal
|
||||
virtual
|
||||
void
|
||||
handle_feedback_message(std::shared_ptr<void> message) = 0;
|
||||
|
||||
/// \internal
|
||||
virtual
|
||||
std::shared_ptr<void>
|
||||
create_status_message() const = 0;
|
||||
|
||||
/// \internal
|
||||
virtual
|
||||
void
|
||||
handle_status_message(std::shared_ptr<void> message) = 0;
|
||||
|
||||
// End API for communication between ClientBase and Client<>
|
||||
// ---------------------------------------------------------
|
||||
|
||||
private:
|
||||
std::unique_ptr<ClientBaseImpl> pimpl_;
|
||||
};
|
||||
|
||||
|
||||
/// Templated Action Client class
|
||||
/// It is responsible for getting the C action type support struct from the C++ type, and
|
||||
/// calling user callbacks with goal handles of the appropriate type.
|
||||
template<typename ACTION>
|
||||
/// Action Client
|
||||
/**
|
||||
* This class creates an action client.
|
||||
*
|
||||
* Create an instance of this server using `rclcpp_action::create_client()`.
|
||||
*
|
||||
* Internally, this class is responsible for:
|
||||
* - coverting between the C++ action type and generic types for `rclcpp_action::ClientBase`, and
|
||||
* - calling user callbacks.
|
||||
*/
|
||||
template<typename ActionT>
|
||||
class Client : public ClientBase
|
||||
{
|
||||
public:
|
||||
RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(Client)
|
||||
RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(Client<ActionT>)
|
||||
|
||||
using FeedbackCallback = std::function<void (
|
||||
std::shared_ptr<ClientGoalHandle<ACTION>>, const typename ACTION::Feedback)>;
|
||||
using Goal = typename ActionT::Goal;
|
||||
using Feedback = typename ActionT::Feedback;
|
||||
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;
|
||||
|
||||
Client(
|
||||
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base,
|
||||
const std::string & name
|
||||
rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging,
|
||||
const std::string & action_name,
|
||||
const rcl_action_client_options_t client_options = rcl_action_client_get_default_options()
|
||||
)
|
||||
: ClientBase(
|
||||
node_base,
|
||||
name,
|
||||
rosidl_typesupport_cpp::get_action_type_support_handle<ACTION>())
|
||||
{
|
||||
// TODO(sloretz) what's the link that causes a feedback topic to be called ?
|
||||
}
|
||||
|
||||
ClientGoalHandle<ACTION>
|
||||
async_send_goal(typename ACTION::Goal * goal, FeedbackCallback callback = nullptr);
|
||||
|
||||
RCLCPP_ACTION_PUBLIC
|
||||
std::future<typename ACTION::CancelGoalService::Response>
|
||||
async_cancel_all_goals();
|
||||
|
||||
RCLCPP_ACTION_PUBLIC
|
||||
std::future<typename ACTION::CancelGoalService::Response>
|
||||
async_cancel_goals_before(rclcpp::Time);
|
||||
|
||||
virtual ~Client()
|
||||
node_base, node_logging, action_name,
|
||||
rosidl_typesupport_cpp::get_action_type_support_handle<ActionT>(),
|
||||
client_options)
|
||||
{
|
||||
}
|
||||
|
||||
std::shared_future<typename GoalHandle::SharedPtr>
|
||||
async_send_goal(
|
||||
const Goal & goal, FeedbackCallback callback = nullptr, bool ignore_result = false)
|
||||
{
|
||||
// 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->uuid = this->generate_goal_id();
|
||||
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;
|
||||
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->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));
|
||||
if (!ignore_result) {
|
||||
try {
|
||||
this->make_result_aware(goal_handle);
|
||||
} catch (...) {
|
||||
promise->set_exception(std::current_exception());
|
||||
return;
|
||||
}
|
||||
}
|
||||
std::lock_guard<std::mutex> guard(goal_handles_mutex_);
|
||||
goal_handles_[goal_handle->get_goal_id()] = goal_handle;
|
||||
promise->set_value(goal_handle);
|
||||
});
|
||||
return future;
|
||||
}
|
||||
|
||||
std::shared_future<Result>
|
||||
async_get_result(typename GoalHandle::SharedPtr goal_handle)
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(goal_handles_mutex_);
|
||||
if (goal_handles_.count(goal_handle->get_goal_id()) == 0) {
|
||||
throw exceptions::UnknownGoalHandleError();
|
||||
}
|
||||
// If the user chose to ignore the result before, then ask the server for the result now.
|
||||
if (!goal_handle->is_result_aware()) {
|
||||
this->make_result_aware(goal_handle);
|
||||
}
|
||||
return goal_handle->async_result();
|
||||
}
|
||||
|
||||
std::shared_future<bool>
|
||||
async_cancel_goal(typename GoalHandle::SharedPtr goal_handle)
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(goal_handles_mutex_);
|
||||
if (goal_handles_.count(goal_handle->get_goal_id()) == 0) {
|
||||
throw exceptions::UnknownGoalHandleError();
|
||||
}
|
||||
// Put promise in the heap to move it around.
|
||||
auto promise = std::make_shared<std::promise<bool>>();
|
||||
std::shared_future<bool> future(promise->get_future());
|
||||
auto cancel_request = std::make_shared<CancelRequest>();
|
||||
// cancel_request->goal_info.goal_id = goal_handle->get_goal_id();
|
||||
cancel_request->goal_info.goal_id.uuid = goal_handle->get_goal_id();
|
||||
this->send_cancel_request(
|
||||
std::static_pointer_cast<void>(cancel_request),
|
||||
[goal_handle, promise](std::shared_ptr<void> response) mutable
|
||||
{
|
||||
auto cancel_response = std::static_pointer_cast<CancelResponse>(response);
|
||||
bool goal_canceled = false;
|
||||
if (!cancel_response->goals_canceling.empty()) {
|
||||
const GoalInfo & canceled_goal_info = cancel_response->goals_canceling[0];
|
||||
// goal_canceled = (canceled_goal_info.goal_id == goal_handle->get_goal_id());
|
||||
goal_canceled = (canceled_goal_info.goal_id.uuid == goal_handle->get_goal_id());
|
||||
}
|
||||
promise->set_value(goal_canceled);
|
||||
});
|
||||
return future;
|
||||
}
|
||||
|
||||
std::shared_future<typename CancelResponse::SharedPtr>
|
||||
async_cancel_all_goals()
|
||||
{
|
||||
auto cancel_request = std::make_shared<CancelRequest>();
|
||||
// std::fill(cancel_request->goal_info.goal_id.uuid, 0u);
|
||||
std::fill(
|
||||
cancel_request->goal_info.goal_id.uuid.begin(),
|
||||
cancel_request->goal_info.goal_id.uuid.end(), 0u);
|
||||
return async_cancel(cancel_request);
|
||||
}
|
||||
|
||||
std::shared_future<typename CancelResponse::SharedPtr>
|
||||
async_cancel_goals_before(const rclcpp::Time & stamp)
|
||||
{
|
||||
auto cancel_request = std::make_shared<CancelRequest>();
|
||||
// std::fill(cancel_request->goal_info.goal_id.uuid, 0u);
|
||||
std::fill(
|
||||
cancel_request->goal_info.goal_id.uuid.begin(),
|
||||
cancel_request->goal_info.goal_id.uuid.end(), 0u);
|
||||
cancel_request->goal_info.stamp = stamp;
|
||||
return async_cancel(cancel_request);
|
||||
}
|
||||
|
||||
virtual
|
||||
~Client()
|
||||
{
|
||||
std::lock_guard<std::mutex> guard(goal_handles_mutex_);
|
||||
auto it = goal_handles_.begin();
|
||||
while (it != goal_handles_.end()) {
|
||||
it->second->invalidate();
|
||||
it = goal_handles_.erase(it);
|
||||
}
|
||||
}
|
||||
|
||||
private:
|
||||
/// \internal
|
||||
std::shared_ptr<void>
|
||||
create_goal_response() const override
|
||||
{
|
||||
using GoalResponse = typename ActionT::GoalRequestService::Response;
|
||||
return std::shared_ptr<void>(new GoalResponse());
|
||||
}
|
||||
|
||||
/// \internal
|
||||
std::shared_ptr<void>
|
||||
create_result_response() const override
|
||||
{
|
||||
using GoalResultResponse = typename ActionT::GoalResultService::Response;
|
||||
return std::shared_ptr<void>(new GoalResultResponse());
|
||||
}
|
||||
|
||||
/// \internal
|
||||
std::shared_ptr<void>
|
||||
create_cancel_response() const override
|
||||
{
|
||||
return std::shared_ptr<void>(new CancelResponse());
|
||||
}
|
||||
|
||||
/// \internal
|
||||
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());
|
||||
}
|
||||
|
||||
/// \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->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);
|
||||
}
|
||||
|
||||
/// \internal
|
||||
std::shared_ptr<void>
|
||||
create_status_message() const override
|
||||
{
|
||||
using GoalStatusMessage = typename ActionT::GoalStatusMessage;
|
||||
return std::shared_ptr<void>(new GoalStatusMessage());
|
||||
}
|
||||
|
||||
/// \internal
|
||||
void
|
||||
handle_status_message(std::shared_ptr<void> message) override
|
||||
{
|
||||
std::lock_guard<std::mutex> guard(goal_handles_mutex_);
|
||||
using GoalStatusMessage = typename ActionT::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;
|
||||
if (goal_handles_.count(goal_id) == 0) {
|
||||
RCLCPP_DEBUG(
|
||||
this->get_logger(),
|
||||
"Received status for unknown goal. Ignoring...");
|
||||
continue;
|
||||
}
|
||||
typename GoalHandle::SharedPtr goal_handle = goal_handles_[goal_id];
|
||||
goal_handle->set_status(status.status);
|
||||
const int8_t goal_status = goal_handle->get_status();
|
||||
if (
|
||||
goal_status == GoalStatus::STATUS_SUCCEEDED ||
|
||||
goal_status == GoalStatus::STATUS_CANCELED ||
|
||||
goal_status == GoalStatus::STATUS_ABORTED)
|
||||
{
|
||||
goal_handles_.erase(goal_id);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// \internal
|
||||
void
|
||||
make_result_aware(typename GoalHandle::SharedPtr goal_handle)
|
||||
{
|
||||
using GoalResultRequest = typename ActionT::GoalResultService::Request;
|
||||
auto goal_result_request = std::make_shared<GoalResultRequest>();
|
||||
// goal_result_request.goal_id = goal_handle->get_goal_id();
|
||||
goal_result_request->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->status);
|
||||
goal_handle->set_result(result);
|
||||
std::lock_guard<std::mutex> lock(goal_handles_mutex_);
|
||||
goal_handles_.erase(goal_handle->get_goal_id());
|
||||
});
|
||||
goal_handle->set_result_awareness(true);
|
||||
}
|
||||
|
||||
/// \internal
|
||||
std::shared_future<typename CancelResponse::SharedPtr>
|
||||
async_cancel(typename CancelRequest::SharedPtr cancel_request)
|
||||
{
|
||||
// Put promise in the heap to move it around.
|
||||
auto promise = std::make_shared<std::promise<typename CancelResponse::SharedPtr>>();
|
||||
std::shared_future<typename CancelResponse::SharedPtr> future(promise->get_future());
|
||||
this->send_cancel_request(
|
||||
std::static_pointer_cast<void>(cancel_request),
|
||||
[promise](std::shared_ptr<void> response) mutable
|
||||
{
|
||||
auto cancel_response = std::static_pointer_cast<CancelResponse>(response);
|
||||
promise->set_value(cancel_response);
|
||||
});
|
||||
return future;
|
||||
}
|
||||
|
||||
std::map<GoalID, typename GoalHandle::SharedPtr> goal_handles_;
|
||||
std::mutex goal_handles_mutex_;
|
||||
};
|
||||
} // namespace rclcpp_action
|
||||
|
||||
#endif // RCLCPP_ACTION__CLIENT_HPP_
|
||||
|
|
|
|||
|
|
@ -17,39 +17,112 @@
|
|||
|
||||
#include <rcl_action/action_client.h>
|
||||
|
||||
#include <functional>
|
||||
#include <memory>
|
||||
#include <action_msgs/msg/goal_status.hpp>
|
||||
#include <rclcpp/macros.hpp>
|
||||
#include <rclcpp/time.hpp>
|
||||
|
||||
#include <functional>
|
||||
#include <future>
|
||||
#include <memory>
|
||||
#include <mutex>
|
||||
|
||||
#include "rclcpp_action/types.hpp"
|
||||
#include "rclcpp_action/visibility_control.hpp"
|
||||
|
||||
namespace rclcpp_action
|
||||
{
|
||||
/// The possible statuses that an action goal can finish with.
|
||||
enum class ResultCode : int8_t
|
||||
{
|
||||
UNKNOWN = action_msgs::msg::GoalStatus::STATUS_UNKNOWN,
|
||||
SUCCEEDED = action_msgs::msg::GoalStatus::STATUS_SUCCEEDED,
|
||||
CANCELED = action_msgs::msg::GoalStatus::STATUS_CANCELED,
|
||||
ABORTED = action_msgs::msg::GoalStatus::STATUS_ABORTED
|
||||
};
|
||||
|
||||
|
||||
// Forward declarations
|
||||
template<typename ACTION>
|
||||
template<typename ActionT>
|
||||
class Client;
|
||||
|
||||
template<typename ACTION>
|
||||
template<typename ActionT>
|
||||
class ClientGoalHandle
|
||||
{
|
||||
public:
|
||||
RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(ClientGoalHandle)
|
||||
|
||||
// A wrapper that defines the result of an action
|
||||
typedef struct Result
|
||||
{
|
||||
/// The unique identifier of the goal
|
||||
GoalID 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;
|
||||
|
||||
using Feedback = typename ActionT::Feedback;
|
||||
using FeedbackCallback =
|
||||
std::function<void (
|
||||
typename ClientGoalHandle<ActionT>::SharedPtr, const std::shared_ptr<const Feedback>)>;
|
||||
|
||||
virtual ~ClientGoalHandle();
|
||||
|
||||
/// TODO(sloretz) examples have this on client as `async_cancel_goal(goal_handle)`
|
||||
std::future<bool>
|
||||
async_cancel();
|
||||
const GoalID &
|
||||
get_goal_id() const;
|
||||
|
||||
std::future<typename ACTION::Result>
|
||||
rclcpp::Time
|
||||
get_goal_stamp() const;
|
||||
|
||||
std::shared_future<Result>
|
||||
async_result();
|
||||
|
||||
int8_t
|
||||
get_status();
|
||||
|
||||
bool
|
||||
is_feedback_aware();
|
||||
|
||||
bool
|
||||
is_result_aware();
|
||||
|
||||
private:
|
||||
// The templated Server creates goal handles
|
||||
friend Client<ACTION>;
|
||||
// The templated Client creates goal handles
|
||||
friend Client<ActionT>;
|
||||
|
||||
ClientGoalHandle(rcl_action_client_t * rcl_client, const rcl_action_goal_info_t rcl_info);
|
||||
ClientGoalHandle(const GoalInfo & info, FeedbackCallback callback);
|
||||
|
||||
// TODO(sloretz) shared pointer to keep rcl_client_ alive while goal handles are alive
|
||||
rcl_action_client_t * rcl_client_;
|
||||
rcl_action_goal_info_t rcl_info_;
|
||||
void
|
||||
set_feedback_callback(FeedbackCallback callback);
|
||||
|
||||
void
|
||||
call_feedback_callback(
|
||||
typename ClientGoalHandle<ActionT>::SharedPtr shared_this,
|
||||
typename std::shared_ptr<const Feedback> feedback_message);
|
||||
|
||||
void
|
||||
set_result_awareness(bool awareness);
|
||||
|
||||
void
|
||||
set_status(int8_t status);
|
||||
|
||||
void
|
||||
set_result(const Result & result);
|
||||
|
||||
void
|
||||
invalidate();
|
||||
|
||||
GoalInfo info_;
|
||||
|
||||
bool is_result_aware_{false};
|
||||
std::promise<Result> result_promise_;
|
||||
std::shared_future<Result> result_future_;
|
||||
|
||||
FeedbackCallback feedback_callback_{nullptr};
|
||||
int8_t status_{GoalStatus::STATUS_ACCEPTED};
|
||||
|
||||
std::mutex handle_mutex_;
|
||||
};
|
||||
} // namespace rclcpp_action
|
||||
|
||||
|
|
|
|||
|
|
@ -17,35 +17,137 @@
|
|||
|
||||
#include <rcl_action/types.h>
|
||||
|
||||
#include <memory>
|
||||
|
||||
#include "rclcpp_action/exceptions.hpp"
|
||||
|
||||
namespace rclcpp_action
|
||||
{
|
||||
template<typename ACTION>
|
||||
ClientGoalHandle<ACTION>::ClientGoalHandle(
|
||||
rcl_action_client_t * rcl_client,
|
||||
const rcl_action_goal_info_t rcl_info
|
||||
)
|
||||
: rcl_client_(rcl_client), rcl_info_(rcl_info)
|
||||
|
||||
template<typename ActionT>
|
||||
ClientGoalHandle<ActionT>::ClientGoalHandle(
|
||||
const GoalInfo & info, FeedbackCallback callback)
|
||||
: info_(info), result_future_(result_promise_.get_future()), feedback_callback_(callback)
|
||||
{
|
||||
}
|
||||
|
||||
template<typename ACTION>
|
||||
ClientGoalHandle<ACTION>::~ClientGoalHandle()
|
||||
template<typename ActionT>
|
||||
ClientGoalHandle<ActionT>::~ClientGoalHandle()
|
||||
{
|
||||
}
|
||||
|
||||
template<typename ACTION>
|
||||
std::future<bool>
|
||||
ClientGoalHandle<ACTION>::async_cancel()
|
||||
template<typename ActionT>
|
||||
const GoalID &
|
||||
ClientGoalHandle<ActionT>::get_goal_id() const
|
||||
{
|
||||
throw std::runtime_error("Failed to cancel goal");
|
||||
// return info_.goal_id;
|
||||
return info_.goal_id.uuid;
|
||||
}
|
||||
|
||||
template<typename ACTION>
|
||||
std::future<typename ACTION::Result>
|
||||
ClientGoalHandle<ACTION>::async_result()
|
||||
template<typename ActionT>
|
||||
rclcpp::Time
|
||||
ClientGoalHandle<ActionT>::get_goal_stamp() const
|
||||
{
|
||||
throw std::runtime_error("Failed to get result future");
|
||||
return info_.stamp;
|
||||
}
|
||||
|
||||
template<typename ActionT>
|
||||
std::shared_future<typename ClientGoalHandle<ActionT>::Result>
|
||||
ClientGoalHandle<ActionT>::async_result()
|
||||
{
|
||||
std::lock_guard<std::mutex> guard(handle_mutex_);
|
||||
if (!is_result_aware_) {
|
||||
throw exceptions::UnawareGoalHandleError();
|
||||
}
|
||||
return result_future_;
|
||||
}
|
||||
|
||||
template<typename ActionT>
|
||||
void
|
||||
ClientGoalHandle<ActionT>::set_result(const Result & result)
|
||||
{
|
||||
std::lock_guard<std::mutex> guard(handle_mutex_);
|
||||
status_ = static_cast<int8_t>(result.code);
|
||||
result_promise_.set_value(result);
|
||||
}
|
||||
|
||||
template<typename ActionT>
|
||||
void
|
||||
ClientGoalHandle<ActionT>::set_feedback_callback(FeedbackCallback callback)
|
||||
{
|
||||
std::lock_guard<std::mutex> guard(handle_mutex_);
|
||||
feedback_callback_ = callback;
|
||||
}
|
||||
|
||||
template<typename ActionT>
|
||||
int8_t
|
||||
ClientGoalHandle<ActionT>::get_status()
|
||||
{
|
||||
std::lock_guard<std::mutex> guard(handle_mutex_);
|
||||
return status_;
|
||||
}
|
||||
|
||||
template<typename ActionT>
|
||||
void
|
||||
ClientGoalHandle<ActionT>::set_status(int8_t status)
|
||||
{
|
||||
std::lock_guard<std::mutex> guard(handle_mutex_);
|
||||
status_ = status;
|
||||
}
|
||||
|
||||
template<typename ActionT>
|
||||
bool
|
||||
ClientGoalHandle<ActionT>::is_feedback_aware()
|
||||
{
|
||||
std::lock_guard<std::mutex> guard(handle_mutex_);
|
||||
return feedback_callback_ != nullptr;
|
||||
}
|
||||
|
||||
template<typename ActionT>
|
||||
bool
|
||||
ClientGoalHandle<ActionT>::is_result_aware()
|
||||
{
|
||||
std::lock_guard<std::mutex> guard(handle_mutex_);
|
||||
return is_result_aware_;
|
||||
}
|
||||
|
||||
template<typename ActionT>
|
||||
void
|
||||
ClientGoalHandle<ActionT>::set_result_awareness(bool awareness)
|
||||
{
|
||||
std::lock_guard<std::mutex> guard(handle_mutex_);
|
||||
is_result_aware_ = awareness;
|
||||
}
|
||||
|
||||
template<typename ActionT>
|
||||
void
|
||||
ClientGoalHandle<ActionT>::invalidate()
|
||||
{
|
||||
std::lock_guard<std::mutex> guard(handle_mutex_);
|
||||
status_ = GoalStatus::STATUS_UNKNOWN;
|
||||
result_promise_.set_exception(std::make_exception_ptr(
|
||||
exceptions::UnawareGoalHandleError()));
|
||||
}
|
||||
|
||||
template<typename ActionT>
|
||||
void
|
||||
ClientGoalHandle<ActionT>::call_feedback_callback(
|
||||
typename ClientGoalHandle<ActionT>::SharedPtr shared_this,
|
||||
typename std::shared_ptr<const Feedback> feedback_message)
|
||||
{
|
||||
if (shared_this.get() != this) {
|
||||
RCLCPP_ERROR(rclcpp::get_logger("rclcpp_action"), "Sent feedback to wrong goal handle.");
|
||||
return;
|
||||
}
|
||||
std::lock_guard<std::mutex> guard(handle_mutex_);
|
||||
if (nullptr == feedback_callback_) {
|
||||
// Normal, some feedback messages may arrive after the goal result.
|
||||
RCLCPP_DEBUG(rclcpp::get_logger("rclcpp_action"), "Received feedback but goal ignores it.");
|
||||
return;
|
||||
}
|
||||
feedback_callback_(shared_this, feedback_message);
|
||||
}
|
||||
|
||||
} // namespace rclcpp_action
|
||||
|
||||
#endif // RCLCPP_ACTION__CLIENT_GOAL_HANDLE_IMPL_HPP_
|
||||
|
|
|
|||
|
|
@ -25,16 +25,50 @@
|
|||
|
||||
namespace rclcpp_action
|
||||
{
|
||||
template<typename ACTION>
|
||||
typename Client<ACTION>::SharedPtr
|
||||
template<typename ActionT>
|
||||
typename Client<ActionT>::SharedPtr
|
||||
create_client(
|
||||
rclcpp::Node * node,
|
||||
const std::string & name)
|
||||
rclcpp::Node::SharedPtr node,
|
||||
const std::string & name,
|
||||
rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr)
|
||||
{
|
||||
return Client<ACTION>::make_shared(
|
||||
node->get_node_base_interface(),
|
||||
name);
|
||||
std::weak_ptr<rclcpp::node_interfaces::NodeWaitablesInterface> weak_node =
|
||||
node->get_node_waitables_interface();
|
||||
std::weak_ptr<rclcpp::callback_group::CallbackGroup> weak_group = group;
|
||||
bool group_is_null = (nullptr == group.get());
|
||||
|
||||
auto deleter = [weak_node, weak_group, group_is_null](Client<ActionT> * ptr)
|
||||
{
|
||||
if (nullptr == ptr) {
|
||||
return;
|
||||
}
|
||||
auto shared_node = weak_node.lock();
|
||||
if (!shared_node) {
|
||||
return;
|
||||
}
|
||||
// API expects a shared pointer, give it one with a deleter that does nothing.
|
||||
std::shared_ptr<Client<ActionT>> fake_shared_ptr(ptr, [](Client<ActionT> *) {});
|
||||
|
||||
if (group_is_null) {
|
||||
// Was added to default group
|
||||
shared_node->remove_waitable(fake_shared_ptr, nullptr);
|
||||
} else {
|
||||
// Was added to a specfic group
|
||||
auto shared_group = weak_group.lock();
|
||||
if (shared_group) {
|
||||
shared_node->remove_waitable(fake_shared_ptr, shared_group);
|
||||
}
|
||||
}
|
||||
delete ptr;
|
||||
};
|
||||
|
||||
std::shared_ptr<Client<ActionT>> action_client(
|
||||
new Client<ActionT>(node->get_node_base_interface(), node->get_node_logging_interface(), name),
|
||||
deleter);
|
||||
|
||||
node->get_node_waitables_interface()->add_waitable(action_client, group);
|
||||
return action_client;
|
||||
}
|
||||
} // namespace rclcpp_action
|
||||
|
||||
#endif // RCLCPP_ACTION__CREATE_CLIENT_HPP_
|
||||
// TODO(sloretz) rclcpp_action::create_client<>(node, action_name);
|
||||
|
|
|
|||
46
rclcpp_action/include/rclcpp_action/exceptions.hpp
Normal file
46
rclcpp_action/include/rclcpp_action/exceptions.hpp
Normal file
|
|
@ -0,0 +1,46 @@
|
|||
// Copyright 2018 Open Source Robotics Foundation, Inc.
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef RCLCPP_ACTION__EXCEPTIONS_HPP_
|
||||
#define RCLCPP_ACTION__EXCEPTIONS_HPP_
|
||||
|
||||
#include <stdexcept>
|
||||
|
||||
namespace rclcpp_action
|
||||
{
|
||||
namespace exceptions
|
||||
{
|
||||
class UnknownGoalHandleError : public std::invalid_argument
|
||||
{
|
||||
public:
|
||||
UnknownGoalHandleError()
|
||||
: std::invalid_argument("Goal handle is not know to this client.")
|
||||
{
|
||||
}
|
||||
};
|
||||
|
||||
class UnawareGoalHandleError : public std::runtime_error
|
||||
{
|
||||
public:
|
||||
UnawareGoalHandleError()
|
||||
: std::runtime_error("Goal handle is not tracking the goal result.")
|
||||
{
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace exceptions
|
||||
|
||||
} // namespace rclcpp_action
|
||||
|
||||
#endif // RCLCPP_ACTION__EXCEPTIONS_HPP_
|
||||
48
rclcpp_action/include/rclcpp_action/types.hpp
Normal file
48
rclcpp_action/include/rclcpp_action/types.hpp
Normal file
|
|
@ -0,0 +1,48 @@
|
|||
// Copyright 2018 Open Source Robotics Foundation, Inc.
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef RCLCPP_ACTION__TYPES_HPP_
|
||||
#define RCLCPP_ACTION__TYPES_HPP_
|
||||
|
||||
#include <rcl_action/types.h>
|
||||
|
||||
#include <action_msgs/msg/goal_status.hpp>
|
||||
#include <action_msgs/msg/goal_info.hpp>
|
||||
|
||||
#include <functional>
|
||||
|
||||
|
||||
namespace rclcpp_action
|
||||
{
|
||||
|
||||
using GoalID = std::array<uint8_t, UUID_SIZE>;
|
||||
using GoalStatus = action_msgs::msg::GoalStatus;
|
||||
using GoalInfo = action_msgs::msg::GoalInfo;
|
||||
|
||||
} // namespace rclcpp_action
|
||||
|
||||
namespace std
|
||||
{
|
||||
template<>
|
||||
struct less<rclcpp_action::GoalID>
|
||||
{
|
||||
bool operator()(
|
||||
const rclcpp_action::GoalID & lhs,
|
||||
const rclcpp_action::GoalID & rhs) const
|
||||
{
|
||||
return lhs < rhs;
|
||||
}
|
||||
};
|
||||
} // namespace std
|
||||
#endif // RCLCPP_ACTION__TYPES_HPP_
|
||||
|
|
@ -12,30 +12,349 @@
|
|||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#include <rclcpp_action/client.hpp>
|
||||
#include <rcl_action/action_client.h>
|
||||
#include <rcl_action/wait.h>
|
||||
#include <rclcpp/node_interfaces/node_base_interface.hpp>
|
||||
#include <rclcpp/node_interfaces/node_logging_interface.hpp>
|
||||
|
||||
#include <algorithm>
|
||||
#include <map>
|
||||
#include <memory>
|
||||
#include <random>
|
||||
#include <string>
|
||||
|
||||
using rclcpp_action::ClientBase;
|
||||
#include "rclcpp_action/client.hpp"
|
||||
#include "rclcpp_action/exceptions.hpp"
|
||||
|
||||
namespace rclcpp_action
|
||||
{
|
||||
|
||||
class ClientBaseImpl
|
||||
{
|
||||
};
|
||||
public:
|
||||
ClientBaseImpl(
|
||||
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base,
|
||||
rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging,
|
||||
const std::string & action_name,
|
||||
const rosidl_action_type_support_t * type_support,
|
||||
const rcl_action_client_options_t & client_options)
|
||||
: node_handle(node_base->get_shared_rcl_node_handle()),
|
||||
logger(node_logging->get_logger().get_child("rclcpp_acton")),
|
||||
random_bytes_generator(std::random_device{} ())
|
||||
{
|
||||
std::weak_ptr<rcl_node_t> weak_node_handle(node_handle);
|
||||
client_handle = std::shared_ptr<rcl_action_client_t>(
|
||||
new rcl_action_client_t, [weak_node_handle](rcl_action_client_t * client)
|
||||
{
|
||||
auto handle = weak_node_handle.lock();
|
||||
if (handle) {
|
||||
if (RCL_RET_OK != rcl_action_client_fini(client, handle.get())) {
|
||||
RCLCPP_ERROR(
|
||||
rclcpp::get_logger(rcl_node_get_logger_name(handle.get())).get_child("rclcpp_action"),
|
||||
"Error in destruction of rcl action client handle: %s", rcl_get_error_string().str);
|
||||
rcl_reset_error();
|
||||
}
|
||||
} else {
|
||||
RCLCPP_ERROR(
|
||||
rclcpp::get_logger("rclcpp_action"),
|
||||
"Error in destruction of rcl action client handle: "
|
||||
"the Node Handle was destructed too early. You will leak memory");
|
||||
}
|
||||
delete client;
|
||||
});
|
||||
*client_handle = rcl_action_get_zero_initialized_client();
|
||||
rcl_ret_t ret = rcl_action_client_init(
|
||||
client_handle.get(), node_handle.get(), type_support,
|
||||
action_name.c_str(), &client_options);
|
||||
if (RCL_RET_OK != ret) {
|
||||
rclcpp::exceptions::throw_from_rcl_error(
|
||||
ret, "could not initialize rcl action client");
|
||||
}
|
||||
|
||||
ret = rcl_action_client_wait_set_get_num_entities(
|
||||
client_handle.get(),
|
||||
&num_subscriptions,
|
||||
&num_guard_conditions,
|
||||
&num_timers,
|
||||
&num_clients,
|
||||
&num_services);
|
||||
if (RCL_RET_OK != ret) {
|
||||
rclcpp::exceptions::throw_from_rcl_error(
|
||||
ret, "could not retrieve rcl action client details");
|
||||
}
|
||||
}
|
||||
|
||||
size_t num_subscriptions{0u};
|
||||
size_t num_guard_conditions{0u};
|
||||
size_t num_timers{0u};
|
||||
size_t num_clients{0u};
|
||||
size_t num_services{0u};
|
||||
|
||||
bool is_feedback_ready{false};
|
||||
bool is_status_ready{false};
|
||||
bool is_goal_response_ready{false};
|
||||
bool is_cancel_response_ready{false};
|
||||
bool is_result_response_ready{false};
|
||||
|
||||
std::shared_ptr<rcl_action_client_t> client_handle{nullptr};
|
||||
std::shared_ptr<rcl_node_t> node_handle{nullptr};
|
||||
rclcpp::Logger logger;
|
||||
|
||||
using ResponseCallback = std::function<void (std::shared_ptr<void> response)>;
|
||||
|
||||
std::map<int64_t, ResponseCallback> pending_goal_responses;
|
||||
std::mutex goal_requests_mutex;
|
||||
|
||||
std::map<int64_t, ResponseCallback> pending_result_responses;
|
||||
std::mutex result_requests_mutex;
|
||||
|
||||
std::map<int64_t, ResponseCallback> pending_cancel_responses;
|
||||
std::mutex cancel_requests_mutex;
|
||||
|
||||
std::independent_bits_engine<
|
||||
std::default_random_engine, 8, unsigned int> random_bytes_generator;
|
||||
};
|
||||
|
||||
ClientBase::ClientBase(
|
||||
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base,
|
||||
const std::string & name,
|
||||
const rosidl_action_type_support_t * type_support)
|
||||
rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging,
|
||||
const std::string & action_name,
|
||||
const rosidl_action_type_support_t * type_support,
|
||||
const rcl_action_client_options_t & client_options)
|
||||
: pimpl_(new ClientBaseImpl(node_base, node_logging, action_name, type_support, client_options))
|
||||
{
|
||||
// TODO(sloretz) use rcl_action API when available
|
||||
(void)node_base;
|
||||
(void)name;
|
||||
(void)type_support;
|
||||
}
|
||||
|
||||
ClientBase::~ClientBase()
|
||||
{
|
||||
}
|
||||
|
||||
rclcpp::Logger
|
||||
ClientBase::get_logger()
|
||||
{
|
||||
return pimpl_->logger;
|
||||
}
|
||||
|
||||
size_t
|
||||
ClientBase::get_number_of_ready_subscriptions()
|
||||
{
|
||||
return pimpl_->num_subscriptions;
|
||||
}
|
||||
|
||||
size_t
|
||||
ClientBase::get_number_of_ready_guard_conditions()
|
||||
{
|
||||
return pimpl_->num_guard_conditions;
|
||||
}
|
||||
|
||||
size_t
|
||||
ClientBase::get_number_of_ready_timers()
|
||||
{
|
||||
return pimpl_->num_timers;
|
||||
}
|
||||
|
||||
size_t
|
||||
ClientBase::get_number_of_ready_clients()
|
||||
{
|
||||
return pimpl_->num_clients;
|
||||
}
|
||||
|
||||
size_t
|
||||
ClientBase::get_number_of_ready_services()
|
||||
{
|
||||
return pimpl_->num_services;
|
||||
}
|
||||
|
||||
bool
|
||||
ClientBase::add_to_wait_set(rcl_wait_set_t * wait_set)
|
||||
{
|
||||
rcl_ret_t ret = rcl_action_wait_set_add_action_client(
|
||||
wait_set, pimpl_->client_handle.get(), nullptr, nullptr);
|
||||
return RCL_RET_OK == ret;
|
||||
}
|
||||
|
||||
bool
|
||||
ClientBase::is_ready(rcl_wait_set_t * wait_set)
|
||||
{
|
||||
rcl_ret_t ret = rcl_action_client_wait_set_get_entities_ready(
|
||||
wait_set, pimpl_->client_handle.get(),
|
||||
&pimpl_->is_feedback_ready,
|
||||
&pimpl_->is_status_ready,
|
||||
&pimpl_->is_goal_response_ready,
|
||||
&pimpl_->is_cancel_response_ready,
|
||||
&pimpl_->is_result_response_ready);
|
||||
if (RCL_RET_OK != ret) {
|
||||
rclcpp::exceptions::throw_from_rcl_error(
|
||||
ret, "failed to check for any ready entities");
|
||||
}
|
||||
return
|
||||
pimpl_->is_feedback_ready ||
|
||||
pimpl_->is_status_ready ||
|
||||
pimpl_->is_goal_response_ready ||
|
||||
pimpl_->is_cancel_response_ready ||
|
||||
pimpl_->is_result_response_ready;
|
||||
}
|
||||
|
||||
void
|
||||
ClientBase::handle_goal_response(
|
||||
const rmw_request_id_t & response_header,
|
||||
std::shared_ptr<void> response)
|
||||
{
|
||||
std::lock_guard<std::mutex> guard(pimpl_->goal_requests_mutex);
|
||||
const int64_t & sequence_number = response_header.sequence_number;
|
||||
if (pimpl_->pending_goal_responses.count(sequence_number) == 0) {
|
||||
RCLCPP_ERROR(pimpl_->logger, "unknown goal response, ignoring...");
|
||||
return;
|
||||
}
|
||||
pimpl_->pending_goal_responses[sequence_number](response);
|
||||
pimpl_->pending_goal_responses.erase(sequence_number);
|
||||
}
|
||||
|
||||
void
|
||||
ClientBase::send_goal_request(std::shared_ptr<void> request, ResponseCallback callback)
|
||||
{
|
||||
std::unique_lock<std::mutex> guard(pimpl_->goal_requests_mutex);
|
||||
int64_t sequence_number;
|
||||
rcl_ret_t ret = rcl_action_send_goal_request(
|
||||
pimpl_->client_handle.get(), request.get(), &sequence_number);
|
||||
if (RCL_RET_OK != ret) {
|
||||
rclcpp::exceptions::throw_from_rcl_error(ret, "failed to send goal request");
|
||||
}
|
||||
assert(pimpl_->pending_goal_responses.count(sequence_number) == 0);
|
||||
pimpl_->pending_goal_responses[sequence_number] = callback;
|
||||
}
|
||||
|
||||
void
|
||||
ClientBase::handle_result_response(
|
||||
const rmw_request_id_t & response_header,
|
||||
std::shared_ptr<void> response)
|
||||
{
|
||||
std::lock_guard<std::mutex> guard(pimpl_->result_requests_mutex);
|
||||
const int64_t & sequence_number = response_header.sequence_number;
|
||||
if (pimpl_->pending_result_responses.count(sequence_number) == 0) {
|
||||
RCLCPP_ERROR(pimpl_->logger, "unknown result response, ignoring...");
|
||||
return;
|
||||
}
|
||||
pimpl_->pending_result_responses[sequence_number](response);
|
||||
pimpl_->pending_result_responses.erase(sequence_number);
|
||||
}
|
||||
|
||||
void
|
||||
ClientBase::send_result_request(std::shared_ptr<void> request, ResponseCallback callback)
|
||||
{
|
||||
std::lock_guard<std::mutex> guard(pimpl_->result_requests_mutex);
|
||||
int64_t sequence_number;
|
||||
rcl_ret_t ret = rcl_action_send_result_request(
|
||||
pimpl_->client_handle.get(), request.get(), &sequence_number);
|
||||
if (RCL_RET_OK != ret) {
|
||||
rclcpp::exceptions::throw_from_rcl_error(ret, "failed to send result request");
|
||||
}
|
||||
assert(pimpl_->pending_result_responses.count(sequence_number) == 0);
|
||||
pimpl_->pending_result_responses[sequence_number] = callback;
|
||||
}
|
||||
|
||||
void
|
||||
ClientBase::handle_cancel_response(
|
||||
const rmw_request_id_t & response_header,
|
||||
std::shared_ptr<void> response)
|
||||
{
|
||||
std::lock_guard<std::mutex> guard(pimpl_->cancel_requests_mutex);
|
||||
const int64_t & sequence_number = response_header.sequence_number;
|
||||
if (pimpl_->pending_cancel_responses.count(sequence_number) == 0) {
|
||||
RCLCPP_ERROR(pimpl_->logger, "unknown cancel response, ignoring...");
|
||||
return;
|
||||
}
|
||||
pimpl_->pending_cancel_responses[sequence_number](response);
|
||||
pimpl_->pending_cancel_responses.erase(sequence_number);
|
||||
}
|
||||
|
||||
void
|
||||
ClientBase::send_cancel_request(std::shared_ptr<void> request, ResponseCallback callback)
|
||||
{
|
||||
std::lock_guard<std::mutex> guard(pimpl_->cancel_requests_mutex);
|
||||
int64_t sequence_number;
|
||||
rcl_ret_t ret = rcl_action_send_cancel_request(
|
||||
pimpl_->client_handle.get(), request.get(), &sequence_number);
|
||||
if (RCL_RET_OK != ret) {
|
||||
rclcpp::exceptions::throw_from_rcl_error(ret, "failed to send cancel request");
|
||||
}
|
||||
assert(pimpl_->pending_cancel_responses.count(sequence_number) == 0);
|
||||
pimpl_->pending_cancel_responses[sequence_number] = callback;
|
||||
}
|
||||
|
||||
GoalID
|
||||
ClientBase::generate_goal_id()
|
||||
{
|
||||
GoalID goal_id;
|
||||
// TODO(hidmic): Do something better than this for UUID generation.
|
||||
// std::generate(
|
||||
// goal_id.uuid.begin(), goal_id.uuid.end(),
|
||||
// std::ref(pimpl_->random_bytes_generator));
|
||||
std::generate(
|
||||
goal_id.begin(), goal_id.end(),
|
||||
std::ref(pimpl_->random_bytes_generator));
|
||||
return goal_id;
|
||||
}
|
||||
|
||||
void
|
||||
ClientBase::execute()
|
||||
{
|
||||
if (pimpl_->is_feedback_ready) {
|
||||
std::shared_ptr<void> feedback_message = this->create_feedback_message();
|
||||
rcl_ret_t ret = rcl_action_take_feedback(
|
||||
pimpl_->client_handle.get(), feedback_message.get());
|
||||
pimpl_->is_feedback_ready = false;
|
||||
if (RCL_RET_OK != ret) {
|
||||
rclcpp::exceptions::throw_from_rcl_error(ret, "error taking feedback");
|
||||
} else {
|
||||
this->handle_feedback_message(feedback_message);
|
||||
}
|
||||
} else if (pimpl_->is_status_ready) {
|
||||
std::shared_ptr<void> status_message = this->create_status_message();
|
||||
rcl_ret_t ret = rcl_action_take_status(
|
||||
pimpl_->client_handle.get(), status_message.get());
|
||||
pimpl_->is_status_ready = false;
|
||||
if (RCL_RET_OK != ret) {
|
||||
rclcpp::exceptions::throw_from_rcl_error(ret, "error taking status");
|
||||
} else {
|
||||
this->handle_status_message(status_message);
|
||||
}
|
||||
} else if (pimpl_->is_goal_response_ready) {
|
||||
rmw_request_id_t response_header;
|
||||
std::shared_ptr<void> goal_response = this->create_goal_response();
|
||||
rcl_ret_t ret = rcl_action_take_goal_response(
|
||||
pimpl_->client_handle.get(), &response_header, goal_response.get());
|
||||
pimpl_->is_goal_response_ready = false;
|
||||
if (RCL_RET_OK != ret) {
|
||||
rclcpp::exceptions::throw_from_rcl_error(ret, "error taking goal response");
|
||||
} else {
|
||||
this->handle_goal_response(response_header, goal_response);
|
||||
}
|
||||
} else if (pimpl_->is_result_response_ready) {
|
||||
rmw_request_id_t response_header;
|
||||
std::shared_ptr<void> result_response = this->create_result_response();
|
||||
rcl_ret_t ret = rcl_action_take_result_response(
|
||||
pimpl_->client_handle.get(), &response_header, result_response.get());
|
||||
pimpl_->is_result_response_ready = false;
|
||||
if (RCL_RET_OK != ret) {
|
||||
rclcpp::exceptions::throw_from_rcl_error(ret, "error taking result response");
|
||||
} else {
|
||||
this->handle_result_response(response_header, result_response);
|
||||
}
|
||||
} else if (pimpl_->is_cancel_response_ready) {
|
||||
rmw_request_id_t response_header;
|
||||
std::shared_ptr<void> cancel_response = this->create_cancel_response();
|
||||
rcl_ret_t ret = rcl_action_take_cancel_response(
|
||||
pimpl_->client_handle.get(), &response_header, cancel_response.get());
|
||||
pimpl_->is_cancel_response_ready = false;
|
||||
if (RCL_RET_OK != ret) {
|
||||
rclcpp::exceptions::throw_from_rcl_error(ret, "error taking cancel response");
|
||||
} else {
|
||||
this->handle_cancel_response(response_header, cancel_response);
|
||||
}
|
||||
} else {
|
||||
throw std::runtime_error("Executing action client but nothing is ready");
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace rclcpp_action
|
||||
|
|
|
|||
|
|
@ -14,29 +14,407 @@
|
|||
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include <rcl/allocator.h>
|
||||
#include <rcl/time.h>
|
||||
#include <rcl/types.h>
|
||||
|
||||
#include <rcl_action/names.h>
|
||||
#include <rcl_action/default_qos.h>
|
||||
|
||||
#include <rclcpp/clock.hpp>
|
||||
#include <rclcpp/exceptions.hpp>
|
||||
#include <rclcpp/executors.hpp>
|
||||
#include <rclcpp/executors/multi_threaded_executor.hpp>
|
||||
#include <rclcpp/node.hpp>
|
||||
#include <rclcpp/publisher.hpp>
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <rclcpp/service.hpp>
|
||||
#include <rclcpp/time.hpp>
|
||||
|
||||
#include <test_msgs/action/fibonacci.hpp>
|
||||
|
||||
#include <map>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <utility>
|
||||
#include <thread>
|
||||
#include <chrono>
|
||||
|
||||
#include "rclcpp_action/exceptions.hpp"
|
||||
#include "rclcpp_action/create_client.hpp"
|
||||
#include "rclcpp_action/client.hpp"
|
||||
#include "rclcpp_action/types.hpp"
|
||||
|
||||
using namespace std::chrono_literals;
|
||||
|
||||
class TestClient : public ::testing::Test
|
||||
{
|
||||
protected:
|
||||
using ActionType = test_msgs::action::Fibonacci;
|
||||
using ActionGoal = ActionType::Goal;
|
||||
using ActionGoalHandle = rclcpp_action::ClientGoalHandle<ActionType>;
|
||||
using ActionGoalRequestService = ActionType::GoalRequestService;
|
||||
using ActionGoalRequest = ActionGoalRequestService::Request;
|
||||
using ActionGoalResponse = ActionGoalRequestService::Response;
|
||||
using ActionGoalResultService = ActionType::GoalResultService;
|
||||
using ActionGoalResultRequest = ActionGoalResultService::Request;
|
||||
using ActionGoalResultResponse = ActionGoalResultService::Response;
|
||||
using ActionCancelGoalService = ActionType::CancelGoalService;
|
||||
using ActionCancelGoalRequest = ActionType::CancelGoalService::Request;
|
||||
using ActionCancelGoalResponse = ActionType::CancelGoalService::Response;
|
||||
using ActionStatusMessage = ActionType::GoalStatusMessage;
|
||||
using ActionFeedbackMessage = ActionType::Feedback;
|
||||
using ActionFeedback = ActionType::Feedback;
|
||||
|
||||
static void SetUpTestCase()
|
||||
{
|
||||
rclcpp::init(0, nullptr);
|
||||
}
|
||||
|
||||
void SetUp()
|
||||
{
|
||||
rcl_allocator_t allocator = rcl_get_default_allocator();
|
||||
|
||||
server_node = std::make_shared<rclcpp::Node>(server_node_name, namespace_name);
|
||||
|
||||
char * goal_service_name = nullptr;
|
||||
rcl_ret_t ret = rcl_action_get_goal_service_name(
|
||||
action_name, allocator, &goal_service_name);
|
||||
ASSERT_EQ(RCL_RET_OK, ret);
|
||||
goal_service = server_node->create_service<ActionGoalRequestService>(
|
||||
goal_service_name,
|
||||
[this](
|
||||
const ActionGoalRequest::SharedPtr request,
|
||||
ActionGoalResponse::SharedPtr response)
|
||||
{
|
||||
response->stamp = clock.now();
|
||||
response->accepted = (request->order >= 0);
|
||||
if (response->accepted) {
|
||||
goals[request->uuid] = {request, response};
|
||||
}
|
||||
});
|
||||
ASSERT_TRUE(goal_service != nullptr);
|
||||
allocator.deallocate(goal_service_name, allocator.state);
|
||||
|
||||
char * result_service_name = nullptr;
|
||||
ret = rcl_action_get_result_service_name(
|
||||
action_name, allocator, &result_service_name);
|
||||
ASSERT_EQ(RCL_RET_OK, ret);
|
||||
result_service = server_node->create_service<ActionGoalResultService>(
|
||||
result_service_name,
|
||||
[this](
|
||||
const ActionGoalResultRequest::SharedPtr request,
|
||||
ActionGoalResultResponse::SharedPtr response)
|
||||
{
|
||||
if (goals.count(request->uuid) == 1) {
|
||||
auto goal_request = goals[request->uuid].first;
|
||||
auto goal_response = goals[request->uuid].second;
|
||||
ActionStatusMessage status_message;
|
||||
rclcpp_action::GoalStatus goal_status;
|
||||
goal_status.goal_info.goal_id.uuid = goal_request->uuid;
|
||||
goal_status.goal_info.stamp = goal_response->stamp;
|
||||
goal_status.status = rclcpp_action::GoalStatus::STATUS_EXECUTING;
|
||||
status_message.status_list.push_back(goal_status);
|
||||
status_publisher->publish(status_message);
|
||||
client_executor.spin_once();
|
||||
ActionFeedbackMessage feedback_message;
|
||||
feedback_message.uuid = goal_request->uuid;
|
||||
feedback_message.sequence.push_back(0);
|
||||
feedback_publisher->publish(feedback_message);
|
||||
client_executor.spin_once();
|
||||
if (goal_request->order > 0) {
|
||||
feedback_message.sequence.push_back(1);
|
||||
feedback_publisher->publish(feedback_message);
|
||||
client_executor.spin_once();
|
||||
for (int i = 1; i < goal_request->order; ++i) {
|
||||
feedback_message.sequence.push_back(
|
||||
feedback_message.sequence[i] + feedback_message.sequence[i - 1]);
|
||||
feedback_publisher->publish(feedback_message);
|
||||
client_executor.spin_once();
|
||||
}
|
||||
}
|
||||
goal_status.status = rclcpp_action::GoalStatus::STATUS_SUCCEEDED;
|
||||
status_message.status_list[0] = goal_status;
|
||||
status_publisher->publish(status_message);
|
||||
client_executor.spin_once();
|
||||
response->sequence = feedback_message.sequence;
|
||||
response->status = rclcpp_action::GoalStatus::STATUS_SUCCEEDED;
|
||||
goals.erase(request->uuid);
|
||||
} else {
|
||||
response->status = rclcpp_action::GoalStatus::STATUS_UNKNOWN;
|
||||
}
|
||||
});
|
||||
ASSERT_TRUE(result_service != nullptr);
|
||||
allocator.deallocate(result_service_name, allocator.state);
|
||||
|
||||
char * cancel_service_name = nullptr;
|
||||
ret = rcl_action_get_cancel_service_name(
|
||||
action_name, allocator, &cancel_service_name);
|
||||
ASSERT_EQ(RCL_RET_OK, ret);
|
||||
cancel_service = server_node->create_service<ActionCancelGoalService>(
|
||||
cancel_service_name,
|
||||
[this](
|
||||
const ActionCancelGoalRequest::SharedPtr request,
|
||||
ActionCancelGoalResponse::SharedPtr response)
|
||||
{
|
||||
rclcpp_action::GoalID zero_uuid;
|
||||
std::fill(zero_uuid.begin(), zero_uuid.end(), 0u);
|
||||
const rclcpp::Time cancel_stamp = request->goal_info.stamp;
|
||||
bool cancel_all = (
|
||||
request->goal_info.goal_id.uuid == zero_uuid &&
|
||||
cancel_stamp == zero_stamp);
|
||||
ActionStatusMessage status_message;
|
||||
auto it = goals.begin();
|
||||
while (it != goals.end()) {
|
||||
auto goal_request = it->second.first;
|
||||
auto goal_response = it->second.second;
|
||||
const rclcpp::Time goal_stamp = goal_response->stamp;
|
||||
bool cancel_this = (
|
||||
request->goal_info.goal_id.uuid == goal_request->uuid ||
|
||||
cancel_stamp > goal_stamp);
|
||||
if (cancel_all || cancel_this) {
|
||||
rclcpp_action::GoalStatus goal_status;
|
||||
goal_status.goal_info.goal_id.uuid = goal_request->uuid;
|
||||
goal_status.goal_info.stamp = goal_response->stamp;
|
||||
goal_status.status = rclcpp_action::GoalStatus::STATUS_CANCELED;
|
||||
status_message.status_list.push_back(goal_status);
|
||||
response->goals_canceling.push_back(goal_status.goal_info);
|
||||
it = goals.erase(it);
|
||||
} else {
|
||||
++it;
|
||||
}
|
||||
}
|
||||
status_publisher->publish(status_message);
|
||||
client_executor.spin_once();
|
||||
});
|
||||
ASSERT_TRUE(cancel_service != nullptr);
|
||||
allocator.deallocate(cancel_service_name, allocator.state);
|
||||
|
||||
char * feedback_topic_name = nullptr;
|
||||
ret = rcl_action_get_feedback_topic_name(
|
||||
action_name, allocator, &feedback_topic_name);
|
||||
ASSERT_EQ(RCL_RET_OK, ret);
|
||||
feedback_publisher = server_node->create_publisher<ActionFeedbackMessage>(feedback_topic_name);
|
||||
ASSERT_TRUE(feedback_publisher != nullptr);
|
||||
allocator.deallocate(feedback_topic_name, allocator.state);
|
||||
|
||||
char * status_topic_name = nullptr;
|
||||
ret = rcl_action_get_status_topic_name(
|
||||
action_name, allocator, &status_topic_name);
|
||||
ASSERT_EQ(RCL_RET_OK, ret);
|
||||
status_publisher = server_node->create_publisher<ActionStatusMessage>(
|
||||
status_topic_name, rcl_action_qos_profile_status_default);
|
||||
ASSERT_TRUE(status_publisher != nullptr);
|
||||
allocator.deallocate(status_topic_name, allocator.state);
|
||||
server_executor.add_node(server_node);
|
||||
|
||||
client_node = std::make_shared<rclcpp::Node>(client_node_name, namespace_name);
|
||||
client_executor.add_node(client_node);
|
||||
|
||||
ASSERT_EQ(RCL_RET_OK, rcl_enable_ros_time_override(clock.get_clock_handle()));
|
||||
ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(clock.get_clock_handle(), RCL_S_TO_NS(1)));
|
||||
}
|
||||
|
||||
void Teardown()
|
||||
{
|
||||
status_publisher.reset();
|
||||
feedback_publisher.reset();
|
||||
cancel_service.reset();
|
||||
result_service.reset();
|
||||
goal_service.reset();
|
||||
server_node.reset();
|
||||
client_node.reset();
|
||||
}
|
||||
|
||||
template<typename FutureT>
|
||||
void dual_spin_until_future_complete(std::shared_future<FutureT> & future)
|
||||
{
|
||||
std::future_status status;
|
||||
do {
|
||||
server_executor.spin_some();
|
||||
client_executor.spin_some();
|
||||
status = future.wait_for(std::chrono::seconds(0));
|
||||
} while (std::future_status::ready != status);
|
||||
}
|
||||
|
||||
rclcpp::Clock clock{RCL_ROS_TIME};
|
||||
const rclcpp::Time zero_stamp{0, 0, RCL_ROS_TIME};
|
||||
|
||||
rclcpp::Node::SharedPtr server_node;
|
||||
rclcpp::executors::SingleThreadedExecutor server_executor;
|
||||
rclcpp::Node::SharedPtr client_node;
|
||||
rclcpp::executors::SingleThreadedExecutor client_executor;
|
||||
const char * const server_node_name{"fibonacci_action_test_server"};
|
||||
const char * const client_node_name{"fibonacci_action_test_client"};
|
||||
const char * const namespace_name{"/rclcpp_action/test/client"};
|
||||
const char * const action_name{"fibonacci_test"};
|
||||
|
||||
std::map<
|
||||
rclcpp_action::GoalID,
|
||||
std::pair<
|
||||
typename ActionGoalRequest::SharedPtr,
|
||||
typename ActionGoalResponse::SharedPtr>> goals;
|
||||
typename rclcpp::Service<ActionGoalRequestService>::SharedPtr goal_service;
|
||||
typename rclcpp::Service<ActionGoalResultService>::SharedPtr result_service;
|
||||
typename rclcpp::Service<ActionCancelGoalService>::SharedPtr cancel_service;
|
||||
typename rclcpp::Publisher<ActionFeedbackMessage>::SharedPtr feedback_publisher;
|
||||
typename rclcpp::Publisher<ActionStatusMessage>::SharedPtr status_publisher;
|
||||
};
|
||||
|
||||
TEST_F(TestClient, construction_and_destruction)
|
||||
{
|
||||
auto node = std::make_shared<rclcpp::Node>("my_node", "/rclcpp_action/test/client");
|
||||
|
||||
auto ac = rclcpp_action::create_client<test_msgs::action::Fibonacci>(node.get(), "fibonacci");
|
||||
(void)ac;
|
||||
ASSERT_NO_THROW(rclcpp_action::create_client<ActionType>(client_node, action_name).reset());
|
||||
}
|
||||
|
||||
TEST_F(TestClient, async_send_goal_but_ignore_feedback_and_result)
|
||||
{
|
||||
auto action_client = rclcpp_action::create_client<ActionType>(client_node, action_name);
|
||||
|
||||
ActionGoal bad_goal;
|
||||
bad_goal.order = -5;
|
||||
auto future_goal_handle = action_client->async_send_goal(bad_goal, nullptr, true);
|
||||
dual_spin_until_future_complete(future_goal_handle);
|
||||
EXPECT_EQ(nullptr, future_goal_handle.get().get());
|
||||
|
||||
ActionGoal good_goal;
|
||||
good_goal.order = 5;
|
||||
future_goal_handle = action_client->async_send_goal(good_goal, nullptr, true);
|
||||
dual_spin_until_future_complete(future_goal_handle);
|
||||
auto goal_handle = future_goal_handle.get();
|
||||
EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_ACCEPTED, goal_handle->get_status());
|
||||
EXPECT_FALSE(goal_handle->is_feedback_aware());
|
||||
EXPECT_FALSE(goal_handle->is_result_aware());
|
||||
EXPECT_THROW(goal_handle->async_result(), rclcpp_action::exceptions::UnawareGoalHandleError);
|
||||
}
|
||||
|
||||
TEST_F(TestClient, async_send_goal_and_ignore_feedback_but_wait_for_result)
|
||||
{
|
||||
auto action_client = rclcpp_action::create_client<ActionType>(client_node, action_name);
|
||||
|
||||
ActionGoal goal;
|
||||
goal.order = 5;
|
||||
auto future_goal_handle = action_client->async_send_goal(goal);
|
||||
dual_spin_until_future_complete(future_goal_handle);
|
||||
auto goal_handle = future_goal_handle.get();
|
||||
EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_ACCEPTED, goal_handle->get_status());
|
||||
EXPECT_FALSE(goal_handle->is_feedback_aware());
|
||||
EXPECT_TRUE(goal_handle->is_result_aware());
|
||||
auto future_result = goal_handle->async_result();
|
||||
dual_spin_until_future_complete(future_result);
|
||||
auto result = future_result.get();
|
||||
ASSERT_EQ(6ul, result.response->sequence.size());
|
||||
EXPECT_EQ(0, result.response->sequence[0]);
|
||||
EXPECT_EQ(1, result.response->sequence[1]);
|
||||
EXPECT_EQ(5, result.response->sequence[5]);
|
||||
}
|
||||
|
||||
TEST_F(TestClient, async_send_goal_with_feedback_and_result)
|
||||
{
|
||||
auto action_client = rclcpp_action::create_client<ActionType>(client_node, action_name);
|
||||
|
||||
ActionGoal goal;
|
||||
goal.order = 4;
|
||||
int feedback_count = 0;
|
||||
auto future_goal_handle = action_client->async_send_goal(
|
||||
goal,
|
||||
[&feedback_count](
|
||||
typename ActionGoalHandle::SharedPtr goal_handle,
|
||||
const std::shared_ptr<const ActionFeedback> feedback) mutable
|
||||
{
|
||||
(void)goal_handle;
|
||||
(void)feedback;
|
||||
feedback_count++;
|
||||
});
|
||||
dual_spin_until_future_complete(future_goal_handle);
|
||||
auto goal_handle = future_goal_handle.get();
|
||||
EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_ACCEPTED, goal_handle->get_status());
|
||||
EXPECT_TRUE(goal_handle->is_feedback_aware());
|
||||
EXPECT_TRUE(goal_handle->is_result_aware());
|
||||
auto future_result = goal_handle->async_result();
|
||||
dual_spin_until_future_complete(future_result);
|
||||
auto result = future_result.get();
|
||||
|
||||
ASSERT_EQ(5u, result.response->sequence.size());
|
||||
EXPECT_EQ(3, result.response->sequence.back());
|
||||
EXPECT_EQ(5, feedback_count);
|
||||
}
|
||||
|
||||
TEST_F(TestClient, async_cancel_one_goal)
|
||||
{
|
||||
auto action_client = rclcpp_action::create_client<ActionType>(client_node, action_name);
|
||||
|
||||
ActionGoal goal;
|
||||
goal.order = 5;
|
||||
auto future_goal_handle = action_client->async_send_goal(goal, nullptr, true);
|
||||
dual_spin_until_future_complete(future_goal_handle);
|
||||
auto goal_handle = future_goal_handle.get();
|
||||
EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_ACCEPTED, goal_handle->get_status());
|
||||
|
||||
auto future_cancel = action_client->async_cancel_goal(goal_handle);
|
||||
dual_spin_until_future_complete(future_cancel);
|
||||
bool goal_canceled = future_cancel.get();
|
||||
EXPECT_TRUE(goal_canceled);
|
||||
}
|
||||
|
||||
TEST_F(TestClient, async_cancel_all_goals)
|
||||
{
|
||||
auto action_client = rclcpp_action::create_client<ActionType>(client_node, action_name);
|
||||
|
||||
ActionGoal goal;
|
||||
goal.order = 6;
|
||||
auto future_goal_handle0 = action_client->async_send_goal(goal, nullptr, true);
|
||||
dual_spin_until_future_complete(future_goal_handle0);
|
||||
auto goal_handle0 = future_goal_handle0.get();
|
||||
|
||||
ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(clock.get_clock_handle(), RCL_S_TO_NS(2)));
|
||||
|
||||
goal.order = 8;
|
||||
auto future_goal_handle1 = action_client->async_send_goal(goal, nullptr, true);
|
||||
dual_spin_until_future_complete(future_goal_handle1);
|
||||
auto goal_handle1 = future_goal_handle1.get();
|
||||
|
||||
if (goal_handle1->get_goal_id() < goal_handle0->get_goal_id()) {
|
||||
goal_handle0.swap(goal_handle1);
|
||||
}
|
||||
|
||||
ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(clock.get_clock_handle(), RCL_S_TO_NS(3)));
|
||||
|
||||
auto future_cancel_all = action_client->async_cancel_all_goals();
|
||||
dual_spin_until_future_complete(future_cancel_all);
|
||||
auto cancel_response = future_cancel_all.get();
|
||||
|
||||
ASSERT_EQ(2ul, cancel_response->goals_canceling.size());
|
||||
EXPECT_EQ(goal_handle0->get_goal_id(), cancel_response->goals_canceling[0].goal_id.uuid);
|
||||
EXPECT_EQ(goal_handle1->get_goal_id(), cancel_response->goals_canceling[1].goal_id.uuid);
|
||||
EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_CANCELED, goal_handle0->get_status());
|
||||
EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_CANCELED, goal_handle1->get_status());
|
||||
}
|
||||
|
||||
TEST_F(TestClient, async_cancel_some_goals)
|
||||
{
|
||||
auto action_client = rclcpp_action::create_client<ActionType>(client_node, action_name);
|
||||
|
||||
ActionGoal goal;
|
||||
goal.order = 6;
|
||||
auto future_goal_handle0 = action_client->async_send_goal(goal, nullptr, true);
|
||||
dual_spin_until_future_complete(future_goal_handle0);
|
||||
auto goal_handle0 = future_goal_handle0.get();
|
||||
|
||||
ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(clock.get_clock_handle(), RCL_S_TO_NS(2)));
|
||||
|
||||
goal.order = 8;
|
||||
auto future_goal_handle1 = action_client->async_send_goal(goal, nullptr, true);
|
||||
dual_spin_until_future_complete(future_goal_handle1);
|
||||
auto goal_handle1 = future_goal_handle1.get();
|
||||
|
||||
ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(clock.get_clock_handle(), RCL_S_TO_NS(3)));
|
||||
|
||||
auto future_cancel_some =
|
||||
action_client->async_cancel_goals_before(goal_handle1->get_goal_stamp());
|
||||
dual_spin_until_future_complete(future_cancel_some);
|
||||
auto cancel_response = future_cancel_some.get();
|
||||
|
||||
ASSERT_EQ(1ul, cancel_response->goals_canceling.size());
|
||||
EXPECT_EQ(goal_handle0->get_goal_id(), cancel_response->goals_canceling[0].goal_id.uuid);
|
||||
EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_CANCELED, goal_handle0->get_status());
|
||||
}
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue