|
| 1 | +// Copyright (c) 2020 Matt Richard. All rights reserved. |
| 2 | +// |
| 3 | +// Licensed under the Apache License, Version 2.0 (the "License"); |
| 4 | +// you may not use this file except in compliance with the License. |
| 5 | +// You may obtain a copy of the License at |
| 6 | +// |
| 7 | +// http://www.apache.org/licenses/LICENSE-2.0 |
| 8 | +// |
| 9 | +// Unless required by applicable law or agreed to in writing, software |
| 10 | +// distributed under the License is distributed on an "AS IS" BASIS, |
| 11 | +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
| 12 | +// See the License for the specific language governing permissions and |
| 13 | +// limitations under the License. |
| 14 | + |
| 15 | +#include "rcl_action_client_bindings.h" |
| 16 | + |
| 17 | +#include <rcl/error_handling.h> |
| 18 | +#include <rcl/rcl.h> |
| 19 | +#include <rcl_action/rcl_action.h> |
| 20 | + |
| 21 | +#include <string> |
| 22 | + |
| 23 | +#include "handle_manager.h" |
| 24 | +#include "macros.h" |
| 25 | +#include "rcl_handle.h" |
| 26 | +#include "rcl_utilities.h" |
| 27 | + |
| 28 | +namespace rclnodejs { |
| 29 | + |
| 30 | +Napi::Value ActionCreateClient(const Napi::CallbackInfo& info) { |
| 31 | + Napi::Env env = info.Env(); |
| 32 | + |
| 33 | + RclHandle* node_handle = RclHandle::Unwrap(info[0].As<Napi::Object>()); |
| 34 | + rcl_node_t* node = reinterpret_cast<rcl_node_t*>(node_handle->ptr()); |
| 35 | + std::string action_name = info[1].As<Napi::String>().Utf8Value(); |
| 36 | + std::string interface_name = info[2].As<Napi::String>().Utf8Value(); |
| 37 | + std::string package_name = info[3].As<Napi::String>().Utf8Value(); |
| 38 | + |
| 39 | + const rosidl_action_type_support_t* ts = |
| 40 | + GetActionTypeSupport(package_name, interface_name); |
| 41 | + |
| 42 | + if (ts) { |
| 43 | + rcl_action_client_options_t action_client_ops = |
| 44 | + rcl_action_client_get_default_options(); |
| 45 | + |
| 46 | + auto goal_service_qos = GetQoSProfile(info[4]); |
| 47 | + auto result_service_qos = GetQoSProfile(info[5]); |
| 48 | + auto cancel_service_qos = GetQoSProfile(info[6]); |
| 49 | + auto feedback_topic_qos = GetQoSProfile(info[7]); |
| 50 | + auto status_topic_qos = GetQoSProfile(info[8]); |
| 51 | + |
| 52 | + if (goal_service_qos) { |
| 53 | + action_client_ops.goal_service_qos = *goal_service_qos; |
| 54 | + } |
| 55 | + if (result_service_qos) { |
| 56 | + action_client_ops.result_service_qos = *result_service_qos; |
| 57 | + } |
| 58 | + if (cancel_service_qos) { |
| 59 | + action_client_ops.cancel_service_qos = *cancel_service_qos; |
| 60 | + } |
| 61 | + if (feedback_topic_qos) { |
| 62 | + action_client_ops.feedback_topic_qos = *feedback_topic_qos; |
| 63 | + } |
| 64 | + if (status_topic_qos) { |
| 65 | + action_client_ops.status_topic_qos = *status_topic_qos; |
| 66 | + } |
| 67 | + |
| 68 | + rcl_action_client_t* action_client = reinterpret_cast<rcl_action_client_t*>( |
| 69 | + malloc(sizeof(rcl_action_client_t))); |
| 70 | + *action_client = rcl_action_get_zero_initialized_client(); |
| 71 | + |
| 72 | + THROW_ERROR_IF_NOT_EQUAL( |
| 73 | + rcl_action_client_init(action_client, node, ts, action_name.c_str(), |
| 74 | + &action_client_ops), |
| 75 | + RCL_RET_OK, rcl_get_error_string().str); |
| 76 | + auto js_obj = RclHandle::NewInstance( |
| 77 | + env, action_client, node_handle, [node](void* ptr) { |
| 78 | + rcl_action_client_t* action_client = |
| 79 | + reinterpret_cast<rcl_action_client_t*>(ptr); |
| 80 | + rcl_ret_t ret = rcl_action_client_fini(action_client, node); |
| 81 | + free(ptr); |
| 82 | + THROW_ERROR_IF_NOT_EQUAL(RCL_RET_OK, ret, rcl_get_error_string().str); |
| 83 | + }); |
| 84 | + |
| 85 | + return js_obj; |
| 86 | + } else { |
| 87 | + Napi::Error::New(env, GetErrorMessageAndClear()) |
| 88 | + .ThrowAsJavaScriptException(); |
| 89 | + return env.Undefined(); |
| 90 | + } |
| 91 | +} |
| 92 | + |
| 93 | +Napi::Value ActionServerIsAvailable(const Napi::CallbackInfo& info) { |
| 94 | + Napi::Env env = info.Env(); |
| 95 | + |
| 96 | + RclHandle* node_handle = RclHandle::Unwrap(info[0].As<Napi::Object>()); |
| 97 | + rcl_node_t* node = reinterpret_cast<rcl_node_t*>(node_handle->ptr()); |
| 98 | + RclHandle* action_client_handle = |
| 99 | + RclHandle::Unwrap(info[1].As<Napi::Object>()); |
| 100 | + rcl_action_client_t* action_client = |
| 101 | + reinterpret_cast<rcl_action_client_t*>(action_client_handle->ptr()); |
| 102 | + |
| 103 | + bool is_available; |
| 104 | + THROW_ERROR_IF_NOT_EQUAL( |
| 105 | + RCL_RET_OK, |
| 106 | + rcl_action_server_is_available(node, action_client, &is_available), |
| 107 | + rcl_get_error_string().str); |
| 108 | + |
| 109 | + return Napi::Boolean::New(env, is_available); |
| 110 | +} |
| 111 | + |
| 112 | +Napi::Value ActionSendGoalRequest(const Napi::CallbackInfo& info) { |
| 113 | + Napi::Env env = info.Env(); |
| 114 | + |
| 115 | + RclHandle* action_client_handle = |
| 116 | + RclHandle::Unwrap(info[0].As<Napi::Object>()); |
| 117 | + rcl_action_client_t* action_client = |
| 118 | + reinterpret_cast<rcl_action_client_t*>(action_client_handle->ptr()); |
| 119 | + void* buffer = info[1].As<Napi::Buffer<char>>().Data(); |
| 120 | + |
| 121 | + int64_t sequence_number; |
| 122 | + THROW_ERROR_IF_NOT_EQUAL( |
| 123 | + rcl_action_send_goal_request(action_client, buffer, &sequence_number), |
| 124 | + RCL_RET_OK, rcl_get_error_string().str); |
| 125 | + |
| 126 | + return Napi::Number::New(env, static_cast<int32_t>(sequence_number)); |
| 127 | +} |
| 128 | + |
| 129 | +Napi::Value ActionTakeCancelRequest(const Napi::CallbackInfo& info) { |
| 130 | + Napi::Env env = info.Env(); |
| 131 | + |
| 132 | + RclHandle* action_server_handle = |
| 133 | + RclHandle::Unwrap(info[0].As<Napi::Object>()); |
| 134 | + rcl_action_server_t* action_server = |
| 135 | + reinterpret_cast<rcl_action_server_t*>(action_server_handle->ptr()); |
| 136 | + rmw_request_id_t* header = |
| 137 | + reinterpret_cast<rmw_request_id_t*>(malloc(sizeof(rmw_request_id_t))); |
| 138 | + |
| 139 | + void* taken_request = info[1].As<Napi::Buffer<char>>().Data(); |
| 140 | + rcl_ret_t ret = |
| 141 | + rcl_action_take_cancel_request(action_server, header, taken_request); |
| 142 | + if (ret != RCL_RET_ACTION_SERVER_TAKE_FAILED) { |
| 143 | + auto js_obj = RclHandle::NewInstance(env, header, nullptr, |
| 144 | + [](void* ptr) { free(ptr); }); |
| 145 | + return js_obj; |
| 146 | + } |
| 147 | + |
| 148 | + return env.Undefined(); |
| 149 | +} |
| 150 | + |
| 151 | +Napi::Value ActionSendResultRequest(const Napi::CallbackInfo& info) { |
| 152 | + Napi::Env env = info.Env(); |
| 153 | + |
| 154 | + RclHandle* action_client_handle = |
| 155 | + RclHandle::Unwrap(info[0].As<Napi::Object>()); |
| 156 | + rcl_action_client_t* action_client = |
| 157 | + reinterpret_cast<rcl_action_client_t*>(action_client_handle->ptr()); |
| 158 | + void* buffer = info[1].As<Napi::Buffer<char>>().Data(); |
| 159 | + |
| 160 | + int64_t sequence_number; |
| 161 | + THROW_ERROR_IF_NOT_EQUAL( |
| 162 | + rcl_action_send_result_request(action_client, buffer, &sequence_number), |
| 163 | + RCL_RET_OK, rcl_get_error_string().str); |
| 164 | + |
| 165 | + return Napi::Number::New(env, static_cast<int32_t>(sequence_number)); |
| 166 | +} |
| 167 | + |
| 168 | +Napi::Value ActionTakeFeedback(const Napi::CallbackInfo& info) { |
| 169 | + Napi::Env env = info.Env(); |
| 170 | + |
| 171 | + RclHandle* action_client_handle = |
| 172 | + RclHandle::Unwrap(info[0].As<Napi::Object>()); |
| 173 | + rcl_action_client_t* action_client = |
| 174 | + reinterpret_cast<rcl_action_client_t*>(action_client_handle->ptr()); |
| 175 | + void* buffer = info[1].As<Napi::Buffer<char>>().Data(); |
| 176 | + |
| 177 | + rcl_ret_t ret = rcl_action_take_feedback(action_client, buffer); |
| 178 | + if (ret != RCL_RET_OK && ret != RCL_RET_ACTION_CLIENT_TAKE_FAILED) { |
| 179 | + Napi::Error::New(env, rcl_get_error_string().str) |
| 180 | + .ThrowAsJavaScriptException(); |
| 181 | + rcl_reset_error(); |
| 182 | + return Napi::Boolean::New(env, false); |
| 183 | + } |
| 184 | + |
| 185 | + if (ret != RCL_RET_ACTION_CLIENT_TAKE_FAILED) { |
| 186 | + return Napi::Boolean::New(env, true); |
| 187 | + } |
| 188 | + return env.Undefined(); |
| 189 | +} |
| 190 | + |
| 191 | +Napi::Value ActionTakeStatus(const Napi::CallbackInfo& info) { |
| 192 | + Napi::Env env = info.Env(); |
| 193 | + |
| 194 | + RclHandle* action_client_handle = |
| 195 | + RclHandle::Unwrap(info[0].As<Napi::Object>()); |
| 196 | + rcl_action_client_t* action_client = |
| 197 | + reinterpret_cast<rcl_action_client_t*>(action_client_handle->ptr()); |
| 198 | + void* buffer = info[1].As<Napi::Buffer<char>>().Data(); |
| 199 | + |
| 200 | + rcl_ret_t ret = rcl_action_take_status(action_client, buffer); |
| 201 | + if (ret != RCL_RET_OK && ret != RCL_RET_ACTION_CLIENT_TAKE_FAILED) { |
| 202 | + rcl_reset_error(); |
| 203 | + Napi::Error::New(env, rcl_get_error_string().str) |
| 204 | + .ThrowAsJavaScriptException(); |
| 205 | + return Napi::Boolean::New(env, false); |
| 206 | + } |
| 207 | + |
| 208 | + if (ret != RCL_RET_ACTION_CLIENT_TAKE_FAILED) { |
| 209 | + return Napi::Boolean::New(env, true); |
| 210 | + } |
| 211 | + return env.Undefined(); |
| 212 | +} |
| 213 | + |
| 214 | +Napi::Object InitActionClientBindings(Napi::Env env, Napi::Object exports) { |
| 215 | + exports.Set("actionCreateClient", |
| 216 | + Napi::Function::New(env, ActionCreateClient)); |
| 217 | + exports.Set("actionServerIsAvailable", |
| 218 | + Napi::Function::New(env, ActionServerIsAvailable)); |
| 219 | + exports.Set("actionSendGoalRequest", |
| 220 | + Napi::Function::New(env, ActionSendGoalRequest)); |
| 221 | + exports.Set("actionTakeCancelRequest", |
| 222 | + Napi::Function::New(env, ActionTakeCancelRequest)); |
| 223 | + exports.Set("actionSendResultRequest", |
| 224 | + Napi::Function::New(env, ActionSendResultRequest)); |
| 225 | + exports.Set("actionTakeFeedback", |
| 226 | + Napi::Function::New(env, ActionTakeFeedback)); |
| 227 | + exports.Set("actionTakeStatus", Napi::Function::New(env, ActionTakeStatus)); |
| 228 | + return exports; |
| 229 | +} |
| 230 | + |
| 231 | +} // namespace rclnodejs |
0 commit comments