|
17 | 17 | #include <cassert> |
18 | 18 | #include <cstdlib> |
19 | 19 | #include <string> |
| 20 | +#include <vector> |
20 | 21 |
|
21 | 22 | #include "rcl/error_handling.h" |
22 | 23 | #include "rcl/node.h" |
@@ -44,30 +45,103 @@ Java_org_ros2_rcljava_RCLJava_nativeCreateContextHandle(JNIEnv *, jclass) |
44 | 45 | return context_handle; |
45 | 46 | } |
46 | 47 |
|
| 48 | +bool parse_arguments(JNIEnv * env, jobject cli_args, rcl_arguments_t * arguments) |
| 49 | +{ |
| 50 | + // TODO(clalancette): we could probably make this faster by just doing these |
| 51 | + // method lookups during some initialization time. But we'd have to add a lot |
| 52 | + // more infrastructure for that, and we don't expect to call |
| 53 | + // 'nativeCreateNodeHandle' that often, so I think this is OK for now. |
| 54 | + jclass java_util_ArrayList = env->FindClass("java/util/ArrayList"); |
| 55 | + jmethodID java_util_ArrayList_size = env->GetMethodID(java_util_ArrayList, "size", "()I"); |
| 56 | + jmethodID java_util_ArrayList_get = env->GetMethodID( |
| 57 | + java_util_ArrayList, "get", "(I)Ljava/lang/Object;"); |
| 58 | + |
| 59 | + *arguments = rcl_get_zero_initialized_arguments(); |
| 60 | + jint argc = env->CallIntMethod(cli_args, java_util_ArrayList_size); |
| 61 | + std::vector<const char *> argv(argc); |
| 62 | + for (jint i = 0; i < argc; ++i) { |
| 63 | + jstring element = |
| 64 | + static_cast<jstring>(env->CallObjectMethod(cli_args, java_util_ArrayList_get, i)); |
| 65 | + argv[i] = env->GetStringUTFChars(element, nullptr); |
| 66 | + env->DeleteLocalRef(element); |
| 67 | + } |
| 68 | + rcl_ret_t ret = rcl_parse_arguments(argc, &argv[0], rcl_get_default_allocator(), arguments); |
| 69 | + for (jint i = 0; i < argc; ++i) { |
| 70 | + jstring element = |
| 71 | + static_cast<jstring>(env->CallObjectMethod(cli_args, java_util_ArrayList_get, i)); |
| 72 | + env->ReleaseStringUTFChars(element, argv[i]); |
| 73 | + env->DeleteLocalRef(element); |
| 74 | + } |
| 75 | + if (ret != RCL_RET_OK) { |
| 76 | + std::string msg = "Failed to parse node arguments: " + std::string(rcl_get_error_string().str); |
| 77 | + rcl_reset_error(); |
| 78 | + rcljava_throw_rclexception(env, ret, msg); |
| 79 | + return false; |
| 80 | + } |
| 81 | + |
| 82 | + return true; |
| 83 | +} |
| 84 | + |
47 | 85 | JNIEXPORT jlong JNICALL |
48 | 86 | Java_org_ros2_rcljava_RCLJava_nativeCreateNodeHandle( |
49 | | - JNIEnv * env, jclass, jstring jnode_name, jstring jnamespace, jlong context_handle) |
| 87 | + JNIEnv * env, jclass, jstring jnode_name, jstring jnamespace, jlong context_handle, |
| 88 | + jobject cli_args, jboolean use_global_arguments, jboolean enable_rosout) |
50 | 89 | { |
51 | | - const char * node_name = env->GetStringUTFChars(jnode_name, 0); |
52 | | - const char * node_namespace = env->GetStringUTFChars(jnamespace, 0); |
| 90 | + const char * node_name_tmp = env->GetStringUTFChars(jnode_name, 0); |
| 91 | + std::string node_name(node_name_tmp); |
| 92 | + env->ReleaseStringUTFChars(jnode_name, node_name_tmp); |
| 93 | + |
| 94 | + const char * namespace_tmp = env->GetStringUTFChars(jnamespace, 0); |
| 95 | + std::string namespace_(namespace_tmp); |
| 96 | + env->ReleaseStringUTFChars(jnamespace, namespace_tmp); |
53 | 97 |
|
54 | 98 | rcl_context_t * context = reinterpret_cast<rcl_context_t *>(context_handle); |
55 | 99 |
|
56 | 100 | rcl_node_t * node = static_cast<rcl_node_t *>(malloc(sizeof(rcl_node_t))); |
| 101 | + if (node == nullptr) { |
| 102 | + env->ThrowNew(env->FindClass("java/lang/OutOfMemoryError"), "Failed to allocate node"); |
| 103 | + return 0; |
| 104 | + } |
57 | 105 | *node = rcl_get_zero_initialized_node(); |
58 | 106 |
|
59 | | - rcl_node_options_t default_options = rcl_node_get_default_options(); |
60 | | - rcl_ret_t ret = rcl_node_init(node, node_name, node_namespace, context, &default_options); |
61 | | - env->ReleaseStringUTFChars(jnode_name, node_name); |
62 | | - env->ReleaseStringUTFChars(jnamespace, node_namespace); |
| 107 | + rcl_arguments_t arguments; |
| 108 | + if (!parse_arguments(env, cli_args, &arguments)) { |
| 109 | + // All of the exception setup was done by parse_arguments, just return here. |
| 110 | + free(node); |
| 111 | + return 0; |
| 112 | + } |
| 113 | + |
| 114 | + rcl_node_options_t options = rcl_node_get_default_options(); |
| 115 | + options.use_global_arguments = use_global_arguments; |
| 116 | + options.arguments = arguments; |
| 117 | + options.enable_rosout = enable_rosout; |
| 118 | + rcl_ret_t ret = rcl_node_init(node, node_name.c_str(), namespace_.c_str(), context, &options); |
63 | 119 | if (ret != RCL_RET_OK) { |
64 | 120 | std::string msg = "Failed to create node: " + std::string(rcl_get_error_string().str); |
65 | 121 | rcl_reset_error(); |
| 122 | + free(node); |
| 123 | + if (rcl_arguments_fini(&arguments) != RCL_RET_OK) { |
| 124 | + // We are already throwing an exception, just ignore the return here |
| 125 | + } |
| 126 | + rcljava_throw_rclexception(env, ret, msg); |
| 127 | + return 0; |
| 128 | + } |
| 129 | + |
| 130 | + ret = rcl_arguments_fini(&arguments); |
| 131 | + if (ret != RCL_RET_OK) { |
| 132 | + // We failed to cleanup |
| 133 | + std::string msg = "Failed to cleanup after creating node: " + std::string( |
| 134 | + rcl_get_error_string().str); |
| 135 | + rcl_reset_error(); |
| 136 | + if (rcl_node_fini(node) != RCL_RET_OK) { |
| 137 | + // We are already throwing an exception, just ignore the return here |
| 138 | + } |
| 139 | + free(node); |
66 | 140 | rcljava_throw_rclexception(env, ret, msg); |
67 | 141 | return 0; |
68 | 142 | } |
69 | | - jlong node_handle = reinterpret_cast<jlong>(node); |
70 | | - return node_handle; |
| 143 | + |
| 144 | + return reinterpret_cast<jlong>(node); |
71 | 145 | } |
72 | 146 |
|
73 | 147 | JNIEXPORT jstring JNICALL |
|
0 commit comments