rclcpp/rclcpp_action/include/rclcpp_action/client.hpp
William Woodall 8bffd25746 add wait_for_action_server() for action clients (#598)
* add wait_for_action_server() for action clients

Signed-off-by: William Woodall <william@osrfoundation.org>

* Handle negative timeouts in wait_for_service() and wait_for_action_server() methods.

* Fix uncrustify errors.

* Ignore take failure on services for connext
2018-12-06 18:57:25 -08:00

544 lines
17 KiB
C++

// 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__CLIENT_HPP_
#define RCLCPP_ACTION__CLIENT_HPP_
#include <rclcpp/macros.hpp>
#include <rclcpp/node_interfaces/node_base_interface.hpp>
#include <rclcpp/node_interfaces/node_logging_interface.hpp>
#include <rclcpp/node_interfaces/node_graph_interface.hpp>
#include <rclcpp/logger.hpp>
#include <rclcpp/time.hpp>
#include <rclcpp/waitable.hpp>
#include <rosidl_generator_c/action_type_support_struct.h>
#include <rosidl_typesupport_cpp/action_type_support.hpp>
#include <algorithm>
#include <chrono>
#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"
namespace rclcpp_action
{
// Forward declaration
class ClientBaseImpl;
/// Base Action Client implementation
/// \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:
RCLCPP_ACTION_PUBLIC
virtual ~ClientBase();
/// Return true if there is an action server that is ready to take goal requests.
RCLCPP_ACTION_PUBLIC
bool
action_server_is_ready() const;
/// Wait for action_server_is_ready() to become true, or until the given timeout is reached.
template<typename RatioT = std::milli>
bool
wait_for_action_server(
std::chrono::duration<int64_t, RatioT> timeout = std::chrono::duration<int64_t, RatioT>(-1))
{
return wait_for_action_server_nanoseconds(
std::chrono::duration_cast<std::chrono::nanoseconds>(timeout)
);
}
// -------------
// 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::NodeGraphInterface::SharedPtr node_graph,
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);
/// Wait for action_server_is_ready() to become true, or until the given timeout is reached.
RCLCPP_ACTION_PUBLIC
bool
wait_for_action_server_nanoseconds(std::chrono::nanoseconds timeout);
// -----------------------------------------------------
// 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_;
};
/// 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<ActionT>)
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,
rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph,
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, node_graph, 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->action_goal_id.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->action_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));
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->action_goal_id.uuid;
if (goal_handles_.count(goal_id) == 0) {
RCLCPP_DEBUG(
this->get_logger(),
"Received feedback for unknown goal. Ignoring...");
return;
}
typename GoalHandle::SharedPtr goal_handle = goal_handles_[goal_id];
// goal_handle->call_feedback_callback(goal_handle, feedback_message->feedback);
goal_handle->call_feedback_callback(goal_handle, feedback_message);
}
/// \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->action_goal_id.uuid = goal_handle->get_goal_id();
this->send_result_request(
std::static_pointer_cast<void>(goal_result_request),
[goal_handle, this](std::shared_ptr<void> response) mutable
{
// Wrap the response in a struct with the fields a user cares about
Result result;
using GoalResultResponse = typename ActionT::GoalResultService::Response;
result.response = std::static_pointer_cast<GoalResultResponse>(response);
result.goal_id = goal_handle->get_goal_id();
result.code = static_cast<ResultCode>(result.response->action_status);
goal_handle->set_result(result);
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_