diff --git a/rcljava/CMakeLists.txt b/rcljava/CMakeLists.txt index b357f0d4..983eabf4 100644 --- a/rcljava/CMakeLists.txt +++ b/rcljava/CMakeLists.txt @@ -2,16 +2,19 @@ cmake_minimum_required(VERSION 3.5) project(rcljava) +find_package(action_msgs REQUIRED) find_package(ament_cmake REQUIRED) find_package(ament_cmake_export_jars REQUIRED) find_package(ament_cmake_export_jni_libraries REQUIRED) find_package(builtin_interfaces REQUIRED) find_package(rcl REQUIRED) +find_package(rcl_action REQUIRED) find_package(rcl_interfaces REQUIRED) find_package(rcljava_common REQUIRED) find_package(rmw REQUIRED) find_package(rmw_implementation_cmake REQUIRED) find_package(rosgraph_msgs REQUIRED) +find_package(unique_identifier_msgs REQUIRED) include(CrossCompilingExtra) @@ -57,6 +60,8 @@ endif() set(${PROJECT_NAME}_jni_sources "src/main/cpp/org_ros2_rcljava_RCLJava.cpp" "src/main/cpp/org_ros2_rcljava_Time.cpp" + "src/main/cpp/org_ros2_rcljava_action_ActionServerImpl.cpp" + "src/main/cpp/org_ros2_rcljava_action_ActionServerImpl_GoalHandleImpl.cpp" "src/main/cpp/org_ros2_rcljava_client_ClientImpl.cpp" "src/main/cpp/org_ros2_rcljava_contexts_ContextImpl.cpp" "src/main/cpp/org_ros2_rcljava_detail_QosIncompatibleStatus.cpp" @@ -105,11 +110,14 @@ foreach(_jni_source ${${PROJECT_NAME}_jni_sources}) endif() ament_target_dependencies(${_target_name} - "rcl" - "rcljava_common" + "action_msgs" "builtin_interfaces" + "rcl" + "rcl_action" "rcl_interfaces" + "rcljava_common" "rosgraph_msgs" + "unique_identifier_msgs" ) target_include_directories(${_target_name} @@ -129,6 +137,12 @@ endforeach() set(${PROJECT_NAME}_sources "src/main/java/org/ros2/rcljava/RCLJava.java" "src/main/java/org/ros2/rcljava/Time.java" + "src/main/java/org/ros2/rcljava/action/ActionServer.java" + "src/main/java/org/ros2/rcljava/action/ActionServerGoalHandle.java" + "src/main/java/org/ros2/rcljava/action/ActionServerImpl.java" + "src/main/java/org/ros2/rcljava/action/CancelCallback.java" + "src/main/java/org/ros2/rcljava/action/GoalCallback.java" + "src/main/java/org/ros2/rcljava/action/GoalStatus.java" "src/main/java/org/ros2/rcljava/client/Client.java" "src/main/java/org/ros2/rcljava/client/ClientImpl.java" "src/main/java/org/ros2/rcljava/concurrent/Callback.java" @@ -205,10 +219,12 @@ add_jar("${PROJECT_NAME}_jar" OUTPUT_NAME ${PROJECT_NAME} INCLUDE_JARS - ${rcljava_common_JARS} + ${action_msgs_JARS} ${builtin_interfaces_JARS} ${rcl_interfaces_JARS} + ${rcljava_common_JARS} ${rosgraph_msgs_JARS} + ${unique_identifier_msgs_JARS} ) install_jar("${PROJECT_NAME}_jar" "share/${PROJECT_NAME}/java") @@ -218,6 +234,7 @@ if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) find_package(std_msgs REQUIRED) find_package(mockito_vendor REQUIRED) + find_package(test_msgs REQUIRED) ament_lint_auto_find_test_dependencies() set(${PROJECT_NAME}_message_files @@ -245,9 +262,11 @@ if(BUILD_TESTING) ${${PROJECT_NAME}_message_files} ${${PROJECT_NAME}_service_files} DEPENDENCIES + action_msgs builtin_interfaces rcl_interfaces rosgraph_msgs + unique_identifier_msgs ${_java_type_supports} SKIP_INSTALL ) @@ -264,6 +283,8 @@ if(BUILD_TESTING) "src/test/java/org/ros2/rcljava/RCLJavaTest.java" "src/test/java/org/ros2/rcljava/SpinTest.java" "src/test/java/org/ros2/rcljava/TimeTest.java" + "src/test/java/org/ros2/rcljava/action/ActionServerTest.java" + "src/test/java/org/ros2/rcljava/action/MockActionClient.java" "src/test/java/org/ros2/rcljava/client/ClientTest.java" "src/test/java/org/ros2/rcljava/contexts/ContextTest.java" "src/test/java/org/ros2/rcljava/node/NodeOptionsTest.java" @@ -283,6 +304,7 @@ if(BUILD_TESTING) "org.ros2.rcljava.RCLJavaTest" "org.ros2.rcljava.SpinTest" "org.ros2.rcljava.TimeTest" + "org.ros2.rcljava.action.ActionServerTest" "org.ros2.rcljava.client.ClientTest" "org.ros2.rcljava.contexts.ContextTest" "org.ros2.rcljava.node.NodeOptionsTest" @@ -344,6 +366,33 @@ if(BUILD_TESTING) list_append_unique(_deps_library_dirs ${_dep_dir}) endforeach() + foreach(_dep_lib ${action_msgs_LIBRARIES}) + get_filename_component(_dep_dir "${_dep_lib}" DIRECTORY) + list_append_unique(_deps_library_dirs ${_dep_dir}) + endforeach() + foreach(_dep_lib ${action_msgs_JNI_LIBRARIES}) + get_filename_component(_dep_dir "${_dep_lib}" DIRECTORY) + list_append_unique(_deps_library_dirs ${_dep_dir}) + endforeach() + + foreach(_dep_lib ${unique_identifier_msgs_LIBRARIES}) + get_filename_component(_dep_dir "${_dep_lib}" DIRECTORY) + list_append_unique(_deps_library_dirs ${_dep_dir}) + endforeach() + foreach(_dep_lib ${unique_identifier_msgs_JNI_LIBRARIES}) + get_filename_component(_dep_dir "${_dep_lib}" DIRECTORY) + list_append_unique(_deps_library_dirs ${_dep_dir}) + endforeach() + + foreach(_dep_lib ${test_msgs_LIBRARIES}) + get_filename_component(_dep_dir "${_dep_lib}" DIRECTORY) + list_append_unique(_deps_library_dirs ${_dep_dir}) + endforeach() + foreach(_dep_lib ${test_msgs_JNI_LIBRARIES}) + get_filename_component(_dep_dir "${_dep_lib}" DIRECTORY) + list_append_unique(_deps_library_dirs ${_dep_dir}) + endforeach() + list_append_unique(_deps_library_dirs ${CMAKE_CURRENT_BINARY_DIR}) list_append_unique(_deps_library_dirs ${CMAKE_CURRENT_BINARY_DIR}/rcljava) list_append_unique(_deps_library_dirs ${CMAKE_CURRENT_BINARY_DIR}/rosidl_generator_java/rcljava/msg/) @@ -359,13 +408,16 @@ if(BUILD_TESTING) TESTS "${testsuite}" INCLUDE_JARS + "${action_msgs_JARS}" "${rcljava_common_JARS}" "${rcljava_test_msgs_JARS}" "${std_msgs_JARS}" "${builtin_interfaces_JARS}" "${rcl_interfaces_JARS}" "${rosgraph_msgs_JARS}" + "${test_msgs_JARS}" "${mockito_vendor_JARS}" + "${unique_identifier_msgs_JARS}" "${_${PROJECT_NAME}_jar_file}" "${_${PROJECT_NAME}_messages_jar_file}" APPEND_LIBRARY_DIRS diff --git a/rcljava/include/org_ros2_rcljava_action_ActionServerImpl.h b/rcljava/include/org_ros2_rcljava_action_ActionServerImpl.h new file mode 100644 index 00000000..28b65441 --- /dev/null +++ b/rcljava/include/org_ros2_rcljava_action_ActionServerImpl.h @@ -0,0 +1,138 @@ +// Copyright 2020 ros2-java contributors +// +// 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. + +#include +/* Header for class org_ros2_rcljava_action_ActionServerImpl */ + +#ifndef ORG_ROS2_RCLJAVA_ACTION_ACTIONSERVERIMPL_H_ +#define ORG_ROS2_RCLJAVA_ACTION_ACTIONSERVERIMPL_H_ +#ifdef __cplusplus +extern "C" { +#endif + +/* + * Class: org_ros2_rcljava_action_ActionServerImpl + * Method: nativeGetNumberOfEntities + * Signature: (L)[I + * Returns array of numbers for each type of entity, + * [subscriptions, guard_conditions, timers, clients, services] + */ +JNIEXPORT jintArray +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeGetNumberOfEntities( + JNIEnv *, jclass, jlong); + +/* + * Class: org_ros2_rcljava_action_ActionServerImpl + * Method: nativeGetReadyEntities + * Signature: (LL)[Z + */ +JNIEXPORT jbooleanArray +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeGetReadyEntities( + JNIEnv *, jclass, jlong, jlong); + +/* + * Class: org_ros2_rcljava_action_ActionServerImpl + * Method: nativeDispose + * Signature: (JJ)V + */ +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeDispose(JNIEnv *, jclass, jlong, jlong); + +/* + * Class: org_ros2_rcljava_action_ActionServerImpl + * Method: nativeCreateActionServer + * Signature: (JLjava/lang/Class;Ljava/lang/String;)J + */ +JNIEXPORT jlong +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeCreateActionServer( + JNIEnv *, jobject, jlong, jlong, jclass, jstring); + +/* + * Class: org_ros2_rcljava_action_ActionServerImpl + * Method: nativeTakeGoalRequest + * Signature: (JJJJLorg/ros2/rcljava/interfaces/MessageDefinition;)Lorg/ros2/rcljava/RMWRequestId; + */ +JNIEXPORT jobject +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeTakeGoalRequest( + JNIEnv *, jclass, jlong, jlong, jlong, jlong, jobject); + +/* + * Class: org_ros2_rcljava_action_ActionServerImpl + * Method: nativeTakeCancelRequest + * Signature: (JJJJLorg/ros2/rcljava/interfaces/MessageDefinition;)Lorg/ros2/rcljava/RMWRequestId; + */ +JNIEXPORT jobject +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeTakeCancelRequest( + JNIEnv *, jclass, jlong, jlong, jlong, jlong, jobject); + +/* + * Class: org_ros2_rcljava_action_ActionServerImpl + * Method: nativeTakeResultRequest + * Signature: (JJJJLorg/ros2/rcljava/interfaces/MessageDefinition;)Lorg/ros2/rcljava/RMWRequestId; + */ +JNIEXPORT jobject +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeTakeResultRequest( + JNIEnv *, jclass, jlong, jlong, jlong, jlong, jobject); + +/* + * Class: org_ros2_rcljava_action_ActionServerImpl + * Method: nativeSendGoalResponse + * Signature: (JLorg/ros2/rcljava/RMWRequestId;JJJLorg/ros2/rcljava/interfaces/MessageDefinition;) + */ +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeSendGoalResponse( + JNIEnv *, jclass, jlong, jobject, jlong, jlong, jlong, jobject); + +/* + * Class: org_ros2_rcljava_action_ActionServerImpl + * Method: nativeSendCancelResponse + * Signature: (JLorg/ros2/rcljava/RMWRequestId;JJJLorg/ros2/rcljava/interfaces/MessageDefinition;) + */ +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeSendCancelResponse( + JNIEnv *, jclass, jlong, jobject, jlong, jlong, jlong, jobject); + +/* + * Class: org_ros2_rcljava_action_ActionServerImpl + * Method: nativeSendResultResponse + * Signature: (JLorg/ros2/rcljava/RMWRequestId;JJJLorg/ros2/rcljava/interfaces/MessageDefinition;) + */ +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeSendResultResponse( + JNIEnv *, jclass, jlong, jobject, jlong, jlong, jlong, jobject); + +/* + * Class: org_ros2_rcljava_action_ActionServerImpl + * Method: nativeProcessCancelRequest + * Signature: (JJJJLorg/ros2/rcljava/interfaces/MessageDefinition;Lorg/ros2/rcljava/interfaces/MessageDefinition;) + */ +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeProcessCancelRequest( + JNIEnv *, jclass, jlong, jlong, jlong, jlong, jobject, jobject); + +/* + * Class: org_ros2_rcljava_action_ActionServerImpl + * Method: nativeCheckGoalExists + * Signature: (JLorg/ros2/rcljava/interfaces/MessageDefinition;JJ)Z + */ + +JNIEXPORT jboolean +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeCheckGoalExists( + JNIEnv * env, jclass, + jlong, jobject, jlong, jlong); + +#ifdef __cplusplus +} +#endif +#endif // ORG_ROS2_RCLJAVA_ACTION_ACTIONSERVERIMPL_H__ diff --git a/rcljava/include/org_ros2_rcljava_action_ActionServerImpl_GoalHandleImpl.h b/rcljava/include/org_ros2_rcljava_action_ActionServerImpl_GoalHandleImpl.h new file mode 100644 index 00000000..94aaab23 --- /dev/null +++ b/rcljava/include/org_ros2_rcljava_action_ActionServerImpl_GoalHandleImpl.h @@ -0,0 +1,99 @@ +// Copyright 2020 ros2-java contributors +// +// 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. + +#include +/* Header for class org_ros2_rcljava_action_ActionServerImpl_GoalHandleImpl */ + +#ifndef ORG_ROS2_RCLJAVA_ACTION_ACTIONSERVERIMPL_GOALHANDLEIMPL_H_ +#define ORG_ROS2_RCLJAVA_ACTION_ACTIONSERVERIMPL_GOALHANDLEIMPL_H_ +#ifdef __cplusplus +extern "C" { +#endif + +/* + * Class: org_ros2_rcljava_action_ActionServerImpl$GoalHandleImpl + * Method: nativeAcceptNewGoal + * Signature: (JJJLorg/ros2/rcljava/interfaces/MessageDefinition;)J + */ +JNIEXPORT jlong +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_00024GoalHandleImpl_nativeAcceptNewGoal( + JNIEnv *, jclass, jlong, jlong, jlong, jobject); + +/* + * Class: org_ros2_rcljava_action_ActionServerImpl$GoalHandleImpl + * Method: nativeGetStatus + * Signature: (J)I + */ +JNIEXPORT int +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_00024GoalHandleImpl_nativeGetStatus( + JNIEnv *, jclass, jlong); + +/* + * Class: org_ros2_rcljava_action_ActionServerImpl$GoalHandleImpl + * Method: nativeGoalEventExecute + * Signature: (J) + */ +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_00024GoalHandleImpl_nativeGoalEventExecute( + JNIEnv * env, jclass, jlong); + +/* + * Class: org_ros2_rcljava_action_ActionServerImpl$GoalHandleImpl + * Method: nativeGoalEventCancelGoal + * Signature: (J) + */ +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_00024GoalHandleImpl_nativeGoalEventCancelGoal( + JNIEnv * env, jclass, jlong); + +/* + * Class: org_ros2_rcljava_action_ActionServerImpl$GoalHandleImpl + * Method: nativeGoalEventSucceed + * Signature: (J) + */ +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_00024GoalHandleImpl_nativeGoalEventSucceed( + JNIEnv * env, jclass, jlong); + +/* + * Class: org_ros2_rcljava_action_ActionServerImpl$GoalHandleImpl + * Method: nativeGoalEventAbort + * Signature: (J) + */ +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_00024GoalHandleImpl_nativeGoalEventAbort( + JNIEnv * env, jclass, jlong); + +/* + * Class: org_ros2_rcljava_action_ActionServerImpl$GoalHandleImpl + * Method: nativeGoalEventCanceled + * Signature: (J) + */ +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_00024GoalHandleImpl_nativeGoalEventCanceled( + JNIEnv * env, jclass, jlong); + +/* + * Class: org_ros2_rcljava_action_ActionServerImpl$GoalHandleImpl + * Method: nativeDispose + * Signature: (J) + */ +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_00024GoalHandleImpl_nativeDipose( + JNIEnv *, jclass, jlong); + +#ifdef __cplusplus +} +#endif +#endif // ORG_ROS2_RCLJAVA_ACTION_ACTIONSERVERIMPL_GOALHANDLEIMPL_H__ diff --git a/rcljava/include/org_ros2_rcljava_executors_BaseExecutor.h b/rcljava/include/org_ros2_rcljava_executors_BaseExecutor.h index 5522ac8d..e96b88c8 100644 --- a/rcljava/include/org_ros2_rcljava_executors_BaseExecutor.h +++ b/rcljava/include/org_ros2_rcljava_executors_BaseExecutor.h @@ -98,6 +98,15 @@ JNIEXPORT void JNICALL Java_org_ros2_rcljava_executors_BaseExecutor_nativeWaitSetAddClient( JNIEnv *, jclass, jlong, jlong); +/* + * Class: org_ros2_rcljava_executors_BaseExecutor + * Method: nativeWaitSetAddActionServer + * Signature: (JJ)V + */ +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_executors_BaseExecutor_nativeWaitSetAddActionServer( + JNIEnv *, jclass, jlong, jlong); + /* * Class: org_ros2_rcljava_executors_BaseExecutor * Method: nativeTakeRequest diff --git a/rcljava/package.xml b/rcljava/package.xml index 9e32719b..d966ac03 100644 --- a/rcljava/package.xml +++ b/rcljava/package.xml @@ -12,15 +12,18 @@ ament_cmake_export_jni_libraries rcljava_common + action_msgs builtin_interfaces - rcl_interfaces rcl + rcl_action + rcl_interfaces rcpputils rmw_implementation_cmake rmw rosgraph_msgs rosidl_generator_c rosidl_typesupport_c + unique_identifier_msgs builtin_interfaces rcl_interfaces rmw @@ -29,15 +32,18 @@ rosidl_generator_java rosidl_typesupport_c + action_msgs builtin_interfaces - rcl_interfaces rcl + rcl_action + rcl_interfaces rcpputils rmw_implementation_cmake rmw_implementation rosgraph_msgs rosidl_runtime_c rosidl_parser + unique_identifier_msgs ament_lint_auto ament_lint_common @@ -50,6 +56,7 @@ rosidl_runtime_c rosidl_generator_java std_msgs + test_msgs ament_cmake diff --git a/rcljava/src/main/cpp/convert.hpp b/rcljava/src/main/cpp/convert.hpp new file mode 100644 index 00000000..31db9655 --- /dev/null +++ b/rcljava/src/main/cpp/convert.hpp @@ -0,0 +1,88 @@ +// Copyright 2017-2018 Esteve Fernandez +// +// 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. +// +#include + +#include +#include + +#include "rmw/rmw.h" + +#ifndef MAIN__CPP__CONVERT_HPP_ +#define MAIN__CPP__CONVERT_HPP_ + +namespace rcljava +{ + +jobject +convert_rmw_request_id_to_java(JNIEnv * env, rmw_request_id_t * request_id) +{ + jclass jrequest_id_class = env->FindClass("org/ros2/rcljava/service/RMWRequestId"); + assert(jrequest_id_class != nullptr); + + jmethodID jconstructor = env->GetMethodID(jrequest_id_class, "", "()V"); + assert(jconstructor != nullptr); + + jobject jrequest_id = env->NewObject(jrequest_id_class, jconstructor); + + jfieldID jsequence_number_field_id = env->GetFieldID(jrequest_id_class, "sequenceNumber", "J"); + jfieldID jwriter_guid_field_id = env->GetFieldID(jrequest_id_class, "writerGUID", "[B"); + + assert(jsequence_number_field_id != nullptr); + assert(jwriter_guid_field_id != nullptr); + + int8_t * writer_guid = request_id->writer_guid; + int64_t sequence_number = request_id->sequence_number; + + env->SetLongField(jrequest_id, jsequence_number_field_id, sequence_number); + + jsize writer_guid_len = sizeof(request_id->writer_guid) / sizeof(request_id->writer_guid[0]); + + jbyteArray jwriter_guid = env->NewByteArray(writer_guid_len); + env->SetByteArrayRegion(jwriter_guid, 0, writer_guid_len, reinterpret_cast(writer_guid)); + env->SetObjectField(jrequest_id, jwriter_guid_field_id, jwriter_guid); + + return jrequest_id; +} + +rmw_request_id_t * +convert_rmw_request_id_from_java(JNIEnv * env, jobject jrequest_id) +{ + assert(jrequest_id != nullptr); + + jclass jrequest_id_class = env->GetObjectClass(jrequest_id); + assert(jrequest_id_class != nullptr); + + jfieldID jsequence_number_field_id = env->GetFieldID(jrequest_id_class, "sequenceNumber", "J"); + jfieldID jwriter_guid_field_id = env->GetFieldID(jrequest_id_class, "writerGUID", "[B"); + + assert(jsequence_number_field_id != nullptr); + assert(jwriter_guid_field_id != nullptr); + + rmw_request_id_t * request_id = static_cast(malloc(sizeof(rmw_request_id_t))); + + int8_t * writer_guid = request_id->writer_guid; + request_id->sequence_number = env->GetLongField(jrequest_id, jsequence_number_field_id); + + jsize writer_guid_len = sizeof(request_id->writer_guid) / sizeof(request_id->writer_guid[0]); + + jbyteArray jwriter_guid = (jbyteArray)env->GetObjectField(jrequest_id, jwriter_guid_field_id); + env->GetByteArrayRegion(jwriter_guid, 0, writer_guid_len, reinterpret_cast(writer_guid)); + + return request_id; +} + +} // namespace rcljava + +#endif // MAIN__CPP__CONVERT_HPP_ diff --git a/rcljava/src/main/cpp/org_ros2_rcljava_action_ActionServerImpl.cpp b/rcljava/src/main/cpp/org_ros2_rcljava_action_ActionServerImpl.cpp new file mode 100644 index 00000000..2d48fef1 --- /dev/null +++ b/rcljava/src/main/cpp/org_ros2_rcljava_action_ActionServerImpl.cpp @@ -0,0 +1,365 @@ +// Copyright 2020 ros2-java contributors +// +// 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. + +#include + +#include +#include +#include + +#include "rcl/error_handling.h" +#include "rcl/rcl.h" +#include "rcl_action/rcl_action.h" +#include "rosidl_runtime_c/message_type_support_struct.h" + +#include "rcljava_common/exceptions.hpp" +#include "rcljava_common/signatures.hpp" + +#include "org_ros2_rcljava_action_ActionServerImpl.h" + +#include "./convert.hpp" + +using rcljava_common::exceptions::rcljava_throw_rclexception; +using rcljava_common::signatures::convert_from_java_signature; +using rcljava_common::signatures::convert_to_java_signature; +using rcljava_common::signatures::destroy_ros_message_signature; + +JNIEXPORT jintArray +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeGetNumberOfEntities( + JNIEnv * env, jclass, jlong action_server_handle) +{ + size_t num_subscriptions; + size_t num_guard_conditions; + size_t num_timers; + size_t num_clients; + size_t num_services; + rcl_action_server_t * action_server = reinterpret_cast( + action_server_handle); + rcl_ret_t ret = rcl_action_server_wait_set_get_num_entities( + action_server, + &num_subscriptions, + &num_guard_conditions, + &num_timers, + &num_clients, + &num_services); + if (ret != RCL_RET_OK) { + std::string msg = + "Failed to get number of entities for an action server: " + + std::string(rcl_get_error_string().str); + rcl_reset_error(); + rcljava_throw_rclexception(env, ret, msg); + } + jintArray result = env->NewIntArray(5); + jint temp_result[5] = { + static_cast(num_subscriptions), + static_cast(num_guard_conditions), + static_cast(num_timers), + static_cast(num_clients), + static_cast(num_services) + }; + env->SetIntArrayRegion(result, 0, 5, temp_result); + return result; +} + +JNIEXPORT jbooleanArray +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeGetReadyEntities( + JNIEnv * env, jclass, jlong action_server_handle, jlong wait_set_handle) +{ + rcl_action_server_t * action_server = reinterpret_cast( + action_server_handle); + rcl_wait_set_t * wait_set = reinterpret_cast(wait_set_handle); + + bool is_goal_request_ready = false; + bool is_cancel_request_ready = false; + bool is_result_request_ready = false; + bool is_goal_expired = false; + rcl_ret_t ret = rcl_action_server_wait_set_get_entities_ready( + wait_set, + action_server, + &is_goal_request_ready, + &is_cancel_request_ready, + &is_result_request_ready, + &is_goal_expired); + if (RCL_RET_OK != ret) { + std::string msg = "Failed to get ready entities for action server: " + + std::string(rcl_get_error_string().str); + rcl_reset_error(); + rcljava_throw_rclexception(env, ret, msg); + return NULL; + } + + jbooleanArray result = env->NewBooleanArray(4); + jboolean temp_result[4] = { + is_goal_request_ready, + is_cancel_request_ready, + is_result_request_ready, + is_goal_expired + }; + env->SetBooleanArrayRegion(result, 0, 4, temp_result); + return result; +} + +JNIEXPORT void JNICALL +Java_org_ros2_rcljava_action_ActionServerImpl_nativeDispose( + JNIEnv * env, jclass, jlong node_handle, jlong action_server_handle) +{ + if (action_server_handle == 0) { + // everything is ok, already destroyed + return; + } + + if (node_handle == 0) { + // TODO(jacobperron): throw exception + return; + } + + rcl_node_t * node = reinterpret_cast(node_handle); + + assert(node != NULL); + + rcl_action_server_t * action_server = reinterpret_cast( + action_server_handle); + + assert(action_server != NULL); + + rcl_ret_t ret = rcl_action_server_fini(action_server, node); + + if (ret != RCL_RET_OK) { + std::string msg = "Failed to destroy action server: " + + std::string(rcl_get_error_string().str); + rcl_reset_error(); + rcljava_throw_rclexception(env, ret, msg); + } +} + +JNIEXPORT jlong JNICALL +Java_org_ros2_rcljava_action_ActionServerImpl_nativeCreateActionServer( + JNIEnv * env, + jobject, + jlong node_handle, + jlong clock_handle, + jclass jaction_class, + jstring jaction_name) +{ + jmethodID mid = env->GetStaticMethodID(jaction_class, "getActionTypeSupport", "()J"); + assert(mid != NULL); + + jlong jts = env->CallStaticLongMethod(jaction_class, mid); + assert(jts != 0); + + const char * action_name = env->GetStringUTFChars(jaction_name, 0); + + rcl_node_t * node = reinterpret_cast(node_handle); + rcl_clock_t * clock = reinterpret_cast(clock_handle); + + rosidl_action_type_support_t * ts = reinterpret_cast(jts); + + rcl_action_server_t * action_server = static_cast( + malloc(sizeof(rcl_action_server_t))); + *action_server = rcl_action_get_zero_initialized_server(); + rcl_action_server_options_t action_server_ops = rcl_action_server_get_default_options(); + + rcl_ret_t ret = rcl_action_server_init( + action_server, node, clock, ts, action_name, &action_server_ops); + env->ReleaseStringUTFChars(jaction_name, action_name); + + if (ret != RCL_RET_OK) { + std::string msg = "Failed to create action server: " + std::string(rcl_get_error_string().str); + rcl_reset_error(); + rcljava_throw_rclexception(env, ret, msg); + free(action_server); + return 0; + } + + jlong jaction_server = reinterpret_cast(action_server); + return jaction_server; +} + +#define RCLJAVA_ACTION_SERVER_TAKE_REQUEST(Type) \ + do { \ + assert(jrequest_from_java_converter_handle != 0); \ + assert(jrequest_to_java_converter_handle != 0); \ + rcl_action_server_t * action_server = reinterpret_cast( \ + action_server_handle); \ + convert_from_java_signature convert_from_java = \ + reinterpret_cast(jrequest_from_java_converter_handle); \ + convert_to_java_signature convert_to_java = \ + reinterpret_cast(jrequest_to_java_converter_handle); \ + destroy_ros_message_signature destroy_ros_message = \ + reinterpret_cast(jrequest_destructor_handle); \ + void * taken_msg = convert_from_java(jrequest_msg, nullptr); \ + rmw_request_id_t header; \ + rcl_ret_t ret = rcl_action_take_ ## Type ## _request(action_server, &header, taken_msg); \ + if (ret != RCL_RET_OK && ret != RCL_RET_ACTION_SERVER_TAKE_FAILED) { \ + destroy_ros_message(taken_msg); \ + std::string msg = \ + "Failed to take " #Type " request: " + std::string(rcl_get_error_string().str); \ + rcl_reset_error(); \ + rcljava_throw_rclexception(env, ret, msg); \ + return nullptr; \ + } \ + if (RCL_RET_OK == ret) { \ + jobject jtaken_msg = convert_to_java(taken_msg, jrequest_msg); \ + destroy_ros_message(taken_msg); \ + assert(jtaken_msg != nullptr); \ + jobject jheader = rcljava::convert_rmw_request_id_to_java(env, &header); \ + return jheader; \ + } \ + destroy_ros_message(taken_msg); \ + return nullptr; \ + } \ + while (0) + +#define RCLJAVA_ACTION_SERVER_SEND_RESPONSE(Type) \ + do { \ + assert(jresponse_from_java_converter_handle != 0); \ + assert(jresponse_to_java_converter_handle != 0); \ + assert(jresponse_destructor_handle != 0); \ + rcl_action_server_t * action_server = reinterpret_cast( \ + action_server_handle); \ + convert_from_java_signature convert_from_java = \ + reinterpret_cast(jresponse_from_java_converter_handle); \ + void * response_msg = convert_from_java(jresponse_msg, nullptr); \ + rmw_request_id_t * request_id = rcljava::convert_rmw_request_id_from_java(env, jrequest_id); \ + rcl_ret_t ret = rcl_action_send_ ## Type ## _response( \ + action_server, request_id, response_msg); \ + destroy_ros_message_signature destroy_ros_message = \ + reinterpret_cast(jresponse_destructor_handle); \ + destroy_ros_message(response_msg); \ + if (ret != RCL_RET_OK) { \ + std::string msg = \ + "Failed to send " #Type " response: " + std::string(rcl_get_error_string().str); \ + rcl_reset_error(); \ + rcljava_throw_rclexception(env, ret, msg); \ + } \ + } \ + while (0) + +JNIEXPORT jobject +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeTakeGoalRequest( + JNIEnv * env, jclass, jlong action_server_handle, jlong jrequest_from_java_converter_handle, + jlong jrequest_to_java_converter_handle, jlong jrequest_destructor_handle, jobject jrequest_msg) +{ + RCLJAVA_ACTION_SERVER_TAKE_REQUEST(goal); +} + +JNIEXPORT jobject +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeTakeCancelRequest( + JNIEnv * env, jclass, jlong action_server_handle, jlong jrequest_from_java_converter_handle, + jlong jrequest_to_java_converter_handle, jlong jrequest_destructor_handle, jobject jrequest_msg) +{ + RCLJAVA_ACTION_SERVER_TAKE_REQUEST(cancel); +} + +JNIEXPORT jobject +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeTakeResultRequest( + JNIEnv * env, jclass, jlong action_server_handle, jlong jrequest_from_java_converter_handle, + jlong jrequest_to_java_converter_handle, jlong jrequest_destructor_handle, jobject jrequest_msg) +{ + RCLJAVA_ACTION_SERVER_TAKE_REQUEST(result); +} + +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeSendGoalResponse( + JNIEnv * env, jclass, jlong action_server_handle, jobject jrequest_id, + jlong jresponse_from_java_converter_handle, jlong jresponse_to_java_converter_handle, + jlong jresponse_destructor_handle, jobject jresponse_msg) +{ + RCLJAVA_ACTION_SERVER_SEND_RESPONSE(goal); +} + +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeSendCancelResponse( + JNIEnv * env, jclass, jlong action_server_handle, jobject jrequest_id, + jlong jresponse_from_java_converter_handle, jlong jresponse_to_java_converter_handle, + jlong jresponse_destructor_handle, jobject jresponse_msg) +{ + RCLJAVA_ACTION_SERVER_SEND_RESPONSE(cancel); +} + +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeSendResultResponse( + JNIEnv * env, jclass, jlong action_server_handle, jobject jrequest_id, + jlong jresponse_from_java_converter_handle, jlong jresponse_to_java_converter_handle, + jlong jresponse_destructor_handle, jobject jresponse_msg) +{ + RCLJAVA_ACTION_SERVER_SEND_RESPONSE(result); +} + +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeProcessCancelRequest( + JNIEnv * env, jclass, + jlong action_server_handle, + jlong jrequest_from_java_converter_handle, + jlong jrequest_destructor_handle, + jlong jresponse_to_java_converter_handle, + jobject jrequest_msg, + jobject jresponse_msg) +{ + assert(jrequest_from_java_converter_handle != 0); + assert(jrequest_destructor_handle != 0); + assert(jresponse_to_java_converter_handle != 0); + + rcl_action_server_t * action_server = reinterpret_cast( + action_server_handle); + convert_from_java_signature request_convert_from_java = + reinterpret_cast(jrequest_from_java_converter_handle); + convert_to_java_signature response_convert_to_java = reinterpret_cast( + jresponse_to_java_converter_handle); + destroy_ros_message_signature request_destroy_ros_message = + reinterpret_cast(jrequest_destructor_handle); + + rcl_action_cancel_request_t * request_msg = reinterpret_cast( + request_convert_from_java(jrequest_msg, nullptr)); + rcl_action_cancel_response_t response_msg = rcl_action_get_zero_initialized_cancel_response(); + + rcl_ret_t ret = rcl_action_process_cancel_request( + action_server, request_msg, &response_msg); + request_destroy_ros_message(request_msg); + if (ret != RCL_RET_OK) { + std::string msg = \ + "Failed to process cancel request: " + std::string(rcl_get_error_string().str); + rcl_reset_error(); + rcljava_throw_rclexception(env, ret, msg); + return; + } + + response_convert_to_java(&response_msg, jresponse_msg); +} + +JNIEXPORT jboolean +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeCheckGoalExists( + JNIEnv *, jclass, + jlong jaction_server, + jobject jgoal_info, + jlong jgoal_info_from_java_converter_handle, + jlong jgoal_info_destructor_handle) +{ + assert(0 != jgoal_info_from_java_converter_handle); + assert(0 != jgoal_info_destructor_handle); + + rcl_action_server_t * action_server = reinterpret_cast( + jaction_server); + convert_from_java_signature convert_from_java = + reinterpret_cast(jgoal_info_from_java_converter_handle); + destroy_ros_message_signature destroy_ros_message = + reinterpret_cast(jgoal_info_destructor_handle); + + rcl_action_goal_info_t * goal_info = + reinterpret_cast(convert_from_java(jgoal_info, nullptr)); + bool exists = rcl_action_server_goal_exists(action_server, goal_info); + destroy_ros_message(goal_info); + + return exists; +} diff --git a/rcljava/src/main/cpp/org_ros2_rcljava_action_ActionServerImpl_GoalHandleImpl.cpp b/rcljava/src/main/cpp/org_ros2_rcljava_action_ActionServerImpl_GoalHandleImpl.cpp new file mode 100644 index 00000000..de1fb4b6 --- /dev/null +++ b/rcljava/src/main/cpp/org_ros2_rcljava_action_ActionServerImpl_GoalHandleImpl.cpp @@ -0,0 +1,149 @@ +// Copyright 2020 ros2-java contributors +// +// 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. + +#include + +#include +#include +#include + +#include "rcl/error_handling.h" +#include "rcl_action/rcl_action.h" + +#include "rcljava_common/exceptions.hpp" +#include "rcljava_common/signatures.hpp" + +#include "org_ros2_rcljava_action_ActionServerImpl_GoalHandleImpl.h" + +using rcljava_common::exceptions::rcljava_throw_rclexception; +using rcljava_common::signatures::convert_from_java_signature; +using rcljava_common::signatures::destroy_ros_message_signature; + +JNIEXPORT jlong +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_00024GoalHandleImpl_nativeAcceptNewGoal( + JNIEnv * env, jclass, jlong action_server_handle, + jlong jgoal_info_from_java_converter_handle, jlong jgoal_info_destructor_handle, + jobject jgoal_info_message) +{ + assert(jgoal_info_from_java_converter_handle != 0); + + rcl_action_server_t * action_server = reinterpret_cast( + action_server_handle); + convert_from_java_signature convert_from_java = + reinterpret_cast(jgoal_info_from_java_converter_handle); + destroy_ros_message_signature destroy_ros_message = + reinterpret_cast(jgoal_info_destructor_handle); + + rcl_action_goal_info_t * goal_info_message = + reinterpret_cast(convert_from_java(jgoal_info_message, nullptr)); + + rcl_action_goal_handle_t * goal_handle = rcl_action_accept_new_goal( + action_server, goal_info_message); + destroy_ros_message(goal_info_message); + if (!goal_handle) { + std::string msg = "Failed to accept new goal: " + std::string(rcl_get_error_string().str); + rcl_reset_error(); + // '1' is arbitrary since we don't have a specific return code + rcljava_throw_rclexception(env, 1, msg); + return 0; + } + + jlong jgoal_handle = reinterpret_cast(goal_handle); + return jgoal_handle; +} + +JNIEXPORT int +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_00024GoalHandleImpl_nativeGetStatus( + JNIEnv * env, jclass, jlong jgoal_handle) +{ + rcl_action_goal_handle_t * goal_handle = reinterpret_cast( + jgoal_handle); + rcl_action_goal_state_t status; + rcl_ret_t ret = rcl_action_goal_handle_get_status(goal_handle, &status); + if (RCL_RET_OK != ret) { + std::string msg = "Failed to get goal status: " + std::string(rcl_get_error_string().str); + rcl_reset_error(); + rcljava_throw_rclexception(env, ret, msg); + return static_cast(GOAL_STATE_UNKNOWN); + } + + return static_cast(status); +} + +static void update_goal_state(JNIEnv * env, jlong jgoal_handle, rcl_action_goal_event_t event) +{ + rcl_action_goal_handle_t * goal_handle = reinterpret_cast( + jgoal_handle); + rcl_ret_t ret = rcl_action_update_goal_state(goal_handle, event); + if (RCL_RET_OK != ret) { + std::string msg = "Failed to update goal state with event: " + + std::string(rcl_get_error_string().str); + rcl_reset_error(); + rcljava_throw_rclexception(env, ret, msg); + } +} + +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_00024GoalHandleImpl_nativeGoalEventExecute( + JNIEnv * env, jclass, jlong jgoal_handle) +{ + update_goal_state(env, jgoal_handle, GOAL_EVENT_EXECUTE); +} + +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_00024GoalHandleImpl_nativeGoalEventCancelGoal( + JNIEnv * env, jclass, jlong jgoal_handle) +{ + update_goal_state(env, jgoal_handle, GOAL_EVENT_CANCEL_GOAL); +} + +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_00024GoalHandleImpl_nativeGoalEventSucceed( + JNIEnv * env, jclass, jlong jgoal_handle) +{ + update_goal_state(env, jgoal_handle, GOAL_EVENT_SUCCEED); +} + +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_00024GoalHandleImpl_nativeGoalEventAbort( + JNIEnv * env, jclass, jlong jgoal_handle) +{ + update_goal_state(env, jgoal_handle, GOAL_EVENT_ABORT); +} + +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_00024GoalHandleImpl_nativeGoalEventCanceled( + JNIEnv * env, jclass, jlong jgoal_handle) +{ + update_goal_state(env, jgoal_handle, GOAL_EVENT_CANCELED); +} + +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_00024GoalHandleImpl_nativeDipose( + JNIEnv * env, jclass, jlong jgoal_handle) +{ + rcl_action_goal_handle_t * goal_handle = reinterpret_cast( + jgoal_handle); + if (!goal_handle) { + // Nothing to dispose + return; + } + + rcl_ret_t ret = rcl_action_goal_handle_fini(goal_handle); + if (RCL_RET_OK != ret) { + std::string msg = "Failed to finalize goal handle: " + std::string(rcl_get_error_string().str); + rcl_reset_error(); + rcljava_throw_rclexception(env, ret, msg); + } +} diff --git a/rcljava/src/main/cpp/org_ros2_rcljava_executors_BaseExecutor.cpp b/rcljava/src/main/cpp/org_ros2_rcljava_executors_BaseExecutor.cpp index 287a175f..2a784dab 100644 --- a/rcljava/src/main/cpp/org_ros2_rcljava_executors_BaseExecutor.cpp +++ b/rcljava/src/main/cpp/org_ros2_rcljava_executors_BaseExecutor.cpp @@ -22,6 +22,7 @@ #include "rcl/node.h" #include "rcl/rcl.h" #include "rcl/timer.h" +#include "rcl_action/rcl_action.h" #include "rmw/rmw.h" #include "rosidl_runtime_c/message_type_support_struct.h" @@ -30,69 +31,13 @@ #include "org_ros2_rcljava_executors_BaseExecutor.h" +#include "./convert.hpp" + using rcljava_common::exceptions::rcljava_throw_rclexception; using rcljava_common::signatures::convert_from_java_signature; using rcljava_common::signatures::convert_to_java_signature; using rcljava_common::signatures::destroy_ros_message_signature; -jobject -convert_rmw_request_id_to_java(JNIEnv * env, rmw_request_id_t * request_id) -{ - jclass jrequest_id_class = env->FindClass("org/ros2/rcljava/service/RMWRequestId"); - assert(jrequest_id_class != nullptr); - - jmethodID jconstructor = env->GetMethodID(jrequest_id_class, "", "()V"); - assert(jconstructor != nullptr); - - jobject jrequest_id = env->NewObject(jrequest_id_class, jconstructor); - - jfieldID jsequence_number_field_id = env->GetFieldID(jrequest_id_class, "sequenceNumber", "J"); - jfieldID jwriter_guid_field_id = env->GetFieldID(jrequest_id_class, "writerGUID", "[B"); - - assert(jsequence_number_field_id != nullptr); - assert(jwriter_guid_field_id != nullptr); - - int8_t * writer_guid = request_id->writer_guid; - int64_t sequence_number = request_id->sequence_number; - - env->SetLongField(jrequest_id, jsequence_number_field_id, sequence_number); - - jsize writer_guid_len = 16; // See rmw/rmw/include/rmw/types.h - - jbyteArray jwriter_guid = env->NewByteArray(writer_guid_len); - env->SetByteArrayRegion(jwriter_guid, 0, writer_guid_len, reinterpret_cast(writer_guid)); - env->SetObjectField(jrequest_id, jwriter_guid_field_id, jwriter_guid); - - return jrequest_id; -} - -rmw_request_id_t * -convert_rmw_request_id_from_java(JNIEnv * env, jobject jrequest_id) -{ - assert(jrequest_id != nullptr); - - jclass jrequest_id_class = env->GetObjectClass(jrequest_id); - assert(jrequest_id_class != nullptr); - - jfieldID jsequence_number_field_id = env->GetFieldID(jrequest_id_class, "sequenceNumber", "J"); - jfieldID jwriter_guid_field_id = env->GetFieldID(jrequest_id_class, "writerGUID", "[B"); - - assert(jsequence_number_field_id != nullptr); - assert(jwriter_guid_field_id != nullptr); - - rmw_request_id_t * request_id = static_cast(malloc(sizeof(rmw_request_id_t))); - - int8_t * writer_guid = request_id->writer_guid; - request_id->sequence_number = env->GetLongField(jrequest_id, jsequence_number_field_id); - - jsize writer_guid_len = 16; // See rmw/rmw/include/rmw/types.h - - jbyteArray jwriter_guid = (jbyteArray)env->GetObjectField(jrequest_id, jwriter_guid_field_id); - env->GetByteArrayRegion(jwriter_guid, 0, writer_guid_len, reinterpret_cast(writer_guid)); - - return request_id; -} - JNIEXPORT jlong JNICALL Java_org_ros2_rcljava_executors_BaseExecutor_nativeGetZeroInitializedWaitSet(JNIEnv *, jclass) { @@ -261,6 +206,23 @@ Java_org_ros2_rcljava_executors_BaseExecutor_nativeWaitSetAddClient( } } +JNIEXPORT void JNICALL +Java_org_ros2_rcljava_executors_BaseExecutor_nativeWaitSetAddActionServer( + JNIEnv * env, jclass, jlong wait_set_handle, jlong action_server_handle) +{ + rcl_wait_set_t * wait_set = reinterpret_cast(wait_set_handle); + rcl_action_server_t * action_server = reinterpret_cast( + action_server_handle); + + rcl_ret_t ret = rcl_action_wait_set_add_action_server(wait_set, action_server, NULL); + if (ret != RCL_RET_OK) { + std::string msg = + "Failed to add action server to wait set: " + std::string(rcl_get_error_string().str); + rcl_reset_error(); + rcljava_throw_rclexception(env, ret, msg); + } +} + JNIEXPORT void JNICALL Java_org_ros2_rcljava_executors_BaseExecutor_nativeWaitSetAddTimer( JNIEnv * env, jclass, jlong wait_set_handle, jlong timer_handle) @@ -335,7 +297,7 @@ Java_org_ros2_rcljava_executors_BaseExecutor_nativeTakeRequest( assert(jtaken_msg != nullptr); - jobject jheader = convert_rmw_request_id_to_java(env, &header); + jobject jheader = rcljava::convert_rmw_request_id_to_java(env, &header); return jheader; } @@ -363,7 +325,7 @@ Java_org_ros2_rcljava_executors_BaseExecutor_nativeSendServiceResponse( void * response_msg = convert_from_java(jresponse_msg, nullptr); - rmw_request_id_t * request_id = convert_rmw_request_id_from_java(env, jrequest_id); + rmw_request_id_t * request_id = rcljava::convert_rmw_request_id_from_java(env, jrequest_id); rcl_ret_t ret = rcl_send_response(service, request_id, response_msg); @@ -425,7 +387,7 @@ Java_org_ros2_rcljava_executors_BaseExecutor_nativeTakeResponse( assert(jtaken_msg != nullptr); - jobject jheader = convert_rmw_request_id_to_java(env, &header); + jobject jheader = rcljava::convert_rmw_request_id_to_java(env, &header); return jheader; } diff --git a/rcljava/src/main/java/org/ros2/rcljava/action/ActionServer.java b/rcljava/src/main/java/org/ros2/rcljava/action/ActionServer.java new file mode 100644 index 00000000..4ab12f2e --- /dev/null +++ b/rcljava/src/main/java/org/ros2/rcljava/action/ActionServer.java @@ -0,0 +1,66 @@ +/* Copyright 2020 ros2-java contributors + * + * 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. + */ + +package org.ros2.rcljava.action; + +import java.util.Collection; + +import org.ros2.rcljava.interfaces.Disposable; +import org.ros2.rcljava.interfaces.ActionDefinition; + +public interface ActionServer extends Disposable { + // TODO(jacobperron): Move most of these methods to a new "Waitable" interface + /** + * Get the number of underlying subscriptions that the action server uses. + * + * @return The number of subscriptions. + */ + int getNumberOfSubscriptions(); + + /** + * Get the number of underlying timers that the action server uses. + * + * @return The number of timers. + */ + int getNumberOfTimers(); + + /** + * Get the number of underlying clients that the action server uses. + * + * @return The number of clients. + */ + int getNumberOfClients(); + + /** + * Get the number of underlying services that the action server uses. + * + * @return The number of services. + */ + int getNumberOfServices(); + + /** + * Check if an entity of the action server is ready in the wait set. + * + * @param waitSetHandle Handle to the rcl wait set that this action server was added to. + * + * @return true if at least one entity is ready, false otherwise. + */ + boolean isReady(long waitSetHandle); + + /** + * Execute any entities that are ready in the underlying wait set. + */ + void execute(); +} diff --git a/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerGoalHandle.java b/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerGoalHandle.java new file mode 100644 index 00000000..aedf0dbe --- /dev/null +++ b/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerGoalHandle.java @@ -0,0 +1,63 @@ +/* Copyright 2020 ros2-java contributors + * + * 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. + */ + +package org.ros2.rcljava.action; + +import org.ros2.rcljava.interfaces.ActionDefinition; +import org.ros2.rcljava.interfaces.Disposable; +import org.ros2.rcljava.interfaces.MessageDefinition; + +public interface ActionServerGoalHandle extends Disposable { + /** + * Get the message containing the timestamp and ID for the goal. + */ + public action_msgs.msg.GoalInfo getGoalInfo(); + + /** + * Get the goal message. + */ + public MessageDefinition getGoal(); + + /** + * Get the goal status. + */ + public GoalStatus getGoalStatus(); + + /** + * Returns true if the goal is in the CANCELING state. + */ + public boolean isCanceling(); + + /** + * Transition the goal to the SUCCEEDED state. + * + * Pre-condition: the goal must be in the EXECUTING or CANCELING state. + */ + public void succeed(); + + /** + * Transition the goal the the CANCELED state. + * + * Pre-condition: the goal must be in the CANCELING state. + */ + public void canceled(); + + /** + * Transition the goal the the CANCELED state. + * + * Pre-condition: the goal must be in the EXCUTING or CANCELING state. + */ + public void abort(); +} diff --git a/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerImpl.java b/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerImpl.java new file mode 100644 index 00000000..8c883f25 --- /dev/null +++ b/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerImpl.java @@ -0,0 +1,547 @@ +/* Copyright 2020 ros2-java contributors + * + * 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. + */ + +package org.ros2.rcljava.action; + +import java.lang.ref.WeakReference; +import java.util.ArrayList; +import java.util.Collection; +import java.util.HashMap; +import java.util.List; +import java.util.Map; + +import org.ros2.rcljava.RCLJava; +import org.ros2.rcljava.common.JNIUtils; +import org.ros2.rcljava.consumers.Consumer; +import org.ros2.rcljava.interfaces.MessageDefinition; +import org.ros2.rcljava.interfaces.ActionDefinition; +import org.ros2.rcljava.interfaces.GoalRequestDefinition; +import org.ros2.rcljava.interfaces.GoalResponseDefinition; +import org.ros2.rcljava.node.Node; +import org.ros2.rcljava.service.RMWRequestId; +import org.ros2.rcljava.time.Clock; +import org.ros2.rcljava.Time; + +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + +public class ActionServerImpl implements ActionServer { + private static final Logger logger = LoggerFactory.getLogger(ActionServerImpl.class); + + static { + try { + JNIUtils.loadImplementation(ActionServerImpl.class); + JNIUtils.loadImplementation(ActionServerImpl.GoalHandleImpl.class); + } catch (UnsatisfiedLinkError ule) { + logger.error("Native code library failed to load.\n" + ule); + System.exit(1); + } + JNIUtils.loadImplementation(ActionServerImpl.class); + } + + class GoalHandleImpl implements ActionServerGoalHandle { + private long handle; + private ActionServer actionServer; + private action_msgs.msg.GoalInfo goalInfo; + private MessageDefinition goal; + + private native long nativeAcceptNewGoal( + long actionServerHandle, + long goalInfoFromJavaConverterHandle, + long goalInfoDestructorHandle, + MessageDefinition goalInfo); + private native int nativeGetStatus(long goalHandle); + private native void nativeGoalEventExecute(long goalHandle); + private native void nativeGoalEventCancelGoal(long goalHandle); + private native void nativeGoalEventSucceed(long goalHandle); + private native void nativeGoalEventAbort(long goalHandle); + private native void nativeGoalEventCanceled(long goalHandle); + private native void nativeDispose(long handle); + + public GoalHandleImpl( + ActionServer actionServer, action_msgs.msg.GoalInfo goalInfo, MessageDefinition goal) + { + this.actionServer = actionServer; + this.goalInfo = goalInfo; + this.goal = goal; + long goalInfoFromJavaConverterHandle = goalInfo.getFromJavaConverterInstance(); + long goalInfoDestructorHandle = goalInfo.getDestructorInstance(); + this.handle = nativeAcceptNewGoal( + actionServer.getHandle(), + goalInfoFromJavaConverterHandle, + goalInfoDestructorHandle, + goalInfo); + } + + /** + * {@inheritDoc} + */ + public action_msgs.msg.GoalInfo getGoalInfo() { + return this.goalInfo; + } + + /** + * {@inheritDoc} + */ + public MessageDefinition getGoal() { + return this.goal; + } + + /** + * {@inheritDoc} + */ + public synchronized GoalStatus getGoalStatus() { + int status = nativeGetStatus(this.handle); + return GoalStatus.fromMessageValue((byte)status); + } + + /** + * {@inheritDoc} + */ + public synchronized boolean isCanceling() { + return this.getGoalStatus() == GoalStatus.CANCELING; + } + + /** + * Transition the goal to the EXECUTING state. + */ + public synchronized void execute() { + // It's possible that there has been a request to cancel the goal prior to executing. + // In this case we want to avoid the illegal state transition to EXECUTING + // but still call the users execute callback to let them handle canceling the goal. + if (!this.isCanceling()) { + nativeGoalEventExecute(this.handle); + } + } + + /** + * Transition the goal to the CANCELING state. + */ + public synchronized void cancelGoal() { + nativeGoalEventCancelGoal(this.handle); + } + + /** + * {@inheritDoc} + */ + public synchronized void succeed() { + nativeGoalEventSucceed(this.handle); + } + + /** + * {@inheritDoc} + */ + public synchronized void canceled() { + nativeGoalEventCanceled(this.handle); + } + + /** + * {@inheritDoc} + */ + public synchronized void abort() { + nativeGoalEventAbort(this.handle); + } + + /** + * {@inheritDoc} + */ + public synchronized final void dispose() { + nativeDispose(this.handle); + this.handle = 0; + } + + /** + * {@inheritDoc} + */ + public final long getHandle() { + return this.handle; + } + } // class GoalHandleImpl + + private final WeakReference nodeReference; + private final Clock clock; + private final T actionTypeInstance; + private final String actionName; + private long handle; + private final GoalCallback goalCallback; + private final CancelCallback cancelCallback; + private final Consumer> acceptedCallback; + + private boolean[] readyEntities; + + private Map, GoalHandleImpl> goalHandles; + + private boolean isGoalRequestReady() { + return this.readyEntities[0]; + } + + private boolean isCancelRequestReady() { + return this.readyEntities[1]; + } + + private boolean isResultRequestReady() { + return this.readyEntities[2]; + } + + private boolean isGoalExpiredReady() { + return this.readyEntities[3]; + } + + private native long nativeCreateActionServer( + long nodeHandle, long clockHandle, Class cls, String actionName); + + /** + * Create an action server. + * + * @param nodeReference A reference to the node to use to create this action server. + * @param actionType The type of the action. + * @param actionName The name of the action. + * @param goalCallback Callback triggered when a new goal request is received. + * @param cancelCallback Callback triggered when a new cancel request is received. + * @param acceptedCallback Callback triggered when a new goal is accepted. + */ + public ActionServerImpl( + final WeakReference nodeReference, + final Class actionType, + final String actionName, + final GoalCallback> goalCallback, + final CancelCallback cancelCallback, + final Consumer> acceptedCallback) throws IllegalArgumentException { + this.nodeReference = nodeReference; + try { + this.actionTypeInstance = actionType.newInstance(); + } catch (Exception ex) { + throw new IllegalArgumentException("Failed to instantiate provided action type", ex); + } + this.actionName = actionName; + this.goalCallback = goalCallback; + this.cancelCallback = cancelCallback; + this.acceptedCallback = acceptedCallback; + + this.goalHandles = new HashMap, GoalHandleImpl>(); + + Node node = nodeReference.get(); + if (node == null) { + throw new IllegalArgumentException("Node reference is null"); + } + + this.clock = node.getClock(); + + this.handle = nativeCreateActionServer( + node.getHandle(), node.getClock().getHandle(), actionType, actionName); + // TODO(jacobperron): Introduce 'Waitable' interface for entities like timers, services, etc + // node.addWaitable(this); + } + + private static native int[] nativeGetNumberOfEntities(long handle); + + /** + * {@inheritDoc} + */ + public int getNumberOfSubscriptions() { + return nativeGetNumberOfEntities(this.handle)[0]; + } + + /** + * {@inheritDoc} + */ + public int getNumberOfTimers() { + return nativeGetNumberOfEntities(this.handle)[2]; + } + + /** + * {@inheritDoc} + */ + public int getNumberOfClients() { + return nativeGetNumberOfEntities(this.handle)[3]; + } + + /** + * {@inheritDoc} + */ + public int getNumberOfServices() { + return nativeGetNumberOfEntities(this.handle)[4]; + } + + private static native boolean[] nativeGetReadyEntities( + long actionServerHandle, long waitSetHandle); + + /** + * {@inheritDoc} + */ + public boolean isReady(long waitSetHandle) { + this.readyEntities = nativeGetReadyEntities(this.handle, waitSetHandle); + for (boolean isReady : this.readyEntities) { + if (isReady) { + return true; + } + } + return false; + } + + private ActionServerGoalHandle executeGoalRequest( + RMWRequestId rmwRequestId, + GoalRequestDefinition requestMessage, + GoalResponseDefinition responseMessage) + { + builtin_interfaces.msg.Time timeRequestHandled = this.clock.now().toMsg(); + responseMessage.setStamp(timeRequestHandled.getSec(), timeRequestHandled.getNanosec()); + + // Create and populate a GoalInfo message + List goalUuid = requestMessage.getGoalUuid(); + action_msgs.msg.GoalInfo goalInfo = new action_msgs.msg.GoalInfo(); + unique_identifier_msgs.msg.UUID uuidMessage= new unique_identifier_msgs.msg.UUID(); + uuidMessage.setUuid(goalUuid); + goalInfo.setGoalId(uuidMessage); + goalInfo.setStamp(timeRequestHandled); + + long goalInfoFromJavaConverterHandle = goalInfo.getFromJavaConverterInstance(); + long goalInfoDestructorHandle = goalInfo.getDestructorInstance(); + + // Check that the goal ID isn't already being used + boolean goalExists = nativeCheckGoalExists( + this.handle, goalInfo, goalInfoFromJavaConverterHandle, goalInfoDestructorHandle); + if (goalExists) { + logger.warn("Received goal request for goal already being tracked by action server. Goal ID: " + goalUuid); + responseMessage.accept(false); + return null; + } + + // Call user callback + GoalCallback.GoalResponse response = this.goalCallback.handleGoal(requestMessage); + + boolean accepted = GoalCallback.GoalResponse.ACCEPT == response; + responseMessage.accept(accepted); + + System.out.println("Goal request handled " + accepted); + if (!accepted) { + return null; + } + + // Create a goal handle and add it to the list of goals + GoalHandleImpl goalHandle = this.new GoalHandleImpl( + this, goalInfo, requestMessage.getGoal()); + this.goalHandles.put(requestMessage.getGoalUuid(), goalHandle); + return goalHandle; + } + + private action_msgs.srv.CancelGoal_Response executeCancelRequest( + action_msgs.srv.CancelGoal_Response inputMessage) + { + action_msgs.srv.CancelGoal_Response outputMessage = new action_msgs.srv.CancelGoal_Response(); + outputMessage.setReturnCode(inputMessage.getReturnCode()); + List goalsToCancel = new ArrayList(); + + // Process user callback for each goal in cancel request + for (action_msgs.msg.GoalInfo goalInfo : inputMessage.getGoalsCanceling()) { + List goalUuid = goalInfo.getGoalId().getUuidAsList(); + // It's possible a goal may not be tracked by the user + if (!this.goalHandles.containsKey(goalUuid)) { + logger.warn("Ignoring cancel request for untracked goal handle with ID '" + goalUuid + "'"); + continue; + } + GoalHandleImpl goalHandle = this.goalHandles.get(goalUuid); + CancelCallback.CancelResponse cancelResponse = this.cancelCallback.handleCancel(goalHandle); + + if (CancelCallback.CancelResponse.ACCEPT == cancelResponse) { + // Update goal state to CANCELING + goalHandle.cancelGoal(); + + // Add to returned response + goalsToCancel.add(goalInfo); + } + } + + outputMessage.setGoalsCanceling(goalsToCancel); + return outputMessage; + } + + private static native RMWRequestId nativeTakeGoalRequest( + long actionServerHandle, + long requestFromJavaConverterHandle, + long requestToJavaConverterHandle, + long requestDestructorHandle, + MessageDefinition requestMessage); + + private static native RMWRequestId nativeTakeCancelRequest( + long actionServerHandle, + long requestFromJavaConverterHandle, + long requestToJavaConverterHandle, + long requestDestructorHandle, + MessageDefinition requestMessage); + + private static native RMWRequestId nativeTakeResultRequest( + long actionServerHandle, + long requestFromJavaConverterHandle, + long requestToJavaConverterHandle, + long requestDestructorHandle, + MessageDefinition requestMessage); + + private static native void nativeSendGoalResponse( + long actionServerHandle, + RMWRequestId header, + long responseFromJavaConverterHandle, + long responseToJavaConverterHandle, + long responseDestructorHandle, + MessageDefinition responseMessage); + + private static native void nativeSendCancelResponse( + long actionServerHandle, + RMWRequestId header, + long responseFromJavaConverterHandle, + long responseToJavaConverterHandle, + long responseDestructorHandle, + MessageDefinition responseMessage); + + private static native void nativeSendResultResponse( + long actionServerHandle, + RMWRequestId header, + long responseFromJavaConverterHandle, + long responseToJavaConverterHandle, + long responseDestructorHandle, + MessageDefinition responseMessage); + + private static native void nativeProcessCancelRequest( + long actionServerHandle, + long requestFromJavaConverterHandle, + long requestDestructorHandle, + long responseToJavaConverterHandle, + MessageDefinition requestMessage, + MessageDefinition responseMessage); + + private static native boolean nativeCheckGoalExists( + long handle, + MessageDefinition goalInfo, + long goalInfoFromJavaConverterHandle, + long goalInfoDestructorHandle); + + /** + * {@inheritDoc} + */ + public void execute() { + if (this.isGoalRequestReady()) { + Class requestType = this.actionTypeInstance.getSendGoalRequestType(); + Class responseType = this.actionTypeInstance.getSendGoalResponseType(); + + GoalRequestDefinition requestMessage = null; + GoalResponseDefinition responseMessage = null; + + try { + requestMessage = requestType.newInstance(); + responseMessage = responseType.newInstance(); + } catch (InstantiationException ie) { + ie.printStackTrace(); + } catch (IllegalAccessException iae) { + iae.printStackTrace(); + } + + if (requestMessage != null && responseMessage != null) { + long requestFromJavaConverterHandle = requestMessage.getFromJavaConverterInstance(); + long requestToJavaConverterHandle = requestMessage.getToJavaConverterInstance(); + long requestDestructorHandle = requestMessage.getDestructorInstance(); + long responseFromJavaConverterHandle = responseMessage.getFromJavaConverterInstance(); + long responseToJavaConverterHandle = responseMessage.getToJavaConverterInstance(); + long responseDestructorHandle = responseMessage.getDestructorInstance(); + + RMWRequestId rmwRequestId = + nativeTakeGoalRequest( + this.handle, + requestFromJavaConverterHandle, requestToJavaConverterHandle, requestDestructorHandle, + requestMessage); + if (rmwRequestId != null) { + ActionServerGoalHandle goalHandle = this.executeGoalRequest( + rmwRequestId, requestMessage, responseMessage); + nativeSendGoalResponse( + this.handle, rmwRequestId, + responseFromJavaConverterHandle, responseToJavaConverterHandle, + responseDestructorHandle, responseMessage); + if (goalHandle != null) { + this.acceptedCallback.accept(goalHandle); + } + } + } + } + + if (this.isCancelRequestReady()) { + action_msgs.srv.CancelGoal_Request requestMessage = new action_msgs.srv.CancelGoal_Request(); + action_msgs.srv.CancelGoal_Response responseMessage = new action_msgs.srv.CancelGoal_Response(); + + long requestFromJavaConverterHandle = requestMessage.getFromJavaConverterInstance(); + long requestToJavaConverterHandle = requestMessage.getToJavaConverterInstance(); + long requestDestructorHandle = requestMessage.getDestructorInstance(); + long responseFromJavaConverterHandle = responseMessage.getFromJavaConverterInstance(); + long responseToJavaConverterHandle = responseMessage.getToJavaConverterInstance(); + long responseDestructorHandle = responseMessage.getDestructorInstance(); + + RMWRequestId rmwRequestId = + nativeTakeCancelRequest( + this.handle, + requestFromJavaConverterHandle, requestToJavaConverterHandle, requestDestructorHandle, + requestMessage); + if (rmwRequestId != null) { + nativeProcessCancelRequest( + this.handle, + requestFromJavaConverterHandle, + requestDestructorHandle, + responseToJavaConverterHandle, + requestMessage, + responseMessage); + responseMessage = executeCancelRequest(responseMessage); + nativeSendCancelResponse( + this.handle, rmwRequestId, + responseFromJavaConverterHandle, responseToJavaConverterHandle, + responseDestructorHandle, responseMessage); + } + } + + if (this.isResultRequestReady()) { + // executeResultRequest(rmwRequestId, requestMessage, responseMessage); + // TODO + } + + if (this.isGoalExpiredReady()) { + // cleanupExpiredGoals(); + // TODO + } + } + + /** + * Destroy the underlying rcl_action_server_t. + * + * @param nodeHandle A pointer to the underlying rcl_node_t handle that + * created this action server. + * @param handle A pointer to the underlying rcl_action_server_t + */ + private static native void nativeDispose(long nodeHandle, long handle); + + /** + * {@inheritDoc} + */ + public final void dispose() { + Node node = this.nodeReference.get(); + if (node != null) { + nativeDispose(node.getHandle(), this.handle); + node.removeActionServer(this); + this.handle = 0; + } + } + + /** + * {@inheritDoc} + */ + public final long getHandle() { + return handle; + } +} diff --git a/rcljava/src/main/java/org/ros2/rcljava/action/CancelCallback.java b/rcljava/src/main/java/org/ros2/rcljava/action/CancelCallback.java new file mode 100644 index 00000000..f16147e6 --- /dev/null +++ b/rcljava/src/main/java/org/ros2/rcljava/action/CancelCallback.java @@ -0,0 +1,33 @@ +/* Copyright 2020 ros2-java contributors + * + * 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. + */ + +package org.ros2.rcljava.action; + +import org.ros2.rcljava.interfaces.ActionDefinition; + +public interface CancelCallback { + enum CancelResponse { + REJECT, + ACCEPT, + }; + + /** + * Called when a new cancel request is received. + * + * @param goalHandle The goal handle. + * @return Cancel response indicating if the cancel request was accepted or not. + */ + CancelResponse handleCancel(ActionServerGoalHandle goalHandle); +} diff --git a/rcljava/src/main/java/org/ros2/rcljava/action/GoalCallback.java b/rcljava/src/main/java/org/ros2/rcljava/action/GoalCallback.java new file mode 100644 index 00000000..afd017b0 --- /dev/null +++ b/rcljava/src/main/java/org/ros2/rcljava/action/GoalCallback.java @@ -0,0 +1,33 @@ +/* Copyright 2020 ros2-java contributors + * + * 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. + */ + +package org.ros2.rcljava.action; + +import org.ros2.rcljava.interfaces.GoalRequestDefinition; + +public interface GoalCallback { + public enum GoalResponse { + REJECT, + ACCEPT, + }; + + /** + * Called when a new goal request is received. + * + * @param goal The action goal request. + * @return Goal response indicating if the goal was accepted or not. + */ + GoalResponse handleGoal(T goal); +} diff --git a/rcljava/src/main/java/org/ros2/rcljava/action/GoalStatus.java b/rcljava/src/main/java/org/ros2/rcljava/action/GoalStatus.java new file mode 100644 index 00000000..aee9e1c9 --- /dev/null +++ b/rcljava/src/main/java/org/ros2/rcljava/action/GoalStatus.java @@ -0,0 +1,47 @@ +/* Copyright 2020 ros2-java contributors + * + * 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. + */ + +package org.ros2.rcljava.action; + +public enum GoalStatus { + UNKNOWN, + ACCEPTED, + EXECUTING, + CANCELING, + SUCCEEDED, + CANCELED, + ABORTED; + + public static GoalStatus fromMessageValue(byte status) { + switch (status) { + case action_msgs.msg.GoalStatus.STATUS_ACCEPTED: + return GoalStatus.ACCEPTED; + case action_msgs.msg.GoalStatus.STATUS_EXECUTING: + return GoalStatus.EXECUTING; + case action_msgs.msg.GoalStatus.STATUS_CANCELING: + return GoalStatus.CANCELING; + case action_msgs.msg.GoalStatus.STATUS_SUCCEEDED: + return GoalStatus.SUCCEEDED; + case action_msgs.msg.GoalStatus.STATUS_CANCELED: + return GoalStatus.CANCELED; + case action_msgs.msg.GoalStatus.STATUS_ABORTED: + return GoalStatus.ABORTED; + case action_msgs.msg.GoalStatus.STATUS_UNKNOWN: + default: + return GoalStatus.UNKNOWN; + } + } +} + diff --git a/rcljava/src/main/java/org/ros2/rcljava/executors/AnyExecutable.java b/rcljava/src/main/java/org/ros2/rcljava/executors/AnyExecutable.java index dd3d3039..701d551b 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/executors/AnyExecutable.java +++ b/rcljava/src/main/java/org/ros2/rcljava/executors/AnyExecutable.java @@ -15,6 +15,7 @@ package org.ros2.rcljava.executors; +import org.ros2.rcljava.action.ActionServer; import org.ros2.rcljava.client.Client; import org.ros2.rcljava.events.EventHandler; import org.ros2.rcljava.subscription.Subscription; @@ -27,4 +28,5 @@ public class AnyExecutable { public Service service; public Client client; public EventHandler eventHandler; + public ActionServer actionServer; } diff --git a/rcljava/src/main/java/org/ros2/rcljava/executors/BaseExecutor.java b/rcljava/src/main/java/org/ros2/rcljava/executors/BaseExecutor.java index 67c15fda..9ad11e52 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/executors/BaseExecutor.java +++ b/rcljava/src/main/java/org/ros2/rcljava/executors/BaseExecutor.java @@ -30,11 +30,13 @@ import org.slf4j.LoggerFactory; import org.ros2.rcljava.RCLJava; +import org.ros2.rcljava.action.ActionServer; import org.ros2.rcljava.client.Client; import org.ros2.rcljava.common.JNIUtils; import org.ros2.rcljava.events.EventHandler; import org.ros2.rcljava.executors.AnyExecutable; import org.ros2.rcljava.executors.Executor; +import org.ros2.rcljava.interfaces.ActionDefinition; import org.ros2.rcljava.interfaces.MessageDefinition; import org.ros2.rcljava.interfaces.ServiceDefinition; import org.ros2.rcljava.node.ComposableNode; @@ -69,6 +71,8 @@ public class BaseExecutor { private List> eventHandles = new ArrayList>(); + private List> actionServerHandles = new ArrayList>(); + protected void addNode(ComposableNode node) { this.nodes.add(node); } @@ -168,6 +172,11 @@ protected void executeAnyExecutable(AnyExecutable anyExecutable) { anyExecutable.eventHandler.executeCallback(); eventHandles.remove(anyExecutable.eventHandler.getHandle()); } + + if (anyExecutable.actionServer != null) { + anyExecutable.actionServer.execute(); + this.actionServerHandles.remove(anyExecutable.actionServer.getHandle()); + } } protected void waitForWork(long timeout) { @@ -176,6 +185,7 @@ protected void waitForWork(long timeout) { this.serviceHandles.clear(); this.clientHandles.clear(); this.eventHandles.clear(); + this.actionServerHandles.clear(); for (ComposableNode node : this.nodes) { for (Subscription subscription : node.getNode().getSubscriptions()) { @@ -209,6 +219,11 @@ protected void waitForWork(long timeout) { this.clientHandles.add( new AbstractMap.SimpleEntry(client.getHandle(), client)); } + + for (ActionServer actionServer : node.getNode().getActionServers()) { + this.actionServerHandles.add( + new AbstractMap.SimpleEntry(actionServer.getHandle(), actionServer)); + } } int subscriptionsSize = 0; @@ -222,6 +237,13 @@ protected void waitForWork(long timeout) { timersSize += node.getNode().getTimers().size(); clientsSize += node.getNode().getClients().size(); servicesSize += node.getNode().getServices().size(); + + for (ActionServer actionServer : node.getNode().getActionServers()) { + subscriptionsSize += actionServer.getNumberOfSubscriptions(); + timersSize += actionServer.getNumberOfTimers(); + clientsSize += actionServer.getNumberOfClients(); + servicesSize += actionServer.getNumberOfServices(); + } } if (subscriptionsSize == 0 && timersSize == 0 && clientsSize == 0 && servicesSize == 0) { @@ -256,6 +278,10 @@ protected void waitForWork(long timeout) { nativeWaitSetAddEvent(waitSetHandle, entry.getKey()); } + for (Map.Entry entry : this.actionServerHandles) { + nativeWaitSetAddActionServer(waitSetHandle, entry.getKey()); + } + nativeWait(waitSetHandle, timeout); for (int i = 0; i < this.subscriptionHandles.size(); ++i) { @@ -288,6 +314,12 @@ protected void waitForWork(long timeout) { } } + for (Map.Entry entry : this.actionServerHandles) { + if (!entry.getValue().isReady(waitSetHandle)) { + entry.setValue(null); + } + } + Iterator> subscriptionIterator = this.subscriptionHandles.iterator(); while (subscriptionIterator.hasNext()) { @@ -329,6 +361,14 @@ protected void waitForWork(long timeout) { } } + Iterator> actionServerIterator = this.actionServerHandles.iterator(); + while (actionServerIterator.hasNext()) { + Map.Entry entry = actionServerIterator.next(); + if (entry.getValue() == null) { + actionServerIterator.remove(); + } + } + nativeDisposeWaitSet(waitSetHandle); } @@ -378,6 +418,14 @@ protected AnyExecutable getNextExecutable() { } } + for (Map.Entry entry : this.actionServerHandles) { + if (entry.getValue() != null) { + anyExecutable.actionServer = entry.getValue(); + entry.setValue(null); + return anyExecutable; + } + } + return null; } @@ -461,6 +509,8 @@ private static native MessageDefinition nativeTake( private static native void nativeWaitSetAddEvent(long waitSetHandle, long eventHandle); + private static native void nativeWaitSetAddActionServer(long waitSetHandle, long actionServerHandle); + private static native RMWRequestId nativeTakeRequest(long serviceHandle, long requestFromJavaConverterHandle, long requestToJavaConverterHandle, long requestDestructorHandle, MessageDefinition requestMessage); @@ -482,4 +532,6 @@ private static native RMWRequestId nativeTakeResponse(long clientHandle, private static native boolean nativeWaitSetServiceIsReady(long waitSetHandle, long index); private static native boolean nativeWaitSetClientIsReady(long waitSetHandle, long index); + + private static native boolean nativeWaitSetActionServerIsReady(long waitSetHandle, long actionServerHandle); } diff --git a/rcljava/src/main/java/org/ros2/rcljava/node/Node.java b/rcljava/src/main/java/org/ros2/rcljava/node/Node.java index b6750e39..63807a81 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/node/Node.java +++ b/rcljava/src/main/java/org/ros2/rcljava/node/Node.java @@ -20,6 +20,10 @@ import java.util.Map; import java.util.concurrent.TimeUnit; +import org.ros2.rcljava.action.ActionServer; +import org.ros2.rcljava.action.ActionServerGoalHandle; +import org.ros2.rcljava.action.CancelCallback; +import org.ros2.rcljava.action.GoalCallback; import org.ros2.rcljava.client.Client; import org.ros2.rcljava.concurrent.Callback; import org.ros2.rcljava.consumers.Consumer; @@ -28,6 +32,8 @@ import org.ros2.rcljava.graph.NameAndTypes; import org.ros2.rcljava.graph.NodeNameInfo; import org.ros2.rcljava.interfaces.Disposable; +import org.ros2.rcljava.interfaces.ActionDefinition; +import org.ros2.rcljava.interfaces.GoalRequestDefinition; import org.ros2.rcljava.interfaces.MessageDefinition; import org.ros2.rcljava.interfaces.ServiceDefinition; import org.ros2.rcljava.parameters.ParameterCallback; @@ -77,6 +83,11 @@ public interface Node extends Disposable { */ Collection getTimers(); + /** + * @return All the @{link ActionServer}s that were created by this instance. + */ + Collection getActionServers(); + /** * Create a Subscription<T>. * @@ -133,6 +144,24 @@ Client createClient( Client createClient(final Class serviceType, final String serviceName) throws NoSuchFieldException, IllegalAccessException; + /** + * Create an ActionServer<T>. + * + * @param The type of action that will be handled by the created @{link ActionServer}. + * @param actionName The name of action that the create @{link ActionServer} will offer. + * @param goalCallback The callback that will be called when the @{link ActionServer} + * receives a new goal request. + * @param cancelCallback The callback that will be called when the @{link ActionServer} + * receives a cancle request for an active goal. + * @param acceptedCallback The callback that will be called when the @{link ActionServer} + * accepts a goal request. + */ + ActionServer createActionServer(final Class actionType, + final String actionName, + final GoalCallback> goalCallback, + final CancelCallback cancelCallback, + final Consumer> acceptedCallback); + /** * Remove a Subscription created by this Node. * @@ -181,6 +210,18 @@ Client createClient(final Class serviceType, */ boolean removeClient(final Client client); + /** + * Remove an @{link ActionServer} created by this Node. + * + * Calling this method effectively invalidates the passed @{link ActionServer}. + * If the server was not created by this Node, then nothing happens. + * + * @param actionServer The object to remove from this node. + * @return true if the server was removed, false if the server was already + * removed or was never created by this Node. + */ + boolean removeActionServer(final ActionServer actionServer); + /** * Create a wall timer. * diff --git a/rcljava/src/main/java/org/ros2/rcljava/node/NodeImpl.java b/rcljava/src/main/java/org/ros2/rcljava/node/NodeImpl.java index 687138cb..7c6dd4b4 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/node/NodeImpl.java +++ b/rcljava/src/main/java/org/ros2/rcljava/node/NodeImpl.java @@ -16,6 +16,11 @@ package org.ros2.rcljava.node; import org.ros2.rcljava.RCLJava; +import org.ros2.rcljava.action.ActionServer; +import org.ros2.rcljava.action.ActionServerImpl; +import org.ros2.rcljava.action.ActionServerGoalHandle; +import org.ros2.rcljava.action.CancelCallback; +import org.ros2.rcljava.action.GoalCallback; import org.ros2.rcljava.client.Client; import org.ros2.rcljava.client.ClientImpl; import org.ros2.rcljava.common.JNIUtils; @@ -27,6 +32,8 @@ import org.ros2.rcljava.graph.EndpointInfo; import org.ros2.rcljava.graph.NameAndTypes; import org.ros2.rcljava.interfaces.Disposable; +import org.ros2.rcljava.interfaces.ActionDefinition; +import org.ros2.rcljava.interfaces.GoalRequestDefinition; import org.ros2.rcljava.interfaces.MessageDefinition; import org.ros2.rcljava.interfaces.ServiceDefinition; import org.ros2.rcljava.node.NodeOptions; @@ -135,6 +142,11 @@ public class NodeImpl implements Node { */ private final Collection timers; + /** + * All the @{link ActionServer}s that have been created through this instance. + */ + private final Collection actionServers; + private Object parametersMutex; class ParameterAndDescriptor { @@ -169,6 +181,7 @@ public NodeImpl(final long handle, final NodeOptions nodeOptions) { this.services = new LinkedBlockingQueue(); this.clients = new LinkedBlockingQueue(); this.timers = new LinkedBlockingQueue(); + this.actionServers = new LinkedBlockingQueue(); this.parametersMutex = new Object(); this.parameters = new ConcurrentHashMap(); this.allowUndeclaredParameters = nodeOptions.getAllowUndeclaredParameters(); @@ -388,6 +401,18 @@ public Client createClient(final Class servi private static native long nativeCreateClientHandle( long handle, Class cls, String serviceName, long qosProfileHandle); + public ActionServer createActionServer(final Class actionType, + final String actionName, + final GoalCallback> goalCallback, + final CancelCallback cancelCallback, + final Consumer> acceptedCallback) throws IllegalArgumentException { + ActionServer actionServer = new ActionServerImpl( + new WeakReference(this), actionType, actionName, + goalCallback, cancelCallback, acceptedCallback); + this.actionServers.add(actionServer); + return actionServer; + } + /** * {@inheritDoc} */ @@ -402,6 +427,13 @@ public boolean removeClient(final Client client) { return this.clients.remove(client); } + /** + * {@inheritDoc} + */ + public boolean removeActionServer(final ActionServer actionServer) { + return this.actionServers.remove(actionServer); + } + /** * {@inheritDoc} */ @@ -479,6 +511,13 @@ public final Collection getTimers() { return this.timers; } + /** + * {@inheritDoc} + */ + public final Collection getActionServers() { + return this.actionServers; + } + /** * {@inheritDoc} */ diff --git a/rcljava/src/test/java/org/ros2/rcljava/action/ActionServerTest.java b/rcljava/src/test/java/org/ros2/rcljava/action/ActionServerTest.java new file mode 100644 index 00000000..36f004b9 --- /dev/null +++ b/rcljava/src/test/java/org/ros2/rcljava/action/ActionServerTest.java @@ -0,0 +1,234 @@ +/* Copyright 2020 ros2-java contributors + * + * 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. + */ + +package org.ros2.rcljava.action; + +import java.time.Duration; +import java.util.concurrent.Future; +import java.util.concurrent.TimeUnit; +import java.util.Collection; +import java.util.List; + +import static org.junit.Assert.assertEquals; +import static org.junit.Assert.assertNotEquals; + +import org.junit.After; +import org.junit.AfterClass; +import org.junit.Before; +import org.junit.BeforeClass; +import org.junit.Test; + +import org.ros2.rcljava.RCLJava; +import org.ros2.rcljava.executors.SingleThreadedExecutor; +import org.ros2.rcljava.consumers.Consumer; +import org.ros2.rcljava.node.ComposableNode; +import org.ros2.rcljava.node.Node; + +public class ActionServerTest { + class MockGoalCallback implements GoalCallback { + public test_msgs.action.Fibonacci_Goal goal; + public GoalResponse handleGoal(test_msgs.action.Fibonacci.SendGoalRequest goal) { + this.goal = goal.getGoal(); + return GoalResponse.ACCEPT; + } + } + + class MockCancelCallback implements CancelCallback { + public ActionServerGoalHandle goalHandle; + public CancelResponse handleCancel(ActionServerGoalHandle goalHandle) { + this.goalHandle = goalHandle; + return CancelResponse.ACCEPT; + } + } + + class MockAcceptedCallback implements Consumer> { + public ActionServerGoalHandle goalHandle; + public void accept(final ActionServerGoalHandle goalHandle) { + this.goalHandle = goalHandle; + } + } + + private SingleThreadedExecutor executor; + private Node node; + private ComposableNode composableNode; + private ActionServer actionServer; + private MockGoalCallback goalCallback; + private MockCancelCallback cancelCallback; + private MockAcceptedCallback acceptedCallback; + private MockActionClient mockActionClient; + + @BeforeClass + public static void setupOnce() { + RCLJava.rclJavaInit(); + org.apache.log4j.BasicConfigurator.configure(); + } + + @AfterClass + public static void tearDownOnce() { + RCLJava.shutdown(); + } + + @Before + public void setUp() throws Exception { + // Create a node + node = RCLJava.createNode("test_action_server_node"); + + assertNotEquals(null, node); + + // Executor requires a ComposableNode type + composableNode = new ComposableNode() { + public Node getNode() { + return node; + } + }; + executor = new SingleThreadedExecutor(); + executor.addNode(composableNode); + + // Create action server callbacks + goalCallback = new MockGoalCallback(); + cancelCallback = new MockCancelCallback(); + acceptedCallback = new MockAcceptedCallback(); + + // Create an action server + actionServer = node.createActionServer( + test_msgs.action.Fibonacci.class, "test_action", + goalCallback, cancelCallback, acceptedCallback); + + // Create mock client + mockActionClient = new MockActionClient(node, "test_action"); + + // Wait for mock client to discover action sever + // TODO(jacobperron): also wait for action server to discover the client + assertEquals(true, mockActionClient.waitForActionServer(Duration.ofSeconds(5))); + } + + @After + public void tearDown() { + // We expect that calling dispose should result in a zero handle + // and the reference is dropped from the Node + actionServer.dispose(); + assertEquals(0, actionServer.getHandle()); + assertEquals(0, this.node.getActionServers().size()); + + mockActionClient.dispose(); + + executor.removeNode(composableNode); + + node.dispose(); + } + + public test_msgs.action.Fibonacci_SendGoal_Response sendGoal(int order) throws Exception { + test_msgs.action.Fibonacci_SendGoal_Request request = + new test_msgs.action.Fibonacci_SendGoal_Request(); + test_msgs.action.Fibonacci_Goal goal = new test_msgs.action.Fibonacci_Goal(); + goal.setOrder(order); + request.setGoal(goal); + + Future future = + this.mockActionClient.sendGoalClient.asyncSendRequest(request); + + test_msgs.action.Fibonacci_SendGoal_Response response = null; + long startTime = System.nanoTime(); + while (RCLJava.ok() && !future.isDone()) { + this.executor.spinOnce(1); + response = future.get(100, TimeUnit.MILLISECONDS); + + // Check for timeout + long duration = System.nanoTime() - startTime; + if (TimeUnit.NANOSECONDS.toSeconds(duration) >= 5) { + break; + } + } + return response; + } + + @Test + public final void testCreateAndDispose() { + assertNotEquals(0, this.actionServer.getHandle()); + assertEquals(1, this.node.getActionServers().size()); + + // Assert no callbacks triggered + assertEquals(null, this.goalCallback.goal); + assertEquals(null, this.cancelCallback.goalHandle); + assertEquals(null, this.acceptedCallback.goalHandle); + } + + @Test + public final void testAcceptGoal() throws Exception { + assertNotEquals(0, this.actionServer.getHandle()); + assertEquals(1, this.node.getActionServers().size()); + + // Send a goal + test_msgs.action.Fibonacci_SendGoal_Response response = sendGoal(42); + + assertNotEquals(null, response); + + // Assert goal callback and accepted callback triggered + assertNotEquals(null, this.goalCallback.goal); + assertNotEquals(null, this.acceptedCallback.goalHandle); + + assertEquals(42, this.goalCallback.goal.getOrder()); + test_msgs.action.Fibonacci_Goal acceptedGoal = (test_msgs.action.Fibonacci_Goal)this.acceptedCallback.goalHandle.getGoal(); + assertEquals(42, acceptedGoal.getOrder()); + + // Assert cancel callback not triggered + assertEquals(null, this.cancelCallback.goalHandle); + } + + @Test + public final void testCancelGoal() throws Exception { + assertNotEquals(0, this.actionServer.getHandle()); + assertEquals(1, this.node.getActionServers().size()); + + // Send a goal + test_msgs.action.Fibonacci_SendGoal_Response response = sendGoal(42); + + assertNotEquals(null, response); + + // Cancel the goal + action_msgs.srv.CancelGoal_Request cancelRequest = new action_msgs.srv.CancelGoal_Request(); + // A zero GoalInfo means cancel all goals + action_msgs.msg.GoalInfo goalInfo = new action_msgs.msg.GoalInfo(); + unique_identifier_msgs.msg.UUID zeroGoalId = new unique_identifier_msgs.msg.UUID(); + // TODO(jacobperron): code generator should zero initialize to 16 elements + zeroGoalId.setUuid(new byte[16]); + goalInfo.setGoalId(zeroGoalId); + cancelRequest.setGoalInfo(goalInfo); + Future cancelResponseFuture = + this.mockActionClient.cancelGoalClient.asyncSendRequest(cancelRequest); + + // Wait for cancel response + long startTime = System.nanoTime(); + while (RCLJava.ok() && !cancelResponseFuture.isDone()) { + this.executor.spinOnce(100000000); // timeout of 100 milliseconds + + // Check for timeout + long duration = System.nanoTime() - startTime; + if (TimeUnit.NANOSECONDS.toSeconds(duration) >= 5) { + break; + } + } + + assertEquals(true, cancelResponseFuture.isDone()); + action_msgs.srv.CancelGoal_Response cancelResponse = cancelResponseFuture.get(); + action_msgs.msg.GoalInfo[] goalsCanceling = cancelResponse.getGoalsCanceling(); + assertEquals(1, goalsCanceling.length); + + // Assert cancel callback was triggered + assertNotEquals(null, this.cancelCallback.goalHandle); + test_msgs.action.Fibonacci_Goal cancelingGoal = (test_msgs.action.Fibonacci_Goal)this.cancelCallback.goalHandle.getGoal(); + assertEquals(42, cancelingGoal.getOrder()); + } +} diff --git a/rcljava/src/test/java/org/ros2/rcljava/action/MockActionClient.java b/rcljava/src/test/java/org/ros2/rcljava/action/MockActionClient.java new file mode 100644 index 00000000..72f0e7e4 --- /dev/null +++ b/rcljava/src/test/java/org/ros2/rcljava/action/MockActionClient.java @@ -0,0 +1,104 @@ +/* Copyright 2020 ros2-java contributors + * + * 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. + */ + +package org.ros2.rcljava.action; + +import java.time.Duration; +import java.util.ArrayList; +import java.util.Collections; +import java.util.List; + +import org.ros2.rcljava.client.Client; +import org.ros2.rcljava.consumers.Consumer; +import org.ros2.rcljava.node.Node; +import org.ros2.rcljava.subscription.Subscription; + +public class MockActionClient { + class FeedbackCallback implements Consumer { + public List feedbackReceived; + + public FeedbackCallback() { + feedbackReceived = Collections.synchronizedList( + new ArrayList()); + } + + public void accept(final test_msgs.action.Fibonacci_Feedback feedback) { + this.feedbackReceived.add(feedback); + } + } + + class StatusCallback implements Consumer { + private action_msgs.msg.GoalStatusArray statusArray; + + public synchronized action_msgs.msg.GoalStatusArray getStatusArrayMessage() { + return this.statusArray; + } + + public synchronized void accept(final action_msgs.msg.GoalStatusArray statusArray) { + this.statusArray = statusArray; + } + } + + public Client sendGoalClient; + public Client getResultClient; + public Client cancelGoalClient; + public FeedbackCallback feedbackCallback; + public StatusCallback statusCallback; + public Subscription feedbackSubscription; + public Subscription statusSubscription; + + public MockActionClient(Node node, String actionName) throws IllegalAccessException, NoSuchFieldException { + // Create mock service clients that make up an action client + sendGoalClient = node.createClient( + test_msgs.action.Fibonacci_SendGoal.class, actionName + "/_action/send_goal"); + getResultClient = node.createClient( + test_msgs.action.Fibonacci_GetResult.class, actionName + "/_action/get_result"); + cancelGoalClient = node.createClient( + action_msgs.srv.CancelGoal.class, actionName + "/_action/cancel_goal"); + // Create mock subscriptions that make up an action client + feedbackCallback = new FeedbackCallback(); + statusCallback = new StatusCallback(); + feedbackSubscription = node.createSubscription( + test_msgs.action.Fibonacci_Feedback.class, + actionName + "/_action/feedback", + feedbackCallback); + statusSubscription = node.createSubscription( + action_msgs.msg.GoalStatusArray.class, + actionName + "/_action/status", + statusCallback); + } + + public void dispose() { + this.sendGoalClient.dispose(); + this.getResultClient.dispose(); + this.cancelGoalClient.dispose(); + this.feedbackSubscription.dispose(); + this.statusSubscription.dispose(); + } + + public boolean waitForActionServer(Duration timeout) { + if (!this.sendGoalClient.waitForService(timeout)) { + return false; + } + if (!this.getResultClient.waitForService(timeout)) { + return false; + } + if (!this.cancelGoalClient.waitForService(timeout)) { + return false; + } + //TODO(jacobperron): wait for feedback and status subscriptions to match publishers, when API is available + return true; + } +} diff --git a/rcljava_common/CMakeLists.txt b/rcljava_common/CMakeLists.txt index 3bc04fde..46715393 100644 --- a/rcljava_common/CMakeLists.txt +++ b/rcljava_common/CMakeLists.txt @@ -38,6 +38,8 @@ set(${PROJECT_NAME}_java_sources "src/main/java/org/ros2/rcljava/exceptions/RCLReturn.java" "src/main/java/org/ros2/rcljava/interfaces/ActionDefinition.java" "src/main/java/org/ros2/rcljava/interfaces/Disposable.java" + "src/main/java/org/ros2/rcljava/interfaces/GoalRequestDefinition.java" + "src/main/java/org/ros2/rcljava/interfaces/GoalResponseDefinition.java" "src/main/java/org/ros2/rcljava/interfaces/MessageDefinition.java" "src/main/java/org/ros2/rcljava/interfaces/ServiceDefinition.java" ) diff --git a/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/ActionDefinition.java b/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/ActionDefinition.java index 9681463b..e39d65d8 100644 --- a/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/ActionDefinition.java +++ b/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/ActionDefinition.java @@ -15,4 +15,9 @@ package org.ros2.rcljava.interfaces; -public interface ActionDefinition {} +public interface ActionDefinition { + Class getSendGoalRequestType(); + Class getSendGoalResponseType(); + Class getGetResultRequestType(); + Class getGetResultResponseType(); +} diff --git a/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/GoalRequestDefinition.java b/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/GoalRequestDefinition.java new file mode 100644 index 00000000..cf6dc177 --- /dev/null +++ b/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/GoalRequestDefinition.java @@ -0,0 +1,23 @@ +/* Copyright 2020 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. + */ + +package org.ros2.rcljava.interfaces; + +import java.util.List; + +public interface GoalRequestDefinition extends MessageDefinition { + MessageDefinition getGoal(); + List getGoalUuid(); +} diff --git a/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/GoalResponseDefinition.java b/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/GoalResponseDefinition.java new file mode 100644 index 00000000..c645cf8f --- /dev/null +++ b/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/GoalResponseDefinition.java @@ -0,0 +1,21 @@ +/* Copyright 2020 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. + */ + +package org.ros2.rcljava.interfaces; + +public interface GoalResponseDefinition extends MessageDefinition { + void accept(boolean accepted); + void setStamp(int sec, int nanosec); +} diff --git a/rosidl_generator_java/resource/action.java.em b/rosidl_generator_java/resource/action.java.em index 5e7ebbe9..cb6c1dd3 100644 --- a/rosidl_generator_java/resource/action.java.em +++ b/rosidl_generator_java/resource/action.java.em @@ -63,8 +63,12 @@ expand_template( template_basepath=template_basepath) action_imports = [ + 'java.util.List', 'org.ros2.rcljava.common.JNIUtils', 'org.ros2.rcljava.interfaces.ActionDefinition', + 'org.ros2.rcljava.interfaces.GoalRequestDefinition', + 'org.ros2.rcljava.interfaces.GoalResponseDefinition', + 'org.ros2.rcljava.interfaces.MessageDefinition', 'org.slf4j.Logger', 'org.slf4j.LoggerFactory', ] @@ -76,6 +80,42 @@ import @(action_import); public class @(type_name) implements ActionDefinition { + public static class SendGoalRequest extends @(type_name)_SendGoal_Request implements GoalRequestDefinition<@(type_name)> { + public List getGoalUuid() { + // Return List since it's hash is based on the values (not the object pointer) + return super.getGoalId().getUuidAsList(); + } + } + + public static class SendGoalResponse extends @(type_name)_SendGoal_Response implements GoalResponseDefinition<@(type_name)> { + public void accept(boolean accepted) { + super.setAccepted(accepted); + } + + public void setStamp(int sec, int nanosec) { + builtin_interfaces.msg.Time msg = new builtin_interfaces.msg.Time(); + msg.setSec(sec); + msg.setNanosec(nanosec); + super.setStamp(msg); + } + } + + public Class getSendGoalRequestType() { + return SendGoalRequest.class; + } + + public Class getSendGoalResponseType() { + return SendGoalResponse.class; + } + + public Class getGetResultRequestType() { + return @(type_name)_GetResult_Request.class; + } + + public Class getGetResultResponseType() { + return @(type_name)_GetResult_Response.class; + } + private static final Logger logger = LoggerFactory.getLogger(@(type_name).class); static { diff --git a/rosidl_generator_java/resource/msg.java.em b/rosidl_generator_java/resource/msg.java.em index c8912488..9dcb2a42 100644 --- a/rosidl_generator_java/resource/msg.java.em +++ b/rosidl_generator_java/resource/msg.java.em @@ -38,7 +38,7 @@ import @('.'.join(member.type.namespaced_name())); @[ end if]@ @[end for]@ -public final class @(type_name) implements MessageDefinition { +public class @(type_name) implements MessageDefinition { private static final Logger logger = LoggerFactory.getLogger(@(type_name).class);