557 if (!initialized_)
return Fut();
560 storage_,
reinterpret_cast<const uint8_t(*)[16]
>(goal_id));
561 if (ret != 0)
return Fut();
565 return Fut(storage_, &nros_cpp_action_client_poll, goal_id, &results_,
590 if (ret != 0) return ::rclcpp::Result(ret);
592 if (FeedbackType::ffi_deserialize(buf, len, &feedback) != 0) {
595 return ::rclcpp::Result::success();
621 if (initialized_ && !feedback_stream_.is_valid()) {
622 feedback_stream_.bind(
this, &Client::take_feedback);
624 return feedback_stream_;
627 const ::rclcpp::Stream<FeedbackType>& feedback_stream()
const {
return feedback_stream_; }
651 void (*
feedback)(
const uint8_t goal_id[16],
const uint8_t* data,
size_t len,
void* ctx);
653 void (*
result)(
const uint8_t goal_id[16], int32_t status,
const uint8_t* data,
size_t len,
682 uint8_t buf[::rclcpp::detail::buffer_bounds<GoalType>::tx];
684 if (GoalType::ffi_serialize(&goal, buf,
sizeof(buf), &len) != 0) {
687 return ::rclcpp::Result(nros_cpp_action_client_send_goal_async(
688 storage_, buf, len,
reinterpret_cast<uint8_t(*)[16]
>(goal_id)));
697#ifdef NROS_CPP_HAS_SHARED_PTR
715 ::rclcpp::Future<GoalAccept>
716 async_send_goal(
const GoalType& goal,
const SendGoalOptions& options = SendGoalOptions()) {
718 if (!send_goal_async(goal, goal_id).ok()) return ::rclcpp::Future<GoalAccept>();
719 UpstreamState* st = upstream_state();
722 g.id = GoalUUID(goal_id);
724 st->goals.push_back(g);
725 return ::rclcpp::Future<GoalAccept>(storage_, &nros_cpp_action_client_poll, goal_id,
726 &accepts_, executor_);
730 template <
typename T =
void>
733 static_assert(::rclcpp::detail::refuse<T>::value, NROS_RCLCPP_REFUSE_ACTION_GOAL_HANDLE);
734 return ::rclcpp::Future<GoalAccept>();
738#ifdef NROS_CPP_HAS_STD_PROMISE
760 detail::DrivenSharedFuture<WrappedResult>
762 ResultCallback result_callback =
nullptr) {
763 if (!initialized_ || !goal_handle)
return detail::DrivenSharedFuture<WrappedResult>();
764 typename GoalHandle::ResultState* rs = goal_handle->result_state();
765 detail::DrivenSharedFuture<WrappedResult> fut(rs->future, pump_cell());
766 if (rs->done)
return fut;
767 UpstreamState* st = upstream_state();
769 bool created =
false;
770 if (upstream_goal(goal_handle->get_goal_id().data(), at) ==
nullptr) {
772 g.id = goal_handle->get_goal_id();
774 st->goals.push_back(g);
775 at = st->goals.size() - 1;
778 if (!goal_handle->result_aware_) {
780 if (created) st->goals.erase(st->goals.begin() +
static_cast<long>(at));
781 return detail::DrivenSharedFuture<WrappedResult>();
783 goal_handle->result_aware_ =
true;
785 UpstreamGoal& g = st->goals[at];
787 g.handle = goal_handle;
789 if (result_callback) g.options.result_callback = result_callback;
809 detail::DrivenSharedFuture<typename CancelResponse::SharedPtr>
811 CancelCallback cancel_callback =
nullptr) {
812 if (!goal_handle)
return detail::DrivenSharedFuture<typename CancelResponse::SharedPtr>();
813 return send_cancel(goal_handle->get_goal_id(), 0, 0, cancel_callback);
819 detail::DrivenSharedFuture<typename CancelResponse::SharedPtr>
821 return send_cancel(GoalUUID(), 0, 0, cancel_callback);
828 detail::DrivenSharedFuture<typename CancelResponse::SharedPtr>
830 CancelCallback cancel_callback =
nullptr) {
831 const int64_t ns = stamp.nanoseconds();
832 const int64_t sec = ns >= 0 ? ns / 1000000000LL : -((-ns + 999999999LL) / 1000000000LL);
833 const int64_t nsec = ns - sec * 1000000000LL;
834 return send_cancel(GoalUUID(),
static_cast<int32_t
>(sec),
static_cast<uint32_t
>(nsec),
838#ifndef NROS_CPP_HAS_STD_PROMISE
843 template <typename T = void, typename... Args> void async_get_result(Args&&...) {
844 static_assert(::rclcpp::detail::refuse<T>::value, NROS_RCLCPP_REFUSE_ACTION_GOAL_HANDLE);
848 static_assert(::rclcpp::detail::refuse<T>::value, NROS_RCLCPP_REFUSE_ACTION_GOAL_HANDLE);
852 static_assert(::rclcpp::detail::refuse<T>::value, NROS_RCLCPP_REFUSE_ACTION_GOAL_HANDLE);
856 static_assert(::rclcpp::detail::refuse<T>::value, NROS_RCLCPP_REFUSE_ACTION_GOAL_HANDLE);
876 return ::rclcpp::Result(nros_cpp_action_client_cancel_goal(
877 storage_,
reinterpret_cast<const uint8_t(*)[16]
>(goal_id)));
901 if (take_stray_cancel(code)) {
903 return ::rclcpp::Result::success();
905 nros_cpp_ret_t ret = nros_cpp_action_client_try_recv_cancel_response(storage_, &code);
906 if (ret != 0) return ::rclcpp::Result(ret);
908 return ::rclcpp::Result::success();
920 return ::rclcpp::Result(nros_cpp_action_client_get_result_async(
921 storage_,
reinterpret_cast<const uint8_t(*)[16]
>(goal_id)));
946 return install_dispatch();
955 if (!initialized_)
return;
957 nros_cpp_action_client_poll(storage_);
978 accepts_.orphan_all();
979 results_.orphan_all();
981 nros_cpp_action_client_destroy(storage_);
982 initialized_ =
false;
997 : executor_(other.executor_), initialized_(other.initialized_), action_name_{} {
998 ::memcpy(action_name_, other.action_name_,
sizeof(action_name_));
1003 if (
this != &other) {
1005 accepts_.orphan_all();
1006 results_.orphan_all();
1008 nros_cpp_action_client_destroy(storage_);
1011 executor_ = other.executor_;
1012 initialized_ = other.initialized_;
1013 ::memcpy(action_name_, other.action_name_,
sizeof(action_name_));
1021 Client() : executor_(nullptr), initialized_(false), action_name_{} {}
1027 friend class ::rclcpp::Node;
1033 accepts_.
forget = &Client::forget_accept;
1035 results_.
forget = &Client::forget_result;
1038 storage_, &Client::on_goal_response, &Client::on_feedback, &Client::on_result,
this);
1039 if (ret != 0) return ::rclcpp::Result(ret);
1042 return ::rclcpp::Result(
1055 static void forget_accept(
void* ctx,
const uint8_t goal[16]) {
1056 static_cast<Client*
>(ctx)->forget_request(NROS_CPP_ACTION_CHANNEL_SEND_GOAL, goal);
1058 static void forget_result(
void* ctx,
const uint8_t goal[16]) {
1059 static_cast<Client*
>(ctx)->forget_request(NROS_CPP_ACTION_CHANNEL_GET_RESULT, goal);
1061 void forget_request(int32_t channel,
const uint8_t goal[16]) {
1062 if (!initialized_)
return;
1063 const bool accept = channel == NROS_CPP_ACTION_CHANNEL_SEND_GOAL;
1065 if (upstream_wants(accept, goal))
return;
1066 (void)nros_cpp_action_client_forget(storage_, channel,
1067 reinterpret_cast<const uint8_t(*)[16]
>(goal));
1071 void adopt(
Client& other) {
1072 user_ = other.user_;
1073 upstream_ =
static_cast<detail::HostedBox&&
>(other.upstream_);
1075 feedback_len_ = other.feedback_len_;
1076 feedback_ready_ = other.feedback_ready_;
1078 stash_codes_[i] = other.stash_codes_[i];
1079 stash_head_ = other.stash_head_;
1080 stash_len_ = other.stash_len_;
1081 other.stash_len_ = 0;
1082 feedback_truncated_ = other.feedback_truncated_;
1083 for (
size_t i = 0; i < feedback_len_; ++i)
1084 feedback_buf_[i] = other.feedback_buf_[i];
1085 other.feedback_ready_ =
false;
1086 if (other.initialized_) {
1087 nros_cpp_action_client_relocate(other.storage_, storage_);
1088 other.accepts_.move_to(accepts_, storage_);
1089 other.results_.move_to(results_, storage_);
1090 other.initialized_ =
false;
1091 (void)install_dispatch();
1102 static void on_goal_response(
bool accepted,
const uint8_t goal_id[16],
void* ctx) {
1105 if (owner !=
nullptr) {
1109 for (
int i = 0; i < 16; ++i)
1110 wire[i] = goal_id[i];
1111 wire[16] = accepted ? 1 : 0;
1112 self->accepts_.deliver(owner, wire,
sizeof(wire));
1114 self->upstream_goal_response(accepted, goal_id);
1115 if (self->user_.goal_response !=
nullptr)
1116 self->user_.goal_response(accepted, goal_id, self->user_.context);
1119 static void on_feedback(
const uint8_t goal_id[16],
const uint8_t* data,
size_t len,
void* ctx) {
1121 const size_t n = len <
sizeof(self->feedback_buf_) ? len : sizeof(self->feedback_buf_);
1122 for (
size_t i = 0; i < n; ++i)
1123 self->feedback_buf_[i] = data[i];
1124 self->feedback_len_ = n;
1125 self->feedback_truncated_ = n < len;
1126 self->feedback_ready_ =
true;
1127 self->upstream_feedback(goal_id, data, len);
1128 if (self->user_.feedback !=
nullptr)
1129 self->user_.feedback(goal_id, data, len, self->user_.context);
1132 static void on_result(
const uint8_t goal_id[16], int32_t status,
const uint8_t* data,
1133 size_t len,
void* ctx) {
1136 if (owner !=
nullptr) self->results_.deliver(owner, data, len);
1137 self->upstream_result(goal_id, status, data, len);
1138 if (self->user_.result !=
nullptr)
1139 self->user_.result(goal_id, status, data, len, self->user_.context);
1144#ifdef NROS_CPP_HAS_SHARED_PTR
1150 struct UpstreamGoal {
1152 SendGoalOptions options;
1154 ::std::weak_ptr<GoalHandle> weak;
1155 bool accepted =
false;
1157#ifdef NROS_CPP_HAS_STD_PROMISE
1160 struct CancelWaiter {
1165 ::std::promise<typename CancelResponse::SharedPtr> promise;
1166 CancelCallback callback;
1169 struct UpstreamState {
1170 ::std::vector<UpstreamGoal> goals;
1171#ifdef NROS_CPP_HAS_STD_PROMISE
1172 ::std::vector<CancelWaiter> cancels;
1174 ::std::shared_ptr<detail::PumpCell> pump;
1177 UpstreamState* upstream_state() {
1178 UpstreamState* st = upstream_.template as<UpstreamState>();
1179 if (st ==
nullptr) {
1180 st =
new UpstreamState();
1181 upstream_.reset(st, [](
void* p) {
delete static_cast<UpstreamState*
>(p); });
1186 UpstreamState* upstream_goal(
const uint8_t goal_id[16],
size_t& at)
const {
1187 UpstreamState* st = upstream_.template as<UpstreamState>();
1188 if (st ==
nullptr)
return nullptr;
1189 const GoalUUID id(goal_id);
1190 for (at = 0; at < st->goals.size(); ++at)
1191 if (st->goals[at].id ==
id)
return st;
1196 static void prune_upstream(UpstreamState* st) {
1197 for (
size_t i = 0; i < st->goals.size();) {
1198 const UpstreamGoal& g = st->goals[i];
1199 if (g.accepted && !g.handle && g.weak.expired())
1200 st->goals.erase(st->goals.begin() +
static_cast<long>(i));
1207 ::std::shared_ptr<detail::PumpCell> pump_cell() {
1208 UpstreamState* st = upstream_state();
1210 st->pump = ::std::make_shared<detail::PumpCell>();
1211 st->pump->fn = &Client::pump_entry;
1213 st->pump->ctx =
this;
1214 st->pump->executor = executor_;
1218 static void pump_entry(
void* ctx) {
1220 if (!self->initialized_)
return;
1221 (void)nros_cpp_action_client_poll(self->storage_);
1230 bool upstream_wants(
bool accept,
const uint8_t goal_id[16]) {
1231#ifdef NROS_CPP_HAS_SHARED_PTR
1233 UpstreamState* st = upstream_goal(goal_id, at);
1234 if (st ==
nullptr)
return false;
1235 UpstreamGoal& g = st->goals[at];
1237 if (g.accepted)
return false;
1238 const bool wanted =
static_cast<bool>(g.options.goal_response_callback) ||
1239 static_cast<bool>(g.options.feedback_callback) ||
1240 static_cast<bool>(g.options.result_callback);
1241 if (!wanted) st->goals.erase(st->goals.begin() +
static_cast<long>(at));
1244 return g.handle && g.handle->result_aware_;
1253 void repoint_pump() {
1254#ifdef NROS_CPP_HAS_SHARED_PTR
1255 UpstreamState* st = upstream_.template as<UpstreamState>();
1256 if (st !=
nullptr && st->pump) {
1257 st->pump->ctx =
this;
1258 st->pump->executor = executor_;
1267 void abandon_upstream() {
1268#ifdef NROS_CPP_HAS_SHARED_PTR
1269 UpstreamState* st = upstream_.template as<UpstreamState>();
1270 if (st ==
nullptr)
return;
1271 if (st->pump) st->pump->ctx =
nullptr;
1272#ifdef NROS_CPP_HAS_STD_PROMISE
1273 for (CancelWaiter& w : st->cancels)
1274 w.promise.set_value(
typename CancelResponse::SharedPtr());
1275 st->cancels.clear();
1276 for (UpstreamGoal& g : st->goals) {
1277 if (g.handle && g.handle->result_aware_) {
1279 wrapped.goal_id = g.id;
1280 wrapped.code = ResultCode::UNKNOWN;
1281 g.handle->set_result(wrapped);
1289#ifdef NROS_CPP_HAS_STD_PROMISE
1290 detail::DrivenSharedFuture<typename CancelResponse::SharedPtr>
1291 send_cancel(
const GoalUUID& key, int32_t sec, uint32_t nanosec, CancelCallback& cb) {
1292 using Fut = detail::DrivenSharedFuture<typename CancelResponse::SharedPtr>;
1293 if (!initialized_)
return Fut();
1295 if (nros_cpp_action_client_cancel_goals(storage_,
1296 reinterpret_cast<const uint8_t(*)[16]
>(key.data()),
1297 sec, nanosec, &seq) != 0)
1299 UpstreamState* st = upstream_state();
1304 ::std::shared_future<typename CancelResponse::SharedPtr> f = w.promise.get_future().share();
1305 st->cancels.push_back(
static_cast<CancelWaiter&&
>(w));
1306 return Fut(f, pump_cell());
1319 static void on_cancel_reply(
const uint8_t goal[16], int64_t seq,
const uint8_t* reply,
1320 size_t len,
void* ctx) {
1326 nros_cpp_action_cancel_reply_decode(reply, len, &code, infos,
1328 if (self->upstream_cancel_reply(goal, seq, taken, code, infos, count))
return;
1329 if (taken) self->stash_cancel(code);
1333 void stash_cancel(int8_t code) {
1334 if (stash_len_ == CANCEL_STASH_CAP) {
1338 stash_codes_[(stash_head_ + stash_len_) % CANCEL_STASH_CAP] = code;
1342 bool take_stray_cancel(int8_t& code) {
1343 if (stash_len_ == 0)
return false;
1344 code = stash_codes_[stash_head_];
1352 bool upstream_cancel_reply(
const uint8_t goal[16], int64_t seq,
bool taken, int8_t code,
1353 const nros_cpp_action_goal_info_t* infos,
size_t count) {
1354#ifdef NROS_CPP_HAS_STD_PROMISE
1355 UpstreamState* st = upstream_.template as<UpstreamState>();
1356 if (st ==
nullptr)
return false;
1357 const GoalUUID key(goal);
1359 while (at < st->cancels.size() &&
1361 : st->cancels[at].key == key))
1363 if (at == st->cancels.size())
return false;
1364 CancelWaiter w =
static_cast<CancelWaiter&&
>(st->cancels[at]);
1365 st->cancels.erase(st->cancels.begin() +
static_cast<long>(at));
1367 w.promise.set_value(
typename CancelResponse::SharedPtr());
1368 if (w.callback) w.callback(
typename CancelResponse::SharedPtr());
1371 typename CancelResponse::SharedPtr resp = ::std::make_shared<CancelResponse>();
1372 resp->return_code = code;
1375 for (
size_t k = 0; k < n; ++k) {
1376 detail::CancelGoalInfo info;
1377 for (
int b = 0; b < 16; ++b)
1378 info.goal_id.uuid[b] = infos[k].uuid[b];
1379 info.stamp.sec = infos[k].stamp_sec;
1380 info.stamp.nanosec = infos[k].stamp_nanosec;
1381 resp->goals_canceling.push_back(info);
1383 w.promise.set_value(resp);
1384 if (w.callback) w.callback(resp);
1397 void upstream_goal_response(
bool accepted,
const uint8_t goal_id[16]) {
1398#ifdef NROS_CPP_HAS_SHARED_PTR
1400 UpstreamState* st = upstream_goal(goal_id, at);
1401 if (st ==
nullptr)
return;
1402 UpstreamGoal& g = st->goals[at];
1403 if (g.accepted)
return;
1404 auto cb = g.options.goal_response_callback;
1406 st->goals.erase(st->goals.begin() +
static_cast<long>(at));
1407 if (cb) cb(
nullptr);
1411 handle->feedback_aware_ =
static_cast<bool>(g.options.feedback_callback);
1412 handle->result_aware_ =
static_cast<bool>(g.options.result_callback);
1414 if (
handle->result_aware_)
1418 if (!
handle->feedback_aware_ && !
handle->result_aware_)
1419 st->goals.erase(st->goals.begin() +
static_cast<long>(at));
1424 handle->result_aware_ =
false;
1425 if (upstream_goal(goal_id, at) !=
nullptr) {
1426 st->goals[at].handle.reset();
1427 st->goals[at].weak =
handle;
1436 void upstream_feedback(
const uint8_t goal_id[16],
const uint8_t* data,
size_t len) {
1437#ifdef NROS_CPP_HAS_SHARED_PTR
1439 UpstreamState* st = upstream_goal(goal_id, at);
1440 if (st ==
nullptr)
return;
1441 UpstreamGoal& g = st->goals[at];
1445 if (g.accepted) st->goals.erase(st->goals.begin() +
static_cast<long>(at));
1448 auto cb = g.options.feedback_callback;
1450 ::std::shared_ptr<FeedbackType> fb = ::std::make_shared<FeedbackType>();
1451 if (FeedbackType::ffi_deserialize(data, len, fb.get()) != 0)
return;
1460 void upstream_result(
const uint8_t goal_id[16], int32_t status,
const uint8_t* data,
1462#ifdef NROS_CPP_HAS_SHARED_PTR
1464 UpstreamState* st = upstream_goal(goal_id, at);
1465 if (st ==
nullptr)
return;
1466 UpstreamGoal g = st->goals[at];
1467 st->goals.erase(st->goals.begin() +
static_cast<long>(at));
1469 if (
handle)
handle->status_ =
static_cast<int8_t
>(status);
1471 wrapped.goal_id = g.id;
1472 wrapped.code = (status == 4 || status == 5 || status == 6) ?
static_cast<ResultCode>(status)
1473 : ResultCode::UNKNOWN;
1474 wrapped.result = ::std::make_shared<ResultType>();
1475 if (ResultType::ffi_deserialize(data, len, wrapped.result.get()) != 0) {
1476 wrapped.result.reset();
1478#ifdef NROS_CPP_HAS_STD_PROMISE
1481 if (g.options.result_callback) g.options.result_callback(wrapped);
1493 static nros_cpp_ret_t take_feedback(
void* ctx, uint8_t* buf,
size_t cap,
size_t* out_len) {
1496 if (!self->initialized_)
1498 if (!self->feedback_ready_) (void)nros_cpp_action_client_poll(self->storage_);
1499 if (!self->feedback_ready_)
return 0;
1500 self->feedback_ready_ =
false;
1501 if (self->feedback_truncated_ || self->feedback_len_ > cap) {
1502 *out_len = self->feedback_len_;
1505 for (
size_t i = 0; i < self->feedback_len_; ++i)
1506 buf[i] = self->feedback_buf_[i];
1507 *out_len = self->feedback_len_;
1511 alignas(8) uint8_t storage_[NROS_CPP_ACTION_CLIENT_STORAGE_SIZE];
1528 SendGoalOptions user_;
1530 uint8_t feedback_buf_[::rclcpp::rx_buffer_capacity<FeedbackType>::value];
1531 size_t feedback_len_ = 0;
1532 bool feedback_ready_ =
false;
1533 bool feedback_truncated_ =
false;
1537 uint8_t stash_head_ = 0;
1538 uint8_t stash_len_ = 0;
1542 detail::HostedBox upstream_;
1553template <
typename A>
1555 const ::rclcpp::QoS& qos) {
1560 &handle_, action_name, A::TYPE_NAME, A::Goal::TYPE_HASH, &hashes, ffi_qos, out.storage_);
1562 out.executor_ = executor_handle_;
1565 out.initialized_ =
true;
1566 (void)out.install_dispatch();
1569 ::rclcpp::detail::assign_entity_name(out.action_name_, action_name);
1570 out.initialized_ =
true;