Add action server with logic for accepting and canceling goals

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

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 <jni.h>
+/* 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__

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 <jni.h>
+/* 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__