diff --git a/rcljava/CMakeLists.txt b/rcljava/CMakeLists.txt index dbd3d649..0e7206bb 100644 --- a/rcljava/CMakeLists.txt +++ b/rcljava/CMakeLists.txt @@ -2,15 +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) @@ -56,23 +60,28 @@ 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" "src/main/cpp/org_ros2_rcljava_executors_BaseExecutor.cpp" "src/main/cpp/org_ros2_rcljava_events_EventHandlerImpl.cpp" + "src/main/cpp/org_ros2_rcljava_graph_EndpointInfo" "src/main/cpp/org_ros2_rcljava_publisher_statuses_LivelinessLost.cpp" "src/main/cpp/org_ros2_rcljava_publisher_statuses_OfferedDeadlineMissed.cpp" "src/main/cpp/org_ros2_rcljava_publisher_statuses_OfferedQosIncompatible.cpp" "src/main/cpp/org_ros2_rcljava_node_NodeImpl.cpp" "src/main/cpp/org_ros2_rcljava_publisher_PublisherImpl.cpp" + "src/main/cpp/org_ros2_rcljava_qos_QoSProfile.cpp" "src/main/cpp/org_ros2_rcljava_service_ServiceImpl.cpp" "src/main/cpp/org_ros2_rcljava_subscription_SubscriptionImpl.cpp" "src/main/cpp/org_ros2_rcljava_subscription_statuses_LivelinessChanged.cpp" + "src/main/cpp/org_ros2_rcljava_subscription_statuses_MessageLost.cpp" "src/main/cpp/org_ros2_rcljava_subscription_statuses_RequestedDeadlineMissed.cpp" "src/main/cpp/org_ros2_rcljava_subscription_statuses_RequestedQosIncompatible.cpp" "src/main/cpp/org_ros2_rcljava_time_Clock.cpp" - "src/main/cpp/org_ros2_rcljava_timer_WallTimerImpl.cpp" + "src/main/cpp/org_ros2_rcljava_timer_TimerImpl.cpp" ) foreach(_jni_source ${${PROJECT_NAME}_jni_sources}) @@ -101,10 +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} @@ -124,8 +137,15 @@ 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/client/ResponseFuture.java" "src/main/java/org/ros2/rcljava/concurrent/Callback.java" "src/main/java/org/ros2/rcljava/concurrent/RCLFuture.java" "src/main/java/org/ros2/rcljava/contexts/Context.java" @@ -139,14 +159,14 @@ set(${PROJECT_NAME}_sources "src/main/java/org/ros2/rcljava/events/EventStatus.java" "src/main/java/org/ros2/rcljava/events/PublisherEventStatus.java" "src/main/java/org/ros2/rcljava/events/SubscriptionEventStatus.java" - "src/main/java/org/ros2/rcljava/publisher/statuses/LivelinessLost.java" - "src/main/java/org/ros2/rcljava/publisher/statuses/OfferedDeadlineMissed.java" - "src/main/java/org/ros2/rcljava/publisher/statuses/OfferedQosIncompatible.java" + "src/main/java/org/ros2/rcljava/graph/NameAndTypes.java" + "src/main/java/org/ros2/rcljava/graph/NodeNameInfo.java" "src/main/java/org/ros2/rcljava/executors/AnyExecutable.java" "src/main/java/org/ros2/rcljava/executors/BaseExecutor.java" "src/main/java/org/ros2/rcljava/executors/Executor.java" "src/main/java/org/ros2/rcljava/executors/MultiThreadedExecutor.java" "src/main/java/org/ros2/rcljava/executors/SingleThreadedExecutor.java" + "src/main/java/org/ros2/rcljava/graph/EndpointInfo.java" "src/main/java/org/ros2/rcljava/node/BaseComposableNode.java" "src/main/java/org/ros2/rcljava/node/ComposableNode.java" "src/main/java/org/ros2/rcljava/node/Node.java" @@ -168,6 +188,9 @@ set(${PROJECT_NAME}_sources "src/main/java/org/ros2/rcljava/parameters/service/ParameterServiceImpl.java" "src/main/java/org/ros2/rcljava/publisher/Publisher.java" "src/main/java/org/ros2/rcljava/publisher/PublisherImpl.java" + "src/main/java/org/ros2/rcljava/publisher/statuses/LivelinessLost.java" + "src/main/java/org/ros2/rcljava/publisher/statuses/OfferedDeadlineMissed.java" + "src/main/java/org/ros2/rcljava/publisher/statuses/OfferedQosIncompatible.java" "src/main/java/org/ros2/rcljava/qos/policies/Durability.java" "src/main/java/org/ros2/rcljava/qos/policies/History.java" "src/main/java/org/ros2/rcljava/qos/policies/Liveliness.java" @@ -180,11 +203,14 @@ set(${PROJECT_NAME}_sources "src/main/java/org/ros2/rcljava/subscription/Subscription.java" "src/main/java/org/ros2/rcljava/subscription/SubscriptionImpl.java" "src/main/java/org/ros2/rcljava/subscription/statuses/LivelinessChanged.java" + "src/main/java/org/ros2/rcljava/subscription/statuses/MessageLost.java" "src/main/java/org/ros2/rcljava/subscription/statuses/RequestedDeadlineMissed.java" "src/main/java/org/ros2/rcljava/subscription/statuses/RequestedQosIncompatible.java" "src/main/java/org/ros2/rcljava/time/Clock.java" "src/main/java/org/ros2/rcljava/time/ClockType.java" + "src/main/java/org/ros2/rcljava/time/TimeSource.java" "src/main/java/org/ros2/rcljava/timer/Timer.java" + "src/main/java/org/ros2/rcljava/timer/TimerImpl.java" "src/main/java/org/ros2/rcljava/timer/WallTimer.java" "src/main/java/org/ros2/rcljava/timer/WallTimerImpl.java" ) @@ -194,17 +220,32 @@ 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") ament_export_jars("share/${PROJECT_NAME}/java/${PROJECT_NAME}.jar") +add_source_jar("${PROJECT_NAME}-source_jar" + ${${PROJECT_NAME}_sources} + OUTPUT_NAME + ${PROJECT_NAME}-source +) + +install_jar("${PROJECT_NAME}-source_jar" "share/${PROJECT_NAME}/java") +ament_export_jars("share/${PROJECT_NAME}/java/${PROJECT_NAME}-source.jar") + + 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 @@ -232,8 +273,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 ) @@ -250,16 +294,20 @@ 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" "src/test/java/org/ros2/rcljava/node/NodeParametersTest.java" "src/test/java/org/ros2/rcljava/node/NodeUndeclaredParametersTest.java" "src/test/java/org/ros2/rcljava/node/NodeTest.java" - # "src/test/java/org/ros2/rcljava/parameters/AsyncParametersClientTest.java" - # "src/test/java/org/ros2/rcljava/parameters/SyncParametersClientTest.java" + "src/test/java/org/ros2/rcljava/parameters/AsyncParametersClientTest.java" + "src/test/java/org/ros2/rcljava/parameters/SyncParametersClientTest.java" "src/test/java/org/ros2/rcljava/publisher/PublisherTest.java" "src/test/java/org/ros2/rcljava/qos/QoSProfileTest.java" "src/test/java/org/ros2/rcljava/subscription/SubscriptionTest.java" + "src/test/java/org/ros2/rcljava/time/TimeSourceTest.java" "src/test/java/org/ros2/rcljava/timer/TimerTest.java" ) @@ -267,15 +315,19 @@ 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" "org.ros2.rcljava.node.NodeParametersTest" "org.ros2.rcljava.node.NodeUndeclaredParametersTest" "org.ros2.rcljava.node.NodeTest" - # "org.ros2.rcljava.parameters.SyncParametersClientTest" + "org.ros2.rcljava.parameters.AsyncParametersClientTest" + "org.ros2.rcljava.parameters.SyncParametersClientTest" "org.ros2.rcljava.publisher.PublisherTest" "org.ros2.rcljava.qos.QoSProfileTest" "org.ros2.rcljava.subscription.SubscriptionTest" + "org.ros2.rcljava.time.TimeSourceTest" "org.ros2.rcljava.timer.TimerTest" ) @@ -316,6 +368,42 @@ if(BUILD_TESTING) list_append_unique(_deps_library_dirs ${_dep_dir}) endforeach() + foreach(_dep_lib ${rosgraph_msgs_interfaces_LIBRARIES}) + get_filename_component(_dep_dir "${_dep_lib}" DIRECTORY) + list_append_unique(_deps_library_dirs ${_dep_dir}) + endforeach() + foreach(_dep_lib ${rosgraph_msgs_JNI_LIBRARIES}) + get_filename_component(_dep_dir "${_dep_lib}" DIRECTORY) + 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/) @@ -331,11 +419,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..ae42dd5e --- /dev/null +++ b/rcljava/include/org_ros2_rcljava_action_ActionServerImpl.h @@ -0,0 +1,173 @@ +// 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, 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, 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, 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); + +/* + * Class: org_ros2_rcljava_action_ActionServerImpl + * Method: nativePublishStatus + * Signature: (J) + */ +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativePublishStatus( + JNIEnv * env, jclass, jlong); + +/* + * Class: org_ros2_rcljava_action_ActionServerImpl + * Method: nativePublishFeedbackMessage + * Signature: (JLorg/ros2/rcljava/interfaces/FeedbackMessageDefinition;JJ) + */ +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativePublishFeedbackMessage( + JNIEnv * env, jclass, jlong, jobject, jlong, jlong); + +/* + * Class: org_ros2_rcljava_action_ActionServerImpl + * Method: nativeNotifyGoalDone + * Signature: (J) + */ +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeNotifyGoalDone( + JNIEnv * env, jclass, jlong); + +/* + * Class: org_ros2_rcljava_action_ActionServerImpl + * Method: nativeExpireGoals + * Signature: (JLaction_msgs/msg/GoalInfo;JLorg/ros2/rcljava/consumers/Consumer;) + */ +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeExpireGoals( + JNIEnv * env, jclass, jlong, jobject, jlong, jobject); + +#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..6f6f98ce --- /dev/null +++ b/rcljava/include/org_ros2_rcljava_action_ActionServerImpl_GoalHandleImpl.h @@ -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. + +#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: nativeUpdateGoalState + * Signature: (JJ) + */ +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_00024GoalHandleImpl_nativeUpdateGoalState( + JNIEnv * env, jclass, jlong jgoal_handle, jlong jevent); + +/* + * 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_client_ClientImpl.h b/rcljava/include/org_ros2_rcljava_client_ClientImpl.h index 9a3b20e5..56bc2082 100644 --- a/rcljava/include/org_ros2_rcljava_client_ClientImpl.h +++ b/rcljava/include/org_ros2_rcljava_client_ClientImpl.h @@ -27,7 +27,7 @@ extern "C" { */ JNIEXPORT jlong JNICALL Java_org_ros2_rcljava_client_ClientImpl_nativeSendClientRequest( - JNIEnv *, jclass, jlong, jlong, jlong, jlong, jobject); + JNIEnv *, jclass, jlong, jlong, jlong, jobject); /* * Class: org_ros2_rcljava_client_ClientImpl diff --git a/rcljava/include/org_ros2_rcljava_contexts_ContextImpl.h b/rcljava/include/org_ros2_rcljava_contexts_ContextImpl.h index 049891d6..b6cef1fb 100644 --- a/rcljava/include/org_ros2_rcljava_contexts_ContextImpl.h +++ b/rcljava/include/org_ros2_rcljava_contexts_ContextImpl.h @@ -27,7 +27,8 @@ extern "C" { * Signature: (J)V */ JNIEXPORT void -JNICALL Java_org_ros2_rcljava_contexts_ContextImpl_nativeInit(JNIEnv *, jclass, jlong); +JNICALL Java_org_ros2_rcljava_contexts_ContextImpl_nativeInit( + JNIEnv *, jclass, jlong, jobjectArray); /* * Class: org_ros2_rcljava_contexts_ContextImpl diff --git a/rcljava/include/org_ros2_rcljava_executors_BaseExecutor.h b/rcljava/include/org_ros2_rcljava_executors_BaseExecutor.h index 5522ac8d..08b95c08 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 @@ -114,7 +123,7 @@ JNICALL Java_org_ros2_rcljava_executors_BaseExecutor_nativeTakeRequest( */ JNIEXPORT void JNICALL Java_org_ros2_rcljava_executors_BaseExecutor_nativeSendServiceResponse( - JNIEnv *, jclass, jlong, jobject, jlong, jlong, jlong, jobject); + JNIEnv *, jclass, jlong, jobject, jlong, jlong, jobject); /* * Class: org_ros2_rcljava_executors_BaseExecutor diff --git a/rcljava/include/org_ros2_rcljava_graph_EndpointInfo.h b/rcljava/include/org_ros2_rcljava_graph_EndpointInfo.h new file mode 100644 index 00000000..e391d84a --- /dev/null +++ b/rcljava/include/org_ros2_rcljava_graph_EndpointInfo.h @@ -0,0 +1,34 @@ +// 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. + +#include +/* Header for class org_ros2_rcljava_graph_EndpointInfo */ + +#ifndef ORG_ROS2_RCLJAVA_GRAPH_ENDPOINTINFO_H_ +#define ORG_ROS2_RCLJAVA_GRAPH_ENDPOINTINFO_H_ +#ifdef __cplusplus +extern "C" { +#endif +/* + * Class: org_ros2_rcljava_graph_EndpointInfo + * Method: nativeFromRCL + * Signature: (J)V + */ +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_graph_EndpointInfo_nativeFromRCL(JNIEnv *, jobject, jlong); + +#ifdef __cplusplus +} +#endif +#endif // ORG_ROS2_RCLJAVA_GRAPH_ENDPOINTINFO_H_ diff --git a/rcljava/include/org_ros2_rcljava_node_NodeImpl.h b/rcljava/include/org_ros2_rcljava_node_NodeImpl.h index 40d40867..8a4bc582 100644 --- a/rcljava/include/org_ros2_rcljava_node_NodeImpl.h +++ b/rcljava/include/org_ros2_rcljava_node_NodeImpl.h @@ -92,6 +92,87 @@ JNIEXPORT jlong JNICALL Java_org_ros2_rcljava_node_NodeImpl_nativeCreateTimerHandle( JNIEnv *, jclass, jlong, jlong, jlong); +/* + * Class: org_ros2_rcljava_node_NodeImpl + * Method: nativeGetNodeNames + * Signature: (JLjava/util/List;)V + */ +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_node_NodeImpl_nativeGetNodeNames( + JNIEnv *, jclass, jlong, jobject); + +/* + * Class: org_ros2_rcljava_node_NodeImpl + * Method: nativeGetTopicNamesAndTypes + * Signature: (JLjava/util/Collection;)V + */ +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_node_NodeImpl_nativeGetTopicNamesAndTypes( + JNIEnv *, jclass, jlong, jobject); + +/* + * Class: org_ros2_rcljava_node_NodeImpl + * Method: nativeGetServiceNamesAndTypes + * Signature: (JLjava/util/Collection;)V + */ +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_node_NodeImpl_nativeGetServiceNamesAndTypes( + JNIEnv *, jclass, jlong, jobject); + +/* + * Class: org_ros2_rcljava_node_NodeImpl + * Method: nativeGetPublishersInfo + * Signature: (JLjava/lang/String;Ljava/util/List;)V + */ +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_node_NodeImpl_nativeGetPublishersInfo( + JNIEnv *, jclass, jlong, jstring, jobject); + +/* + * Class: org_ros2_rcljava_node_NodeImpl + * Method: nativeGetSubscriptionsInfo + * Signature: (JLjava/lang/String;Ljava/util/List;)V + */ +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_node_NodeImpl_nativeGetSubscriptionsInfo( + JNIEnv *, jclass, jlong, jstring, jobject); + +/* + * Class: org_ros2_rcljava_node_NodeImpl + * Method: nativeGetPublisherNamesAndTypesByNode + * Signature: (JLjava/lang/String;Ljava/lang/String;Ljava/util/Collection;)V + */ +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_node_NodeImpl_nativeGetPublisherNamesAndTypesByNode( + JNIEnv *, jclass, jlong, jstring, jstring, jobject); + +/* + * Class: org_ros2_rcljava_node_NodeImpl + * Method: nativeGetSubscriptionNamesAndTypesByNode + * Signature: (JLjava/lang/String;Ljava/lang/String;Ljava/util/Collection;)V + */ +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_node_NodeImpl_nativeGetSubscriptionNamesAndTypesByNode( + JNIEnv *, jclass, jlong, jstring, jstring, jobject); + +/* + * Class: org_ros2_rcljava_node_NodeImpl + * Method: nativeServiceNamesAndTypesByNode + * Signature: (JLjava/lang/String;Ljava/lang/String;Ljava/util/Collection;)V + */ +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_node_NodeImpl_nativeGetServiceNamesAndTypesByNode( + JNIEnv *, jclass, jlong, jstring, jstring, jobject); + +/* + * Class: org_ros2_rcljava_node_NodeImpl + * Method: nativeGetClientNamesAndTypesByNode + * Signature: (JLjava/lang/String;Ljava/lang/String;Ljava/util/Collection;)V + */ +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_node_NodeImpl_nativeGetClientNamesAndTypesByNode( + JNIEnv *, jclass, jlong, jstring, jstring, jobject); + #ifdef __cplusplus } #endif diff --git a/rcljava/include/org_ros2_rcljava_qos_QoSProfile.h b/rcljava/include/org_ros2_rcljava_qos_QoSProfile.h new file mode 100644 index 00000000..ada8004c --- /dev/null +++ b/rcljava/include/org_ros2_rcljava_qos_QoSProfile.h @@ -0,0 +1,41 @@ +// 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. + +#include +/* Header for class org_ros2_rcljava_qos_QoSProfile */ + +#ifndef ORG_ROS2_RCLJAVA_QOS_QOSPROFILE_H_ +#define ORG_ROS2_RCLJAVA_QOS_QOSPROFILE_H_ +#ifdef __cplusplus +extern "C" { +#endif +/* + * Class: org_ros2_rcljava_qos_QoSProfile + * Method: nativeFromRCL + * Signature: (J)V + */ +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_qos_QoSProfile_nativeFromRCL(JNIEnv *, jobject, jlong); + +/* + * Class: org_ros2_rcljava_qos_QoSProfile + * Method: nativeGetHandleFromName + * Signature: (Ljava/lang/String;)J + */ +JNIEXPORT jlong +JNICALL Java_org_ros2_rcljava_qos_QoSProfile_nativeGetHandleFromName(JNIEnv *, jclass, jstring); +#ifdef __cplusplus +} +#endif +#endif // ORG_ROS2_RCLJAVA_QOS_QOSPROFILE_H_ diff --git a/rcljava/include/org_ros2_rcljava_subscription_statuses_MessageLost.h b/rcljava/include/org_ros2_rcljava_subscription_statuses_MessageLost.h new file mode 100644 index 00000000..3139afa9 --- /dev/null +++ b/rcljava/include/org_ros2_rcljava_subscription_statuses_MessageLost.h @@ -0,0 +1,63 @@ +// 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. + +#include +/* Header for class org_ros2_rcljava_subscription_statuses_MessageLost */ + +#ifndef ORG_ROS2_RCLJAVA_SUBSCRIPTION_STATUSES_MESSAGELOST_H_ +#define ORG_ROS2_RCLJAVA_SUBSCRIPTION_STATUSES_MESSAGELOST_H_ +#ifdef __cplusplus +extern "C" { +#endif + +/* + * Class: org_ros2_rcljava_subscription_statuses_MessageLost + * Method: nativeAllocateRCLStatusEvent + * Signature: ()J + */ +JNIEXPORT jlong JNICALL +Java_org_ros2_rcljava_subscription_statuses_MessageLost_nativeAllocateRCLStatusEvent( + JNIEnv *, jclass); + +/* + * Class: org_ros2_rcljava_subscription_statuses_MessageLost + * Method: nativeDeallocateRCLStatusEvent + * Signature: (J)V + */ +JNIEXPORT void JNICALL +Java_org_ros2_rcljava_subscription_statuses_MessageLost_nativeDeallocateRCLStatusEvent( + JNIEnv *, jclass, jlong); + +/* + * Class: org_ros2_rcljava_subscription_statuses_MessageLost + * Method: nativeFromRCLEvent + * Signature: (J)V + */ +JNIEXPORT void JNICALL +Java_org_ros2_rcljava_subscription_statuses_MessageLost_nativeFromRCLEvent( + JNIEnv *, jobject, jlong); + +/* + * Class: org_ros2_rcljava_subscription_statuses_MessageLost + * Method: nativeGetsubscriptionEventType + * Signature: ()I + */ +JNIEXPORT jint JNICALL +Java_org_ros2_rcljava_subscription_statuses_MessageLost_nativeGetSubscriptionEventType( + JNIEnv *, jclass); + +#ifdef __cplusplus +} +#endif +#endif // ORG_ROS2_RCLJAVA_SUBSCRIPTION_STATUSES_MESSAGELOST_H_ diff --git a/rcljava/include/org_ros2_rcljava_time_Clock.h b/rcljava/include/org_ros2_rcljava_time_Clock.h index 430ca4b0..e903f2e1 100644 --- a/rcljava/include/org_ros2_rcljava_time_Clock.h +++ b/rcljava/include/org_ros2_rcljava_time_Clock.h @@ -28,6 +28,40 @@ extern "C" { JNIEXPORT jlong JNICALL Java_org_ros2_rcljava_time_Clock_nativeCreateClockHandle(JNIEnv *, jclass, jobject); +/* + * Class: org_ros2_rcljava_time_Clock + * Method: nativeGetNow + * Signature: (J)J + */ +JNIEXPORT jlong +JNICALL Java_org_ros2_rcljava_time_Clock_nativeGetNow(JNIEnv * env, jclass, jlong); + +/* + * Class: org_ros2_rcljava_time_Clock + * Method: nativeRosTimeOverrideEnabled + * Signature: (J)Z + */ +JNIEXPORT jboolean +JNICALL Java_org_ros2_rcljava_time_Clock_nativeRosTimeOverrideEnabled(JNIEnv * env, jclass, jlong); + +/* + * Class: org_ros2_rcljava_time_Clock + * Method: nativeSetRosTimeOverrideEnabled + * Signature: (JZ)V + */ +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_time_Clock_nativeSetRosTimeOverrideEnabled( + JNIEnv * env, jclass, jlong, jboolean); + +/* + * Class: org_ros2_rcljava_time_Clock + * Method: nativeSetRosTimeOverride + * Signature: (JJ)V + */ +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_time_Clock_nativeSetRosTimeOverride( + JNIEnv * env, jclass, jlong, jlong); + /* * Class: org_ros2_rcljava_time_Clock * Method: nativeDispose diff --git a/rcljava/include/org_ros2_rcljava_timer_TimerImpl.h b/rcljava/include/org_ros2_rcljava_timer_TimerImpl.h new file mode 100644 index 00000000..d02a7efa --- /dev/null +++ b/rcljava/include/org_ros2_rcljava_timer_TimerImpl.h @@ -0,0 +1,108 @@ +// 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 +/* Header for class org_ros2_rcljava_timer_TimerImpl */ + +#ifndef ORG_ROS2_RCLJAVA_TIMER_TIMERIMPL_H_ +#define ORG_ROS2_RCLJAVA_TIMER_TIMERIMPL_H_ +#ifdef __cplusplus +extern "C" { +#endif + +/* + * Class: org_ros2_rcljava_timer_TimerImpl + * Method: nativeDispose + * Signature: (J)V + */ +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_timer_TimerImpl_nativeDispose(JNIEnv *, jclass, jlong); + +/* + * Class: org_ros2_rcljava_timer_TimerImpl + * Method: nativeIsReady + * Signature: (J)Z + */ +JNIEXPORT jboolean +JNICALL Java_org_ros2_rcljava_timer_TimerImpl_nativeIsReady(JNIEnv *, jclass, jlong); + +/* + * Class: org_ros2_rcljava_timer_TimerImpl + * Method: nativeIsCanceled + * Signature: (J)Z + */ +JNIEXPORT jboolean +JNICALL Java_org_ros2_rcljava_timer_TimerImpl_nativeIsCanceled(JNIEnv *, jclass, jlong); + +/* + * Class: org_ros2_rcljava_timer_TimerImpl + * Method: nativeReset + * Signature: (J)Z + */ +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_timer_TimerImpl_nativeReset(JNIEnv *, jclass, jlong); + +/* + * Class: org_ros2_rcljava_timer_TimerImpl + * Method: nativeCancel + * Signature: (J)Z + */ +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_timer_TimerImpl_nativeCancel(JNIEnv *, jclass, jlong); + +/* + * Class: org_ros2_rcljava_timer_TimerImpl + * Method: nativeTimeUntilNextCall + * Signature: (J)J + */ +JNIEXPORT jlong +JNICALL Java_org_ros2_rcljava_timer_TimerImpl_nativeTimeUntilNextCall(JNIEnv *, jclass, jlong); + +/* + * Class: org_ros2_rcljava_timer_TimerImpl + * Method: nativeTimeSinceLastCall + * Signature: (J)J + */ +JNIEXPORT jlong +JNICALL Java_org_ros2_rcljava_timer_TimerImpl_nativeTimeSinceLastCall(JNIEnv *, jclass, jlong); + +/* + * Class: org_ros2_rcljava_timer_TimerImpl + * Method: nativeGetTimerPeriodNS + * Signature: (J)J + */ +JNIEXPORT jlong +JNICALL Java_org_ros2_rcljava_timer_TimerImpl_nativeGetTimerPeriodNS(JNIEnv *, jclass, jlong); + +/* + * Class: org_ros2_rcljava_timer_TimerImpl + * Method: nativeSetTimerPeriodNS + * Signature: (JJ)Z + */ +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_timer_TimerImpl_nativeSetTimerPeriodNS( + JNIEnv *, jclass, jlong, jlong); + +/* + * Class: org_ros2_rcljava_timer_TimerImpl + * Method: nativeCallTimer + * Signature: (J)Z + */ +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_timer_TimerImpl_nativeCallTimer(JNIEnv *, jclass, jlong); + +#ifdef __cplusplus +} +#endif +#endif // ORG_ROS2_RCLJAVA_TIMER_TIMERIMPL_H_ diff --git a/rcljava/include/org_ros2_rcljava_timer_WallTimerImpl.h b/rcljava/include/org_ros2_rcljava_timer_WallTimerImpl.h deleted file mode 100644 index 88c9daeb..00000000 --- a/rcljava/include/org_ros2_rcljava_timer_WallTimerImpl.h +++ /dev/null @@ -1,108 +0,0 @@ -// 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 -/* Header for class org_ros2_rcljava_timer_WallTimerImpl */ - -#ifndef ORG_ROS2_RCLJAVA_TIMER_WALLTIMERIMPL_H_ -#define ORG_ROS2_RCLJAVA_TIMER_WALLTIMERIMPL_H_ -#ifdef __cplusplus -extern "C" { -#endif - -/* - * Class: org_ros2_rcljava_timer_WallTimerImpl - * Method: nativeDispose - * Signature: (J)V - */ -JNIEXPORT void -JNICALL Java_org_ros2_rcljava_timer_WallTimerImpl_nativeDispose(JNIEnv *, jclass, jlong); - -/* - * Class: org_ros2_rcljava_timer_WallTimerImpl - * Method: nativeIsReady - * Signature: (J)Z - */ -JNIEXPORT jboolean -JNICALL Java_org_ros2_rcljava_timer_WallTimerImpl_nativeIsReady(JNIEnv *, jclass, jlong); - -/* - * Class: org_ros2_rcljava_timer_WallTimerImpl - * Method: nativeIsCanceled - * Signature: (J)Z - */ -JNIEXPORT jboolean -JNICALL Java_org_ros2_rcljava_timer_WallTimerImpl_nativeIsCanceled(JNIEnv *, jclass, jlong); - -/* - * Class: org_ros2_rcljava_timer_WallTimerImpl - * Method: nativeReset - * Signature: (J)Z - */ -JNIEXPORT void -JNICALL Java_org_ros2_rcljava_timer_WallTimerImpl_nativeReset(JNIEnv *, jclass, jlong); - -/* - * Class: org_ros2_rcljava_timer_WallTimerImpl - * Method: nativeCancel - * Signature: (J)Z - */ -JNIEXPORT void -JNICALL Java_org_ros2_rcljava_timer_WallTimerImpl_nativeCancel(JNIEnv *, jclass, jlong); - -/* - * Class: org_ros2_rcljava_timer_WallTimerImpl - * Method: nativeTimeUntilNextCall - * Signature: (J)J - */ -JNIEXPORT jlong -JNICALL Java_org_ros2_rcljava_timer_WallTimerImpl_nativeTimeUntilNextCall(JNIEnv *, jclass, jlong); - -/* - * Class: org_ros2_rcljava_timer_WallTimerImpl - * Method: nativeTimeSinceLastCall - * Signature: (J)J - */ -JNIEXPORT jlong -JNICALL Java_org_ros2_rcljava_timer_WallTimerImpl_nativeTimeSinceLastCall(JNIEnv *, jclass, jlong); - -/* - * Class: org_ros2_rcljava_timer_WallTimerImpl - * Method: nativeGetTimerPeriodNS - * Signature: (J)J - */ -JNIEXPORT jlong -JNICALL Java_org_ros2_rcljava_timer_WallTimerImpl_nativeGetTimerPeriodNS(JNIEnv *, jclass, jlong); - -/* - * Class: org_ros2_rcljava_timer_WallTimerImpl - * Method: nativeSetTimerPeriodNS - * Signature: (JJ)Z - */ -JNIEXPORT void -JNICALL Java_org_ros2_rcljava_timer_WallTimerImpl_nativeSetTimerPeriodNS( - JNIEnv *, jclass, jlong, jlong); - -/* - * Class: org_ros2_rcljava_timer_WallTimerImpl - * Method: nativeCallTimer - * Signature: (J)Z - */ -JNIEXPORT void -JNICALL Java_org_ros2_rcljava_timer_WallTimerImpl_nativeCallTimer(JNIEnv *, jclass, jlong); - -#ifdef __cplusplus -} -#endif -#endif // ORG_ROS2_RCLJAVA_TIMER_WALLTIMERIMPL_H_ diff --git a/rcljava/package.xml b/rcljava/package.xml index 340edf33..fa218c0c 100644 --- a/rcljava/package.xml +++ b/rcljava/package.xml @@ -12,31 +12,43 @@ 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 + rosgraph_msgs rosidl_runtime_c 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 builtin_interfaces + mockito_vendor rcl_interfaces rcljava_common rmw_implementation_cmake @@ -46,6 +58,7 @@ rosidl_generator_cpp 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..7ed6d699 --- /dev/null +++ b/rcljava/src/main/cpp/org_ros2_rcljava_action_ActionServerImpl.cpp @@ -0,0 +1,467 @@ +// 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 "rcpputils/scope_exit.hpp" +#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_exception; +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) { \ + convert_to_java(taken_msg, jrequest_msg); \ + destroy_ros_message(taken_msg); \ + 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_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_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_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_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; +} + +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativePublishStatus( + JNIEnv * env, jclass, jlong jaction_server) +{ + assert(0 != jaction_server); + auto * action_server = reinterpret_cast(jaction_server); + rcl_action_goal_status_array_t status_message = + rcl_action_get_zero_initialized_goal_status_array(); + rcl_ret_t ret = rcl_action_get_goal_status_array(action_server, &status_message); + if (ret != RCL_RET_OK) { + std::string msg = \ + "Failed to get goal status array: " + std::string(rcl_get_error_string().str); + rcl_reset_error(); + rcljava_throw_rclexception(env, ret, msg); + return; + } + + ret = rcl_action_publish_status(action_server, &status_message); + + if (ret != RCL_RET_OK) { + std::string msg = \ + "Failed to publish status array: " + std::string(rcl_get_error_string().str); + rcl_reset_error(); + rcljava_throw_rclexception(env, ret, msg); + return; + } +} + +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativePublishFeedbackMessage( + JNIEnv * env, jclass, jlong jaction_server, jobject jfeedback_msg, + jlong jfeedback_from_java, jlong jfeedback_destroy) +{ + assert(0 != jaction_server); + assert(nullptr != jfeedback_msg); + assert(0 != jfeedback_from_java); + assert(0 != jfeedback_destroy); + + auto * action_server = reinterpret_cast(jaction_server); + auto destroy_feedback = reinterpret_cast(jfeedback_destroy); + auto from_java_feedback = reinterpret_cast(jfeedback_from_java); + void * feedback = from_java_feedback(jfeedback_msg, nullptr); + auto destroy_feedback_scope_exit = rcpputils::make_scope_exit( + [feedback, destroy_feedback]() {destroy_feedback(feedback);}); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + if (!feedback) { + return; + } + rcl_ret_t ret = rcl_action_publish_feedback(action_server, feedback); + RCLJAVA_COMMON_THROW_FROM_RCL(env, ret, "Failed to publish feedback"); +} + +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeNotifyGoalDone( + JNIEnv * env, jclass, jlong jaction_server) +{ + assert(0 != jaction_server); + auto * action_server = reinterpret_cast(jaction_server); + rcl_ret_t ret = rcl_action_notify_goal_done(action_server); + RCLJAVA_COMMON_THROW_FROM_RCL(env, ret, "Failed to notify goal done"); +} + +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeExpireGoals( + JNIEnv * env, jclass, jlong jaction_server, jobject jgoal_info, + jlong jgoal_info_to_java, jobject jaccept) +{ + assert(0 != jaction_server); + auto * action_server = reinterpret_cast(jaction_server); + + rcl_action_goal_info_t expired_goal; + size_t num_expired = 1; + + // Loop in case more than 1 goal expired + jclass jaccept_class = env->GetObjectClass(jaccept); + if (!jaccept_class) { + rcljava_throw_exception( + env, "java/lang/IllegalStateException", + "nativeExpireGoals(): could not find jaccept class"); + return; + } + jmethodID jaccept_mid = env->GetMethodID( + jaccept_class, "accept", "(Laction_msgs/msg/GoalInfo;)V"); + if (!jaccept_mid) { + rcljava_throw_exception( + env, "java/lang/IllegalStateException", + "nativeExpireGoals(): could not find jaccept accept method"); + return; + } + while (num_expired > 0u) { + rcl_ret_t ret; + ret = rcl_action_expire_goals(action_server, &expired_goal, 1, &num_expired); + RCLJAVA_COMMON_THROW_FROM_RCL(env, ret, "Failed to expire goals"); + if (num_expired) { + auto goal_info_to_java = reinterpret_cast(jgoal_info_to_java); + goal_info_to_java(&expired_goal, jgoal_info); + env->CallVoidMethod(jaccept, jaccept_mid, jgoal_info); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + } + } +} 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..85185eaa --- /dev/null +++ b/rcljava/src/main/cpp/org_ros2_rcljava_action_ActionServerImpl_GoalHandleImpl.cpp @@ -0,0 +1,145 @@ +// 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_exception; +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); +} + +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_00024GoalHandleImpl_nativeUpdateGoalState( + JNIEnv * env, jclass, jlong jgoal_handle, jlong jto_status) +{ + assert(0 != jgoal_handle); + rcl_action_goal_event_t event = GOAL_EVENT_NUM_EVENTS; + switch (jto_status) { + case GOAL_STATE_EXECUTING: + event = GOAL_EVENT_EXECUTE; + break; + case GOAL_STATE_CANCELING: + event = GOAL_EVENT_CANCEL_GOAL; + break; + case GOAL_STATE_SUCCEEDED: + event = GOAL_EVENT_SUCCEED; + break; + case GOAL_STATE_CANCELED: + event = GOAL_EVENT_CANCELED; + break; + case GOAL_STATE_ABORTED: + event = GOAL_EVENT_ABORT; + break; + case GOAL_STATE_ACCEPTED: // fallthrough + case GOAL_STATE_UNKNOWN: // fallthrough + default: + break; + } + if (event >= GOAL_EVENT_NUM_EVENTS) { + rcljava_throw_exception( + env, "java/lang/IllegalStateException", "nativeUpdateGoalState(): Unknown event"); + return; + } + + auto * 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_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_client_ClientImpl.cpp b/rcljava/src/main/cpp/org_ros2_rcljava_client_ClientImpl.cpp index 842b7d99..b4fe8e09 100644 --- a/rcljava/src/main/cpp/org_ros2_rcljava_client_ClientImpl.cpp +++ b/rcljava/src/main/cpp/org_ros2_rcljava_client_ClientImpl.cpp @@ -36,13 +36,11 @@ using rcljava_common::signatures::destroy_ros_message_signature; JNIEXPORT jlong JNICALL Java_org_ros2_rcljava_client_ClientImpl_nativeSendClientRequest( - JNIEnv * env, jclass, jlong client_handle, - jlong jrequest_from_java_converter_handle, jlong jrequest_to_java_converter_handle, + JNIEnv * env, jclass, jlong client_handle, jlong jrequest_from_java_converter_handle, jlong jrequest_destructor_handle, jobject jrequest_msg) { assert(client_handle != 0); assert(jrequest_from_java_converter_handle != 0); - assert(jrequest_to_java_converter_handle != 0); assert(jrequest_destructor_handle != 0); assert(jrequest_msg != nullptr); diff --git a/rcljava/src/main/cpp/org_ros2_rcljava_contexts_ContextImpl.cpp b/rcljava/src/main/cpp/org_ros2_rcljava_contexts_ContextImpl.cpp index 8f70afad..4ae9d471 100644 --- a/rcljava/src/main/cpp/org_ros2_rcljava_contexts_ContextImpl.cpp +++ b/rcljava/src/main/cpp/org_ros2_rcljava_contexts_ContextImpl.cpp @@ -14,7 +14,9 @@ #include +#include #include +#include #include "rcl/context.h" #include "rcl/error_handling.h" @@ -25,6 +27,7 @@ #include "org_ros2_rcljava_contexts_ContextImpl.h" +using rcljava_common::exceptions::rcljava_throw_exception; using rcljava_common::exceptions::rcljava_throw_rclexception; JNIEXPORT jboolean JNICALL @@ -36,7 +39,8 @@ Java_org_ros2_rcljava_contexts_ContextImpl_nativeIsValid(JNIEnv *, jclass, jlong } JNIEXPORT void JNICALL -Java_org_ros2_rcljava_contexts_ContextImpl_nativeInit(JNIEnv * env, jclass, jlong context_handle) +Java_org_ros2_rcljava_contexts_ContextImpl_nativeInit( + JNIEnv * env, jclass, jlong context_handle, jobjectArray jargs) { // TODO(jacobperron): Encapsulate init options into a Java class rcl_init_options_t init_options = rcl_get_zero_initialized_init_options(); @@ -49,8 +53,42 @@ Java_org_ros2_rcljava_contexts_ContextImpl_nativeInit(JNIEnv * env, jclass, jlon } rcl_context_t * context = reinterpret_cast(context_handle); - // TODO(esteve): parse args - ret = rcl_init(0, nullptr, &init_options, context); + + // jsize should always fit in a size_t, so the following cast is safe. + const auto argc = static_cast(env->GetArrayLength(jargs)); + // rcl_init takes an int, check for overflow just in case + if (argc > static_cast(std::numeric_limits::max())) { + std::ostringstream oss( + "args length is longer than expected, maximum length is ", std::ios_base::ate); + oss << std::numeric_limits::max(); + rcljava_throw_exception( + env, "java/lang/IllegalArgumentException", oss.str().c_str()); + return; + } + const char ** argv = nullptr; + if (argc) { + argv = static_cast(malloc(argc * sizeof(char *))); + for (size_t i = 0; i < argc; ++i) { + auto item = static_cast(env->GetObjectArrayElement(jargs, i)); + // an exception here should never happen, + // as the only possible exception is array out of bounds + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + argv[i] = env->GetStringUTFChars(item, NULL); + } + } + + ret = rcl_init(static_cast(argc), argv, &init_options, context); + if (argc) { + for (size_t i = 0; i < argc; ++i) { + auto item = static_cast(env->GetObjectArrayElement(jargs, i)); + // an exception here should never happen, + // as the only possible exception is array out of bounds + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + env->ReleaseStringUTFChars(item, argv[i]); + argv[i] = env->GetStringUTFChars(item, NULL); + } + free(argv); + } if (RCL_RET_OK != ret) { std::string msg = "Failed to init context: " + std::string(rcl_get_error_string().str); rcl_ret_t ignored_ret = rcl_init_options_fini(&init_options); @@ -98,8 +136,15 @@ Java_org_ros2_rcljava_contexts_ContextImpl_nativeDispose(JNIEnv * env, jclass, j rcl_context_t * context = reinterpret_cast(context_handle); - rcl_ret_t ret = rcl_context_fini(context); + // TODO(ivanpauno): Currently, calling `rcl_context_fini` in a zero initialized context fails: + // That's incongruent with most other rcl objects. + // rcl issue: https://github.com/ros2/rcl/issues/814 + if (!context->impl) { + return; + } + + rcl_ret_t ret = rcl_context_fini(context); if (RCL_RET_OK != ret) { std::string msg = "Failed to destroy context: " + std::string(rcl_get_error_string().str); rcl_reset_error(); diff --git a/rcljava/src/main/cpp/org_ros2_rcljava_detail_QosIncompatibleStatus.cpp b/rcljava/src/main/cpp/org_ros2_rcljava_detail_QosIncompatibleStatus.cpp index 149b90c6..a09d7867 100644 --- a/rcljava/src/main/cpp/org_ros2_rcljava_detail_QosIncompatibleStatus.cpp +++ b/rcljava/src/main/cpp/org_ros2_rcljava_detail_QosIncompatibleStatus.cpp @@ -75,7 +75,7 @@ Java_org_ros2_rcljava_detail_QosIncompatibleStatus_nativeFromRCLEvent( return; } - jfieldID enum_value_fid; + jfieldID enum_value_fid = nullptr; switch (p->last_policy_kind) { case RMW_QOS_POLICY_INVALID: enum_value_fid = env->GetStaticFieldID(qos_kind_clazz, "INVALID", enum_class_path); 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..7cbdb6cc 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) { @@ -220,9 +165,7 @@ Java_org_ros2_rcljava_executors_BaseExecutor_nativeTake( reinterpret_cast(jto_java_converter); jobject jtaken_msg = convert_to_java(taken_msg, nullptr); - destroy_ros_message(taken_msg); - return jtaken_msg; } @@ -261,6 +204,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) @@ -329,13 +289,10 @@ Java_org_ros2_rcljava_executors_BaseExecutor_nativeTakeRequest( } if (ret != RCL_RET_SERVICE_TAKE_FAILED) { - jobject jtaken_msg = convert_to_java(taken_msg, jrequest_msg); - + convert_to_java(taken_msg, jrequest_msg); destroy_ros_message(taken_msg); - 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; } @@ -347,12 +304,11 @@ Java_org_ros2_rcljava_executors_BaseExecutor_nativeTakeRequest( JNIEXPORT void JNICALL Java_org_ros2_rcljava_executors_BaseExecutor_nativeSendServiceResponse( JNIEnv * env, jclass, jlong service_handle, jobject jrequest_id, - jlong jresponse_from_java_converter_handle, jlong jresponse_to_java_converter_handle, - jlong jresponse_destructor_handle, jobject jresponse_msg) + jlong jresponse_from_java_converter_handle, jlong jresponse_destructor_handle, + jobject jresponse_msg) { assert(service_handle != 0); assert(jresponse_from_java_converter_handle != 0); - assert(jresponse_to_java_converter_handle != 0); assert(jresponse_destructor_handle != 0); assert(jresponse_msg != nullptr); @@ -363,7 +319,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); @@ -419,13 +375,10 @@ Java_org_ros2_rcljava_executors_BaseExecutor_nativeTakeResponse( } if (ret != RCL_RET_CLIENT_TAKE_FAILED) { - jobject jtaken_msg = convert_to_java(taken_msg, jresponse_msg); - + convert_to_java(taken_msg, jresponse_msg); destroy_ros_message(taken_msg); - 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/cpp/org_ros2_rcljava_graph_EndpointInfo.cpp b/rcljava/src/main/cpp/org_ros2_rcljava_graph_EndpointInfo.cpp new file mode 100644 index 00000000..db0f48dc --- /dev/null +++ b/rcljava/src/main/cpp/org_ros2_rcljava_graph_EndpointInfo.cpp @@ -0,0 +1,109 @@ +// 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. + +#include "org_ros2_rcljava_graph_EndpointInfo.h" + +#include +#include + +#include "rcl/graph.h" +#include "rcljava_common/exceptions.hpp" + +using rcljava_common::exceptions::rcljava_throw_exception; + +JNIEXPORT void JNICALL +Java_org_ros2_rcljava_graph_EndpointInfo_nativeFromRCL(JNIEnv * env, jobject self, jlong handle) +{ + auto * p = reinterpret_cast(handle); + if (!p) { + rcljava_throw_exception( + env, "java/lang/IllegalArgumentException", "passed rcl endpoint info handle is NULL"); + return; + } + const char * endpoint_type_enum_path = + "Lorg/ros2/rcljava/graph/EndpointInfo$EndpointType;"; + // TODO(ivanpauno): class and field lookup could be done at startup time + jclass endpoint_type_clazz = env->FindClass("org/ros2/rcljava/graph/EndpointInfo$EndpointType"); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + jclass clazz = env->GetObjectClass(self); + jfieldID node_name_fid = env->GetFieldID(clazz, "nodeName", "Ljava/lang/String;"); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + jfieldID node_namespace_fid = env->GetFieldID(clazz, "nodeNamespace", "Ljava/lang/String;"); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + jfieldID topic_type_fid = env->GetFieldID(clazz, "topicType", "Ljava/lang/String;"); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + jfieldID endpoint_type_fid = env->GetFieldID(clazz, "endpointType", endpoint_type_enum_path); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + jfieldID endpoint_gid_fid = env->GetFieldID(clazz, "endpointGID", "[B"); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + jfieldID qos_fid = env->GetFieldID(clazz, "qos", "Lorg/ros2/rcljava/qos/QoSProfile;"); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + + jstring jnode_name = env->NewStringUTF(p->node_name); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + env->SetObjectField(self, node_name_fid, jnode_name); + jstring jnode_namespace = env->NewStringUTF(p->node_namespace); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + env->SetObjectField(self, node_namespace_fid, jnode_namespace); + jstring jtopic_type = env->NewStringUTF(p->topic_type); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + env->SetObjectField(self, topic_type_fid, jtopic_type); + jfieldID enum_value_fid = nullptr; + switch (p->endpoint_type) { + case RMW_ENDPOINT_INVALID: + enum_value_fid = env->GetStaticFieldID( + endpoint_type_clazz, "INVALID", endpoint_type_enum_path); + break; + case RMW_ENDPOINT_PUBLISHER: + enum_value_fid = env->GetStaticFieldID( + endpoint_type_clazz, "PUBLISHER", endpoint_type_enum_path); + break; + case RMW_ENDPOINT_SUBSCRIPTION: + enum_value_fid = env->GetStaticFieldID( + endpoint_type_clazz, "SUBSCRIPTION", endpoint_type_enum_path); + break; + default: + rcljava_throw_exception( + env, "java/lang/IllegalArgumentException", "unknown endpoint type"); + break; + } + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + jobject enum_value = env->GetStaticObjectField(endpoint_type_clazz, enum_value_fid); + env->SetObjectField(self, endpoint_type_fid, enum_value); + jbyteArray jgid = env->NewByteArray(RMW_GID_STORAGE_SIZE); + if (jgid == NULL) { + rcljava_throw_exception( + env, "java/lang/OutOfMemoryError", "cannot allocate java gid byte array"); + return; + } + jbyte * gid_content = env->GetByteArrayElements(jgid, nullptr); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + for (size_t i = 0; i < RMW_GID_STORAGE_SIZE; ++i) { + gid_content[i] = p->endpoint_gid[i]; + } + env->ReleaseByteArrayElements(jgid, gid_content, 0); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + env->SetObjectField(self, endpoint_gid_fid, jgid); + jclass qos_clazz = env->FindClass("org/ros2/rcljava/qos/QoSProfile"); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + jmethodID qos_init_mid = env->GetMethodID(qos_clazz, "", "()V"); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + jobject jqos = env->NewObject(qos_clazz, qos_init_mid); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + jmethodID qos_from_rcl_mid = env->GetMethodID(qos_clazz, "nativeFromRCL", "(J)V"); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + env->CallObjectMethod(jqos, qos_from_rcl_mid, &p->qos_profile); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + env->SetObjectField(self, qos_fid, jqos); +} diff --git a/rcljava/src/main/cpp/org_ros2_rcljava_node_NodeImpl.cpp b/rcljava/src/main/cpp/org_ros2_rcljava_node_NodeImpl.cpp index e2c2e829..a85bd726 100644 --- a/rcljava/src/main/cpp/org_ros2_rcljava_node_NodeImpl.cpp +++ b/rcljava/src/main/cpp/org_ros2_rcljava_node_NodeImpl.cpp @@ -19,8 +19,10 @@ #include #include "rcl/error_handling.h" +#include "rcl/graph.h" #include "rcl/node.h" #include "rcl/rcl.h" +#include "rcpputils/scope_exit.hpp" #include "rmw/rmw.h" #include "rosidl_runtime_c/message_type_support_struct.h" @@ -29,6 +31,7 @@ #include "org_ros2_rcljava_node_NodeImpl.h" +using rcljava_common::exceptions::rcljava_throw_exception; using rcljava_common::exceptions::rcljava_throw_rclexception; JNIEXPORT jstring JNICALL @@ -238,3 +241,397 @@ Java_org_ros2_rcljava_node_NodeImpl_nativeCreateTimerHandle( jlong jtimer = reinterpret_cast(timer); return jtimer; } + +JNIEXPORT void JNICALL +Java_org_ros2_rcljava_node_NodeImpl_nativeGetNodeNames( + JNIEnv * env, jclass, jlong handle, jobject jnode_names_info) +{ + rcl_node_t * node = reinterpret_cast(handle); + if (!node) { + rcljava_throw_exception(env, "java/lang/IllegalArgumentException", "node handle is NULL"); + return; + } + + jclass list_clazz = env->GetObjectClass(jnode_names_info); + jmethodID list_add_mid = env->GetMethodID(list_clazz, "add", "(Ljava/lang/Object;)Z"); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + jclass node_info_clazz = env->FindClass("org/ros2/rcljava/graph/NodeNameInfo"); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + jmethodID node_info_init_mid = env->GetMethodID(node_info_clazz, "", "()V"); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + jfieldID name_fid = env->GetFieldID(node_info_clazz, "name", "Ljava/lang/String;"); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + jfieldID namespace_fid = env->GetFieldID(node_info_clazz, "namespace", "Ljava/lang/String;"); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + jfieldID enclave_fid = env->GetFieldID(node_info_clazz, "enclave", "Ljava/lang/String;"); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + + rcl_allocator_t allocator = rcl_get_default_allocator(); + rcutils_string_array_t node_names = rcutils_get_zero_initialized_string_array(); + rcutils_string_array_t node_namespaces = rcutils_get_zero_initialized_string_array(); + rcutils_string_array_t enclaves = rcutils_get_zero_initialized_string_array(); + + rcl_ret_t ret = rcl_get_node_names_with_enclaves( + node, + allocator, + &node_names, + &node_namespaces, + &enclaves); + RCLJAVA_COMMON_THROW_FROM_RCL(env, ret, "rcl_get_node_names_with_enclaves failed"); + auto on_scope_exit = rcpputils::make_scope_exit( + [pnames = &node_names, pnamespaces = &node_namespaces, penclaves = &enclaves, env]() { + rcl_ret_t ret = rcutils_string_array_fini(pnames); + if (!env->ExceptionCheck() && RCL_RET_OK != ret) { + rcljava_throw_rclexception(env, ret, "failed to fini node names string array"); + } + ret = rcutils_string_array_fini(pnamespaces); + if (!env->ExceptionCheck() && RCL_RET_OK != ret) { + rcljava_throw_rclexception(env, ret, "failed to fini node namespaces string array"); + } + ret = rcutils_string_array_fini(penclaves); + if (!env->ExceptionCheck() && RCL_RET_OK != ret) { + rcljava_throw_rclexception(env, ret, "failed to fini enclaves string array"); + } + } + ); + + if (node_names.size != node_namespaces.size || node_names.size != enclaves.size) { + rcljava_throw_exception( + env, + "java/lang/IllegalStateException", + "names, namespaces and enclaves array leghts don't match"); + return; + } + + for (size_t i = 0; i < node_names.size; i++) { + jstring jnode_name = env->NewStringUTF(node_names.data[i]); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + jstring jnode_namespace = env->NewStringUTF(node_namespaces.data[i]); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + jstring jenclave = env->NewStringUTF(enclaves.data[i]); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + jobject jitem = env->NewObject(node_info_clazz, node_info_init_mid); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + env->SetObjectField(jitem, name_fid, jnode_name); + env->SetObjectField(jitem, namespace_fid, jnode_namespace); + env->SetObjectField(jitem, enclave_fid, jenclave); + env->CallBooleanMethod(jnode_names_info, list_add_mid, jitem); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + } +} + +void +fill_jnames_and_types( + JNIEnv * env, const rcl_names_and_types_t & names_and_types, jobject jnames_and_types) +{ + jclass collection_clazz = env->FindClass("java/util/Collection"); + jmethodID collection_add_mid = env->GetMethodID( + collection_clazz, "add", "(Ljava/lang/Object;)Z"); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + jclass name_and_types_clazz = env->FindClass("org/ros2/rcljava/graph/NameAndTypes"); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + jmethodID name_and_types_init_mid = env->GetMethodID(name_and_types_clazz, "", "()V"); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + jfieldID name_fid = env->GetFieldID(name_and_types_clazz, "name", "Ljava/lang/String;"); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + jfieldID types_fid = env->GetFieldID(name_and_types_clazz, "types", "Ljava/util/Collection;"); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + + for (size_t i = 0; i < names_and_types.names.size; i++) { + jobject jitem = env->NewObject(name_and_types_clazz, name_and_types_init_mid); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + jstring jname = env->NewStringUTF(names_and_types.names.data[i]); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + env->SetObjectField(jitem, name_fid, jname); + // the default constructor already inits types to an empty ArrayList + jobject jtypes = env->GetObjectField(jitem, types_fid); + for (size_t j = 0; j < names_and_types.types[i].size; j++) { + jstring jtype = env->NewStringUTF(names_and_types.types[i].data[j]); + env->CallBooleanMethod(jtypes, collection_add_mid, jtype); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + } + env->CallBooleanMethod(jnames_and_types, collection_add_mid, jitem); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + } +} + +JNIEXPORT void JNICALL +Java_org_ros2_rcljava_node_NodeImpl_nativeGetTopicNamesAndTypes( + JNIEnv * env, jclass, jlong handle, jobject jnames_and_types) +{ + rcl_node_t * node = reinterpret_cast(handle); + if (!node) { + rcljava_throw_exception(env, "java/lang/IllegalArgumentException", "node handle is NULL"); + return; + } + + rcl_allocator_t allocator = rcl_get_default_allocator(); + rcl_names_and_types_t topic_names_and_types = rcl_get_zero_initialized_names_and_types(); + + rcl_ret_t ret = rcl_get_topic_names_and_types( + node, + &allocator, + false, + &topic_names_and_types); + RCLJAVA_COMMON_THROW_FROM_RCL(env, ret, "failed to get topic names and types"); + fill_jnames_and_types(env, topic_names_and_types, jnames_and_types); + + ret = rcl_names_and_types_fini(&topic_names_and_types); + if (!env->ExceptionCheck() && RCL_RET_OK != ret) { + rcljava_throw_rclexception(env, ret, "failed to fini topic names and types structure"); + } +} + +JNIEXPORT void JNICALL +Java_org_ros2_rcljava_node_NodeImpl_nativeGetServiceNamesAndTypes( + JNIEnv * env, jclass, jlong handle, jobject jnames_and_types) +{ + rcl_node_t * node = reinterpret_cast(handle); + if (!node) { + rcljava_throw_exception(env, "java/lang/IllegalArgumentException", "node handle is NULL"); + return; + } + + rcl_allocator_t allocator = rcl_get_default_allocator(); + rcl_names_and_types_t service_names_and_types = rcl_get_zero_initialized_names_and_types(); + + rcl_ret_t ret = rcl_get_service_names_and_types( + node, + &allocator, + &service_names_and_types); + RCLJAVA_COMMON_THROW_FROM_RCL(env, ret, "failed to get service names and types"); + fill_jnames_and_types(env, service_names_and_types, jnames_and_types); + + ret = rcl_names_and_types_fini(&service_names_and_types); + if (!env->ExceptionCheck() && RCL_RET_OK != ret) { + rcljava_throw_rclexception(env, ret, "failed to fini service names and types structure"); + } +} + +template +void +get_endpoint_info_common( + JNIEnv * env, jlong handle, jstring jtopic_name, jobject jendpoints_info, FunctorT get_info) +{ + rcl_node_t * node = reinterpret_cast(handle); + if (!node) { + rcljava_throw_exception( + env, "java/lang/IllegalArgumentException", "passed node handle is NULL"); + return; + } + + rcutils_allocator_t allocator = rcutils_get_default_allocator(); + rcl_topic_endpoint_info_array_t endpoints_info = + rcl_get_zero_initialized_topic_endpoint_info_array(); + + const char * topic_name = env->GetStringUTFChars(jtopic_name, NULL); + if (!topic_name) { + rcljava_throw_exception( + env, "java/lang/IllegalArgumentException", "failed to convert jstring to utf chars"); + return; + } + + rcl_ret_t ret = get_info( + node, + &allocator, + topic_name, + false, // use ros mangling conventions + &endpoints_info); + + env->ReleaseStringUTFChars(jtopic_name, topic_name); + + RCLJAVA_COMMON_THROW_FROM_RCL(env, ret, "failed to get publisher info"); + auto cleanup_info_array = rcpputils::make_scope_exit( + [info_ptr = &endpoints_info, allocator_ptr = &allocator, env]() { + rcl_ret_t ret = rcl_topic_endpoint_info_array_fini(info_ptr, allocator_ptr); + if (!env->ExceptionCheck() && RCL_RET_OK != ret) { + rcljava_throw_rclexception(env, ret, "failed to destroy rcl endpoints info"); + } + } + ); + + jclass list_clazz = env->GetObjectClass(jendpoints_info); + jmethodID list_add_mid = env->GetMethodID(list_clazz, "add", "(Ljava/lang/Object;)Z"); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + jclass endpoint_info_clazz = env->FindClass("org/ros2/rcljava/graph/EndpointInfo"); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + jmethodID endpoint_info_init_mid = env->GetMethodID(endpoint_info_clazz, "", "()V"); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + jmethodID endpoint_info_from_rcl_mid = env->GetMethodID( + endpoint_info_clazz, "nativeFromRCL", "(J)V"); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + + for (size_t i = 0; i < endpoints_info.size; i++) { + jobject item = env->NewObject(endpoint_info_clazz, endpoint_info_init_mid); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + env->CallVoidMethod(item, endpoint_info_from_rcl_mid, &endpoints_info.info_array[i]); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + env->CallBooleanMethod(jendpoints_info, list_add_mid, item); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + env->DeleteLocalRef(item); + } +} + +JNIEXPORT void JNICALL +Java_org_ros2_rcljava_node_NodeImpl_nativeGetPublishersInfo( + JNIEnv * env, jclass, jlong handle, jstring jtopic_name, jobject jpublishers_info) +{ + get_endpoint_info_common( + env, handle, jtopic_name, jpublishers_info, rcl_get_publishers_info_by_topic); +} + +JNIEXPORT void JNICALL +Java_org_ros2_rcljava_node_NodeImpl_nativeGetSubscriptionsInfo( + JNIEnv * env, jclass, jlong handle, jstring jtopic_name, jobject jsubscriptions_info) +{ + get_endpoint_info_common( + env, handle, jtopic_name, jsubscriptions_info, rcl_get_subscriptions_info_by_topic); +} + +JNIEXPORT void JNICALL +Java_org_ros2_rcljava_node_NodeImpl_nativeGetPublisherNamesAndTypesByNode( + JNIEnv * env, jclass, jlong handle, jstring jname, jstring jnamespace, jobject jnames_and_types) +{ + rcl_node_t * node = reinterpret_cast(handle); + if (!node) { + rcljava_throw_exception(env, "java/lang/IllegalArgumentException", "node handle is NULL"); + return; + } + + const char * name = env->GetStringUTFChars(jname, NULL); + auto release_jname = rcpputils::make_scope_exit( + [jname, name, env]() {env->ReleaseStringUTFChars(jname, name);}); + const char * namespace_ = env->GetStringUTFChars(jnamespace, NULL); + auto release_jnamespace = rcpputils::make_scope_exit( + [jnamespace, namespace_, env]() {env->ReleaseStringUTFChars(jnamespace, namespace_);}); + + rcl_allocator_t allocator = rcl_get_default_allocator(); + rcl_names_and_types_t publisher_names_and_types = rcl_get_zero_initialized_names_and_types(); + auto fini_names_and_types = rcpputils::make_scope_exit( + [pnames_and_types = &publisher_names_and_types, env]() { + rcl_ret_t ret = rcl_names_and_types_fini(pnames_and_types); + if (!env->ExceptionCheck() && RCL_RET_OK != ret) { + rcljava_throw_rclexception(env, ret, "failed to fini publisher names and types structure"); + } + }); + + rcl_ret_t ret = rcl_get_publisher_names_and_types_by_node( + node, + &allocator, + false, + name, + namespace_, + &publisher_names_and_types); + RCLJAVA_COMMON_THROW_FROM_RCL(env, ret, "failed to get publisher names and types"); + fill_jnames_and_types(env, publisher_names_and_types, jnames_and_types); +} + +JNIEXPORT void JNICALL +Java_org_ros2_rcljava_node_NodeImpl_nativeGetSubscriptionNamesAndTypesByNode( + JNIEnv * env, jclass, jlong handle, jstring jname, jstring jnamespace, jobject jnames_and_types) +{ + rcl_node_t * node = reinterpret_cast(handle); + if (!node) { + rcljava_throw_exception(env, "java/lang/IllegalArgumentException", "node handle is NULL"); + return; + } + + const char * name = env->GetStringUTFChars(jname, NULL); + auto release_jname = rcpputils::make_scope_exit( + [jname, name, env]() {env->ReleaseStringUTFChars(jname, name);}); + const char * namespace_ = env->GetStringUTFChars(jnamespace, NULL); + auto release_jnamespace = rcpputils::make_scope_exit( + [jnamespace, namespace_, env]() {env->ReleaseStringUTFChars(jnamespace, namespace_);}); + rcl_allocator_t allocator = rcl_get_default_allocator(); + rcl_names_and_types_t subscription_names_and_types = rcl_get_zero_initialized_names_and_types(); + auto fini_names_and_types = rcpputils::make_scope_exit( + [pnames_and_types = &subscription_names_and_types, env]() { + rcl_ret_t ret = rcl_names_and_types_fini(pnames_and_types); + if (!env->ExceptionCheck() && RCL_RET_OK != ret) { + rcljava_throw_rclexception( + env, ret, "failed to fini subscription names and types structure"); + } + }); + + rcl_ret_t ret = rcl_get_subscriber_names_and_types_by_node( + node, + &allocator, + false, + name, + namespace_, + &subscription_names_and_types); + RCLJAVA_COMMON_THROW_FROM_RCL(env, ret, "failed to get subscription names and types"); + fill_jnames_and_types(env, subscription_names_and_types, jnames_and_types); +} + +JNIEXPORT void JNICALL Java_org_ros2_rcljava_node_NodeImpl_nativeGetServiceNamesAndTypesByNode( + JNIEnv * env, jclass, jlong handle, jstring jname, jstring jnamespace, jobject jnames_and_types) +{ + rcl_node_t * node = reinterpret_cast(handle); + if (!node) { + rcljava_throw_exception(env, "java/lang/IllegalArgumentException", "node handle is NULL"); + return; + } + + const char * name = env->GetStringUTFChars(jname, NULL); + auto release_jname = rcpputils::make_scope_exit( + [jname, name, env]() {env->ReleaseStringUTFChars(jname, name);}); + const char * namespace_ = env->GetStringUTFChars(jnamespace, NULL); + auto release_jnamespace = rcpputils::make_scope_exit( + [jnamespace, namespace_, env]() {env->ReleaseStringUTFChars(jnamespace, namespace_);}); + rcl_allocator_t allocator = rcl_get_default_allocator(); + rcl_names_and_types_t service_names_and_types = rcl_get_zero_initialized_names_and_types(); + auto fini_names_and_types = rcpputils::make_scope_exit( + [pnames_and_types = &service_names_and_types, env]() { + rcl_ret_t ret = rcl_names_and_types_fini(pnames_and_types); + if (!env->ExceptionCheck() && RCL_RET_OK != ret) { + rcljava_throw_rclexception( + env, ret, "failed to fini service names and types structure"); + } + }); + + rcl_ret_t ret = rcl_get_service_names_and_types_by_node( + node, + &allocator, + name, + namespace_, + &service_names_and_types); + RCLJAVA_COMMON_THROW_FROM_RCL(env, ret, "failed to get service names and types"); + fill_jnames_and_types(env, service_names_and_types, jnames_and_types); +} + +JNIEXPORT void JNICALL Java_org_ros2_rcljava_node_NodeImpl_nativeGetClientNamesAndTypesByNode( + JNIEnv * env, jclass, jlong handle, jstring jname, jstring jnamespace, jobject jnames_and_types) +{ + rcl_node_t * node = reinterpret_cast(handle); + if (!node) { + rcljava_throw_exception(env, "java/lang/IllegalArgumentException", "node handle is NULL"); + return; + } + + const char * name = env->GetStringUTFChars(jname, NULL); + auto release_jname = rcpputils::make_scope_exit( + [jname, name, env]() {env->ReleaseStringUTFChars(jname, name);}); + const char * namespace_ = env->GetStringUTFChars(jnamespace, NULL); + auto release_jnamespace = rcpputils::make_scope_exit( + [jnamespace, namespace_, env]() {env->ReleaseStringUTFChars(jnamespace, namespace_);}); + rcl_allocator_t allocator = rcl_get_default_allocator(); + rcl_names_and_types_t client_names_and_types = rcl_get_zero_initialized_names_and_types(); + auto fini_names_and_types = rcpputils::make_scope_exit( + [pnames_and_types = &client_names_and_types, env]() { + rcl_ret_t ret = rcl_names_and_types_fini(pnames_and_types); + if (!env->ExceptionCheck() && RCL_RET_OK != ret) { + rcljava_throw_rclexception( + env, ret, "failed to fini client names and types structure"); + } + }); + + rcl_ret_t ret = rcl_get_client_names_and_types_by_node( + node, + &allocator, + name, + namespace_, + &client_names_and_types); + RCLJAVA_COMMON_THROW_FROM_RCL(env, ret, "failed to get client names and types"); + fill_jnames_and_types(env, client_names_and_types, jnames_and_types); +} diff --git a/rcljava/src/main/cpp/org_ros2_rcljava_qos_QoSProfile.cpp b/rcljava/src/main/cpp/org_ros2_rcljava_qos_QoSProfile.cpp new file mode 100644 index 00000000..e082dca5 --- /dev/null +++ b/rcljava/src/main/cpp/org_ros2_rcljava_qos_QoSProfile.cpp @@ -0,0 +1,250 @@ +// 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. + +#include "org_ros2_rcljava_qos_QoSProfile.h" + +#include +#include + +#include "rcljava_common/exceptions.hpp" +#include "rmw/qos_profiles.h" +#include "rmw/types.h" + +using rcljava_common::exceptions::rcljava_throw_exception; + +static void +qos_set_duration(JNIEnv * env, uint64_t seconds, uint64_t nanos, jobject jqos, jfieldID fid) +{ + if (static_cast(std::numeric_limits::max()) < seconds) { + // Throwing an exception here would be weird, as we cannot control the durability that was set + // on all other endpoints in the network. Set jqos seconds to jlong max. + seconds = static_cast(std::numeric_limits::max()); + } + if (static_cast(std::numeric_limits::max()) < nanos) { + // This should never happen, as nanoseconds within a second can perfectly be represented + // in a jlong. + nanos = static_cast(std::numeric_limits::max()); + } + jclass duration_clazz = env->FindClass("java/time/Duration"); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + jmethodID factory_mid = env->GetStaticMethodID( + duration_clazz, "ofSeconds", "(JJ)Ljava/time/Duration;"); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + jobject jduration = env->CallStaticObjectMethod( + duration_clazz, factory_mid, static_cast(seconds), static_cast(nanos)); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + env->SetObjectField(jqos, fid, jduration); +} + +static void +qos_from_rcl(JNIEnv * env, const rmw_qos_profile_t & qos, jobject jqos) +{ + // TODO(ivanpauno): class and field lookup could be done at startup time + jclass clazz = env->GetObjectClass(jqos); + const char * history_class_path = "Lorg/ros2/rcljava/qos/policies/History;"; + jfieldID history_fid = env->GetFieldID(clazz, "history", history_class_path); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + jfieldID depth_fid = env->GetFieldID(clazz, "depth", "I"); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + const char * reliability_class_path = "Lorg/ros2/rcljava/qos/policies/Reliability;"; + jfieldID reliability_fid = env->GetFieldID(clazz, "reliability", reliability_class_path); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + const char * durability_class_path = "Lorg/ros2/rcljava/qos/policies/Durability;"; + jfieldID durability_fid = env->GetFieldID(clazz, "durability", durability_class_path); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + jfieldID deadline_fid = env->GetFieldID(clazz, "deadline", "Ljava/time/Duration;"); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + jfieldID lifespan_fid = env->GetFieldID(clazz, "lifespan", "Ljava/time/Duration;"); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + const char * liveliness_class_path = "Lorg/ros2/rcljava/qos/policies/Liveliness;"; + jfieldID liveliness_fid = env->GetFieldID(clazz, "liveliness", liveliness_class_path); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + jfieldID liveliness_lease_fid = env->GetFieldID( + clazz, "livelinessLeaseDuration", "Ljava/time/Duration;"); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + jfieldID avoid_ros_conventions_fid = env->GetFieldID( + clazz, "avoidROSNamespaceConventions", "Z"); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + + jclass history_clazz = env->FindClass("org/ros2/rcljava/qos/policies/History"); + jfieldID history_value_fid = nullptr; + switch (qos.history) { + case RMW_QOS_POLICY_HISTORY_SYSTEM_DEFAULT: + history_value_fid = env->GetStaticFieldID( + history_clazz, "SYSTEM_DEFAULT", history_class_path); + break; + case RMW_QOS_POLICY_HISTORY_KEEP_LAST: + history_value_fid = env->GetStaticFieldID( + history_clazz, "KEEP_LAST", history_class_path); + break; + case RMW_QOS_POLICY_HISTORY_KEEP_ALL: + history_value_fid = env->GetStaticFieldID( + history_clazz, "KEEP_ALL", history_class_path); + break; + case RMW_QOS_POLICY_HISTORY_UNKNOWN: + history_value_fid = env->GetStaticFieldID( + history_clazz, "UNKNOWN", history_class_path); + break; + default: + rcljava_throw_exception( + env, "java/lang/IllegalStateException", "unknown history policy value"); + break; + } + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + jobject history_value = env->GetStaticObjectField(history_clazz, history_value_fid); + env->SetObjectField(jqos, history_fid, history_value); + + env->SetIntField(jqos, depth_fid, qos.depth); + + jclass reliability_clazz = env->FindClass("org/ros2/rcljava/qos/policies/Reliability"); + jfieldID reliability_value_fid = nullptr; + switch (qos.reliability) { + case RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT: + reliability_value_fid = env->GetStaticFieldID( + reliability_clazz, "SYSTEM_DEFAULT", reliability_class_path); + break; + case RMW_QOS_POLICY_RELIABILITY_RELIABLE: + reliability_value_fid = env->GetStaticFieldID( + reliability_clazz, "RELIABLE", reliability_class_path); + break; + case RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT: + reliability_value_fid = env->GetStaticFieldID( + reliability_clazz, "BEST_EFFORT", reliability_class_path); + break; + case RMW_QOS_POLICY_RELIABILITY_UNKNOWN: + reliability_value_fid = env->GetStaticFieldID( + reliability_clazz, "UNKNOWN", reliability_class_path); + break; + default: + rcljava_throw_exception( + env, "java/lang/IllegalStateException", "unknown reliability policy value"); + break; + } + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + jobject reliability_value = env->GetStaticObjectField(reliability_clazz, reliability_value_fid); + env->SetObjectField(jqos, reliability_fid, reliability_value); + + jclass durability_clazz = env->FindClass("org/ros2/rcljava/qos/policies/Durability"); + jfieldID durability_value_fid = nullptr; + switch (qos.durability) { + case RMW_QOS_POLICY_DURABILITY_SYSTEM_DEFAULT: + durability_value_fid = env->GetStaticFieldID( + durability_clazz, "SYSTEM_DEFAULT", durability_class_path); + break; + case RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL: + durability_value_fid = env->GetStaticFieldID( + durability_clazz, "TRANSIENT_LOCAL", durability_class_path); + break; + case RMW_QOS_POLICY_DURABILITY_VOLATILE: + durability_value_fid = env->GetStaticFieldID( + durability_clazz, "VOLATILE", durability_class_path); + break; + case RMW_QOS_POLICY_DURABILITY_UNKNOWN: + durability_value_fid = env->GetStaticFieldID( + durability_clazz, "UNKNOWN", durability_class_path); + break; + default: + rcljava_throw_exception( + env, "java/lang/IllegalStateException", "unknown durability policy value"); + break; + } + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + jobject durability_value = env->GetStaticObjectField(durability_clazz, durability_value_fid); + env->SetObjectField(jqos, durability_fid, durability_value); + + qos_set_duration(env, qos.deadline.sec, qos.deadline.nsec, jqos, deadline_fid); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + qos_set_duration(env, qos.lifespan.sec, qos.lifespan.nsec, jqos, lifespan_fid); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + qos_set_duration( + env, qos.liveliness_lease_duration.sec, + qos.liveliness_lease_duration.nsec, jqos, liveliness_lease_fid); + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + + jclass liveliness_clazz = env->FindClass("org/ros2/rcljava/qos/policies/Liveliness"); + jfieldID liveliness_value_fid = nullptr; + switch (qos.liveliness) { + case RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT: + liveliness_value_fid = env->GetStaticFieldID( + liveliness_clazz, "SYSTEM_DEFAULT", liveliness_class_path); + break; + case RMW_QOS_POLICY_LIVELINESS_AUTOMATIC: + liveliness_value_fid = env->GetStaticFieldID( + liveliness_clazz, "AUTOMATIC", liveliness_class_path); + break; + case RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC: + liveliness_value_fid = env->GetStaticFieldID( + liveliness_clazz, "MANUAL_BY_TOPIC", liveliness_class_path); + break; + case RMW_QOS_POLICY_LIVELINESS_UNKNOWN: + liveliness_value_fid = env->GetStaticFieldID( + liveliness_clazz, "UNKNOWN", liveliness_class_path); + break; + default: + rcljava_throw_exception( + env, "java/lang/IllegalStateException", "unknown liveliness policy value"); + break; + } + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env); + jobject liveliness_value = env->GetStaticObjectField(liveliness_clazz, liveliness_value_fid); + env->SetObjectField(jqos, liveliness_fid, liveliness_value); + + env->SetBooleanField(jqos, avoid_ros_conventions_fid, qos.avoid_ros_namespace_conventions); +} + +JNIEXPORT void JNICALL +Java_org_ros2_rcljava_qos_QoSProfile_nativeFromRCL(JNIEnv * env, jobject jqos, jlong handle) +{ + auto * qos_profile = reinterpret_cast(handle); + if (!handle) { + rcljava_throw_exception( + env, "java/lang/IllegalArgumentException", "rmw qos profile handle is NULL"); + return; + } + qos_from_rcl(env, *qos_profile, jqos); +} + +JNIEXPORT jlong JNICALL +Java_org_ros2_rcljava_qos_QoSProfile_nativeGetHandleFromName( + JNIEnv * env, jclass, jstring jprofile_name) +{ + const rmw_qos_profile_t * qos = NULL; + const char * profile_name = env->GetStringUTFChars(jprofile_name, NULL); + if (strcmp(profile_name, "default") == 0) { + qos = &rmw_qos_profile_default; + } + if (strcmp(profile_name, "system_default") == 0) { + qos = &rmw_qos_profile_system_default; + } + if (strcmp(profile_name, "sensor_data") == 0) { + qos = &rmw_qos_profile_sensor_data; + } + if (strcmp(profile_name, "parameters") == 0) { + qos = &rmw_qos_profile_parameters; + } + if (strcmp(profile_name, "services") == 0) { + qos = &rmw_qos_profile_services_default; + } + if (strcmp(profile_name, "parameter_events") == 0) { + qos = &rmw_qos_profile_parameter_events; + } + + env->ReleaseStringUTFChars(jprofile_name, profile_name); + if (qos) { + return reinterpret_cast(qos); + } + rcljava_throw_exception( + env, "java/lang/IllegalArgumentException", "unexpected profile identifier"); + return 0; +} diff --git a/rcljava/src/main/cpp/org_ros2_rcljava_subscription_statuses_MessageLost.cpp b/rcljava/src/main/cpp/org_ros2_rcljava_subscription_statuses_MessageLost.cpp new file mode 100644 index 00000000..9fdee7ab --- /dev/null +++ b/rcljava/src/main/cpp/org_ros2_rcljava_subscription_statuses_MessageLost.cpp @@ -0,0 +1,74 @@ +// 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. + +#include "org_ros2_rcljava_subscription_statuses_MessageLost.h" + +#include +#include + +#include "rmw/types.h" +#include "rcl/event.h" +#include "rcljava_common/exceptions.hpp" + +using rcljava_common::exceptions::rcljava_throw_exception; + +JNIEXPORT jlong JNICALL +Java_org_ros2_rcljava_subscription_statuses_MessageLost_nativeAllocateRCLStatusEvent( + JNIEnv * env, jclass) +{ + void * p = malloc(sizeof(rmw_message_lost_status_t)); + if (!p) { + rcljava_throw_exception( + env, "java/lang/OutOfMemoryError", "failed to allocate missed deadline status"); + } + return reinterpret_cast(p); +} + +JNIEXPORT void JNICALL +Java_org_ros2_rcljava_subscription_statuses_MessageLost_nativeDeallocateRCLStatusEvent( + JNIEnv *, jclass, jlong handle) +{ + free(reinterpret_cast(handle)); +} + +JNIEXPORT void JNICALL +Java_org_ros2_rcljava_subscription_statuses_MessageLost_nativeFromRCLEvent( + JNIEnv * env, jobject self, jlong handle) +{ + auto * p = reinterpret_cast(handle); + if (!p) { + rcljava_throw_exception( + env, "java/lang/IllegalArgumentException", "passed rmw object handle is NULL"); + return; + } + // TODO(ivanpauno): class and field lookup could be done at startup time + jclass clazz = env->GetObjectClass(self); + jfieldID total_count_fid = env->GetFieldID(clazz, "totalCount", "I"); + if (env->ExceptionCheck()) { + return; + } + jfieldID total_count_change_fid = env->GetFieldID(clazz, "totalCountChange", "I"); + if (env->ExceptionCheck()) { + return; + } + env->SetIntField(self, total_count_fid, p->total_count); + env->SetIntField(self, total_count_change_fid, p->total_count_change); +} + +JNIEXPORT jint JNICALL +Java_org_ros2_rcljava_subscription_statuses_MessageLost_nativeGetSubscriptionEventType( + JNIEnv *, jclass) +{ + return RCL_SUBSCRIPTION_MESSAGE_LOST; +} diff --git a/rcljava/src/main/cpp/org_ros2_rcljava_time_Clock.cpp b/rcljava/src/main/cpp/org_ros2_rcljava_time_Clock.cpp index 3a4638b2..8e954c22 100644 --- a/rcljava/src/main/cpp/org_ros2_rcljava_time_Clock.cpp +++ b/rcljava/src/main/cpp/org_ros2_rcljava_time_Clock.cpp @@ -62,8 +62,89 @@ Java_org_ros2_rcljava_time_Clock_nativeCreateClockHandle(JNIEnv * env, jclass, j return 0; } - jlong clock_handle = reinterpret_cast(clock); - return clock_handle; + return reinterpret_cast(clock); +} + +JNIEXPORT jlong JNICALL +Java_org_ros2_rcljava_time_Clock_nativeGetNow(JNIEnv * env, jclass, jlong clock_handle) +{ + rcl_clock_t * clock = reinterpret_cast(clock_handle); + rcl_time_point_value_t nanoseconds; + rcl_ret_t ret = rcl_clock_get_now(clock, &nanoseconds); + if (ret != RCL_RET_OK) { + std::string msg = "Failed to get time: " + std::string(rcl_get_error_string().str); + rcl_reset_error(); + rcljava_throw_rclexception(env, ret, msg); + return 0; + } + + return static_cast(nanoseconds); +} + +JNIEXPORT jboolean JNICALL +Java_org_ros2_rcljava_time_Clock_nativeRosTimeOverrideEnabled( + JNIEnv * env, jclass, jlong clock_handle) +{ + rcl_clock_t * clock = reinterpret_cast(clock_handle); + + if (!rcl_clock_valid(clock)) { + return false; + } + + bool is_enabled = false; + rcl_ret_t ret = rcl_is_enabled_ros_time_override(clock, &is_enabled); + if (ret != RCL_RET_OK) { + std::string msg = "Failed to check ros_time_override_status: " + + std::string(rcl_get_error_string().str); + rcl_reset_error(); + rcljava_throw_rclexception(env, ret, msg); + return false; + } + + return is_enabled; +} + +JNIEXPORT void JNICALL +Java_org_ros2_rcljava_time_Clock_nativeSetRosTimeOverrideEnabled( + JNIEnv * env, jclass, jlong clock_handle, jboolean enabled) +{ + rcl_clock_t * clock = reinterpret_cast(clock_handle); + + if (!rcl_clock_valid(clock)) { + return; + } + + rcl_ret_t ret; + if (enabled) { + ret = rcl_enable_ros_time_override(clock); + } else { + ret = rcl_disable_ros_time_override(clock); + } + if (ret != RCL_RET_OK) { + std::string msg = "Failed to set ROS time override enable for clock: " + + std::string(rcl_get_error_string().str); + rcl_reset_error(); + rcljava_throw_rclexception(env, ret, msg); + } +} + +JNIEXPORT void JNICALL +Java_org_ros2_rcljava_time_Clock_nativeSetRosTimeOverride( + JNIEnv * env, jclass, jlong clock_handle, jlong nanos) +{ + rcl_clock_t * clock = reinterpret_cast(clock_handle); + + if (!rcl_clock_valid(clock)) { + return; + } + + rcl_ret_t ret = rcl_set_ros_time_override(clock, static_cast(nanos)); + if (ret != RCL_RET_OK) { + std::string msg = "Failed to set ROS time override for clock: " + + std::string(rcl_get_error_string().str); + rcl_reset_error(); + rcljava_throw_rclexception(env, ret, msg); + } } JNIEXPORT void JNICALL diff --git a/rcljava/src/main/cpp/org_ros2_rcljava_timer_WallTimerImpl.cpp b/rcljava/src/main/cpp/org_ros2_rcljava_timer_TimerImpl.cpp similarity index 67% rename from rcljava/src/main/cpp/org_ros2_rcljava_timer_WallTimerImpl.cpp rename to rcljava/src/main/cpp/org_ros2_rcljava_timer_TimerImpl.cpp index 8a7559af..d2c45c3a 100644 --- a/rcljava/src/main/cpp/org_ros2_rcljava_timer_WallTimerImpl.cpp +++ b/rcljava/src/main/cpp/org_ros2_rcljava_timer_TimerImpl.cpp @@ -27,19 +27,19 @@ #include "rcljava_common/exceptions.hpp" #include "rcljava_common/signatures.hpp" -#include "org_ros2_rcljava_timer_WallTimerImpl.h" +#include "org_ros2_rcljava_timer_TimerImpl.h" using rcljava_common::exceptions::rcljava_throw_exception; using rcljava_common::signatures::convert_from_java_signature; using rcljava_common::signatures::destroy_ros_message_signature; JNIEXPORT jboolean JNICALL -Java_org_ros2_rcljava_timer_WallTimerImpl_nativeIsReady( - JNIEnv * env, jclass, jlong wall_timer_handle) +Java_org_ros2_rcljava_timer_TimerImpl_nativeIsReady( + JNIEnv * env, jclass, jlong timer_handle) { - assert(wall_timer_handle != 0); + assert(timer_handle != 0); - rcl_timer_t * timer = reinterpret_cast(wall_timer_handle); + rcl_timer_t * timer = reinterpret_cast(timer_handle); bool is_ready; rcl_ret_t ret = rcl_timer_is_ready(timer, &is_ready); @@ -54,12 +54,12 @@ Java_org_ros2_rcljava_timer_WallTimerImpl_nativeIsReady( } JNIEXPORT jboolean JNICALL -Java_org_ros2_rcljava_timer_WallTimerImpl_nativeIsCanceled( - JNIEnv * env, jclass, jlong wall_timer_handle) +Java_org_ros2_rcljava_timer_TimerImpl_nativeIsCanceled( + JNIEnv * env, jclass, jlong timer_handle) { - assert(wall_timer_handle != 0); + assert(timer_handle != 0); - rcl_timer_t * timer = reinterpret_cast(wall_timer_handle); + rcl_timer_t * timer = reinterpret_cast(timer_handle); bool is_canceled; rcl_ret_t ret = rcl_timer_is_canceled(timer, &is_canceled); @@ -74,15 +74,15 @@ Java_org_ros2_rcljava_timer_WallTimerImpl_nativeIsCanceled( } JNIEXPORT void JNICALL -Java_org_ros2_rcljava_timer_WallTimerImpl_nativeDispose( - JNIEnv * env, jclass, jlong wall_timer_handle) +Java_org_ros2_rcljava_timer_TimerImpl_nativeDispose( + JNIEnv * env, jclass, jlong timer_handle) { - if (wall_timer_handle == 0) { + if (timer_handle == 0) { // everything is ok, already destroyed return; } - rcl_timer_t * timer = reinterpret_cast(wall_timer_handle); + rcl_timer_t * timer = reinterpret_cast(timer_handle); assert(timer != NULL); @@ -96,11 +96,11 @@ Java_org_ros2_rcljava_timer_WallTimerImpl_nativeDispose( } JNIEXPORT void JNICALL -Java_org_ros2_rcljava_timer_WallTimerImpl_nativeReset(JNIEnv * env, jclass, jlong wall_timer_handle) +Java_org_ros2_rcljava_timer_TimerImpl_nativeReset(JNIEnv * env, jclass, jlong timer_handle) { - assert(wall_timer_handle != 0); + assert(timer_handle != 0); - rcl_timer_t * timer = reinterpret_cast(wall_timer_handle); + rcl_timer_t * timer = reinterpret_cast(timer_handle); assert(timer != NULL); @@ -114,12 +114,12 @@ Java_org_ros2_rcljava_timer_WallTimerImpl_nativeReset(JNIEnv * env, jclass, jlon } JNIEXPORT void JNICALL -Java_org_ros2_rcljava_timer_WallTimerImpl_nativeCancel( - JNIEnv * env, jclass, jlong wall_timer_handle) +Java_org_ros2_rcljava_timer_TimerImpl_nativeCancel( + JNIEnv * env, jclass, jlong timer_handle) { - assert(wall_timer_handle != 0); + assert(timer_handle != 0); - rcl_timer_t * timer = reinterpret_cast(wall_timer_handle); + rcl_timer_t * timer = reinterpret_cast(timer_handle); assert(timer != NULL); @@ -133,12 +133,12 @@ Java_org_ros2_rcljava_timer_WallTimerImpl_nativeCancel( } JNIEXPORT jlong JNICALL -Java_org_ros2_rcljava_timer_WallTimerImpl_nativeTimeUntilNextCall( - JNIEnv * env, jclass, jlong wall_timer_handle) +Java_org_ros2_rcljava_timer_TimerImpl_nativeTimeUntilNextCall( + JNIEnv * env, jclass, jlong timer_handle) { - assert(wall_timer_handle != 0); + assert(timer_handle != 0); - rcl_timer_t * timer = reinterpret_cast(wall_timer_handle); + rcl_timer_t * timer = reinterpret_cast(timer_handle); assert(timer != NULL); @@ -157,12 +157,12 @@ Java_org_ros2_rcljava_timer_WallTimerImpl_nativeTimeUntilNextCall( } JNIEXPORT jlong JNICALL -Java_org_ros2_rcljava_timer_WallTimerImpl_nativeTimeSinceLastCall( - JNIEnv * env, jclass, jlong wall_timer_handle) +Java_org_ros2_rcljava_timer_TimerImpl_nativeTimeSinceLastCall( + JNIEnv * env, jclass, jlong timer_handle) { - assert(wall_timer_handle != 0); + assert(timer_handle != 0); - rcl_timer_t * timer = reinterpret_cast(wall_timer_handle); + rcl_timer_t * timer = reinterpret_cast(timer_handle); assert(timer != NULL); @@ -181,12 +181,12 @@ Java_org_ros2_rcljava_timer_WallTimerImpl_nativeTimeSinceLastCall( } JNIEXPORT jlong JNICALL -Java_org_ros2_rcljava_timer_WallTimerImpl_nativeGetTimerPeriodNS( - JNIEnv * env, jclass, jlong wall_timer_handle) +Java_org_ros2_rcljava_timer_TimerImpl_nativeGetTimerPeriodNS( + JNIEnv * env, jclass, jlong timer_handle) { - assert(wall_timer_handle != 0); + assert(timer_handle != 0); - rcl_timer_t * timer = reinterpret_cast(wall_timer_handle); + rcl_timer_t * timer = reinterpret_cast(timer_handle); assert(timer != NULL); @@ -204,12 +204,12 @@ Java_org_ros2_rcljava_timer_WallTimerImpl_nativeGetTimerPeriodNS( } JNIEXPORT void JNICALL -Java_org_ros2_rcljava_timer_WallTimerImpl_nativeSetTimerPeriodNS( - JNIEnv * env, jclass, jlong wall_timer_handle, jlong timer_period) +Java_org_ros2_rcljava_timer_TimerImpl_nativeSetTimerPeriodNS( + JNIEnv * env, jclass, jlong timer_handle, jlong timer_period) { - assert(wall_timer_handle != 0); + assert(timer_handle != 0); - rcl_timer_t * timer = reinterpret_cast(wall_timer_handle); + rcl_timer_t * timer = reinterpret_cast(timer_handle); assert(timer != NULL); @@ -224,12 +224,12 @@ Java_org_ros2_rcljava_timer_WallTimerImpl_nativeSetTimerPeriodNS( } JNIEXPORT void JNICALL -Java_org_ros2_rcljava_timer_WallTimerImpl_nativeCallTimer( - JNIEnv * env, jclass, jlong wall_timer_handle) +Java_org_ros2_rcljava_timer_TimerImpl_nativeCallTimer( + JNIEnv * env, jclass, jlong timer_handle) { - assert(wall_timer_handle != 0); + assert(timer_handle != 0); - rcl_timer_t * timer = reinterpret_cast(wall_timer_handle); + rcl_timer_t * timer = reinterpret_cast(timer_handle); assert(timer != NULL); diff --git a/rcljava/src/main/java/org/ros2/rcljava/RCLJava.java b/rcljava/src/main/java/org/ros2/rcljava/RCLJava.java index 15a703c8..eadbba7c 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/RCLJava.java +++ b/rcljava/src/main/java/org/ros2/rcljava/RCLJava.java @@ -22,6 +22,7 @@ import java.util.Collection; import java.util.Map; import java.util.concurrent.ConcurrentSkipListMap; +import java.util.concurrent.Future; import java.util.concurrent.LinkedBlockingQueue; import org.ros2.rcljava.client.Client; @@ -43,7 +44,7 @@ import org.ros2.rcljava.timer.Timer; /** - * Entry point for the ROS2 Java API, similar to the rclcpp API. + * Entry point for the ROS 2 Java API, similar to the rclcpp API. */ public final class RCLJava { private static final Logger logger = LoggerFactory.getLogger(RCLJava.class); @@ -132,17 +133,25 @@ public static boolean isInitialized() { /** * Initialize the RCLJava API. If successful, a valid RMW implementation will - * be loaded and accessible, enabling the creating of ROS2 entities + * be loaded and accessible, enabling the creating of ROS 2 entities * (@{link Node}s, @{link Publisher}s and @{link Subscription}s. * This also initializes the default context. */ public static synchronized void rclJavaInit() { + String args[] = {}; + rclJavaInit(args); + } + + /** + * Initialize the RCLJava API, passing command line arguments. + */ + public static synchronized void rclJavaInit(String args[]) { if (RCLJava.ok()) { return; } // Initialize default context - getDefaultContext().init(); + getDefaultContext().init(args); logger.info("Using RMW implementation: {}", RCLJava.getRMWIdentifier()); } @@ -155,18 +164,18 @@ public static synchronized void rclJavaInit() { private static native long nativeCreateContextHandle(); /** - * Create a ROS2 node (rcl_node_t) and return a pointer to it as an integer. + * Create a ROS 2 node (rcl_node_t) and return a pointer to it as an integer. * - * @param nodeName The name that will identify this node in a ROS2 graph. + * @param nodeName The name that will identify this node in a ROS 2 graph. * @param namespace The namespace of the node. * @param contextHandle Pointer to a context (rcl_context_t) with which to associated the node. - * @return A pointer to the underlying ROS2 node structure. + * @return A pointer to the underlying ROS 2 node structure. */ private static native long nativeCreateNodeHandle(String nodeName, String namespace, long contextHandle, ArrayList arguments, boolean useGlobalArguments, boolean enableRosout); /** * @return The identifier of the currently active RMW implementation via the - * native ROS2 API. + * native ROS 2 API. */ private static native String nativeGetRMWIdentifier(); @@ -178,7 +187,7 @@ public static String getRMWIdentifier() { } /** - * Call the underlying ROS2 rcl mechanism to check if ROS2 has been shut + * Call the underlying ROS 2 rcl mechanism to check if ROS 2 has been shut * down. * * @return true if RCLJava hasn't been shut down, false otherwise. @@ -214,29 +223,50 @@ public static Context createContext() { /** * Create a @{link Node}. * - * @param nodeName The name that will identify this node in a ROS2 graph. - * @return A @{link Node} that represents the underlying ROS2 node - * structure. + * @param nodeName The name that will identify this node in a ROS 2 graph. + * @return A @{link Node} that represents the underlying ROS 2 node structure. */ public static Node createNode(final String nodeName) { - return createNode(nodeName, "", RCLJava.getDefaultContext(), new NodeOptions()); + return createNode(nodeName, "", new NodeOptions()); + } + + /** + * Create a @{link Node}. + * + * @param nodeName The name that will identify this node in a ROS 2 graph. + * @param namespace The namespace of the node. + * @return A @{link Node} that represents the underlying ROS 2 node structure. + */ + public static Node createNode(final String nodeName, final String namespace) { + return createNode(nodeName, namespace, new NodeOptions()); } /** * Create a @{link Node}. * - * @param nodeName The name that will identify this node in a ROS2 graph. + * @deprecated Use `RCLJava.createNode(nodeName, namespace, new NodeOptions.setContext(context))` instead. + * @param nodeName The name that will identify this node in a ROS 2 graph. * @param namespace The namespace of the node. - * @return A @{link Node} that represents the underlying ROS2 node - * structure. + * @param context Context used for creating the node, @link{RCLJava.getDefaultContext()} will be used if `null`. + * @return A @{link Node} that represents the underlying ROS 2 node structure. */ + @Deprecated public static Node createNode(final String nodeName, final String namespace, final Context context) { - return createNode(nodeName, namespace, context, new NodeOptions()); + return createNode(nodeName, namespace, new NodeOptions().setContext(context)); } - public static Node createNode(final String nodeName, final String namespace, final Context context, final NodeOptions options) { + /** + * Create a @{link Node}. + * + * @param nodeName The name that will identify this node in a ROS 2 graph. + * @param namespace The namespace of the node. + * @param options Additional options to customize the Node creation. See @link{org.ros2.rcljava.node.NodeOptions}. + * @return A @{link Node} that represents the underlying ROS 2 node structure. + */ + public static Node createNode(final String nodeName, final String namespace, final NodeOptions options) { + Context context = options.getContext() == null ? RCLJava.getDefaultContext() : options.getContext(); long nodeHandle = nativeCreateNodeHandle(nodeName, namespace, context.getHandle(), options.getCliArgs(), options.getUseGlobalArguments(), options.getEnableRosout()); - Node node = new NodeImpl(nodeHandle, context, options.getAllowUndeclaredParameters()); + Node node = new NodeImpl(nodeHandle, options); nodes.add(node); return node; } @@ -258,22 +288,61 @@ public static void spin(final ComposableNode composableNode) { getGlobalExecutor().removeNode(composableNode); } - public static void spinOnce(final Node node) { + /** + * Execute one callback for a @{link Node}. + * + * Wait until a callback becomes ready and then execute it. + * + * @param node The node to spin on. + * @param timeout The time to wait for a callback to become ready in nanoseconds. + * If a timeout occurs, then nothing happens and this method returns. + */ + public static void spinOnce(final Node node, long timeout) { ComposableNode composableNode = new ComposableNode() { public Node getNode() { return node; } }; getGlobalExecutor().addNode(composableNode); - getGlobalExecutor().spinOnce(); + getGlobalExecutor().spinOnce(timeout); getGlobalExecutor().removeNode(composableNode); } - public static void spinOnce(final ComposableNode composableNode) { + /** + * Execute one callback for a @{link Node}. + * + * Wait until a callback becomes ready and then execute it. + * + * @param node The node to spin on. + */ + public static void spinOnce(final Node node) { + RCLJava.spinOnce(node, -1); + } + + /** + * Execute one callback for a @{link ComposableNode}. + * + * Wait until a callback becomes ready and then execute it. + * + * @param composableNode The composable node to spin on. + * @param timeout The time to wait for a callback to become ready in nanoseconds. + * If a timeout occurs, then nothing happens and this method returns. + */ + public static void spinOnce(final ComposableNode composableNode, long timeout) { getGlobalExecutor().addNode(composableNode); - getGlobalExecutor().spinOnce(); + getGlobalExecutor().spinOnce(timeout); getGlobalExecutor().removeNode(composableNode); } + /** + * Execute one callback for a @{link ComposableNode}. + * + * Wait until a callback becomes ready and then execute it. + * + * @param composableNode The composable node to spin on. + */ + public static void spinOnce(final ComposableNode composableNode) { + RCLJava.spinOnce(composableNode, -1); + } public static void spinSome(final Node node) { ComposableNode composableNode = new ComposableNode() { @@ -292,6 +361,34 @@ public static void spinSome(final ComposableNode composableNode) { getGlobalExecutor().removeNode(composableNode); } + public static void spinUntilComplete(final Node node, final Future future, long timeoutNs) { + ComposableNode composableNode = new ComposableNode() { + public Node getNode() { + return node; + } + }; + RCLJava.spinUntilComplete(composableNode, future, timeoutNs); + } + + public static void spinUntilComplete(final Node node, final Future future) + { + RCLJava.spinUntilComplete(node, future, -1); + } + + public static void spinUntilComplete( + final ComposableNode node, final Future future, long timeoutNs) + { + getGlobalExecutor().addNode(node); + getGlobalExecutor().spinUntilComplete(future, timeoutNs); + getGlobalExecutor().removeNode(node); + } + + public static void spinUntilComplete( + final ComposableNode node, final Future future) + { + RCLJava.spinUntilComplete(node, future, -1); + } + public static synchronized void shutdown() { cleanup(); if (RCLJava.defaultContext != null) { diff --git a/rcljava/src/main/java/org/ros2/rcljava/Time.java b/rcljava/src/main/java/org/ros2/rcljava/Time.java index 4bca10af..02d517ed 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/Time.java +++ b/rcljava/src/main/java/org/ros2/rcljava/Time.java @@ -35,56 +35,45 @@ public final class Time { } } - /** - * Private constructor so this cannot be instantiated. - */ - private Time() {} + private final ClockType clockType; - public static builtin_interfaces.msg.Time now() { - return Time.now(ClockType.SYSTEM_TIME); - } - - public static builtin_interfaces.msg.Time now(ClockType clockType) { - long rclTime = 0; - - switch (clockType) { - case ROS_TIME: - throw new UnsupportedOperationException("ROS Time is currently not implemented"); - case SYSTEM_TIME: - rclTime = rclSystemTimeNow(); - break; - case STEADY_TIME: - rclTime = rclSteadyTimeNow(); - break; - } + private final long nanoseconds; - builtin_interfaces.msg.Time msgTime = new builtin_interfaces.msg.Time(); - msgTime.setSec((int) TimeUnit.SECONDS.convert(rclTime, TimeUnit.NANOSECONDS)); - msgTime.setNanosec((int) rclTime % (1000 * 1000 * 1000)); - return msgTime; + public Time() { + this(0, 0, ClockType.SYSTEM_TIME); } - public static long toNanoseconds(builtin_interfaces.msg.Time msgTime) { - return TimeUnit.NANOSECONDS.convert(msgTime.getSec(), TimeUnit.SECONDS) - + (msgTime.getNanosec() & 0x00000000ffffffffL); + public Time(final long nanos, final ClockType ct) { + this(0, nanos, ct); } - public static long difference( - builtin_interfaces.msg.Time msgTime1, builtin_interfaces.msg.Time msgTime2) { - long difference = - (Time.toNanoseconds(msgTime1) - Time.toNanoseconds(msgTime2)) & 0x00000000ffffffffL; - return difference; + public Time(final builtin_interfaces.msg.Time time_msg, final ClockType ct) { + this(time_msg.getSec(), time_msg.getNanosec(), ct); } - private static long rclSystemTimeNow() { - return nativeRCLSystemTimeNow(); // new RuntimeException("Could not get current time"); + public Time(final long secs, final long nanos, final ClockType ct) { + if (secs < 0 || nanos < 0) { + // TODO(clalancette): Make this a custom exception + throw new IllegalArgumentException("seconds and nanoseconds must not be negative"); + } + this.clockType = ct; + this.nanoseconds = TimeUnit.SECONDS.toNanos(secs) + nanos; } - private static long rclSteadyTimeNow() { - return nativeRCLSteadyTimeNow(); // new RuntimeException("Could not get current time"); + public builtin_interfaces.msg.Time toMsg() { + long seconds = this.nanoseconds / 1000000000; + long nanos = this.nanoseconds % 1000000000; + builtin_interfaces.msg.Time msg = new builtin_interfaces.msg.Time(); + msg.setSec((int)seconds); + msg.setNanosec((int)nanos); + return msg; } - private static native long nativeRCLSystemTimeNow(); + public long nanoseconds() { + return nanoseconds; + } - private static native long nativeRCLSteadyTimeNow(); + public ClockType clockType() { + return clockType; + } } 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..6f207319 --- /dev/null +++ b/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerGoalHandle.java @@ -0,0 +1,89 @@ +/* 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.GoalDefinition; +import org.ros2.rcljava.interfaces.FeedbackDefinition; +import org.ros2.rcljava.interfaces.ResultDefinition; + +public interface ActionServerGoalHandle extends Disposable { + /** + * Get the action result type. + */ + public Class getResultType(); + + /** + * Get the action feedback type. + */ + public Class getFeedbackType(); + + /** + * Get the message containing the timestamp and ID for the goal. + */ + public action_msgs.msg.GoalInfo getGoalInfo(); + + /** + * Get the goal message. + */ + public GoalDefinition 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 EXECUTING state. + * + * Pre-condition: the goal must be in the ACCEPTED state. + */ + public void execute(); + + /** + * Transition the goal to the SUCCEEDED state. + * + * Pre-condition: the goal must be in the EXECUTING or CANCELING state. + */ + public void succeed(ResultDefinition result); + + /** + * Transition the goal the the CANCELED state. + * + * Pre-condition: the goal must be in the CANCELING state. + */ + public void canceled(ResultDefinition result); + + /** + * Transition the goal the the CANCELED state. + * + * Pre-condition: the goal must be in the EXCUTING or CANCELING state. + */ + public void abort(ResultDefinition result); + + /** + * Send an update about the progress of a goal. + * + * Pre-condition: the goal must be in the EXCUTING state. + */ + public void publishFeedback(FeedbackDefinition result); +} 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..bd6e5f2d --- /dev/null +++ b/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerImpl.java @@ -0,0 +1,756 @@ +/* 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.lang.SuppressWarnings; +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.GoalDefinition; +import org.ros2.rcljava.interfaces.GoalRequestDefinition; +import org.ros2.rcljava.interfaces.GoalResponseDefinition; +import org.ros2.rcljava.interfaces.FeedbackDefinition; +import org.ros2.rcljava.interfaces.FeedbackMessageDefinition; +import org.ros2.rcljava.interfaces.ResultDefinition; +import org.ros2.rcljava.interfaces.ResultRequestDefinition; +import org.ros2.rcljava.interfaces.ResultResponseDefinition; +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 action_msgs.msg.GoalInfo goalInfo; + private GoalDefinition goal; + + private native long nativeAcceptNewGoal( + long actionServerHandle, + long goalInfoFromJavaConverterHandle, + long goalInfoDestructorHandle, + MessageDefinition goalInfo); + private native int nativeGetStatus(long goalHandle); + private native void nativeUpdateGoalState(long goalHandle, long event); + private native void nativeDispose(long handle); + + public GoalHandleImpl( + action_msgs.msg.GoalInfo goalInfo, GoalDefinition goal) + { + this.goalInfo = goalInfo; + this.goal = goal; + long goalInfoFromJavaConverterHandle = goalInfo.getFromJavaConverterInstance(); + long goalInfoDestructorHandle = goalInfo.getDestructorInstance(); + this.handle = nativeAcceptNewGoal( + ActionServerImpl.this.getHandle(), + goalInfoFromJavaConverterHandle, + goalInfoDestructorHandle, + goalInfo); + } + + /** + * {@inheritDoc} + */ + public Class getResultType() { + return ActionServerImpl.this.actionTypeInstance.getResultType(); + } + + /** + * {@inheritDoc} + */ + public Class getFeedbackType() { + return ActionServerImpl.this.actionTypeInstance.getFeedbackType(); + } + + /** + * {@inheritDoc} + */ + public action_msgs.msg.GoalInfo getGoalInfo() { + return this.goalInfo; + } + + /** + * {@inheritDoc} + */ + public GoalDefinition 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()) { + nativeUpdateGoalState(this.handle, action_msgs.msg.GoalStatus.STATUS_EXECUTING); + } + } + + /** + * Transition the goal to the CANCELING state. + */ + public synchronized void cancelGoal() { + nativeUpdateGoalState(this.handle, action_msgs.msg.GoalStatus.STATUS_CANCELING); + } + + /** + * {@inheritDoc} + */ + public synchronized void succeed(ResultDefinition result) { + this.toTerminalState(action_msgs.msg.GoalStatus.STATUS_SUCCEEDED, result); + } + + /** + * {@inheritDoc} + */ + public synchronized void canceled(ResultDefinition result) { + this.toTerminalState(action_msgs.msg.GoalStatus.STATUS_CANCELED, result); + } + + /** + * {@inheritDoc} + */ + public synchronized void abort(ResultDefinition result) { + this.toTerminalState(action_msgs.msg.GoalStatus.STATUS_ABORTED, result); + } + + /** + * {@inheritDoc} + */ + // TODO(ivanpauno): Improve generated code API so we don't need this. + @SuppressWarnings("unchecked") + public synchronized void publishFeedback(FeedbackDefinition feedback) { + Class feedbackMessageType = + ActionServerImpl.this.actionTypeInstance.getFeedbackMessageType(); + FeedbackMessageDefinition feedbackMessage = null; + try { + feedbackMessage = feedbackMessageType.getDeclaredConstructor().newInstance(); + } catch (ReflectiveOperationException ex) { + throw new IllegalArgumentException("Failed to instantiate feedback message: ", ex); + } + feedbackMessage.setFeedback(feedback); + feedbackMessage.setGoalUuid(this.goalInfo.getGoalId().getUuidAsList()); + ActionServerImpl.this.publishFeedbackMessage(feedbackMessage); + } + + /** + * {@inheritDoc} + */ + public synchronized final void dispose() { + nativeDispose(this.handle); + this.handle = 0; + } + + /** + * {@inheritDoc} + */ + public final long getHandle() { + return this.handle; + } + + private synchronized final void toTerminalState(byte status, ResultDefinition result) { + nativeUpdateGoalState(this.handle, status); + ResultResponseDefinition resultResponse = ActionServerImpl.this.createResultResponseUnchecked(); + resultResponse.setGoalStatus(status); + resultResponse.setResult(result); + ActionServerImpl.this.sendResult(goalInfo.getGoalId().getUuidAsList(), resultResponse); + ActionServerImpl.this.publishStatus(); + ActionServerImpl.this.notifyGoalDone(); + ActionServerImpl.this.goalHandles.remove(this.goalInfo.getGoalId().getUuidAsList()); + } + } // 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 Map, List> goalRequests; + private Map, ResultResponseDefinition> goalResults; + + 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.getDeclaredConstructor().newInstance(); + } catch (ReflectiveOperationException 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>(); + this.goalRequests = new HashMap, List>(); + this.goalResults = new HashMap, ResultResponseDefinition>(); + + 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; + } + + @SuppressWarnings("unchecked") + private GoalCallback.GoalResponse + handleGoalUnchecked(GoalRequestDefinition requestMessage) { + return this.goalCallback.handleGoal(requestMessage); + } + + 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 = this.createGoalInfo(goalUuid); + // Check that the goal ID isn't already being used + boolean goalExists = this.goalExists(goalInfo); + 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.handleGoalUnchecked(requestMessage); + + boolean accepted = GoalCallback.GoalResponse.ACCEPT_AND_DEFER == response + || GoalCallback.GoalResponse.ACCEPT_AND_EXECUTE == response; + responseMessage.accept(accepted); + + if (!accepted) { + return null; + } + + // Create a goal handle and add it to the list of goals + GoalHandleImpl goalHandle = this.new GoalHandleImpl( + goalInfo, requestMessage.getGoal()); + this.goalHandles.put(requestMessage.getGoalUuid(), goalHandle); + if (GoalCallback.GoalResponse.ACCEPT_AND_EXECUTE == response) { + goalHandle.execute(); + this.acceptedCallback.accept(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 void sendResultResponse( + RMWRequestId rmwRequestId, + ResultResponseDefinition resultResponse) + { + long resultFromJavaConverterHandle = resultResponse.getFromJavaConverterInstance(); + long resultDestructorHandle = resultResponse.getDestructorInstance(); + + nativeSendResultResponse( + this.handle, + rmwRequestId, + resultFromJavaConverterHandle, + resultDestructorHandle, + resultResponse); + } + + 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 responseDestructorHandle, + MessageDefinition responseMessage); + + private static native void nativeSendCancelResponse( + long actionServerHandle, + RMWRequestId header, + long responseFromJavaConverterHandle, + long responseDestructorHandle, + MessageDefinition responseMessage); + + private static native void nativeSendResultResponse( + long actionServerHandle, + RMWRequestId header, + long responseFromJavaConverterHandle, + 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); + + private boolean goalExists(action_msgs.msg.GoalInfo goalInfo) { + long goalInfoFromJavaConverterHandle = goalInfo.getFromJavaConverterInstance(); + long goalInfoDestructorHandle = goalInfo.getDestructorInstance(); + + return nativeCheckGoalExists( + this.handle, goalInfo, goalInfoFromJavaConverterHandle, goalInfoDestructorHandle); + } + + private static native void nativePublishStatus(long handle); + + private void publishStatus() { + this.nativePublishStatus(this.handle); + } + + private static native void nativePublishFeedbackMessage( + long handle, + FeedbackMessageDefinition feedbackMessage, + long feedbackMessageFromJavaConverterHandle, + long feedbackMessageDestructorHandle); + + private void publishFeedbackMessage(FeedbackMessageDefinition feedbackMessage) { + this.nativePublishFeedbackMessage( + this.handle, + feedbackMessage, + feedbackMessage.getFromJavaConverterInstance(), + feedbackMessage.getDestructorInstance()); + } + + private static native void nativeNotifyGoalDone(long handle); + + private void notifyGoalDone() { + this.nativeNotifyGoalDone(this.handle); + } + + private static native void nativeExpireGoals( + long handle, action_msgs.msg.GoalInfo goalInfo, + long goalInfoToJavaConverterHandle, Consumer onExpiredGoal); + + private void expireGoals() { + action_msgs.msg.GoalInfo goalInfo = new action_msgs.msg.GoalInfo(); + long goalInfoToJavaConverterHandle = goalInfo.getFromJavaConverterInstance(); + nativeExpireGoals( + this.handle, goalInfo, goalInfoToJavaConverterHandle, + new Consumer() { + public void accept(action_msgs.msg.GoalInfo goalInfo) { + List goalUuid = goalInfo.getGoalId().getUuidAsList(); + ActionServerImpl.this.goalResults.remove(goalUuid); + ActionServerImpl.this.goalRequests.remove(goalUuid); + ActionServerImpl.this.goalHandles.remove(goalUuid); + } + }); + } + + private action_msgs.msg.GoalInfo createGoalInfo(List goalUuid) { + 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); + return goalInfo; + } + + // This will store the result, so it can be sent to future result requests, and + // will also send a result response to all requests that were already made. + private void sendResult(List goalUuid, ResultResponseDefinition resultResponse) { + boolean goalExists = this.goalExists(this.createGoalInfo(goalUuid)); + if (!goalExists) { + throw new IllegalStateException("Asked to publish result for goal that does not exist"); + } + this.goalResults.put(goalUuid, resultResponse); + + // if there are clients who already asked for the result, send it to them + List requests = this.goalRequests.get(goalUuid); + if (requests != null) { + for (RMWRequestId request : requests) { + this.sendResultResponse(request, resultResponse); + } + } + } + + // TODO(ivanpauno): Improve generated code API so we don't need this. + @SuppressWarnings("unchecked") + private ResultRequestDefinition createResultRequestUnchecked() { + ResultRequestDefinition resultRequest; + try { + resultRequest = + this.actionTypeInstance.getGetResultRequestType().getDeclaredConstructor().newInstance(); + } catch (ReflectiveOperationException ex) { + throw new IllegalStateException("Failed to instantiate provided action type: ", ex); + } + return resultRequest; + } + + // TODO(ivanpauno): Improve generated code API so we don't need this. + @SuppressWarnings("unchecked") + private ResultResponseDefinition createResultResponseUnchecked() { + ResultResponseDefinition resultResponse; + try { + resultResponse = + this.actionTypeInstance.getGetResultResponseType().getDeclaredConstructor().newInstance(); + } catch (ReflectiveOperationException ex) { + throw new IllegalStateException("Failed to instantiate provided action type: ", ex); + } + return resultResponse; + } + + // TODO(ivanpauno): Improve generated code API so we don't need this. + @SuppressWarnings("unchecked") + private GoalRequestDefinition newRequestUnchecked() { + Class requestType = this.actionTypeInstance.getSendGoalRequestType(); + try { + return requestType.getDeclaredConstructor().newInstance(); + } catch (ReflectiveOperationException ex) { + throw new IllegalStateException("Failed to instantiate request: ", ex); + } + } + + // TODO(ivanpauno): Improve generated code API so we don't need this. + @SuppressWarnings("unchecked") + private GoalResponseDefinition newResponseUnchecked() { + Class responseType = this.actionTypeInstance.getSendGoalResponseType(); + try { + return responseType.getDeclaredConstructor().newInstance(); + } catch (ReflectiveOperationException ex) { + throw new IllegalStateException("Failed to instantiate responce: ", ex); + } + } + + /** + * {@inheritDoc} + */ + public void execute() { + if (this.isGoalRequestReady()) { + GoalRequestDefinition requestMessage = newRequestUnchecked(); + GoalResponseDefinition responseMessage = newResponseUnchecked(); + + if (requestMessage != null && responseMessage != null) { + long requestFromJavaConverterHandle = requestMessage.getFromJavaConverterInstance(); + long requestToJavaConverterHandle = requestMessage.getToJavaConverterInstance(); + long requestDestructorHandle = requestMessage.getDestructorInstance(); + long responseFromJavaConverterHandle = responseMessage.getFromJavaConverterInstance(); + 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, + responseDestructorHandle, responseMessage); + } + } + } + + 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, responseDestructorHandle, responseMessage); + } + } + + if (this.isResultRequestReady()) { + ResultRequestDefinition requestMessage = createResultRequestUnchecked(); + + if (requestMessage != null) { + long requestFromJavaConverterHandle = requestMessage.getFromJavaConverterInstance(); + long requestToJavaConverterHandle = requestMessage.getToJavaConverterInstance(); + long requestDestructorHandle = requestMessage.getDestructorInstance(); + + RMWRequestId rmwRequestId = + nativeTakeResultRequest( + this.handle, + requestFromJavaConverterHandle, requestToJavaConverterHandle, requestDestructorHandle, + requestMessage); + + if (rmwRequestId == null) { + return; + } + + List goalUuid = requestMessage.getGoalUuid(); + boolean goalExists = this.goalExists(this.createGoalInfo(goalUuid)); + + ResultResponseDefinition resultResponse = null; + if (!goalExists) { + resultResponse = this.createResultResponseUnchecked(); + resultResponse.setGoalStatus(action_msgs.msg.GoalStatus.STATUS_UNKNOWN); + } else { + resultResponse = this.goalResults.get(goalUuid); + } + + if (null == resultResponse) { + List requestIds = null; + requestIds = this.goalRequests.get(goalUuid); + if (requestIds == null) { + requestIds = new ArrayList(); + this.goalRequests.put(goalUuid, requestIds); + } + requestIds.add(rmwRequestId); + } else { + this.sendResultResponse(rmwRequestId, resultResponse); + } + } + } + + if (this.isGoalExpiredReady()) { + this.expireGoals(); + } + } + + /** + * 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..a448ac82 --- /dev/null +++ b/rcljava/src/main/java/org/ros2/rcljava/action/GoalCallback.java @@ -0,0 +1,34 @@ +/* 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_AND_EXECUTE, + ACCEPT_AND_DEFER, + }; + + /** + * 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..abacc6df --- /dev/null +++ b/rcljava/src/main/java/org/ros2/rcljava/action/GoalStatus.java @@ -0,0 +1,46 @@ +/* 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(action_msgs.msg.GoalStatus.STATUS_UNKNOWN), + ACCEPTED(action_msgs.msg.GoalStatus.STATUS_ACCEPTED), + EXECUTING(action_msgs.msg.GoalStatus.STATUS_EXECUTING), + CANCELING(action_msgs.msg.GoalStatus.STATUS_CANCELING), + SUCCEEDED(action_msgs.msg.GoalStatus.STATUS_SUCCEEDED), + CANCELED(action_msgs.msg.GoalStatus.STATUS_CANCELED), + ABORTED(action_msgs.msg.GoalStatus.STATUS_ABORTED); + + public static GoalStatus fromMessageValue(byte value) { + for (GoalStatus status: GoalStatus.values()) { + if (status.status == value) { + return status; + } + } + return GoalStatus.ABORTED; + } + + public byte toMessageValue() { + return this.status; + } + + private GoalStatus(byte status) { + this.status = status; + } + + private byte status; +} + diff --git a/rcljava/src/main/java/org/ros2/rcljava/client/Client.java b/rcljava/src/main/java/org/ros2/rcljava/client/Client.java index 774ecec6..2e7f8f95 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/client/Client.java +++ b/rcljava/src/main/java/org/ros2/rcljava/client/Client.java @@ -26,17 +26,31 @@ import org.ros2.rcljava.service.RMWRequestId; public interface Client extends Disposable { - Class getRequestType(); - - Class getResponseType(); + ServiceDefinition getServiceDefinition(); void handleResponse(RMWRequestId header, U response); - Future asyncSendRequest( + ResponseFuture asyncSendRequest( final U request); - Future asyncSendRequest( + ResponseFuture asyncSendRequest( final U request, final Consumer> callback); + + boolean removePendingRequest(ResponseFuture future); + + /** + * Remove old pending requests that this client has done. + * Future responses to removed requests will be ignored. + */ + long prunePendingRequests(); + + /** + * Remove old pending requests that where done before the specified time point. + * Future responses to removed requests will be ignored. + * + * @param nanoTime requests done before this timepoint will be removed. + */ + long prunePendingRequestsOlderThan(long nanoTime); /** * Check if the service server is available. diff --git a/rcljava/src/main/java/org/ros2/rcljava/client/ClientImpl.java b/rcljava/src/main/java/org/ros2/rcljava/client/ClientImpl.java index 70ff3074..507dd71e 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/client/ClientImpl.java +++ b/rcljava/src/main/java/org/ros2/rcljava/client/ClientImpl.java @@ -22,13 +22,13 @@ import java.lang.Long; import java.util.AbstractMap; import java.util.HashMap; +import java.util.Iterator; import java.util.Map; import java.util.concurrent.Future; import java.util.concurrent.TimeUnit; import org.ros2.rcljava.RCLJava; import org.ros2.rcljava.common.JNIUtils; -import org.ros2.rcljava.concurrent.RCLFuture; import org.ros2.rcljava.consumers.Consumer; import org.ros2.rcljava.interfaces.MessageDefinition; import org.ros2.rcljava.interfaces.ServiceDefinition; @@ -53,52 +53,107 @@ public class ClientImpl implements Client { private final WeakReference nodeReference; private long handle; private final String serviceName; - private Map> pendingRequests; - private final Class requestType; - private final Class responseType; + private class PendingRequest + { + public Consumer callback; + public ResponseFuture future; + public long requestTimestamp; - public ClientImpl(final WeakReference nodeReference, final long handle, - final String serviceName, final Class requestType, - final Class responseType) { + public PendingRequest( + final Consumer callback, + final ResponseFuture future, + final long requestTimestamp) + { + this.callback = callback; + this.future = future; + this.requestTimestamp = requestTimestamp; + } + } + + private Map pendingRequests; + + private final ServiceDefinition serviceDefinition; + + public ClientImpl( + final ServiceDefinition serviceDefinition, + final WeakReference nodeReference, + final long handle, + final String serviceName) + { this.nodeReference = nodeReference; this.handle = handle; this.serviceName = serviceName; - this.requestType = requestType; - this.responseType = responseType; - this.pendingRequests = new HashMap>(); + this.serviceDefinition = serviceDefinition; + this.pendingRequests = new HashMap(); + } + + public ServiceDefinition getServiceDefinition() { + return this.serviceDefinition; } - public final Future + public final ResponseFuture asyncSendRequest(final U request) { return asyncSendRequest(request, new Consumer>() { public void accept(Future input) {} }); } - public final Future + public final ResponseFuture asyncSendRequest(final U request, final Consumer> callback) { synchronized (pendingRequests) { long sequenceNumber = nativeSendClientRequest( handle, request.getFromJavaConverterInstance(), - request.getToJavaConverterInstance(), request.getDestructorInstance(), request); - RCLFuture future = new RCLFuture(this.nodeReference); + request.getDestructorInstance(), request); + ResponseFuture future = new ResponseFuture(sequenceNumber); - Map.Entry entry = - new AbstractMap.SimpleEntry(callback, future); + PendingRequest entry = new PendingRequest(callback, future, System.nanoTime()); pendingRequests.put(sequenceNumber, entry); return future; } } + public final boolean + removePendingRequest(ResponseFuture future) { + synchronized (pendingRequests) { + PendingRequest entry = pendingRequests.remove( + future.getRequestSequenceNumber()); + return entry != null; + } + } + + public final long + prunePendingRequests() { + synchronized (pendingRequests) { + long size = pendingRequests.size(); + pendingRequests.clear(); + return size; + } + } + + public final long + prunePendingRequestsOlderThan(long nanoTime) { + synchronized (pendingRequests) { + Iterator> iter = pendingRequests.entrySet().iterator(); + long removed = 0; + while(iter.hasNext()) { + if(iter.next().getValue().requestTimestamp < nanoTime) { + iter.remove(); + ++removed; + } + } + return removed; + } + } + public final void handleResponse( final RMWRequestId header, final U response) { synchronized (pendingRequests) { long sequenceNumber = header.sequenceNumber; - Map.Entry entry = pendingRequests.remove(sequenceNumber); + PendingRequest entry = pendingRequests.remove(sequenceNumber); if (entry != null) { - Consumer callback = entry.getKey(); - RCLFuture future = entry.getValue(); + Consumer callback = entry.callback; + ResponseFuture future = entry.future; future.set(response); callback.accept(future); return; @@ -109,16 +164,8 @@ public final void handleResponse( } private static native long nativeSendClientRequest( - long handle, long requestFromJavaConverterHandle, long requestToJavaConverterHandle, - long requestDestructorHandle, MessageDefinition requestMessage); - - public final Class getRequestType() { - return this.requestType; - } - - public final Class getResponseType() { - return this.responseType; - } + long handle, long requestFromJavaConverterHandle, long requestDestructorHandle, + MessageDefinition requestMessage); /** * Destroy a ROS2 client (rcl_client_t). @@ -135,11 +182,13 @@ public final Class getResponseType() { */ public final void dispose() { Node node = this.nodeReference.get(); - if (node != null) { - node.removeClient(this); - nativeDispose(node.getHandle(), this.handle); - this.handle = 0; + if (node == null) { + logger.error("Node reference is null. Failed to dispose of Client."); + return; } + node.removeClient(this); + nativeDispose(node.getHandle(), this.handle); + this.handle = 0; } /** diff --git a/rcljava/src/main/java/org/ros2/rcljava/client/ResponseFuture.java b/rcljava/src/main/java/org/ros2/rcljava/client/ResponseFuture.java new file mode 100644 index 00000000..9c89fbef --- /dev/null +++ b/rcljava/src/main/java/org/ros2/rcljava/client/ResponseFuture.java @@ -0,0 +1,29 @@ +/* Copyright 2021 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.client; + +import org.ros2.rcljava.concurrent.RCLFuture; + + +public class ResponseFuture extends RCLFuture { + public ResponseFuture(long requestSequenceNumber) { + this.requestSequenceNumber = requestSequenceNumber; + } + public long getRequestSequenceNumber() { + return this.requestSequenceNumber; + } + private long requestSequenceNumber; +} diff --git a/rcljava/src/main/java/org/ros2/rcljava/concurrent/RCLFuture.java b/rcljava/src/main/java/org/ros2/rcljava/concurrent/RCLFuture.java index 1ac88b55..2b21891f 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/concurrent/RCLFuture.java +++ b/rcljava/src/main/java/org/ros2/rcljava/concurrent/RCLFuture.java @@ -15,6 +15,7 @@ package org.ros2.rcljava.concurrent; +import java.lang.Deprecated; import java.lang.ref.WeakReference; import java.util.concurrent.ExecutionException; import java.util.concurrent.Future; @@ -22,73 +23,50 @@ import java.util.concurrent.TimeoutException; import org.ros2.rcljava.RCLJava; -import org.ros2.rcljava.executors.Executor; import org.ros2.rcljava.node.Node; public class RCLFuture implements Future { private WeakReference nodeReference; private boolean done = false; private V value = null; - private Executor executor = null; - public RCLFuture(final WeakReference nodeReference) { - this.nodeReference = nodeReference; - } - - public RCLFuture(final Executor executor) { - this.executor = executor; - } + public RCLFuture() {} - public final V get() throws InterruptedException, ExecutionException { + public final synchronized V get() throws InterruptedException, ExecutionException { if(this.value != null) { return this.value; } while (RCLJava.ok() && !isDone()) { - if (executor != null) { - executor.spinOnce(); - } else { - Node node = nodeReference.get(); - if (node == null) { - return null; // TODO(esteve) do something - } - RCLJava.spinOnce(node); - } + this.wait(); } return this.value; } - public final V get(final long timeout, final TimeUnit unit) + public final synchronized V get(final long timeout, final TimeUnit unit) throws InterruptedException, ExecutionException, TimeoutException { if (isDone()) { return value; } - long endTime = TimeUnit.NANOSECONDS.convert(System.currentTimeMillis(), TimeUnit.MILLISECONDS); - - long timeoutNS = TimeUnit.NANOSECONDS.convert(timeout, unit); + long endTime = System.nanoTime(); + long timeoutNS = unit.toNanos(timeout); if (timeoutNS > 0) { endTime += timeoutNS; } while (RCLJava.ok()) { - if (executor != null) { - executor.spinOnce(); - } else { - Node node = nodeReference.get(); - if (node == null) { - return null; // TODO(esteve) do something - } - RCLJava.spinOnce(node); - } + this.wait(TimeUnit.NANOSECONDS.toMillis(timeoutNS), (int) (timeoutNS % 1000000l)); if (isDone()) { return value; } - long now = TimeUnit.NANOSECONDS.convert(System.currentTimeMillis(), TimeUnit.MILLISECONDS); + long now = System.nanoTime(); if (now >= endTime) { throw new TimeoutException(); + } else { + timeoutNS = endTime - now; } } throw new InterruptedException(); @@ -109,5 +87,6 @@ public final boolean cancel(final boolean mayInterruptIfRunning) { public final synchronized void set(final V value) { this.value = value; done = true; + this.notify(); } } diff --git a/rcljava/src/main/java/org/ros2/rcljava/contexts/Context.java b/rcljava/src/main/java/org/ros2/rcljava/contexts/Context.java index 717eea77..3a458cb1 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/contexts/Context.java +++ b/rcljava/src/main/java/org/ros2/rcljava/contexts/Context.java @@ -32,11 +32,15 @@ public interface Context extends Disposable { /** * Initialize the context. - * // TODO(jacobperron): Pass arguments for parsing * // TODO(jacobperron): Pass in InitOptions object */ void init(); + /** + * Initialize the context passing command line arguments. + */ + void init(String args[]); + /** * Shutdown the context. */ diff --git a/rcljava/src/main/java/org/ros2/rcljava/contexts/ContextImpl.java b/rcljava/src/main/java/org/ros2/rcljava/contexts/ContextImpl.java index 3d6bf9f0..d39e09d0 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/contexts/ContextImpl.java +++ b/rcljava/src/main/java/org/ros2/rcljava/contexts/ContextImpl.java @@ -61,10 +61,14 @@ public ContextImpl(final long handle) { * {@inheritDoc} */ public final void dispose() { - // Ensure the context is shutdown first - shutdown(); - nativeDispose(this.handle); - this.handle = 0; + if (0 != this.handle) { + // Ensure the context is shutdown first. + if (this.isValid()) { + shutdown(); + } + nativeDispose(this.handle); + this.handle = 0; + } } /** @@ -79,13 +83,21 @@ public final long getHandle() { * * @param contextHandle The pointer to the context structure. */ - private static native void nativeInit(long contextHandle); + private static native void nativeInit(long contextHandle, String args[]); /** * {@inheritDoc} */ public final void init() { - nativeInit(this.handle); + String args[] = {}; + nativeInit(this.handle, args); + } + + /** + * {@inheritDoc} + */ + public final void init(String args[]) { + nativeInit(this.handle, args); } /** 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 1ed9218e..e500f9ac 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/executors/BaseExecutor.java +++ b/rcljava/src/main/java/org/ros2/rcljava/executors/BaseExecutor.java @@ -15,26 +15,31 @@ package org.ros2.rcljava.executors; +import java.lang.Math; +import java.lang.SuppressWarnings; import java.util.AbstractMap; import java.util.ArrayList; import java.util.Iterator; import java.util.List; import java.util.Map; - import java.util.Collection; import java.util.concurrent.BlockingQueue; import java.util.concurrent.ConcurrentHashMap; +import java.util.concurrent.Future; import java.util.concurrent.LinkedBlockingQueue; + import org.slf4j.Logger; 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 +74,8 @@ public class BaseExecutor { private List> eventHandles = new ArrayList>(); + private List> actionServerHandles = new ArrayList>(); + protected void addNode(ComposableNode node) { this.nodes.add(node); } @@ -77,6 +84,23 @@ protected void removeNode(ComposableNode node) { this.nodes.remove(node); } + @SuppressWarnings("unchecked") + protected static void executeSubscriptionCallbackUnchecked( + Subscription subscription, + MessageDefinition message) + { + subscription.executeCallback(message); + } + + @SuppressWarnings("unchecked") + protected static void clientHandleResponseUnchecked( + Client client, + RMWRequestId rmwRequestId, + MessageDefinition response) + { + client.handleResponse(rmwRequestId, response); + } + protected void executeAnyExecutable(AnyExecutable anyExecutable) { if (anyExecutable.timer != null) { anyExecutable.timer.callTimer(); @@ -88,26 +112,17 @@ protected void executeAnyExecutable(AnyExecutable anyExecutable) { MessageDefinition message = nativeTake( anyExecutable.subscription.getHandle(), anyExecutable.subscription.getMessageType()); if (message != null) { - anyExecutable.subscription.executeCallback(message); + // Safety: nativeTake() will return the correct type here. + // We can't do much better here, as subscriptions are type erased. + executeSubscriptionCallbackUnchecked(anyExecutable.subscription, message); } subscriptionHandles.remove(anyExecutable.subscription.getHandle()); } if (anyExecutable.service != null) { - Class requestType = anyExecutable.service.getRequestType(); - Class responseType = anyExecutable.service.getResponseType(); - - MessageDefinition requestMessage = null; - MessageDefinition responseMessage = null; - - try { - requestMessage = requestType.newInstance(); - responseMessage = responseType.newInstance(); - } catch (InstantiationException ie) { - ie.printStackTrace(); - } catch (IllegalAccessException iae) { - iae.printStackTrace(); - } + ServiceDefinition serviceDefinition = anyExecutable.service.getServiceDefinition(); + MessageDefinition requestMessage = serviceDefinition.newRequestInstance(); + MessageDefinition responseMessage = serviceDefinition.newResponseInstance(); if (requestMessage != null && responseMessage != null) { long requestFromJavaConverterHandle = requestMessage.getFromJavaConverterInstance(); @@ -118,37 +133,23 @@ protected void executeAnyExecutable(AnyExecutable anyExecutable) { long responseDestructorHandle = responseMessage.getDestructorInstance(); RMWRequestId rmwRequestId = - nativeTakeRequest(anyExecutable.service.getHandle(), requestFromJavaConverterHandle, - requestToJavaConverterHandle, requestDestructorHandle, requestMessage); + nativeTakeRequest(anyExecutable.service.getHandle(), requestFromJavaConverterHandle, + requestToJavaConverterHandle, requestDestructorHandle, requestMessage); if (rmwRequestId != null) { anyExecutable.service.executeCallback(rmwRequestId, requestMessage, responseMessage); - nativeSendServiceResponse(anyExecutable.service.getHandle(), rmwRequestId, - responseFromJavaConverterHandle, responseToJavaConverterHandle, - responseDestructorHandle, responseMessage); + nativeSendServiceResponse( + anyExecutable.service.getHandle(), rmwRequestId, + responseFromJavaConverterHandle, responseDestructorHandle, responseMessage); } } serviceHandles.remove(anyExecutable.service.getHandle()); } if (anyExecutable.client != null) { - Class requestType = anyExecutable.client.getRequestType(); - Class responseType = anyExecutable.client.getResponseType(); - - MessageDefinition requestMessage = null; - MessageDefinition responseMessage = null; - - try { - requestMessage = requestType.newInstance(); - responseMessage = responseType.newInstance(); - } catch (InstantiationException ie) { - ie.printStackTrace(); - } catch (IllegalAccessException iae) { - iae.printStackTrace(); - } + ServiceDefinition serviceDefinition = anyExecutable.client.getServiceDefinition(); + MessageDefinition responseMessage = serviceDefinition.newResponseInstance(); - if (requestMessage != null && responseMessage != null) { - long requestFromJavaConverterHandle = requestMessage.getFromJavaConverterInstance(); - long requestToJavaConverterHandle = requestMessage.getToJavaConverterInstance(); + if (responseMessage != null) { long responseFromJavaConverterHandle = responseMessage.getFromJavaConverterInstance(); long responseToJavaConverterHandle = responseMessage.getToJavaConverterInstance(); long responseDestructorHandle = responseMessage.getDestructorInstance(); @@ -158,7 +159,9 @@ protected void executeAnyExecutable(AnyExecutable anyExecutable) { responseToJavaConverterHandle, responseDestructorHandle, responseMessage); if (rmwRequestId != null) { - anyExecutable.client.handleResponse(rmwRequestId, responseMessage); + // Safety: nativeTakeResponse() will return the correct type here. + // We can't do much better here, as subscriptions are type erased. + clientHandleResponseUnchecked(anyExecutable.client, rmwRequestId, responseMessage); } } clientHandles.remove(anyExecutable.client.getHandle()); @@ -168,6 +171,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,9 +184,10 @@ 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()) { + for (Subscription subscription : node.getNode().getSubscriptions()) { this.subscriptionHandles.add(new AbstractMap.SimpleEntry( subscription.getHandle(), subscription)); Collection eventHandlers = subscription.getEventHandlers(); @@ -200,15 +209,20 @@ protected void waitForWork(long timeout) { this.timerHandles.add(new AbstractMap.SimpleEntry(timer.getHandle(), timer)); } - for (Service service : node.getNode().getServices()) { + for (Service service : node.getNode().getServices()) { this.serviceHandles.add( new AbstractMap.SimpleEntry(service.getHandle(), service)); } - for (Client client : node.getNode().getClients()) { + for (Client client : node.getNode().getClients()) { 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 +236,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 +277,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 +313,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 +360,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 +417,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; } @@ -394,6 +441,30 @@ private boolean maxDurationNotElapsed(long maxDurationNs, long startNs) { return false; } + public void spinUntilComplete(Future future, long maxDurationNs) { + long startNs = System.nanoTime(); + // only use a blocking call to waitForWork when maxDurationNs < 0 + long waitTimeout = -1; + if (maxDurationNs > 0) { + // We cannot be waiting for work forever, if not we're not going to respect the passed timeout. + // We can neither do a non-blocking call to waitForWork(), because if the future has not yet + // been completed it will result in a busy loop. + // Use an arbitrary timeout to relax cpu usage. + waitTimeout = Math.min(maxDurationNs / 10, 10000000 /* 1ms*/); + } + while (RCLJava.ok() && (maxDurationNs < 0 || maxDurationNotElapsed(maxDurationNs, startNs))) { + waitForWork(waitTimeout); + AnyExecutable anyExecutable = getNextExecutable(); + while (anyExecutable != null) { + executeAnyExecutable(anyExecutable); + if (future.isDone()) { + return; + } + anyExecutable = getNextExecutable(); + } + } + } + private void spinSomeImpl(long maxDurationNs, boolean exhaustive) { long startNs = System.nanoTime(); boolean workAvailable = false; @@ -404,6 +475,7 @@ private void spinSomeImpl(long maxDurationNs, boolean exhaustive) { AnyExecutable anyExecutable = getNextExecutable(); if (anyExecutable != null) { executeAnyExecutable(anyExecutable); + workAvailable = true; } else { if (!workAvailable || !exhaustive) { break; @@ -450,7 +522,7 @@ private static native void nativeWaitSetAddSubscription( private static native void nativeWait(long waitSetHandle, long timeout); private static native MessageDefinition nativeTake( - long subscriptionHandle, Class messageType); + long subscriptionHandle, Class messageType); private static native void nativeWaitSetAddService(long waitSetHandle, long serviceHandle); @@ -460,12 +532,14 @@ 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); - private static native void nativeSendServiceResponse(long serviceHandle, RMWRequestId header, - long responseFromJavaConverterHandle, long responseToJavaConverterHandle, + private static native void nativeSendServiceResponse( + long serviceHandle, RMWRequestId header, long responseFromJavaConverterHandle, long responseDestructorHandle, MessageDefinition responseMessage); private static native RMWRequestId nativeTakeResponse(long clientHandle, @@ -481,4 +555,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/executors/Executor.java b/rcljava/src/main/java/org/ros2/rcljava/executors/Executor.java index fe9810c9..5268e531 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/executors/Executor.java +++ b/rcljava/src/main/java/org/ros2/rcljava/executors/Executor.java @@ -15,6 +15,8 @@ package org.ros2.rcljava.executors; +import java.util.concurrent.Future; + import org.ros2.rcljava.node.ComposableNode; public interface Executor { @@ -26,6 +28,10 @@ public interface Executor { public void spinOnce(long timeout); + public void spinUntilComplete(Future future, long maxDurationNs); + + public void spinUntilComplete(Future future); + public void spinSome(); public void spinSome(long maxDurationNs); diff --git a/rcljava/src/main/java/org/ros2/rcljava/executors/MultiThreadedExecutor.java b/rcljava/src/main/java/org/ros2/rcljava/executors/MultiThreadedExecutor.java index 1bd41e31..5e813701 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/executors/MultiThreadedExecutor.java +++ b/rcljava/src/main/java/org/ros2/rcljava/executors/MultiThreadedExecutor.java @@ -17,6 +17,7 @@ import java.util.concurrent.Executors; import java.util.concurrent.ExecutorService; +import java.util.concurrent.Future; import org.ros2.rcljava.RCLJava; import org.ros2.rcljava.node.ComposableNode; @@ -55,6 +56,14 @@ public void spinOnce(long timeout) { this.baseExecutor.spinOnce(timeout); } + public void spinUntilComplete(Future future, long timeoutNs) { + this.baseExecutor.spinUntilComplete(future, timeoutNs); + } + + public void spinUntilComplete(Future future) { + this.baseExecutor.spinUntilComplete(future, -1); + } + public void spinSome() { this.spinSome(0); } diff --git a/rcljava/src/main/java/org/ros2/rcljava/executors/SingleThreadedExecutor.java b/rcljava/src/main/java/org/ros2/rcljava/executors/SingleThreadedExecutor.java index 93b41d06..a55259d4 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/executors/SingleThreadedExecutor.java +++ b/rcljava/src/main/java/org/ros2/rcljava/executors/SingleThreadedExecutor.java @@ -15,6 +15,8 @@ package org.ros2.rcljava.executors; +import java.util.concurrent.Future; + import org.ros2.rcljava.RCLJava; import org.ros2.rcljava.node.ComposableNode; import org.ros2.rcljava.executors.BaseExecutor; @@ -38,6 +40,14 @@ public void spinOnce(long timeout) { this.baseExecutor.spinOnce(timeout); } + public void spinUntilComplete(Future future, long timeoutNs) { + this.baseExecutor.spinUntilComplete(future, timeoutNs); + } + + public void spinUntilComplete(Future future) { + this.baseExecutor.spinUntilComplete(future, -1); + } + public void spinSome() { this.spinSome(0); } diff --git a/rcljava/src/main/java/org/ros2/rcljava/graph/EndpointInfo.java b/rcljava/src/main/java/org/ros2/rcljava/graph/EndpointInfo.java new file mode 100644 index 00000000..f0891093 --- /dev/null +++ b/rcljava/src/main/java/org/ros2/rcljava/graph/EndpointInfo.java @@ -0,0 +1,58 @@ +/* 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.graph; + +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + +import org.ros2.rcljava.common.JNIUtils; +import org.ros2.rcljava.qos.QoSProfile; + +/** + * Class that represents queried information of an endpoint. + */ +public class EndpointInfo { + /// Name of the node that created the endpoint. + public String nodeName; + /// Namespace of the node that created the endpoint. + public String nodeNamespace; + /// Topic type. + public String topicType; + /// Kind of endpoint, i.e.: publisher or subscription. + public EndpointType endpointType; + /// Gid of the endpoint. + public byte[] endpointGID; + /// Quality of service of the endpoint. + public QoSProfile qos; + + public enum EndpointType { + INVALID, + PUBLISHER, + SUBSCRIPTION; + } + + private native final void nativeFromRCL(long handle); + + private static final Logger logger = LoggerFactory.getLogger(EndpointInfo.class); + static { + try { + JNIUtils.loadImplementation(EndpointInfo.class); + } catch (UnsatisfiedLinkError ule) { + logger.error("Native code library failed to load.\n" + ule); + System.exit(1); + } + } +} diff --git a/rcljava/src/main/java/org/ros2/rcljava/graph/NameAndTypes.java b/rcljava/src/main/java/org/ros2/rcljava/graph/NameAndTypes.java new file mode 100644 index 00000000..12ec3d78 --- /dev/null +++ b/rcljava/src/main/java/org/ros2/rcljava/graph/NameAndTypes.java @@ -0,0 +1,73 @@ +/* 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.graph; + +import java.util.ArrayList; +import java.util.Collection; +import java.util.Objects; + +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + +import org.ros2.rcljava.common.JNIUtils; + +/** + * Class that represents a topic/service/action name, together with all the types + * of message/service/action that are using that name. + */ +public class NameAndTypes { + /// Name of the topic/service/action. + public String name; + /// Types of the topic/service/action with the given name. + public Collection types; + + /** + * Construct from given name and types. + * + * @param name name of the topic/service/action. + * @param types types of the given topic/service/action. + * A shallow copy of the given collection will be stored, + * but given that String is immutable, this is not a problem. + * @param typesSize size of the \a typesHandle array. + */ + public NameAndTypes(final String name, final Collection types) { + this.name = name; + this.types = new ArrayList(types); + } + + /// @internal Default constructor, only used from jni code. + private NameAndTypes() { + this.types = new ArrayList(); + } + + @Override + public boolean equals(final Object o) { + if (o == this) { + return true; + } + if (!(o instanceof NameAndTypes)) { + return false; + } + NameAndTypes other = (NameAndTypes) o; + return Objects.equals(this.name, other.name) && + Objects.equals(this.types, other.types); + } + + @Override + public int hashCode() { + return Objects.hash(this.name, this.types); + } +} diff --git a/rcljava/src/main/java/org/ros2/rcljava/graph/NodeNameInfo.java b/rcljava/src/main/java/org/ros2/rcljava/graph/NodeNameInfo.java new file mode 100644 index 00000000..2cd29806 --- /dev/null +++ b/rcljava/src/main/java/org/ros2/rcljava/graph/NodeNameInfo.java @@ -0,0 +1,68 @@ +/* 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.graph; + +import java.util.Objects; + +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + +import org.ros2.rcljava.common.JNIUtils; + +/** + * Class that represents the queried information of a node in the graph. + */ +public class NodeNameInfo { + /// The node name + public String name; + /// The node namespace + public String namespace; + /// The security enclave of the node. + /** + * For further details, see: + * @{link http://design.ros2.org/articles/ros2_security_enclaves.html} + */ + public String enclave; + + /// Constructor + public NodeNameInfo(final String name, final String namespace, final String enclave) { + this.name = name; + this.namespace = namespace; + this.enclave = enclave; + } + + /// Default constructor, used from jni. + private NodeNameInfo() {} + + @Override + public boolean equals(Object o) { + if (o == this) { + return true; + } + if (!(o instanceof NodeNameInfo)) { + return false; + } + NodeNameInfo other = (NodeNameInfo) o; + return Objects.equals(this.name, other.name) && + Objects.equals(this.namespace, other.namespace) && + Objects.equals(this.enclave, other.enclave); + } + + @Override + public int hashCode() { + return Objects.hash(this.name, this.namespace, this.enclave); + } +} diff --git a/rcljava/src/main/java/org/ros2/rcljava/node/BaseComposableNode.java b/rcljava/src/main/java/org/ros2/rcljava/node/BaseComposableNode.java index 271a746c..4c328aaa 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/node/BaseComposableNode.java +++ b/rcljava/src/main/java/org/ros2/rcljava/node/BaseComposableNode.java @@ -27,6 +27,11 @@ public BaseComposableNode(String name) { node = RCLJava.createNode(this.name); } + public BaseComposableNode(String name, String namespace) { + this.name = name; + node = RCLJava.createNode(name, namespace); + } + public Node getNode() { return node; } 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 f413ab04..43d470d6 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/node/Node.java +++ b/rcljava/src/main/java/org/ros2/rcljava/node/Node.java @@ -20,21 +20,33 @@ 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; import org.ros2.rcljava.consumers.TriConsumer; -import org.ros2.rcljava.qos.QoSProfile; +import org.ros2.rcljava.graph.EndpointInfo; +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; import org.ros2.rcljava.parameters.ParameterType; import org.ros2.rcljava.parameters.ParameterVariant; +import org.ros2.rcljava.parameters.client.AsyncParametersClient; +import org.ros2.rcljava.parameters.client.SyncParametersClient; import org.ros2.rcljava.publisher.Publisher; +import org.ros2.rcljava.qos.QoSProfile; import org.ros2.rcljava.service.RMWRequestId; import org.ros2.rcljava.service.Service; import org.ros2.rcljava.subscription.Subscription; +import org.ros2.rcljava.time.Clock; import org.ros2.rcljava.timer.Timer; import org.ros2.rcljava.timer.WallTimer; @@ -43,6 +55,11 @@ * A Node must be created via @{link RCLJava#createNode(String)} */ public interface Node extends Disposable { + /** + * @return The nodes @{link Clock}. + */ + Clock getClock(); + /** * return All the @{link Client}s that have been created by this instance. */ @@ -68,6 +85,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>. * @@ -106,23 +128,42 @@ Publisher createPublisher( Publisher createPublisher( final Class messageType, final String topic); - Service createService(final Class serviceType, + Service createService( + final Class serviceType, final String serviceName, final TriConsumer callback, - final QoSProfile qosProfile) throws NoSuchFieldException, IllegalAccessException; + final QoSProfile qosProfile); - Service createService(final Class serviceType, + Service createService( + final Class serviceType, final String serviceName, final TriConsumer - callback) throws NoSuchFieldException, IllegalAccessException; + callback); + + Client createClient( + final Class serviceType, final String serviceName, final QoSProfile qosProfile); Client createClient( - final Class serviceType, final String serviceName, final QoSProfile qosProfile) - throws NoSuchFieldException, IllegalAccessException; + final Class serviceType, final String serviceName); - 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 cancel 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. @@ -172,9 +213,44 @@ 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. + * + * A wall timer uses steady time. + * To create a timer that uses ROS time, see @{link Node#createTimer}. + * + * @param period Time between calls to the provided callback function. + * @param unit Time unit of the timer period. + * @param callback Function that is called when the timer expires. + * @return The created timer. + */ + @SuppressWarnings("deprecation") WallTimer createWallTimer(final long period, final TimeUnit unit, final Callback callback); + /** + * Create a timer. + * + * The timer will use the same time source as the node, ie. ROS time. + * + * @param period Time between calls to the provided callback function. + * @param unit Time unit of the timer period. + * @param callback Function that is called when the timer expires. + * @return The created timer. + */ + Timer createTimer(final long period, final TimeUnit unit, final Callback callback); + /** Get the name of the node. * * @return The name of the node. @@ -190,6 +266,16 @@ Client createClient(final Class serviceType, */ String getNamespace(); + /** + * Create an asynchronous parameter client. + */ + AsyncParametersClient createAsyncParametersClient(String nodeName); + + /** + * Create an synchronous parameter client. + */ + SyncParametersClient createSyncParametersClient(String nodeName); + /** * Declare and initialize a parameter, return the effective value. * @@ -549,4 +635,97 @@ Client createClient(final Class serviceType, * @return rcl_interfaces.msg.ListParametersResult */ rcl_interfaces.msg.ListParametersResult listParameters(List prefixes, long depth); + + /** + * Returns a collection of node names that were detected in the ROS graph. + * See @{link NodeNameInfo} for more information about the return value. + */ + Collection getNodeNames(); + + /** + * Return the topics names and types that were detected in the graph. + * See @{link graph#NameAndTypes} for more information about the returned value. + * + * @return the detected topic names and types. + */ + Collection getTopicNamesAndTypes(); + + /** + * Return the service names and types that were detected in the graph. + * See @{link graph#NameAndTypes} for more information about the returned value. + * + * @return the detected service names and types. + */ + Collection getServiceNamesAndTypes(); + + /** + * Get information of all publishers in a topic. + * + * The queried information includes the node that created the publisher, its qos, etc. + * For more info, see @{link EndpointInfo}. + * + * @param topicName The topic name of interest. + * @return A collection of `EndpointInfo` instances, describing all publishers in the + * passed topic. + */ + Collection getPublishersInfo(final String topicName); + + /** + * Get information of all subscriptions in a topic. + * + * The queried information includes the node that created the publisher, its qos, etc. + * For more info, see @{link EndpointInfo}. + * + * @param topicName The topic name of interest. + * @return A collection of `EndpointInfo` instances, describing all subscriptions in the + * passed topic. + */ + Collection getSubscriptionsInfo(final String topicName); + + /** + * Return the publisher names and types that were created from the node specified by the given + * node name and namespace. + * See @{link graph#NameAndTypes} for more information about the returned value. + * + * @param nodeName name of the node we want to know its publishers. + * @param nodeNamespace namespace of the node we want to know its publishers. + * @return the detected publisher names and types. + */ + Collection getPublisherNamesAndTypesByNode(String nodeName, String nodeNamespace); + + /** + * Return the subscription names and types that were created from the node specified by the given + * node name and namespace. + * See @{link graph#NameAndTypes} for more information about the returned value. + * + * @param nodeName name of the node we want to know its subscriptions. + * @param nodeNamespace namespace of the node we want to know its subscriptions. + * @return the detected subscription names and types. + */ + Collection getSubscriptionNamesAndTypesByNode( + String nodeName, String nodeNamespace); + + /** + * Return the service server names and types that were created from the node specified by the + * given node name and namespace. + * See @{link graph#NameAndTypes} for more information about the returned value. + * + * @param nodeName name of the node we want to know its services. + * @param nodeNamespace namespace of the node we want to know its services. + * @return the detected service server names and types. + */ + Collection getServiceNamesAndTypesByNode( + String nodeName, String nodeNamespace); + + /** + * Return the client names and types that were created from the node specified by the + * given node name and namespace. + * See @{link graph#NameAndTypes} for more information about the returned value. + * + * @param nodeName name of the node we want to know its clients. + * @param nodeNamespace namespace of the node we want to know its clients. + * @return the detected client names and types. + */ + Collection getClientNamesAndTypesByNode( + String nodeName, String nodeNamespace); } 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 1f638133..0ac0ee9f 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/node/NodeImpl.java +++ b/rcljava/src/main/java/org/ros2/rcljava/node/NodeImpl.java @@ -16,17 +16,27 @@ 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; import org.ros2.rcljava.concurrent.Callback; import org.ros2.rcljava.consumers.Consumer; import org.ros2.rcljava.consumers.TriConsumer; +import org.ros2.rcljava.graph.NodeNameInfo; import org.ros2.rcljava.contexts.Context; -import org.ros2.rcljava.qos.QoSProfile; +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; import org.ros2.rcljava.parameters.InvalidParametersException; import org.ros2.rcljava.parameters.InvalidParameterValueException; import org.ros2.rcljava.parameters.ParameterAlreadyDeclaredException; @@ -34,8 +44,15 @@ import org.ros2.rcljava.parameters.ParameterNotDeclaredException; import org.ros2.rcljava.parameters.ParameterType; import org.ros2.rcljava.parameters.ParameterVariant; +import org.ros2.rcljava.parameters.client.AsyncParametersClient; +import org.ros2.rcljava.parameters.client.AsyncParametersClientImpl; +import org.ros2.rcljava.parameters.client.SyncParametersClient; +import org.ros2.rcljava.parameters.client.SyncParametersClientImpl; +import org.ros2.rcljava.parameters.service.ParameterService; +import org.ros2.rcljava.parameters.service.ParameterServiceImpl; import org.ros2.rcljava.publisher.Publisher; import org.ros2.rcljava.publisher.PublisherImpl; +import org.ros2.rcljava.qos.QoSProfile; import org.ros2.rcljava.service.RMWRequestId; import org.ros2.rcljava.service.Service; import org.ros2.rcljava.service.ServiceImpl; @@ -43,7 +60,9 @@ import org.ros2.rcljava.subscription.SubscriptionImpl; import org.ros2.rcljava.time.Clock; import org.ros2.rcljava.time.ClockType; +import org.ros2.rcljava.time.TimeSource; import org.ros2.rcljava.timer.Timer; +import org.ros2.rcljava.timer.TimerImpl; import org.ros2.rcljava.timer.WallTimer; import org.ros2.rcljava.timer.WallTimerImpl; @@ -95,6 +114,13 @@ public class NodeImpl implements Node { */ private Clock clock; + /** + * The clock used for creating wall timers. + */ + private Clock wall_clock; + + private TimeSource timeSource; + /** * All the @{link Subscription}s that have been created through this instance. */ @@ -120,9 +146,18 @@ 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 { + public ParameterAndDescriptor() { + this.parameter = new ParameterVariant(); + this.descriptor = new rcl_interfaces.msg.ParameterDescriptor(); + } public ParameterVariant parameter; public rcl_interfaces.msg.ParameterDescriptor descriptor; } @@ -133,26 +168,35 @@ class ParameterAndDescriptor { private Object parameterCallbacksMutex; private List parameterCallbacks; + private final ParameterService parameterService; + /** * Constructor. * * @param handle A pointer to the underlying ROS2 node structure. Must not * be zero. */ - public NodeImpl(final long handle, final Context context, final boolean allowUndeclaredParameters) { - this.clock = new Clock(ClockType.SYSTEM_TIME); + public NodeImpl(final long handle, final NodeOptions nodeOptions) { this.handle = handle; - this.context = context; + this.context = nodeOptions.getContext() == null ? + RCLJava.getDefaultContext() : nodeOptions.getContext(); this.publishers = new LinkedBlockingQueue(); this.subscriptions = new LinkedBlockingQueue(); 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 = allowUndeclaredParameters; + this.allowUndeclaredParameters = nodeOptions.getAllowUndeclaredParameters(); this.parameterCallbacksMutex = new Object(); this.parameterCallbacks = new ArrayList(); + this.clock = new Clock(ClockType.ROS_TIME); + this.wall_clock = new Clock(ClockType.STEADY_TIME); + this.timeSource = new TimeSource(this); + this.timeSource.attachClock(this.clock); + this.parameterService = nodeOptions.getStartParameterServices() ? + new ParameterServiceImpl(this) : null; } /** @@ -205,6 +249,13 @@ private static native long nativeCreatePublisherHa private static native long nativeCreateSubscriptionHandle( long handle, Class messageType, String topic, long qosProfileHandle); + /** + * {@inheritDoc} + */ + public final Clock getClock() { + return this.clock; + } + /** * {@inheritDoc} */ @@ -283,30 +334,39 @@ private static native long nativeCreateServiceHand long handle, Class cls, String serviceName, long qosProfileHandle); public final Service createService(final Class serviceType, - final String serviceName, - final TriConsumer - callback, - final QoSProfile qosProfile) throws NoSuchFieldException, IllegalAccessException { - Class requestType = (Class) serviceType.getField("RequestType").get(null); - - Class responseType = (Class) serviceType.getField("ResponseType").get(null); - + final String serviceName, + final TriConsumer + callback, + final QoSProfile qosProfile) + { + T serviceDefinition; + try { + serviceDefinition = serviceType.getDeclaredConstructor().newInstance(); + } catch (ReflectiveOperationException e) { + throw new IllegalStateException("Failed to instantiate service definition"); + } long qosProfileHandle = RCLJava.convertQoSProfileToHandle(qosProfile); long serviceHandle = nativeCreateServiceHandle(this.handle, serviceType, serviceName, qosProfileHandle); RCLJava.disposeQoSProfile(qosProfileHandle); - Service service = new ServiceImpl(new WeakReference(this), serviceHandle, - serviceName, callback, requestType, responseType); + Service service = new ServiceImpl( + serviceDefinition, + new WeakReference(this), + serviceHandle, + serviceName, + callback); this.services.add(service); return service; } - public Service createService(final Class serviceType, - final String serviceName, - final TriConsumer - callback) throws NoSuchFieldException, IllegalAccessException { + public Service createService( + final Class serviceType, + final String serviceName, + final TriConsumer + callback) + { return this.createService(serviceType, serviceName, callback, QoSProfile.SERVICES_DEFAULT); } @@ -318,32 +378,47 @@ public final Collection getServices() { } public final Client createClient( - final Class serviceType, final String serviceName, final QoSProfile qosProfile) - throws NoSuchFieldException, IllegalAccessException { - Class requestType = (Class) serviceType.getField("RequestType").get(null); - - Class responseType = (Class) serviceType.getField("ResponseType").get(null); - + final Class serviceType, final String serviceName, final QoSProfile qosProfile) + { long qosProfileHandle = RCLJava.convertQoSProfileToHandle(qosProfile); long clientHandle = nativeCreateClientHandle(this.handle, serviceType, serviceName, qosProfileHandle); RCLJava.disposeQoSProfile(qosProfileHandle); + T serviceDefinition; + try { + serviceDefinition = serviceType.getDeclaredConstructor().newInstance(); + } catch (ReflectiveOperationException e) { + throw new IllegalStateException("Failed to instantiate service definition"); + } + Client client = new ClientImpl( - new WeakReference(this), clientHandle, serviceName, requestType, responseType); + serviceDefinition, new WeakReference(this), clientHandle, serviceName); this.clients.add(client); return client; } public Client createClient(final Class serviceType, - final String serviceName) throws NoSuchFieldException, IllegalAccessException { + final String serviceName) { return this.createClient(serviceType, serviceName, QoSProfile.SERVICES_DEFAULT); } 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} */ @@ -358,6 +433,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} */ @@ -406,16 +488,30 @@ public final long getHandle() { private static native long nativeCreateTimerHandle(long clockHandle, long contextHandle, long timerPeriod); - public WallTimer createWallTimer( - final long period, final TimeUnit unit, final Callback callback) { + @SuppressWarnings("deprecation") + private Timer createTimer(Clock clock, final long period, final TimeUnit unit, final Callback callback) { long timerPeriodNS = TimeUnit.NANOSECONDS.convert(period, unit); - long timerHandle = nativeCreateTimerHandle(clock.getHandle(), context.getHandle(), timerPeriodNS); - WallTimer timer = - new WallTimerImpl(new WeakReference(this), timerHandle, callback, timerPeriodNS); + long timerHandle = nativeCreateTimerHandle(clock.getHandle(), this.context.getHandle(), timerPeriodNS); + Timer timer = new WallTimerImpl(new WeakReference(this), timerHandle, callback, timerPeriodNS); this.timers.add(timer); return timer; } + /** + * {@inheritDoc} + */ + @SuppressWarnings("deprecation") + public WallTimer createWallTimer(final long period, final TimeUnit unit, final Callback callback) { + return (WallTimer) this.createTimer(this.wall_clock, period, unit, callback); + } + + /** + * {@inheritDoc} + */ + public Timer createTimer(final long period, final TimeUnit unit, final Callback callback) { + return this.createTimer(this.clock, period, unit, callback); + } + /** * {@inheritDoc} */ @@ -423,6 +519,13 @@ public final Collection getTimers() { return this.timers; } + /** + * {@inheritDoc} + */ + public final Collection getActionServers() { + return this.actionServers; + } + /** * {@inheritDoc} */ @@ -434,6 +537,14 @@ public final String getNamespace() { return nativeGetNamespace(this.handle); } + public AsyncParametersClient createAsyncParametersClient(String nodeName) { + return new AsyncParametersClientImpl(this, nodeName); + } + + public SyncParametersClient createSyncParametersClient(String nodeName) { + return new SyncParametersClientImpl(this, nodeName); + } + public ParameterVariant declareParameter(ParameterVariant parameter) { return declareParameter(parameter, new rcl_interfaces.msg.ParameterDescriptor().setName(parameter.getName())); } @@ -581,7 +692,7 @@ private rcl_interfaces.msg.SetParametersResult internalSetParametersAtomically( // them in the parameter list before setting them to the new value. We // do this multi-stage thing so that all of the parameters are set, or // none of them. - List parametersToDeclare = new ArrayList(); + ListparametersToDeclare = new ArrayList(); List parametersToUndeclare = new ArrayList(); for (ParameterVariant parameter : parameters) { if (this.parameters.containsKey(parameter.getName())) { @@ -590,7 +701,7 @@ private rcl_interfaces.msg.SetParametersResult internalSetParametersAtomically( } } else { if (this.allowUndeclaredParameters) { - parametersToDeclare.add(parameter.getName()); + parametersToDeclare.add(parameter); } else { throw new ParameterNotDeclaredException(String.format("Parameter '%s' is not declared", parameter.getName())); } @@ -599,7 +710,8 @@ private rcl_interfaces.msg.SetParametersResult internalSetParametersAtomically( // Check to make sure that a parameter isn't both trying to be declared // and undeclared simultaneously. - for (String name : parametersToDeclare) { + for (ParameterVariant parameter : parametersToDeclare) { + String name = parameter.getName(); if (parametersToUndeclare.contains(name)) { throw new IllegalArgumentException(String.format("Cannot both declare and undeclare the same parameter name '%s'", name)); } @@ -614,8 +726,16 @@ private rcl_interfaces.msg.SetParametersResult internalSetParametersAtomically( this.parameters.remove(name); } - for (String name : parametersToDeclare) { - this.parameters.put(name, new ParameterAndDescriptor()); + for (ParameterVariant parameter : parametersToDeclare) { + String name = parameter.getName(); + ParameterAndDescriptor pandd = new ParameterAndDescriptor(); + rcl_interfaces.msg.ParameterDescriptor descriptor = + new rcl_interfaces.msg.ParameterDescriptor() + .setName(name) + .setType(parameter.getType().getValue()); + pandd.parameter = parameter; + pandd.descriptor = descriptor; + this.parameters.put(name, pandd); } for (ParameterVariant parameter : parameters) { @@ -716,6 +836,8 @@ public rcl_interfaces.msg.ListParametersResult listParameters( rcl_interfaces.msg.ListParametersResult result = new rcl_interfaces.msg.ListParametersResult(); + List resultNames = new ArrayList(); + List resultPrefixes = new ArrayList(); synchronized (parametersMutex) { for (Map.Entry entry : this.parameters.entrySet()) { boolean getAll = @@ -740,17 +862,107 @@ public rcl_interfaces.msg.ListParametersResult listParameters( } if (getAll || prefixMatches) { - result.getNames().add(entry.getKey()); + resultNames.add(entry.getKey()); int lastSeparator = entry.getKey().lastIndexOf(separator); if (-1 != lastSeparator) { String prefix = entry.getKey().substring(0, lastSeparator); - if (!result.getPrefixes().contains(prefix)) { - result.getPrefixes().add(prefix); + if (!resultPrefixes.contains(prefix)) { + resultPrefixes.add(prefix); } } } } + result.setNames(resultNames); + result.setPrefixes(resultPrefixes); return result; } } + + public final Collection getNodeNames() { + ArrayList nodeNames = new ArrayList(); + nativeGetNodeNames(this.handle, nodeNames); + return nodeNames; + } + + private native static final void nativeGetNodeNames(long handle, ArrayList nodeNames); + + public final Collection getTopicNamesAndTypes() { + Collection namesAndTypes = new ArrayList(); + nativeGetTopicNamesAndTypes(this.handle, namesAndTypes); + return namesAndTypes; + } + + private static native final void nativeGetTopicNamesAndTypes( + long handle, Collection namesAndTypes); + + public final Collection getServiceNamesAndTypes() { + Collection namesAndTypes = new ArrayList(); + nativeGetServiceNamesAndTypes(this.handle, namesAndTypes); + return namesAndTypes; + } + + private static native final void nativeGetServiceNamesAndTypes( + long handle, Collection namesAndTypes); + + public final Collection getPublishersInfo(final String topicName) { + ArrayList returnValue = new ArrayList(); + nativeGetPublishersInfo(this.handle, topicName, returnValue); + return returnValue; + } + + private native static final void nativeGetPublishersInfo( + final long handle, final String topicName, ArrayList endpointInfo); + + public final Collection getSubscriptionsInfo(final String topicName) { + ArrayList returnValue = new ArrayList(); + nativeGetSubscriptionsInfo(this.handle, topicName, returnValue); + return returnValue; + } + + private native static final void nativeGetSubscriptionsInfo( + final long handle, final String topicName, ArrayList endpointInfo); + + public final Collection getPublisherNamesAndTypesByNode( + String nodeName, String nodeNamespace) + { + Collection namesAndTypes = new ArrayList(); + nativeGetPublisherNamesAndTypesByNode(this.handle, nodeName, nodeNamespace, namesAndTypes); + return namesAndTypes; + } + + private static native final Collection nativeGetPublisherNamesAndTypesByNode( + long handle, String nodeName, String nodeNamespace, Collection namesAndTypes); + + public final Collection getSubscriptionNamesAndTypesByNode( + String nodeName, String nodeNamespace) + { + Collection namesAndTypes = new ArrayList(); + nativeGetSubscriptionNamesAndTypesByNode(this.handle, nodeName, nodeNamespace, namesAndTypes); + return namesAndTypes; + } + + private static native final Collection nativeGetSubscriptionNamesAndTypesByNode( + long handle, String nodeName, String nodeNamespace, Collection namesAndTypes); + + public final Collection getServiceNamesAndTypesByNode( + String nodeName, String nodeNamespace) + { + Collection namesAndTypes = new ArrayList(); + nativeGetServiceNamesAndTypesByNode(this.handle, nodeName, nodeNamespace, namesAndTypes); + return namesAndTypes; + } + + private static native final Collection nativeGetServiceNamesAndTypesByNode( + long handle, String nodeName, String nodeNamespace, Collection namesAndTypes); + + public final Collection getClientNamesAndTypesByNode( + String nodeName, String nodeNamespace) + { + Collection namesAndTypes = new ArrayList(); + nativeGetClientNamesAndTypesByNode(this.handle, nodeName, nodeNamespace, namesAndTypes); + return namesAndTypes; + } + + private static native final Collection nativeGetClientNamesAndTypesByNode( + long handle, String nodeName, String nodeNamespace, Collection namesAndTypes); } diff --git a/rcljava/src/main/java/org/ros2/rcljava/node/NodeOptions.java b/rcljava/src/main/java/org/ros2/rcljava/node/NodeOptions.java index c64d65e1..cd230123 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/node/NodeOptions.java +++ b/rcljava/src/main/java/org/ros2/rcljava/node/NodeOptions.java @@ -17,18 +17,15 @@ import java.util.ArrayList; +import org.ros2.rcljava.contexts.Context; + public class NodeOptions { - private boolean useGlobalArguments; - private boolean enableRosout; - private boolean allowUndeclaredParameters; - private ArrayList cliArgs; - - public NodeOptions() { - this.useGlobalArguments = true; - this.enableRosout = true; - this.allowUndeclaredParameters = false; - this.cliArgs = new ArrayList(); - } + private boolean useGlobalArguments = true; + private boolean enableRosout = true; + private boolean allowUndeclaredParameters = false; + private boolean startParameterServices = true; + private Context context = null; + private ArrayList cliArgs = new ArrayList(); public final boolean getUseGlobalArguments() { return this.useGlobalArguments; @@ -57,6 +54,24 @@ public NodeOptions setAllowUndeclaredParameters(boolean allow) { return this; } + public final boolean getStartParameterServices() { + return this.startParameterServices; + } + + public NodeOptions setStartParameterServices(boolean startParameterServices) { + this.startParameterServices = startParameterServices; + return this; + } + + public final Context getContext() { + return this.context; + } + + public NodeOptions setContext(Context context) { + this.context = context; + return this; + } + public final ArrayList getCliArgs() { return this.cliArgs; } diff --git a/rcljava/src/main/java/org/ros2/rcljava/parameters/ParameterVariant.java b/rcljava/src/main/java/org/ros2/rcljava/parameters/ParameterVariant.java index 8f7484c7..3f865ae3 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/parameters/ParameterVariant.java +++ b/rcljava/src/main/java/org/ros2/rcljava/parameters/ParameterVariant.java @@ -100,38 +100,38 @@ public ParameterVariant(final String name, final String stringValue) { this.value.setType(ParameterType.PARAMETER_STRING.getValue()); } - public ParameterVariant(final String name, final Byte[] byteArrayValue) { + public ParameterVariant(final String name, final byte[] byteArrayValue) { this.name = name; this.value = new rcl_interfaces.msg.ParameterValue(); - this.value.setByteArrayValue(Arrays.asList(byteArrayValue)); + this.value.setByteArrayValue(byteArrayValue); this.value.setType(ParameterType.PARAMETER_BYTE_ARRAY.getValue()); } - public ParameterVariant(final String name, final Boolean[] boolArrayValue) { + public ParameterVariant(final String name, final boolean[] boolArrayValue) { this.name = name; this.value = new rcl_interfaces.msg.ParameterValue(); - this.value.setBoolArrayValue(Arrays.asList(boolArrayValue)); + this.value.setBoolArrayValue(boolArrayValue); this.value.setType(ParameterType.PARAMETER_BOOL_ARRAY.getValue()); } - public ParameterVariant(final String name, final Long[] integerArrayValue) { + public ParameterVariant(final String name, final long[] integerArrayValue) { this.name = name; this.value = new rcl_interfaces.msg.ParameterValue(); - this.value.setIntegerArrayValue(Arrays.asList(integerArrayValue)); + this.value.setIntegerArrayValue(integerArrayValue); this.value.setType(ParameterType.PARAMETER_INTEGER_ARRAY.getValue()); } - public ParameterVariant(final String name, final Double[] doubleArrayValue) { + public ParameterVariant(final String name, final double[] doubleArrayValue) { this.name = name; this.value = new rcl_interfaces.msg.ParameterValue(); - this.value.setDoubleArrayValue(Arrays.asList(doubleArrayValue)); + this.value.setDoubleArrayValue(doubleArrayValue); this.value.setType(ParameterType.PARAMETER_DOUBLE_ARRAY.getValue()); } public ParameterVariant(final String name, final String[] stringArrayValue) { this.name = name; this.value = new rcl_interfaces.msg.ParameterValue(); - this.value.setStringArrayValue(Arrays.asList(stringArrayValue)); + this.value.setStringArrayValue(stringArrayValue); this.value.setType(ParameterType.PARAMETER_STRING_ARRAY.getValue()); } @@ -203,39 +203,39 @@ public final boolean asBool() { return this.value.getBoolValue(); } - public final Byte[] asByteArray() { + public final byte[] asByteArray() { if (getType() != ParameterType.PARAMETER_BYTE_ARRAY) { throw new IllegalArgumentException("Invalid type"); } - return this.value.getByteArrayValue().toArray(new Byte[0]); + return this.value.getByteArrayValue(); } - public final Boolean[] asBooleanArray() { + public final boolean[] asBooleanArray() { if (getType() != ParameterType.PARAMETER_BOOL_ARRAY) { throw new IllegalArgumentException("Invalid type"); } - return this.value.getBoolArrayValue().toArray(new Boolean[0]); + return this.value.getBoolArrayValue(); } - public final Long[] asIntegerArray() { + public final long[] asIntegerArray() { if (getType() != ParameterType.PARAMETER_INTEGER_ARRAY) { throw new IllegalArgumentException("Invalid type"); } - return this.value.getIntegerArrayValue().toArray(new Long[0]); + return this.value.getIntegerArrayValue(); } - public final Double[] asDoubleArray() { + public final double[] asDoubleArray() { if (getType() != ParameterType.PARAMETER_DOUBLE_ARRAY) { throw new IllegalArgumentException("Invalid type"); } - return this.value.getDoubleArrayValue().toArray(new Double[0]); + return this.value.getDoubleArrayValue(); } public final String[] asStringArray() { if (getType() != ParameterType.PARAMETER_STRING_ARRAY) { throw new IllegalArgumentException("Invalid type"); } - return this.value.getStringArrayValue().toArray(new String[0]); + return this.value.getStringArrayValue(); } public final rcl_interfaces.msg.Parameter toParameter() { @@ -256,15 +256,15 @@ public static ParameterVariant fromParameter(final rcl_interfaces.msg.Parameter case rcl_interfaces.msg.ParameterType.PARAMETER_STRING: return new ParameterVariant(parameter.getName(), parameter.getValue().getStringValue()); case rcl_interfaces.msg.ParameterType.PARAMETER_BYTE_ARRAY: - return new ParameterVariant(parameter.getName(), parameter.getValue().getByteArrayValue().toArray(new Byte[0])); + return new ParameterVariant(parameter.getName(), parameter.getValue().getByteArrayValue()); case rcl_interfaces.msg.ParameterType.PARAMETER_BOOL_ARRAY: - return new ParameterVariant(parameter.getName(), parameter.getValue().getBoolArrayValue().toArray(new Boolean[0])); + return new ParameterVariant(parameter.getName(), parameter.getValue().getBoolArrayValue()); case rcl_interfaces.msg.ParameterType.PARAMETER_INTEGER_ARRAY: - return new ParameterVariant(parameter.getName(), parameter.getValue().getIntegerArrayValue().toArray(new Long[0])); + return new ParameterVariant(parameter.getName(), parameter.getValue().getIntegerArrayValue()); case rcl_interfaces.msg.ParameterType.PARAMETER_DOUBLE_ARRAY: - return new ParameterVariant(parameter.getName(), parameter.getValue().getDoubleArrayValue().toArray(new Double[0])); + return new ParameterVariant(parameter.getName(), parameter.getValue().getDoubleArrayValue()); case rcl_interfaces.msg.ParameterType.PARAMETER_STRING_ARRAY: - return new ParameterVariant(parameter.getName(), parameter.getValue().getStringArrayValue().toArray(new String[0])); + return new ParameterVariant(parameter.getName(), parameter.getValue().getStringArrayValue()); case rcl_interfaces.msg.ParameterType.PARAMETER_NOT_SET: throw new IllegalArgumentException("Type from ParameterValue is not set"); default: diff --git a/rcljava/src/main/java/org/ros2/rcljava/parameters/client/AsyncParametersClient.java b/rcljava/src/main/java/org/ros2/rcljava/parameters/client/AsyncParametersClient.java index 5a7a74e7..86a766a8 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/parameters/client/AsyncParametersClient.java +++ b/rcljava/src/main/java/org/ros2/rcljava/parameters/client/AsyncParametersClient.java @@ -19,6 +19,7 @@ import java.util.concurrent.Future; import org.ros2.rcljava.consumers.Consumer; +import org.ros2.rcljava.node.Node; import org.ros2.rcljava.parameters.ParameterType; import org.ros2.rcljava.parameters.ParameterVariant; @@ -59,4 +60,6 @@ public Future> describeParameters( public Future> describeParameters( final List names, final Consumer>> callback); + + public Node getNode(); } \ No newline at end of file diff --git a/rcljava/src/main/java/org/ros2/rcljava/parameters/client/AsyncParametersClientImpl.java b/rcljava/src/main/java/org/ros2/rcljava/parameters/client/AsyncParametersClientImpl.java index 6d60c226..a9ece715 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/parameters/client/AsyncParametersClientImpl.java +++ b/rcljava/src/main/java/org/ros2/rcljava/parameters/client/AsyncParametersClientImpl.java @@ -49,7 +49,7 @@ public class AsyncParametersClientImpl implements AsyncParametersClient { private final Client describeParametersClient; public AsyncParametersClientImpl(final Node node, final String remoteName, - final QoSProfile qosProfile) throws NoSuchFieldException, IllegalAccessException { + final QoSProfile qosProfile) { this.node = node; if (remoteName != "") { this.remoteName = remoteName; @@ -84,17 +84,17 @@ public AsyncParametersClientImpl(final Node node, final String remoteName, } public AsyncParametersClientImpl(final Node node, final QoSProfile qosProfile) - throws NoSuchFieldException, IllegalAccessException { + { this(node, "", qosProfile); } public AsyncParametersClientImpl(final Node node, final String remoteName) - throws NoSuchFieldException, IllegalAccessException { + { this(node, remoteName, QoSProfile.PARAMETERS); } public AsyncParametersClientImpl(final Node node) - throws NoSuchFieldException, IllegalAccessException { + { this(node, "", QoSProfile.PARAMETERS); } @@ -104,8 +104,7 @@ public Future> getParameters(final List names) { public Future> getParameters( final List names, final Consumer>> callback) { - final RCLFuture> futureResult = - new RCLFuture>(new WeakReference(this.node)); + final RCLFuture> futureResult = new RCLFuture>(); final rcl_interfaces.srv.GetParameters_Request request = new rcl_interfaces.srv.GetParameters_Request(); request.setNames(names); @@ -114,16 +113,16 @@ public Future> getParameters( request, new Consumer>() { public void accept(final Future future) { List parameterVariants = new ArrayList(); - List pvalues = null; + rcl_interfaces.msg.ParameterValue[] pvalues = new rcl_interfaces.msg.ParameterValue[0]; try { pvalues = future.get().getValues(); } catch (Exception e) { // TODO(esteve): do something } - for (int i = 0; i < pvalues.size(); i++) { + for (int i = 0; i < pvalues.length; i++) { rcl_interfaces.msg.Parameter parameter = new rcl_interfaces.msg.Parameter(); - parameter.setName(request.getNames().get(i)); - parameter.setValue(pvalues.get(i)); + parameter.setName(request.getNames()[i]); + parameter.setValue(pvalues[i]); parameterVariants.add(ParameterVariant.fromParameter(parameter)); } futureResult.set(parameterVariants); @@ -141,8 +140,7 @@ public Future> getParameterTypes(final List names) { public Future> getParameterTypes( final List names, final Consumer>> callback) { - final RCLFuture> futureResult = - new RCLFuture>(new WeakReference(this.node)); + final RCLFuture> futureResult = new RCLFuture>(); final rcl_interfaces.srv.GetParameterTypes_Request request = new rcl_interfaces.srv.GetParameterTypes_Request(); request.setNames(names); @@ -151,13 +149,13 @@ public Future> getParameterTypes( request, new Consumer>() { public void accept(final Future future) { List parameterTypes = new ArrayList(); - List pts = null; + byte[] pts = new byte[0]; try { pts = future.get().getTypes(); } catch (Exception e) { // TODO(esteve): do something } - for (Byte pt : pts) { + for (byte pt : pts) { parameterTypes.add(ParameterType.fromByte(pt)); } futureResult.set(parameterTypes); @@ -178,8 +176,7 @@ public Future> setParameters( final List parameters, final Consumer>> callback) { final RCLFuture> futureResult = - new RCLFuture>( - new WeakReference(this.node)); + new RCLFuture>(); final rcl_interfaces.srv.SetParameters_Request request = new rcl_interfaces.srv.SetParameters_Request(); List requestParameters = @@ -188,13 +185,12 @@ public Future> setParameters( requestParameters.add(parameterVariant.toParameter()); } request.setParameters(requestParameters); - setParametersClient.asyncSendRequest( request, new Consumer>() { public void accept(final Future future) { List setParametersResult = null; try { - setParametersResult = future.get().getResults(); + setParametersResult = future.get().getResultsAsList(); } catch (Exception e) { // TODO(esteve): do something } @@ -216,7 +212,7 @@ public Future setParametersAtomically( final List parameters, final Consumer> callback) { final RCLFuture futureResult = - new RCLFuture(new WeakReference(this.node)); + new RCLFuture(); final rcl_interfaces.srv.SetParametersAtomically_Request request = new rcl_interfaces.srv.SetParametersAtomically_Request(); List requestParameters = @@ -253,7 +249,7 @@ public Future listParameters( public Future listParameters(final List prefixes, long depth, final Consumer> callback) { final RCLFuture futureResult = - new RCLFuture(new WeakReference(this.node)); + new RCLFuture(); final rcl_interfaces.srv.ListParameters_Request request = new rcl_interfaces.srv.ListParameters_Request(); request.setPrefixes(prefixes); @@ -286,8 +282,7 @@ public Future> describeParameters( final List names, final Consumer>> callback) { final RCLFuture> futureResult = - new RCLFuture>( - new WeakReference(this.node)); + new RCLFuture>(); final rcl_interfaces.srv.DescribeParameters_Request request = new rcl_interfaces.srv.DescribeParameters_Request(); request.setNames(names); @@ -297,7 +292,7 @@ public Future> describeParameters( public void accept(final Future future) { List parameterDescriptors = null; try { - parameterDescriptors = future.get().getDescriptors(); + parameterDescriptors = future.get().getDescriptorsAsList(); } catch (Exception e) { // TODO(esteve): do something } @@ -309,4 +304,8 @@ public void accept(final Future }); return futureResult; } + + public Node getNode() { + return this.node; + } } diff --git a/rcljava/src/main/java/org/ros2/rcljava/parameters/client/SyncParametersClientImpl.java b/rcljava/src/main/java/org/ros2/rcljava/parameters/client/SyncParametersClientImpl.java index d2b4a78f..5ee58cb8 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/parameters/client/SyncParametersClientImpl.java +++ b/rcljava/src/main/java/org/ros2/rcljava/parameters/client/SyncParametersClientImpl.java @@ -23,6 +23,7 @@ import java.util.concurrent.ExecutionException; import java.util.concurrent.Future; +import org.ros2.rcljava.RCLJava; import org.ros2.rcljava.client.Client; import org.ros2.rcljava.concurrent.RCLFuture; import org.ros2.rcljava.consumers.Consumer; @@ -33,142 +34,110 @@ import org.ros2.rcljava.parameters.ParameterVariant; public class SyncParametersClientImpl implements SyncParametersClient { - private static class ConsumerHelper implements Consumer> { - private RCLFuture resultFuture; - - public void accept(final Future future) { - T result = null; - try { - result = future.get(); - } catch (Exception e) { - // TODO(esteve): do something - } - this.resultFuture.set(result); - } - - public ConsumerHelper(RCLFuture resultFuture) { - this.resultFuture = resultFuture; - } - } - private Executor executor; public AsyncParametersClient asyncParametersClient; - public SyncParametersClientImpl(final Node node, final String remoteName, - final QoSProfile qosProfile) throws NoSuchFieldException, IllegalAccessException { + public SyncParametersClientImpl( + final Node node, + final String remoteName, + final QoSProfile qosProfile) + { this.asyncParametersClient = new AsyncParametersClientImpl(node, remoteName, qosProfile); } public SyncParametersClientImpl(final Node node, final QoSProfile qosProfile) - throws NoSuchFieldException, IllegalAccessException { + { this(node, "", qosProfile); } public SyncParametersClientImpl(final Node node, final String remoteName) - throws NoSuchFieldException, IllegalAccessException { + { this(node, remoteName, QoSProfile.PARAMETERS); } public SyncParametersClientImpl(final Node node) - throws NoSuchFieldException, IllegalAccessException { + { this(node, "", QoSProfile.PARAMETERS); } - public SyncParametersClientImpl(final Executor executor, final Node node, final String remoteName, - final QoSProfile qosProfile) throws NoSuchFieldException, IllegalAccessException { + public SyncParametersClientImpl( + final Executor executor, + final Node node, + final String remoteName, + final QoSProfile qosProfile) + { this.executor = executor; this.asyncParametersClient = new AsyncParametersClientImpl(node, remoteName, qosProfile); } - public SyncParametersClientImpl(final Executor executor, final Node node, - final QoSProfile qosProfile) throws NoSuchFieldException, IllegalAccessException { + public SyncParametersClientImpl( + final Executor executor, + final Node node, + final QoSProfile qosProfile) + { this(executor, node, "", qosProfile); } - public SyncParametersClientImpl(final Executor executor, final Node node, final String remoteName) - throws NoSuchFieldException, IllegalAccessException { + public SyncParametersClientImpl( + final Executor executor, final Node node, final String remoteName) + { this(executor, node, remoteName, QoSProfile.PARAMETERS); } public SyncParametersClientImpl(final Executor executor, final Node node) - throws NoSuchFieldException, IllegalAccessException { + { this(executor, node, "", QoSProfile.PARAMETERS); } - public List getParameters(final List names) - throws InterruptedException, ExecutionException { + private T spinUntilComplete(Future future) + throws InterruptedException, ExecutionException + { if (executor != null) { - RCLFuture> future = new RCLFuture>(executor); - asyncParametersClient.getParameters( - names, new ConsumerHelper>(future)); - return future.get(); + executor.spinUntilComplete(future); } else { - return asyncParametersClient.getParameters(names, null).get(); + RCLJava.spinUntilComplete(this.asyncParametersClient.getNode(), future); } + return future.get(); + } + + public List getParameters(final List names) + throws InterruptedException, ExecutionException { + Future> future = asyncParametersClient.getParameters(names, null); + return spinUntilComplete(future); } public List getParameterTypes(final List names) throws InterruptedException, ExecutionException { - if (executor != null) { - RCLFuture> future = new RCLFuture>(executor); - asyncParametersClient.getParameterTypes( - names, new ConsumerHelper>(future)); - return future.get(); - } else { - return asyncParametersClient.getParameterTypes(names, null).get(); - } + Future> future = asyncParametersClient.getParameterTypes(names, null); + return spinUntilComplete(future); } public List setParameters( final List parameters) throws InterruptedException, ExecutionException { - if (executor != null) { - RCLFuture> future = - new RCLFuture>(executor); - asyncParametersClient.setParameters( - parameters, new ConsumerHelper>(future)); - return future.get(); - } else { - return asyncParametersClient.setParameters(parameters, null).get(); - } + Future> future = asyncParametersClient.setParameters( + parameters, null); + return spinUntilComplete(future); } public rcl_interfaces.msg.SetParametersResult setParametersAtomically( final List parameters) throws InterruptedException, ExecutionException { - if (executor != null) { - RCLFuture future = - new RCLFuture(executor); - asyncParametersClient.setParametersAtomically( - parameters, new ConsumerHelper(future)); - return future.get(); - } else { - return asyncParametersClient.setParametersAtomically(parameters, null).get(); - } + Future future = asyncParametersClient.setParametersAtomically( + parameters, null); + return spinUntilComplete(future); } public rcl_interfaces.msg.ListParametersResult listParameters( final List prefixes, long depth) throws InterruptedException, ExecutionException { - if (executor != null) { - RCLFuture future = - new RCLFuture(executor); - asyncParametersClient.listParameters( - prefixes, depth, new ConsumerHelper(future)); - return future.get(); - } else { - return asyncParametersClient.listParameters(prefixes, depth, null).get(); - } + Future future = asyncParametersClient.listParameters( + prefixes, depth, null); + return spinUntilComplete(future); } public List describeParameters(final List names) throws InterruptedException, ExecutionException { - if (executor != null) { - RCLFuture> future = - new RCLFuture>(executor); - asyncParametersClient.describeParameters( - names, new ConsumerHelper>(future)); - return future.get(); - } else { - return asyncParametersClient.describeParameters(names, null).get(); - } + Future> future = asyncParametersClient.describeParameters( + names, null); + return spinUntilComplete(future); } } \ No newline at end of file diff --git a/rcljava/src/main/java/org/ros2/rcljava/parameters/service/ParameterService.java b/rcljava/src/main/java/org/ros2/rcljava/parameters/service/ParameterService.java index 0be72380..8e2106f7 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/parameters/service/ParameterService.java +++ b/rcljava/src/main/java/org/ros2/rcljava/parameters/service/ParameterService.java @@ -15,4 +15,4 @@ package org.ros2.rcljava.parameters.service; -public interface ParameterService {} \ No newline at end of file +public interface ParameterService {} diff --git a/rcljava/src/main/java/org/ros2/rcljava/parameters/service/ParameterServiceImpl.java b/rcljava/src/main/java/org/ros2/rcljava/parameters/service/ParameterServiceImpl.java index b13d8e86..bc03b121 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/parameters/service/ParameterServiceImpl.java +++ b/rcljava/src/main/java/org/ros2/rcljava/parameters/service/ParameterServiceImpl.java @@ -38,7 +38,7 @@ public class ParameterServiceImpl implements ParameterService { Service listParametersService; public ParameterServiceImpl(final Node node, final QoSProfile qosProfile) - throws NoSuchFieldException, IllegalAccessException { + { this.node = node; this.getParametersService = node.createService( rcl_interfaces.srv.GetParameters.class, @@ -48,7 +48,7 @@ public ParameterServiceImpl(final Node node, final QoSProfile qosProfile) public void accept(RMWRequestId rmwRequestId, rcl_interfaces.srv.GetParameters_Request request, rcl_interfaces.srv.GetParameters_Response response) { - List values = node.getParameters(request.getNames()); + List values = node.getParameters(request.getNamesAsList()); List pvalues = new ArrayList(); for (ParameterVariant pvariant : values) { @@ -67,7 +67,7 @@ public void accept(RMWRequestId rmwRequestId, public void accept(RMWRequestId rmwRequestId, rcl_interfaces.srv.GetParameterTypes_Request request, rcl_interfaces.srv.GetParameterTypes_Response response) { - List types = node.getParameterTypes(request.getNames()); + List types = node.getParameterTypes(request.getNamesAsList()); List ptypes = new ArrayList(); for (ParameterType type : types) { ptypes.add(type.getValue()); @@ -124,7 +124,7 @@ public void accept(RMWRequestId rmwRequestId, rcl_interfaces.srv.DescribeParameters_Request request, rcl_interfaces.srv.DescribeParameters_Response response) { List descriptors = - node.describeParameters(request.getNames()); + node.describeParameters(request.getNamesAsList()); response.setDescriptors(descriptors); } }, @@ -139,14 +139,14 @@ public void accept(RMWRequestId rmwRequestId, rcl_interfaces.srv.ListParameters_Request request, rcl_interfaces.srv.ListParameters_Response response) { rcl_interfaces.msg.ListParametersResult result = - node.listParameters(request.getPrefixes(), request.getDepth()); + node.listParameters(request.getPrefixesAsList(), request.getDepth()); response.setResult(result); } }, qosProfile); } - public ParameterServiceImpl(final Node node) throws NoSuchFieldException, IllegalAccessException { + public ParameterServiceImpl(final Node node) { this(node, QoSProfile.PARAMETERS); } -} \ No newline at end of file +} diff --git a/rcljava/src/main/java/org/ros2/rcljava/publisher/PublisherImpl.java b/rcljava/src/main/java/org/ros2/rcljava/publisher/PublisherImpl.java index 0e93531e..1bd493fa 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/publisher/PublisherImpl.java +++ b/rcljava/src/main/java/org/ros2/rcljava/publisher/PublisherImpl.java @@ -117,8 +117,8 @@ public final WeakReference getNodeReference() { public final EventHandler createEventHandler(Supplier factory, Consumer callback) { - final WeakReference> weakEventHandlers = new WeakReference( - this.eventHandlers); + final WeakReference> weakEventHandlers = + new WeakReference>(this.eventHandlers); Consumer disposeCallback = new Consumer() { public void accept(EventHandler eventHandler) { Collection eventHandlers = weakEventHandlers.get(); @@ -129,7 +129,7 @@ public void accept(EventHandler eventHandler) { }; T status = factory.get(); long eventHandle = nativeCreateEvent(this.handle, status.getPublisherEventType()); - EventHandler eventHandler = new EventHandlerImpl( + EventHandler eventHandler = new EventHandlerImpl( new WeakReference(this), eventHandle, factory, callback, disposeCallback); this.eventHandlers.add(eventHandler); return eventHandler; @@ -187,10 +187,12 @@ public final void dispose() { } this.eventHandlers.clear(); Node node = this.nodeReference.get(); - if (node != null) { - node.removePublisher(this); - nativeDispose(node.getHandle(), this.handle); - this.handle = 0; + if (node == null) { + logger.error("Node reference is null. Failed to dispose of Publisher."); + return; } + node.removePublisher(this); + nativeDispose(node.getHandle(), this.handle); + this.handle = 0; } } diff --git a/rcljava/src/main/java/org/ros2/rcljava/qos/QoSProfile.java b/rcljava/src/main/java/org/ros2/rcljava/qos/QoSProfile.java index 15b81712..73961faa 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/qos/QoSProfile.java +++ b/rcljava/src/main/java/org/ros2/rcljava/qos/QoSProfile.java @@ -17,37 +17,56 @@ import java.time.Duration; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + +import org.ros2.rcljava.common.JNIUtils; import org.ros2.rcljava.qos.policies.Durability; import org.ros2.rcljava.qos.policies.History; import org.ros2.rcljava.qos.policies.Liveliness; import org.ros2.rcljava.qos.policies.QoSPolicy; import org.ros2.rcljava.qos.policies.Reliability; +/** + * Implementation of the ROS QoS profile abstraction. + * + * It works as a bridge with rmw_qos_profile_t. + * This class provides several static methods to instantiate the default profiles, + * getters for the different policies, and setters that allow method chaining. + * + * Examples: + * + * QoSProfile.keepLast(10); // default qos profile with depth 10 + * // keep all qos profile with best effort reliability and transient local durability + * QoSProfile.keepAll(). + * setReliability(Reliability.BEST_EFFORT).setDurability(Durability.TRANSIENT_LOCAL); + * QoSProfile.sensorData(); // get predefined sensor data qos profile + */ public class QoSProfile { // TODO(ivanpauno): Update all qos policies in a way that the objects are created from RCL, // to avoid depending on the enum values. - private History history; - - private int depth; - - private Reliability reliability; - - private Durability durability; - - private Duration deadline = Duration.ofSeconds(0, 0); - - private Duration lifespan = Duration.ofSeconds(0, 0); - - private Liveliness liveliness = Liveliness.SYSTEM_DEFAULT; - private Duration livelinessLeaseDuration = Duration.ofSeconds(0, 0); - - private boolean avoidROSNamespaceConventions; + /** + * Construct a keep last QoS profile with the specified depth. + * + * @param depth history depth. + * @return a new QoSProfile. + */ + public static QoSProfile keepLast(int depth) { + return defaultProfile().setDepth(depth); + } - public QoSProfile(int depth) { - this(History.KEEP_LAST, depth, Reliability.RELIABLE, Durability.VOLATILE, false); + /** + * Construct a keep all qos profile. + * + * @param depth history depth. + * @return a new QoSProfile. + */ + public static QoSProfile keepAll() { + return defaultProfile().setHistory(History.KEEP_ALL); } + // TODO(ivanpauno): Please deprecate me public QoSProfile(History history, int depth, Reliability reliability, Durability durability, boolean avoidROSNamespaceConventions) { this.history = history; @@ -57,131 +76,263 @@ public QoSProfile(History history, int depth, Reliability reliability, Durabilit this.avoidROSNamespaceConventions = avoidROSNamespaceConventions; } + /** + * @return history policy of the profile. + */ public final History getHistory() { return this.history; } + /** + * @param history history policy to be set. + * @return reference to `this` object, allowing method chaining. + */ public final QoSProfile setHistory(final History history) { this.history = history; return this; } + /** + * @return history depth of the profile. + */ public final int getDepth() { return this.depth; } + /** + * @param depth history depth to be set. + * @return reference to `this` object, allowing method chaining. + */ public final QoSProfile setDepth(final int depth) { this.depth = depth; return this; } + /** + * @return reliability policy of the profile. + */ public final Reliability getReliability() { return this.reliability; } + /** + * @param reliability reliability policy to be set. + * @return reference to `this` object, allowing method chaining. + */ public final QoSProfile setReliability(final Reliability reliability) { this.reliability = reliability; return this; } + /** + * @return durability policy of the profile. + */ public final Durability getDurability() { return this.durability; } + /** + * @param durability durability policy to be set. + * @return reference to `this` object, allowing method chaining. + */ public final QoSProfile setDurability(final Durability durability) { this.durability = durability; return this; } + /** + * @return deadline policy of the profile. + */ public final Duration getDeadline() { return this.deadline; } + /** + * @param deadline deadline policy to be set. See + * here. + * @return reference to `this` object, allowing method chaining. + */ public final QoSProfile setDeadline(final Duration deadline) { this.deadline = deadline; return this; } + /** + * @return lifespan policy of the profile. + */ public final Duration getLifespan() { return this.lifespan; } + /** + * @param lifespan lifespan policy to be set. See + * here. + * @return reference to `this` object, allowing method chaining. + */ public final QoSProfile setLifespan(final Duration lifespan) { this.lifespan = lifespan; return this; } + /** + * @return liveliness policy of the profile. + */ public final Liveliness getLiveliness() { return this.liveliness; } + /** + * @param liveliness liveliness policy to be set. See + * here. + * @return reference to `this` object, allowing method chaining. + */ public final QoSProfile setLiveliness(final Liveliness liveliness) { this.liveliness = liveliness; return this; } + /** + * @return liveliness lease duration of the profile. + */ public final Duration getLivelinessLeaseDuration() { return this.livelinessLeaseDuration; } + /** + * @param livelinessLeaseDuration liveliness lease duration to be set. See + * here. + * @return reference to `this` object, allowing method chaining. + */ public final QoSProfile setLivelinessLeaseDuration(final Duration livelinessLeaseDuration) { this.livelinessLeaseDuration = livelinessLeaseDuration; return this; } + /** + * @return `true` if ROS namespace conventions are ignored, else `false`. + */ public final boolean getAvoidROSNamespaceConventions() { return this.avoidROSNamespaceConventions; } + /** + * @param avoidROSConventions when `true`, ROS name mangling conventions will be ignored. + * @return reference to `this` object, allowing method chaining. + */ public final QoSProfile setAvoidROSNamespaceConventions(final boolean avoidROSConventions) { this.avoidROSNamespaceConventions = avoidROSConventions; return this; } - // TODO(ivanpauno): refactor all static default profiles methods, - // so that the return value is get from the rmw definition directly - // (leveraging a native function). + private static final Logger logger = LoggerFactory.getLogger(QoSProfile.class); + static { + try { + JNIUtils.loadImplementation(QoSProfile.class); + } catch (UnsatisfiedLinkError ule) { + logger.error("Native code library failed to load.\n" + ule); + System.exit(1); + } + } + + // TODO(ivanpauno): Deprecate and use a private alternative, to match rclcpp/rclpy. public static final QoSProfile defaultProfile() { - return new QoSProfile(10); + QoSProfile qos = new QoSProfile(); + qos.nativeFromRCL(qos.nativeGetHandleFromName("default")); + return qos; } + /** + * @return Construct a new instance of the predefined system default profile. + */ public static final QoSProfile systemDefault() { - return new QoSProfile( - History.SYSTEM_DEFAULT, QoSProfile.DEPTH_SYSTEM_DEFAULT, - Reliability.SYSTEM_DEFAULT, Durability.SYSTEM_DEFAULT, false); + QoSProfile qos = new QoSProfile(); + qos.nativeFromRCL(qos.nativeGetHandleFromName("system_default")); + return qos; } + /** + * @return Construct a new instance of the predefined sensor data profile. + */ public static final QoSProfile sensorData() { - return new QoSProfile( - History.KEEP_LAST, 5, Reliability.BEST_EFFORT, Durability.VOLATILE, false); + QoSProfile qos = new QoSProfile(); + qos.nativeFromRCL(qos.nativeGetHandleFromName("sensor_data")); + return qos; } + /** + * @return Construct a new instance of the predefined parameters default profile. + */ public static final QoSProfile parametersDefault() { - return new QoSProfile( - History.KEEP_LAST, 1000, Reliability.RELIABLE, Durability.VOLATILE, false); + QoSProfile qos = new QoSProfile(); + qos.nativeFromRCL(qos.nativeGetHandleFromName("parameters")); + return qos; } + /** + * @return Construct a new instance of the predefined services default profile. + */ public static final QoSProfile servicesDefault() { - return new QoSProfile( - History.KEEP_LAST, 10, Reliability.RELIABLE, Durability.VOLATILE, false); + QoSProfile qos = new QoSProfile(); + qos.nativeFromRCL(qos.nativeGetHandleFromName("services")); + return qos; } + /** + * @return Construct a new instance of the predefined parameter events default profile. + */ public static final QoSProfile parameterEventsDefault() { - return new QoSProfile( - History.KEEP_ALL, 1000, Reliability.RELIABLE, Durability.VOLATILE, false); + QoSProfile qos = new QoSProfile(); + qos.nativeFromRCL(qos.nativeGetHandleFromName("parameter_events")); + return qos; } + // TODO(ivanpauno): Deprecate. public static final int DEPTH_SYSTEM_DEFAULT = 0; + // TODO(ivanpauno): Deprecate. public static final QoSProfile SENSOR_DATA = sensorData(); + // TODO(ivanpauno): Deprecate. public static final QoSProfile PARAMETERS = parametersDefault(); + // TODO(ivanpauno): Deprecate. public static final QoSProfile DEFAULT = defaultProfile(); + // TODO(ivanpauno): Deprecate. public static final QoSProfile SERVICES_DEFAULT = servicesDefault(); + // TODO(ivanpauno): Deprecate. public static final QoSProfile PARAMETER_EVENTS = parameterEventsDefault(); + // TODO(ivanpauno): Deprecate. public static final QoSProfile SYSTEM_DEFAULT = systemDefault(); + + /** + * @internal + * Private default constructor. + * + * Used from native code or private methods. + * Typically combined with @{link QoSProfile#nativeFromRCL(handle)}. + */ + private QoSProfile() {} + + /** + * @internal + * Load from an rmw_qos_profile_t. + * + * @param handle A `rmw_qos_profile_t *`, from where the profile is loaded. + * The handle is only borrowed, and a reference to it is NOT kept. + */ + private final native void nativeFromRCL(long handle); + private final static native long nativeGetHandleFromName(String profileName); + + private History history; + private int depth; + private Reliability reliability; + private Durability durability; + private Duration deadline = Duration.ofSeconds(0, 0); + private Duration lifespan = Duration.ofSeconds(0, 0); + private Liveliness liveliness = Liveliness.SYSTEM_DEFAULT; + private Duration livelinessLeaseDuration = Duration.ofSeconds(0, 0); + private boolean avoidROSNamespaceConventions; } diff --git a/rcljava/src/main/java/org/ros2/rcljava/qos/policies/Durability.java b/rcljava/src/main/java/org/ros2/rcljava/qos/policies/Durability.java index 215602df..08a45329 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/qos/policies/Durability.java +++ b/rcljava/src/main/java/org/ros2/rcljava/qos/policies/Durability.java @@ -18,7 +18,8 @@ public enum Durability implements QoSPolicy { SYSTEM_DEFAULT(0), TRANSIENT_LOCAL(1), - VOLATILE(2); + VOLATILE(2), + UNKNOWN(3); private final int value; diff --git a/rcljava/src/main/java/org/ros2/rcljava/qos/policies/History.java b/rcljava/src/main/java/org/ros2/rcljava/qos/policies/History.java index 461d13d9..69d5b4c3 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/qos/policies/History.java +++ b/rcljava/src/main/java/org/ros2/rcljava/qos/policies/History.java @@ -18,7 +18,8 @@ public enum History implements QoSPolicy { SYSTEM_DEFAULT(0), KEEP_LAST(1), - KEEP_ALL(2); + KEEP_ALL(2), + UNKNOWN(3); private final int value; diff --git a/rcljava/src/main/java/org/ros2/rcljava/qos/policies/Liveliness.java b/rcljava/src/main/java/org/ros2/rcljava/qos/policies/Liveliness.java index 0a814e7f..3b25997f 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/qos/policies/Liveliness.java +++ b/rcljava/src/main/java/org/ros2/rcljava/qos/policies/Liveliness.java @@ -18,7 +18,8 @@ public enum Liveliness implements QoSPolicy { SYSTEM_DEFAULT(0), AUTOMATIC(1), - MANUAL_BY_TOPIC(3); + MANUAL_BY_TOPIC(3), + UNKNOWN(4); private final int value; diff --git a/rcljava/src/main/java/org/ros2/rcljava/qos/policies/Reliability.java b/rcljava/src/main/java/org/ros2/rcljava/qos/policies/Reliability.java index 3764d0bb..ea53f124 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/qos/policies/Reliability.java +++ b/rcljava/src/main/java/org/ros2/rcljava/qos/policies/Reliability.java @@ -18,7 +18,8 @@ public enum Reliability implements QoSPolicy { SYSTEM_DEFAULT(0), RELIABLE(1), - BEST_EFFORT(2); + BEST_EFFORT(2), + UNKNOWN(3); private final int value; diff --git a/rcljava/src/main/java/org/ros2/rcljava/service/Service.java b/rcljava/src/main/java/org/ros2/rcljava/service/Service.java index dfde9a09..289bad2b 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/service/Service.java +++ b/rcljava/src/main/java/org/ros2/rcljava/service/Service.java @@ -21,9 +21,7 @@ import org.ros2.rcljava.interfaces.ServiceDefinition; public interface Service extends Disposable { - Class getRequestType(); - - Class getResponseType(); + ServiceDefinition getServiceDefinition(); void executeCallback(RMWRequestId rmwRequestId, MessageDefinition request, MessageDefinition response); diff --git a/rcljava/src/main/java/org/ros2/rcljava/service/ServiceImpl.java b/rcljava/src/main/java/org/ros2/rcljava/service/ServiceImpl.java index 8d7a3254..e25d11e7 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/service/ServiceImpl.java +++ b/rcljava/src/main/java/org/ros2/rcljava/service/ServiceImpl.java @@ -45,28 +45,25 @@ public class ServiceImpl implements Service { private final TriConsumer callback; - private final Class requestType; - private final Class responseType; - - public ServiceImpl(final WeakReference nodeReference, final long handle, - final String serviceName, - final TriConsumer - callback, - final Class requestType, final Class responseType) { + private final ServiceDefinition serviceDefinition; + + public ServiceImpl( + final ServiceDefinition serviceDefinition, + final WeakReference nodeReference, + final long handle, + final String serviceName, + final TriConsumer + callback) + { this.nodeReference = nodeReference; this.handle = handle; this.serviceName = serviceName; this.callback = callback; - this.requestType = requestType; - this.responseType = responseType; - } - - public final Class getRequestType() { - return this.requestType; + this.serviceDefinition = serviceDefinition; } - public final Class getResponseType() { - return this.responseType; + public final ServiceDefinition getServiceDefinition() { + return this.serviceDefinition; } /** @@ -84,11 +81,13 @@ public final Class getResponseType() { */ public final void dispose() { Node node = this.nodeReference.get(); - if (node != null) { - node.removeService(this); - nativeDispose(node.getHandle(), this.handle); - this.handle = 0; + if (node == null) { + logger.error("Node reference is null. Failed to dispose of Service."); + return; } + node.removeService(this); + nativeDispose(node.getHandle(), this.handle); + this.handle = 0; } /** diff --git a/rcljava/src/main/java/org/ros2/rcljava/subscription/SubscriptionImpl.java b/rcljava/src/main/java/org/ros2/rcljava/subscription/SubscriptionImpl.java index 8e20d81d..34c3e3cf 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/subscription/SubscriptionImpl.java +++ b/rcljava/src/main/java/org/ros2/rcljava/subscription/SubscriptionImpl.java @@ -129,8 +129,8 @@ public final WeakReference getNodeReference() { public final EventHandler createEventHandler(Supplier factory, Consumer callback) { - final WeakReference> weakEventHandlers = new WeakReference( - this.eventHandlers); + final WeakReference> weakEventHandlers = + new WeakReference>(this.eventHandlers); Consumer disposeCallback = new Consumer() { public void accept(EventHandler eventHandler) { Collection eventHandlers = weakEventHandlers.get(); @@ -141,8 +141,9 @@ public void accept(EventHandler eventHandler) { }; T status = factory.get(); long eventHandle = nativeCreateEvent(this.handle, status.getSubscriptionEventType()); - EventHandler eventHandler = new EventHandlerImpl( - new WeakReference(this), eventHandle, factory, callback, disposeCallback); + EventHandler eventHandler = + new EventHandlerImpl( + new WeakReference(this), eventHandle, factory, callback, disposeCallback); this.eventHandlers.add(eventHandler); return eventHandler; } @@ -200,11 +201,13 @@ public final void dispose() { } this.eventHandlers.clear(); Node node = this.nodeReference.get(); - if (node != null) { - node.removeSubscription(this); - nativeDispose(node.getHandle(), this.handle); - this.handle = 0; + if (node == null) { + logger.error("Node reference is null. Failed to dispose of Subscription."); + return; } + node.removeSubscription(this); + nativeDispose(node.getHandle(), this.handle); + this.handle = 0; } public void executeCallback(T message) { diff --git a/rcljava/src/main/java/org/ros2/rcljava/subscription/statuses/MessageLost.java b/rcljava/src/main/java/org/ros2/rcljava/subscription/statuses/MessageLost.java new file mode 100644 index 00000000..eb789b4c --- /dev/null +++ b/rcljava/src/main/java/org/ros2/rcljava/subscription/statuses/MessageLost.java @@ -0,0 +1,65 @@ +// 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.subscription.statuses; + +import java.util.function.Supplier; + +import org.ros2.rcljava.common.JNIUtils; +import org.ros2.rcljava.events.SubscriptionEventStatus; + +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + +/** + * This class serves as a bridge between a rmw_message_lost_status_t and RCLJava. + */ +public class MessageLost implements SubscriptionEventStatus { + public int totalCount; + public int totalCountChange; + + public final long allocateRCLStatusEvent() { + return nativeAllocateRCLStatusEvent(); + } + public final void deallocateRCLStatusEvent(long handle) { + nativeDeallocateRCLStatusEvent(handle); + } + public final void fromRCLEvent(long handle) { + nativeFromRCLEvent(handle); + } + public final int getSubscriptionEventType() { + return nativeGetSubscriptionEventType(); + } + // TODO(ivanpauno): Remove this when -source 8 can be used (method references for the win) + public static final Supplier factory = new Supplier() { + public MessageLost get() { + return new MessageLost(); + } + }; + + private static native long nativeAllocateRCLStatusEvent(); + private static native void nativeDeallocateRCLStatusEvent(long handle); + private native void nativeFromRCLEvent(long handle); + private static native int nativeGetSubscriptionEventType(); + + private static final Logger logger = LoggerFactory.getLogger(MessageLost.class); + static { + try { + JNIUtils.loadImplementation(MessageLost.class); + } catch (UnsatisfiedLinkError ule) { + logger.error("Native code library failed to load.\n" + ule); + System.exit(1); + } + } +} diff --git a/rcljava/src/main/java/org/ros2/rcljava/time/Clock.java b/rcljava/src/main/java/org/ros2/rcljava/time/Clock.java index 69936c33..f4428121 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/time/Clock.java +++ b/rcljava/src/main/java/org/ros2/rcljava/time/Clock.java @@ -15,13 +15,14 @@ package org.ros2.rcljava.time; -import org.ros2.rcljava.common.JNIUtils; - -import org.ros2.rcljava.interfaces.Disposable; import org.slf4j.Logger; import org.slf4j.LoggerFactory; +import org.ros2.rcljava.common.JNIUtils; +import org.ros2.rcljava.interfaces.Disposable; +import org.ros2.rcljava.Time; + public class Clock implements Disposable { private static final Logger logger = LoggerFactory.getLogger(Clock.class); @@ -44,6 +45,8 @@ public class Clock implements Disposable { */ private final ClockType clockType; + private Object clockHandleMutex; + /** * Create an RCL clock (rcl_clock_t). * @@ -52,6 +55,10 @@ public class Clock implements Disposable { */ private static native long nativeCreateClockHandle(ClockType clockType); + public Clock() { + this(ClockType.SYSTEM_TIME); + } + /** * Constructor. * @@ -60,6 +67,31 @@ public class Clock implements Disposable { public Clock(ClockType clockType) { this.clockType = clockType; this.handle = Clock.nativeCreateClockHandle(clockType); + this.clockHandleMutex = new Object(); + } + + public Time now() { + synchronized(this.clockHandleMutex) { + return new Time(0, nativeGetNow(this.handle), this.clockType); + } + } + + public boolean getRosTimeIsActive() { + synchronized(this.clockHandleMutex) { + return nativeRosTimeOverrideEnabled(this.handle); + } + } + + public void setRosTimeIsActive(boolean enabled) { + synchronized(this.clockHandleMutex) { + nativeSetRosTimeOverrideEnabled(this.handle, enabled); + } + } + + public void setRosTimeOverride(Time time) { + synchronized(this.clockHandleMutex) { + nativeSetRosTimeOverride(this.handle, time.nanoseconds()); + } } /** @@ -69,6 +101,18 @@ public ClockType getClockType() { return clockType; } + // TODO(clalancette): The rclcpp and rclpy implementations of the Clock class + // both have the ability to register "time jump" callbacks with rcl. That is + // only used with tf2, so we don't need it in rcljava until we have tf2 support. + + private static native long nativeGetNow(long handle); + + private static native boolean nativeRosTimeOverrideEnabled(long handle); + + private static native void nativeSetRosTimeOverrideEnabled(long handle, boolean enabled); + + private static native void nativeSetRosTimeOverride(long handle, long nanos); + /** * Destroy an RCL clock (rcl_clock_t). * @@ -80,7 +124,9 @@ public ClockType getClockType() { * {@inheritDoc} */ public final void dispose() { - Clock.nativeDispose(this.handle); + synchronized(this.clockHandleMutex) { + nativeDispose(this.handle); + } this.handle = 0; } diff --git a/rcljava/src/main/java/org/ros2/rcljava/time/TimeSource.java b/rcljava/src/main/java/org/ros2/rcljava/time/TimeSource.java new file mode 100644 index 00000000..862cacb7 --- /dev/null +++ b/rcljava/src/main/java/org/ros2/rcljava/time/TimeSource.java @@ -0,0 +1,236 @@ +/* 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.time; + +import org.ros2.rcljava.consumers.Consumer; +import org.ros2.rcljava.node.Node; +import org.ros2.rcljava.parameters.ParameterCallback; +import org.ros2.rcljava.parameters.ParameterType; +import org.ros2.rcljava.parameters.ParameterVariant; +import org.ros2.rcljava.subscription.Subscription; +import org.ros2.rcljava.time.Clock; +import org.ros2.rcljava.time.ClockType; +import org.ros2.rcljava.Time; + +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + +import java.util.Collection; +import java.util.List; + +import java.util.concurrent.LinkedBlockingQueue; + +/** + * Implementation of the ROS time source abstraction. + * + * This class updates the time of one or more @{link Clock}s with a ROS time source. + * A ROS time source is expected to be published to the "/clock" topic. + * A TimeSource object needs a @{link Node} in order create a subscription to the "/clock" topic. + * + * There are two ways to enable (or disable) using a ROS time source: + * + * 1. Calling the method @{link #setRosTimeIsActive(boolean)}, or + * 2. Setting the 'use_sim_time' parameter on the @{link Node} that is attached to this TimeSource + * + * More information can be found in the design document + * Clock and Time. + */ +public final class TimeSource { + private Node node; + private boolean rosTimeIsActive; + private final Collection associatedClocks; + private Subscription clockSub; + private Time lastTimeSet; + private ParameterCallback simTimeCB; + + private static final Logger logger = LoggerFactory.getLogger(TimeSource.class); + + /** + * Default constructor. + */ + public TimeSource() { + this(null); + } + + /** + * Attach a node to the time source. + * + * @param node The node to attach to the time source. + */ + public TimeSource(Node node) { + this.rosTimeIsActive = false; + this.associatedClocks = new LinkedBlockingQueue(); + this.clockSub = null; + this.lastTimeSet = new Time(0, 0, ClockType.ROS_TIME); + if (node != null) { + this.attachNode(node); + } + } + + /** + * @return true if ROS time is active, false otherwise. + * Being active means we are listening to the "/clock" topic. + */ + public boolean getRosTimeIsActive() { + return this.rosTimeIsActive; + } + + class SubscriptionCallback implements Consumer { + private TimeSource timeSource; + + SubscriptionCallback(TimeSource ts) { + this.timeSource = ts; + } + + public void accept(final rosgraph_msgs.msg.Clock msg) { + Time timeFromMsg = new Time(msg.getClock(), ClockType.ROS_TIME); + this.timeSource.lastTimeSet = timeFromMsg; + for (Clock clock : this.timeSource.associatedClocks) { + clock.setRosTimeOverride(timeFromMsg); + } + } + } + + /** + * Enable or disable ROS time. + * + * @param enabled Flag to enable or disable ROS time. + */ + public void setRosTimeIsActive(boolean enabled) { + if (this.rosTimeIsActive == enabled) { + return; + } + this.rosTimeIsActive = enabled; + for (Clock clock : this.associatedClocks) { + clock.setRosTimeIsActive(enabled); + } + if (enabled) { + if (this.node != null) { + this.clockSub = node.createSubscription(rosgraph_msgs.msg.Clock.class, "/clock", new SubscriptionCallback(this)); + } + } else { + if (this.node != null && this.clockSub != null) { + this.node.removeSubscription(this.clockSub); + this.clockSub = null; + } + } + } + + /** + * Attach a @{link Node} to the time source. + * + * This node is used to create a subscription to the "/clock" topic. + * Any previously attached @{link Node} is detached. + * + * @param node The node to attach to the time source. + */ + public void attachNode(Node node) { + if (this.node != null) { + detachNode(); + } + + this.node = node; + + if (!this.node.hasParameter("use_sim_time")) { + this.node.declareParameter(new ParameterVariant("use_sim_time", false)); + } + + ParameterVariant useSimTime = this.node.getParameter("use_sim_time"); + ParameterType useSimTimeType = useSimTime.getType(); + if (useSimTimeType != ParameterType.PARAMETER_NOT_SET) { + if (useSimTimeType == ParameterType.PARAMETER_BOOL) { + this.rosTimeIsActive = useSimTime.asBool(); + } else { + logger.warn("The 'use_sim_time' parameter must be a boolean"); + } + } + + class SimTimeCB implements ParameterCallback { + private TimeSource timeSource; + + public SimTimeCB(TimeSource ts) { + this.timeSource = ts; + } + + public rcl_interfaces.msg.SetParametersResult callback(List parameters) { + rcl_interfaces.msg.SetParametersResult result = new rcl_interfaces.msg.SetParametersResult(); + result.setSuccessful(true); + for (ParameterVariant param : parameters) { + if (param.getName() == "use_sim_time") { + if (param.getType() == ParameterType.PARAMETER_BOOL) { + this.timeSource.setRosTimeIsActive(param.asBool()); + } else { + result.setSuccessful(false); + result.setReason("'use_sim_time' parameter must be a boolean"); + break; + } + } + } + return result; + } + } + + this.simTimeCB = new SimTimeCB(this); + + this.node.addOnSetParametersCallback(this.simTimeCB); + } + + /** + * Detach a @{link Node} from the time source. + * + * Unsubscribes from the "/clock" topic, effectively disabling ROS time. + * + * If no Node is attached, nothing happens. + */ + public void detachNode() { + if (this.node == null) { + return; + } + this.setRosTimeIsActive(false); + this.node.removeOnSetParametersCallback(this.simTimeCB); + this.node = null; + } + + /** + * Attach a @{link Clock} to the time source. + * + * More than one clock may be attached to a time source. + * Attached clocks will be updated to the the time received on the "/clock" topic. + * + * @param clock The Clock to attach. + * Must be of type @{link ClockType.ROS_TIME}, otherwise an exception is thrown. + */ + public void attachClock(Clock clock) { + if (clock.getClockType() != ClockType.ROS_TIME) { + // TODO(clalancette): Make this a custom exception + throw new IllegalArgumentException("Cannot attach a clock that is not ROS_TIME to a timesource"); + } + clock.setRosTimeOverride(this.lastTimeSet); + clock.setRosTimeIsActive(this.rosTimeIsActive); + this.associatedClocks.add(clock); + } + + /** + * Detach a @{link Clock} from the time source. + * + * The Clock will no longer receive time updates from this time source. + * + * @param clock The Clock to detach. + */ + public void detachClock(Clock clock) { + this.associatedClocks.remove(clock); + } +} diff --git a/rcljava/src/main/java/org/ros2/rcljava/timer/Timer.java b/rcljava/src/main/java/org/ros2/rcljava/timer/Timer.java index 46aa6d43..dc2b47ab 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/timer/Timer.java +++ b/rcljava/src/main/java/org/ros2/rcljava/timer/Timer.java @@ -23,4 +23,18 @@ public interface Timer extends Disposable { void executeCallback(); boolean isReady(); + + long getTimerPeriodNS(); + + void setTimerPeriodNS(long period); + + boolean isCanceled(); + + void cancel(); + + void reset(); + + long timeSinceLastCall(); + + long timeUntilNextCall(); } diff --git a/rcljava/src/main/java/org/ros2/rcljava/timer/TimerImpl.java b/rcljava/src/main/java/org/ros2/rcljava/timer/TimerImpl.java new file mode 100644 index 00000000..5f9084de --- /dev/null +++ b/rcljava/src/main/java/org/ros2/rcljava/timer/TimerImpl.java @@ -0,0 +1,131 @@ +/* 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. + */ + +package org.ros2.rcljava.timer; + +import java.lang.ref.WeakReference; + +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + +import org.ros2.rcljava.common.JNIUtils; +import org.ros2.rcljava.concurrent.Callback; +import org.ros2.rcljava.node.Node; + +public class TimerImpl implements Timer { + private static final Logger logger = LoggerFactory.getLogger(TimerImpl.class); + + static { + try { + JNIUtils.loadImplementation(TimerImpl.class); + } catch (UnsatisfiedLinkError ule) { + logger.error("Native code library failed to load.\n" + ule); + System.exit(1); + } + } + + private long timerPeriodNS; + + private final WeakReference nodeReference; + + private long handle; + + private final Callback callback; + + private static native boolean nativeIsReady(long handle); + + private static native boolean nativeIsCanceled(long handle); + + private static native long nativeTimeSinceLastCall(long handle); + + private static native long nativeTimeUntilNextCall(long handle); + + private static native long nativeReset(long handle); + + private static native long nativeCancel(long handle); + + private static native long nativeGetTimerPeriodNS(long handle); + + private static native void nativeSetTimerPeriodNS(long handle, long period); + + private static native long nativeCallTimer(long handle); + + public TimerImpl(final WeakReference nodeReference, final long handle, + final Callback callback, final long timerPeriodNS) { + this.nodeReference = nodeReference; + this.handle = handle; + this.callback = callback; + this.timerPeriodNS = timerPeriodNS; + } + + public long timeSinceLastCall() { + return nativeTimeSinceLastCall(this.handle); + } + + public long timeUntilNextCall() { + return nativeTimeUntilNextCall(this.handle); + } + + public void reset() { + nativeReset(this.handle); + } + + public void cancel() { + nativeCancel(this.handle); + } + + public boolean isCanceled() { + return nativeIsCanceled(this.handle); + } + + public boolean isReady() { + return nativeIsReady(this.handle); + } + + public void setTimerPeriodNS(final long timerPeriodNS) { + nativeSetTimerPeriodNS(this.handle, timerPeriodNS); + this.timerPeriodNS = timerPeriodNS; + } + + public long getTimerPeriodNS() { + long timerPeriodNS = nativeGetTimerPeriodNS(this.handle); + this.timerPeriodNS = timerPeriodNS; + return this.timerPeriodNS; + } + + public long getHandle() { + return this.handle; + } + + private static native void nativeDispose(long handle); + + public void dispose() { + Node node = this.nodeReference.get(); + if (node == null) { + logger.error("Node reference is null. Failed to dispose of Timer."); + return; + } + nativeDispose(this.handle); + this.handle = 0; + } + + public void callTimer() { + nativeCallTimer(this.handle); + } + + public void executeCallback() { + this.callback.call(); + } +} diff --git a/rcljava/src/main/java/org/ros2/rcljava/timer/WallTimer.java b/rcljava/src/main/java/org/ros2/rcljava/timer/WallTimer.java index f21a96f4..49de0910 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/timer/WallTimer.java +++ b/rcljava/src/main/java/org/ros2/rcljava/timer/WallTimer.java @@ -17,18 +17,10 @@ import org.ros2.rcljava.concurrent.Callback; +/** + * @deprecated + * Use @{link Timer} interface instead. + */ +@Deprecated public interface WallTimer extends Timer { - long getTimerPeriodNS(); - - void setTimerPeriodNS(long period); - - boolean isCanceled(); - - void cancel(); - - void reset(); - - long timeSinceLastCall(); - - long timeUntilNextCall(); } diff --git a/rcljava/src/main/java/org/ros2/rcljava/timer/WallTimerImpl.java b/rcljava/src/main/java/org/ros2/rcljava/timer/WallTimerImpl.java index 560c4548..077c6386 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/timer/WallTimerImpl.java +++ b/rcljava/src/main/java/org/ros2/rcljava/timer/WallTimerImpl.java @@ -17,113 +17,19 @@ import java.lang.ref.WeakReference; -import org.slf4j.Logger; -import org.slf4j.LoggerFactory; - -import org.ros2.rcljava.common.JNIUtils; import org.ros2.rcljava.concurrent.Callback; import org.ros2.rcljava.node.Node; +import org.ros2.rcljava.timer.TimerImpl; +import org.ros2.rcljava.timer.WallTimer; -public class WallTimerImpl implements WallTimer { - private static final Logger logger = LoggerFactory.getLogger(WallTimerImpl.class); - - static { - try { - JNIUtils.loadImplementation(WallTimerImpl.class); - } catch (UnsatisfiedLinkError ule) { - logger.error("Native code library failed to load.\n" + ule); - System.exit(1); - } - } - - private long timerPeriodNS; - - private final WeakReference nodeReference; - - private long handle; - - private final Callback callback; - - private static native boolean nativeIsReady(long handle); - - private static native boolean nativeIsCanceled(long handle); - - private static native long nativeTimeSinceLastCall(long handle); - - private static native long nativeTimeUntilNextCall(long handle); - - private static native long nativeReset(long handle); - - private static native long nativeCancel(long handle); - - private static native long nativeGetTimerPeriodNS(long handle); - - private static native void nativeSetTimerPeriodNS(long handle, long period); - - private static native long nativeCallTimer(long handle); - +/** + * @deprecated + * Use @{link TimerImpl} instead. + */ +@Deprecated +public class WallTimerImpl extends TimerImpl implements WallTimer { public WallTimerImpl(final WeakReference nodeReference, final long handle, final Callback callback, final long timerPeriodNS) { - this.nodeReference = nodeReference; - this.handle = handle; - this.callback = callback; - this.timerPeriodNS = timerPeriodNS; - } - - public long timeSinceLastCall() { - return nativeTimeSinceLastCall(this.handle); - } - - public long timeUntilNextCall() { - return nativeTimeUntilNextCall(this.handle); - } - - public void reset() { - nativeReset(this.handle); - } - - public void cancel() { - nativeCancel(this.handle); - } - - public boolean isCanceled() { - return nativeIsCanceled(this.handle); - } - - public boolean isReady() { - return nativeIsReady(this.handle); - } - - public void setTimerPeriodNS(final long timerPeriodNS) { - nativeSetTimerPeriodNS(this.handle, timerPeriodNS); - this.timerPeriodNS = timerPeriodNS; - } - - public long getTimerPeriodNS() { - long timerPeriodNS = nativeGetTimerPeriodNS(this.handle); - this.timerPeriodNS = timerPeriodNS; - return this.timerPeriodNS; - } - - public long getHandle() { - return this.handle; - } - - private static native void nativeDispose(long handle); - - public void dispose() { - Node node = this.nodeReference.get(); - if (node != null) { - nativeDispose(this.handle); - this.handle = 0; - } - } - - public void callTimer() { - nativeCallTimer(this.handle); - } - - public void executeCallback() { - this.callback.call(); + super(nodeReference, handle, callback, timerPeriodNS); } } diff --git a/rcljava/src/test/java/org/ros2/rcljava/SpinTest.java b/rcljava/src/test/java/org/ros2/rcljava/SpinTest.java index c5b8beb3..21740e87 100644 --- a/rcljava/src/test/java/org/ros2/rcljava/SpinTest.java +++ b/rcljava/src/test/java/org/ros2/rcljava/SpinTest.java @@ -35,7 +35,7 @@ import org.ros2.rcljava.executors.SingleThreadedExecutor; import org.ros2.rcljava.node.ComposableNode; import org.ros2.rcljava.node.Node; -import org.ros2.rcljava.timer.WallTimer; +import org.ros2.rcljava.timer.Timer; import org.ros2.rcljava.publisher.Publisher; import org.ros2.rcljava.qos.policies.Durability; import org.ros2.rcljava.qos.policies.History; @@ -97,7 +97,7 @@ public final void testSpinOnce() { Executor executor = new SingleThreadedExecutor(); final Node node = RCLJava.createNode("spin_once_node"); TimerCallback timerCallback = new TimerCallback(0); - WallTimer timer = node.createWallTimer(100, TimeUnit.MILLISECONDS, timerCallback); + Timer timer = node.createWallTimer(100, TimeUnit.MILLISECONDS, timerCallback); assertNotEquals(0, timer.getHandle()); ComposableNode composableNode = new ComposableNode() { @@ -117,7 +117,7 @@ public final void testSpinOnceTimeout() { Executor executor = new SingleThreadedExecutor(); final Node node = RCLJava.createNode("spin_once_timeout"); TimerCallback timerCallback = new TimerCallback(0); - WallTimer timer = node.createWallTimer(100, TimeUnit.MILLISECONDS, timerCallback); + Timer timer = node.createWallTimer(100, TimeUnit.MILLISECONDS, timerCallback); assertNotEquals(0, timer.getHandle()); ComposableNode composableNode = new ComposableNode() { @@ -137,7 +137,7 @@ public final void testSpinOnceTimeoutTooShort() { Executor executor = new SingleThreadedExecutor(); final Node node = RCLJava.createNode("spin_once_timeout_too_short_node"); TimerCallback timerCallback = new TimerCallback(0); - WallTimer timer = node.createWallTimer(100, TimeUnit.MILLISECONDS, timerCallback); + Timer timer = node.createWallTimer(100, TimeUnit.MILLISECONDS, timerCallback); assertNotEquals(0, timer.getHandle()); ComposableNode composableNode = new ComposableNode() { @@ -157,7 +157,7 @@ public final void testSpinSome() { Executor executor = new SingleThreadedExecutor(); final Node node = RCLJava.createNode("spin_some_node"); TimerCallback timerCallback = new TimerCallback(0); - WallTimer timer = node.createWallTimer(100, TimeUnit.MILLISECONDS, timerCallback); + Timer timer = node.createWallTimer(100, TimeUnit.MILLISECONDS, timerCallback); assertNotEquals(0, timer.getHandle()); ComposableNode composableNode = new ComposableNode() { @@ -185,7 +185,7 @@ public final void testSpinSomeMaxDuration() { Executor executor = new SingleThreadedExecutor(); final Node node = RCLJava.createNode("spin_some_max_duration_node"); TimerCallback timerCallback = new TimerCallback(0); - WallTimer timer = node.createWallTimer(100, TimeUnit.MILLISECONDS, timerCallback); + Timer timer = node.createWallTimer(100, TimeUnit.MILLISECONDS, timerCallback); assertNotEquals(0, timer.getHandle()); ComposableNode composableNode = new ComposableNode() { @@ -213,8 +213,8 @@ public final void testSpinSomeMaxDurationMultipleTimers() { Executor executor = new SingleThreadedExecutor(); final Node node = RCLJava.createNode("spin_some_max_duration_multiple_timers_node"); TimerCallback timerCallback = new TimerCallback(200); - WallTimer timer = node.createWallTimer(100, TimeUnit.MILLISECONDS, timerCallback); - WallTimer timer2 = node.createWallTimer(100, TimeUnit.MILLISECONDS, timerCallback); + Timer timer = node.createWallTimer(100, TimeUnit.MILLISECONDS, timerCallback); + Timer timer2 = node.createWallTimer(100, TimeUnit.MILLISECONDS, timerCallback); assertNotEquals(0, timer.getHandle()); assertNotEquals(0, timer2.getHandle()); @@ -243,8 +243,8 @@ public final void testSpinSomeMaxDurationTooShortMultipleTimers() { Executor executor = new SingleThreadedExecutor(); final Node node = RCLJava.createNode("spin_some_max_duration_too_short_node"); TimerCallback timerCallback = new TimerCallback(200); - WallTimer timer = node.createWallTimer(100, TimeUnit.MILLISECONDS, timerCallback); - WallTimer timer2 = node.createWallTimer(100, TimeUnit.MILLISECONDS, timerCallback); + Timer timer = node.createWallTimer(100, TimeUnit.MILLISECONDS, timerCallback); + Timer timer2 = node.createWallTimer(100, TimeUnit.MILLISECONDS, timerCallback); assertNotEquals(0, timer.getHandle()); assertNotEquals(0, timer2.getHandle()); @@ -273,7 +273,7 @@ public final void testSpinAll() { Executor executor = new SingleThreadedExecutor(); final Node node = RCLJava.createNode("spin_all_node"); TimerCallback timerCallback = new TimerCallback(0); - WallTimer timer = node.createWallTimer(100, TimeUnit.MILLISECONDS, timerCallback); + Timer timer = node.createWallTimer(100, TimeUnit.MILLISECONDS, timerCallback); assertNotEquals(0, timer.getHandle()); ComposableNode composableNode = new ComposableNode() { @@ -302,8 +302,8 @@ public final void testSpinAllMaxDurationMultipleTimers() { Executor executor = new SingleThreadedExecutor(); final Node node = RCLJava.createNode("spin_all_max_duration_multiple_timers_node"); TimerCallback timerCallback = new TimerCallback(200); - WallTimer timer = node.createWallTimer(100, TimeUnit.MILLISECONDS, timerCallback); - WallTimer timer2 = node.createWallTimer(100, TimeUnit.MILLISECONDS, timerCallback); + Timer timer = node.createWallTimer(100, TimeUnit.MILLISECONDS, timerCallback); + Timer timer2 = node.createWallTimer(100, TimeUnit.MILLISECONDS, timerCallback); assertNotEquals(0, timer.getHandle()); assertNotEquals(0, timer2.getHandle()); @@ -332,8 +332,8 @@ public final void testSpinAllMaxDurationTooShortMultipleTimers() { Executor executor = new SingleThreadedExecutor(); final Node node = RCLJava.createNode("spin_all_max_duration_too_short_node"); TimerCallback timerCallback = new TimerCallback(200); - WallTimer timer = node.createWallTimer(100, TimeUnit.MILLISECONDS, timerCallback); - WallTimer timer2 = node.createWallTimer(100, TimeUnit.MILLISECONDS, timerCallback); + Timer timer = node.createWallTimer(100, TimeUnit.MILLISECONDS, timerCallback); + Timer timer2 = node.createWallTimer(100, TimeUnit.MILLISECONDS, timerCallback); assertNotEquals(0, timer.getHandle()); assertNotEquals(0, timer2.getHandle()); diff --git a/rcljava/src/test/java/org/ros2/rcljava/TimeTest.java b/rcljava/src/test/java/org/ros2/rcljava/TimeTest.java index 9bd0e01c..62aa37a8 100644 --- a/rcljava/src/test/java/org/ros2/rcljava/TimeTest.java +++ b/rcljava/src/test/java/org/ros2/rcljava/TimeTest.java @@ -16,10 +16,8 @@ package org.ros2.rcljava; import static org.junit.Assert.assertEquals; -import static org.junit.Assert.fail; import java.lang.reflect.Method; -import java.util.concurrent.TimeUnit; import org.junit.BeforeClass; import org.junit.Test; @@ -45,50 +43,69 @@ public static void setupOnce() throws Exception { } } - // @Test - public final void testSystemTime() { - builtin_interfaces.msg.Time now = Time.now(ClockType.SYSTEM_TIME); - long javaNowMillis = System.currentTimeMillis(); - - long nowNanos = Time.toNanoseconds(now); - long javaNowNanos = TimeUnit.NANOSECONDS.convert(javaNowMillis, TimeUnit.MILLISECONDS); - - long tolerance = TimeUnit.NANOSECONDS.convert(1000, TimeUnit.MILLISECONDS); - assertEquals(javaNowNanos, nowNanos, tolerance); + @Test + public final void testTimeNoArgConstructor() { + Time time = new Time(); + assertEquals(0, time.nanoseconds()); + assertEquals(ClockType.SYSTEM_TIME, time.clockType()); } - // @Test - public final void testSteadyTime() { - int millisecondsToSleep = 100; - builtin_interfaces.msg.Time now = Time.now(ClockType.STEADY_TIME); - long javaNowNanos = System.nanoTime(); - - try { - Thread.sleep(millisecondsToSleep); - } catch (InterruptedException iex) { - fail("Failed to sleep for " + millisecondsToSleep + " milliseconds"); - } + @Test + public final void testTimeFromMsgConstructor() { + builtin_interfaces.msg.Time timeMsg = new builtin_interfaces.msg.Time(); + timeMsg.setSec(42); + timeMsg.setNanosec(100); + Time time = new Time(timeMsg, ClockType.SYSTEM_TIME); + assertEquals(42000000100L, time.nanoseconds()); + assertEquals(ClockType.SYSTEM_TIME, time.clockType()); + } - builtin_interfaces.msg.Time later = Time.now(ClockType.STEADY_TIME); + @Test + public final void testTimeNanos() { + Time time = new Time(45, ClockType.SYSTEM_TIME); + assertEquals(45, time.nanoseconds()); + assertEquals(ClockType.SYSTEM_TIME, time.clockType()); + } - long javaLaterNanos = System.nanoTime(); + @Test + public final void testTimeAllArgs() { + Time time = new Time(0, 45, ClockType.SYSTEM_TIME); + assertEquals(45, time.nanoseconds()); + assertEquals(ClockType.SYSTEM_TIME, time.clockType()); + } - long tolerance = TimeUnit.NANOSECONDS.convert(1, TimeUnit.MILLISECONDS); + @Test(expected = IllegalArgumentException.class) + public final void testTimeBadSecs() { + Time time = new Time(-1, 0, ClockType.SYSTEM_TIME); + } - long javaDifference = javaLaterNanos - javaNowNanos; - long difference = Time.difference(later, now); - assertEquals(javaDifference, difference, tolerance); + @Test(expected = IllegalArgumentException.class) + public final void testTimeBadNanos() { + Time time = new Time(0, -45, ClockType.SYSTEM_TIME); } @Test - public final void testROSTime() { - builtin_interfaces.msg.Time startTime; - try { - startTime = Time.now(ClockType.ROS_TIME); - } catch (UnsupportedOperationException uoe) { - return; + public final void testTimeToMsg() { + { + Time time = new Time(); + builtin_interfaces.msg.Time timeMsgOut = time.toMsg(); + assertEquals(0, timeMsgOut.getSec()); + assertEquals(0, timeMsgOut.getNanosec()); + } + { + builtin_interfaces.msg.Time timeMsg = new builtin_interfaces.msg.Time(); + timeMsg.setSec(42); + timeMsg.setNanosec(100); + Time time = new Time(timeMsg, ClockType.SYSTEM_TIME); + builtin_interfaces.msg.Time timeMsgOut = time.toMsg(); + assertEquals(42, timeMsgOut.getSec()); + assertEquals(100, timeMsgOut.getNanosec()); + } + { + Time time = new Time(0, 45, ClockType.SYSTEM_TIME); + builtin_interfaces.msg.Time timeMsgOut = time.toMsg(); + assertEquals(0, timeMsgOut.getSec()); + assertEquals(45, timeMsgOut.getNanosec()); } - - fail("ROS time is not supported yet"); } } 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..2e49b6ad --- /dev/null +++ b/rcljava/src/test/java/org/ros2/rcljava/action/ActionServerTest.java @@ -0,0 +1,216 @@ +/* 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_AND_EXECUTE; + } + } + + 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); + + long startTime = System.nanoTime(); + this.executor.spinUntilComplete(future, TimeUnit.SECONDS.toNanos(5)); + return future.get(); + } + + @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(); + this.executor.spinUntilComplete(cancelResponseFuture, TimeUnit.SECONDS.toNanos(5)); + + 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..3ff19de4 --- /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_FeedbackMessage 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_FeedbackMessage.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/src/test/java/org/ros2/rcljava/client/ClientTest.java b/rcljava/src/test/java/org/ros2/rcljava/client/ClientTest.java index cf01c02e..49b3ac6e 100644 --- a/rcljava/src/test/java/org/ros2/rcljava/client/ClientTest.java +++ b/rcljava/src/test/java/org/ros2/rcljava/client/ClientTest.java @@ -16,6 +16,7 @@ package org.ros2.rcljava.client; import static org.junit.Assert.assertEquals; +import static org.junit.Assert.assertFalse; import static org.junit.Assert.assertNotEquals; import static org.junit.Assert.assertTrue; @@ -36,7 +37,9 @@ import org.ros2.rcljava.RCLJava; import org.ros2.rcljava.concurrent.RCLFuture; +import org.ros2.rcljava.consumers.Consumer; import org.ros2.rcljava.consumers.TriConsumer; +import org.ros2.rcljava.executors.Executor; import org.ros2.rcljava.node.Node; import org.ros2.rcljava.service.RMWRequestId; import org.ros2.rcljava.service.Service; @@ -51,7 +54,7 @@ public static void setupOnce() throws Exception { { // Configure log4j. Doing this dynamically so that Android does not complain about missing // the log4j JARs, SLF4J uses Android's native logging mechanism instead. - Class c = Class.forName("org.apache.log4j.BasicConfigurator"); + Class c = Class.forName("org.apache.log4j.BasicConfigurator"); Method m = c.getDeclaredMethod("configure", (Class[]) null); Object o = m.invoke(null, (Object[]) null); } @@ -97,7 +100,7 @@ public static void tearDownOnce() { @Test public final void testAdd() throws Exception { RCLFuture consumerFuture = - new RCLFuture(new WeakReference(node)); + new RCLFuture(); TestClientConsumer clientConsumer = new TestClientConsumer(consumerFuture); @@ -115,7 +118,8 @@ public final void testAdd() throws Exception { Future responseFuture = client.asyncSendRequest(request); - rcljava.srv.AddTwoInts_Response response = responseFuture.get(10, TimeUnit.SECONDS); + RCLJava.spinUntilComplete(node, responseFuture, TimeUnit.SECONDS.toNanos(10)); + rcljava.srv.AddTwoInts_Response response = responseFuture.get(); // Check that the message was received by the service assertTrue(consumerFuture.isDone()); @@ -135,4 +139,61 @@ public final void testAdd() throws Exception { assertEquals(0, service.getHandle()); assertEquals(0, node.getServices().size()); } + + @Test + public final void testRemovePendingRequest() throws Exception { + RCLFuture consumerFuture = + new RCLFuture(); + + TestClientConsumer clientConsumer = new TestClientConsumer(consumerFuture); + + Service service = node.createService( + rcljava.srv.AddTwoInts.class, "add_two_ints", clientConsumer); + + rcljava.srv.AddTwoInts_Request request = new rcljava.srv.AddTwoInts_Request(); + request.setA(2); + request.setB(3); + + Client client = + node.createClient(rcljava.srv.AddTwoInts.class, "add_two_ints"); + + assertTrue(client.waitForService(Duration.ofSeconds(10))); + + ResponseFuture responseFuture = client.asyncSendRequest(request); + + assertTrue(client.removePendingRequest(responseFuture)); + assertFalse(client.removePendingRequest(responseFuture)); + } + + @Test + public final void testPrunePendingRequestsOlderThan() throws Exception { + RCLFuture consumerFuture = + new RCLFuture(); + + TestClientConsumer clientConsumer = new TestClientConsumer(consumerFuture); + + Service service = node.createService( + rcljava.srv.AddTwoInts.class, "add_two_ints", clientConsumer); + + rcljava.srv.AddTwoInts_Request request = new rcljava.srv.AddTwoInts_Request(); + request.setA(2); + request.setB(3); + + Client client = + node.createClient( + rcljava.srv.AddTwoInts.class, "add_two_ints"); + + assertTrue(client.waitForService(Duration.ofSeconds(10))); + + long oldNanoTime = System.nanoTime(); + ResponseFuture responseFuture = client.asyncSendRequest( + request, + new Consumer>() { + public final void accept(final Future futureResponse) {} + }); + + assertEquals(0, client.prunePendingRequestsOlderThan(oldNanoTime)); + assertEquals(1, client.prunePendingRequestsOlderThan(System.nanoTime())); + assertEquals(0, client.prunePendingRequestsOlderThan(System.nanoTime())); + } } diff --git a/rcljava/src/test/java/org/ros2/rcljava/contexts/ContextTest.java b/rcljava/src/test/java/org/ros2/rcljava/contexts/ContextTest.java new file mode 100644 index 00000000..52216587 --- /dev/null +++ b/rcljava/src/test/java/org/ros2/rcljava/contexts/ContextTest.java @@ -0,0 +1,82 @@ +/* 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.contexts; + +import static org.junit.Assert.assertEquals; + +import org.junit.After; +import org.junit.AfterClass; +import org.junit.BeforeClass; +import org.junit.Before; +import org.junit.Test; + +import org.ros2.rcljava.RCLJava; +import org.ros2.rcljava.contexts.Context; + +public class ContextTest { + public Context context = null; + + @BeforeClass + public static void setupOnce() throws Exception { + // Just to quiet down warnings + org.apache.log4j.BasicConfigurator.configure(); + + RCLJava.rclJavaInit(); + } + + @AfterClass + public static void tearDownOnce() { + RCLJava.shutdown(); + } + + @Before + public void setup() throws Exception { + this.context = RCLJava.createContext(); + } + + @After + public void tearDown() { + this.context.dispose(); + } + + @Test + public final void testCreateAndDispose() { + assertEquals(false, this.context.isValid()); + } + + @Test + public final void testInitShutdown() { + assertEquals(false, this.context.isValid()); + this.context.init(); + assertEquals(true, this.context.isValid()); + this.context.shutdown(); + assertEquals(false, this.context.isValid()); + } + + @Test + public final void testInitWithArgsAndShutdown() { + String args[] = { + "--user-arg-1", "user-arg-2", "-p,", + "--ros-args", "-r", "asd:=bsd" + }; + assertEquals(false, this.context.isValid()); + this.context.init(args); + assertEquals(true, this.context.isValid()); + this.context.shutdown(); + assertEquals(false, this.context.isValid()); + } +} + diff --git a/rcljava/src/test/java/org/ros2/rcljava/node/NodeOptionsTest.java b/rcljava/src/test/java/org/ros2/rcljava/node/NodeOptionsTest.java index bde90763..3fe2ef94 100644 --- a/rcljava/src/test/java/org/ros2/rcljava/node/NodeOptionsTest.java +++ b/rcljava/src/test/java/org/ros2/rcljava/node/NodeOptionsTest.java @@ -58,7 +58,7 @@ public static void tearDownOnce() { public final void testCreateNodeWithArgs() { NodeOptions options = new NodeOptions(); options.setCliArgs(new ArrayList(Arrays.asList("--ros-args", "-r", "__ns:=/foo"))); - Node node = RCLJava.createNode("test_node", "", RCLJava.getDefaultContext(), options); + Node node = RCLJava.createNode("test_node", "", options); assertEquals("test_node", node.getName()); assertEquals("/foo", node.getNamespace()); diff --git a/rcljava/src/test/java/org/ros2/rcljava/node/NodeParametersTest.java b/rcljava/src/test/java/org/ros2/rcljava/node/NodeParametersTest.java index 72af9f8d..5843b359 100644 --- a/rcljava/src/test/java/org/ros2/rcljava/node/NodeParametersTest.java +++ b/rcljava/src/test/java/org/ros2/rcljava/node/NodeParametersTest.java @@ -349,7 +349,8 @@ public final void testListParameters() { node.declareParameter(new ParameterVariant("thing.do", true)); rcl_interfaces.msg.ListParametersResult result = node.listParameters(new ArrayList(), rcl_interfaces.srv.ListParameters_Request.DEPTH_RECURSIVE); - assertEquals(2, result.getNames().size()); + // 2 we added above plus the implicit "use_sim_time" + assertEquals(3, result.getNames().length); } @Test diff --git a/rcljava/src/test/java/org/ros2/rcljava/node/NodeTest.java b/rcljava/src/test/java/org/ros2/rcljava/node/NodeTest.java index 8df0cb7f..e1c84690 100644 --- a/rcljava/src/test/java/org/ros2/rcljava/node/NodeTest.java +++ b/rcljava/src/test/java/org/ros2/rcljava/node/NodeTest.java @@ -16,7 +16,9 @@ package org.ros2.rcljava.node; import static org.junit.Assert.assertEquals; +import static org.junit.Assert.assertFalse; import static org.junit.Assert.assertNotEquals; +import static org.junit.Assert.assertNotNull; import static org.junit.Assert.assertTrue; import org.junit.After; @@ -28,17 +30,33 @@ import java.lang.ref.WeakReference; import java.lang.reflect.Method; +import java.util.concurrent.TimeUnit; +import java.util.ArrayList; import java.util.Arrays; +import java.util.Collection; +import java.util.Iterator; import java.util.List; +import java.util.concurrent.TimeUnit; import org.ros2.rcljava.RCLJava; +import org.ros2.rcljava.client.Client; import org.ros2.rcljava.concurrent.RCLFuture; import org.ros2.rcljava.consumers.Consumer; +import org.ros2.rcljava.consumers.BiConsumer; +import org.ros2.rcljava.consumers.TriConsumer; import org.ros2.rcljava.executors.Executor; import org.ros2.rcljava.executors.MultiThreadedExecutor; import org.ros2.rcljava.executors.SingleThreadedExecutor; +import org.ros2.rcljava.graph.EndpointInfo; +import org.ros2.rcljava.graph.NameAndTypes; +import org.ros2.rcljava.graph.NodeNameInfo; import org.ros2.rcljava.node.Node; +import org.ros2.rcljava.node.NodeOptions; import org.ros2.rcljava.publisher.Publisher; +import org.ros2.rcljava.qos.policies.Reliability; +import org.ros2.rcljava.qos.QoSProfile; +import org.ros2.rcljava.service.RMWRequestId; +import org.ros2.rcljava.service.Service; import org.ros2.rcljava.subscription.Subscription; public class NodeTest { @@ -91,7 +109,7 @@ public static void setupOnce() throws Exception { { // Configure log4j. Doing this dynamically so that Android does not complain about missing // the log4j JARs, SLF4J uses Android's native logging mechanism instead. - Class c = Class.forName("org.apache.log4j.BasicConfigurator"); + Class c = Class.forName("org.apache.log4j.BasicConfigurator"); Method m = c.getDeclaredMethod("configure", (Class[]) null); Object o = m.invoke(null, (Object[]) null); } @@ -119,7 +137,8 @@ public final void accept(final T msg) { @Before public void setUp() { - node = RCLJava.createNode("test_node"); + node = RCLJava.createNode( + "test_node", "/", new NodeOptions().setStartParameterServices(false)); primitives1 = new rcljava.msg.Primitives(); primitives2 = new rcljava.msg.Primitives(); @@ -206,7 +225,7 @@ public final void testPubSubStdString() throws Exception { node.createPublisher(std_msgs.msg.String.class, "test_topic_string"); RCLFuture future = - new RCLFuture(new WeakReference(node)); + new RCLFuture(); Subscription subscription = node.createSubscription(std_msgs.msg.String.class, "test_topic_string", @@ -236,7 +255,7 @@ public final void testPubSubBoundedArrayNested() throws Exception { rcljava.msg.BoundedArrayNested.class, "test_topic_bounded_array_nested"); RCLFuture future = - new RCLFuture(new WeakReference(node)); + new RCLFuture(); Subscription subscription = node.createSubscription( @@ -254,8 +273,8 @@ public final void testPubSubBoundedArrayNested() throws Exception { rcljava.msg.BoundedArrayNested value = future.get(); assertNotEquals(null, value.getPrimitiveValues()); - rcljava.msg.Primitives primitivesValue1 = value.getPrimitiveValues().get(0); - rcljava.msg.Primitives primitivesValue2 = value.getPrimitiveValues().get(1); + rcljava.msg.Primitives primitivesValue1 = value.getPrimitiveValues()[0]; + rcljava.msg.Primitives primitivesValue2 = value.getPrimitiveValues()[1]; assertTrue(checkPrimitives(primitivesValue1, boolValue1, byteValue1, charValue1, float32Value1, float64Value1, int8Value1, uint8Value1, int16Value1, uint16Value1, int32Value1, @@ -278,7 +297,7 @@ public final void testPubSubBoundedArrayPrimitives() throws Exception { rcljava.msg.BoundedArrayPrimitives.class, "test_topic_bounded_array_primitives"); RCLFuture future = - new RCLFuture(new WeakReference(node)); + new RCLFuture(); Subscription subscription = node.createSubscription( @@ -324,20 +343,20 @@ public final void testPubSubBoundedArrayPrimitives() throws Exception { rcljava.msg.BoundedArrayPrimitives value = future.get(); - assertEquals(boolValues, value.getBoolValues()); - assertEquals(byteValues, value.getByteValues()); - assertEquals(charValues, value.getCharValues()); - assertEquals(float32Values, value.getFloat32Values()); - assertEquals(float64Values, value.getFloat64Values()); - assertEquals(int8Values, value.getInt8Values()); - assertEquals(uint8Values, value.getUint8Values()); - assertEquals(int16Values, value.getInt16Values()); - assertEquals(uint16Values, value.getUint16Values()); - assertEquals(int32Values, value.getInt32Values()); - assertEquals(uint32Values, value.getUint32Values()); - assertEquals(int64Values, value.getInt64Values()); - assertEquals(uint64Values, value.getUint64Values()); - assertEquals(stringValues, value.getStringValues()); + assertEquals(boolValues, value.getBoolValuesAsList()); + assertEquals(byteValues, value.getByteValuesAsList()); + assertEquals(charValues, value.getCharValuesAsList()); + assertEquals(float32Values, value.getFloat32ValuesAsList()); + assertEquals(float64Values, value.getFloat64ValuesAsList()); + assertEquals(int8Values, value.getInt8ValuesAsList()); + assertEquals(uint8Values, value.getUint8ValuesAsList()); + assertEquals(int16Values, value.getInt16ValuesAsList()); + assertEquals(uint16Values, value.getUint16ValuesAsList()); + assertEquals(int32Values, value.getInt32ValuesAsList()); + assertEquals(uint32Values, value.getUint32ValuesAsList()); + assertEquals(int64Values, value.getInt64ValuesAsList()); + assertEquals(uint64Values, value.getUint64ValuesAsList()); + assertEquals(stringValues, value.getStringValuesAsList()); publisher.dispose(); assertEquals(0, publisher.getHandle()); @@ -351,7 +370,7 @@ public final void testPubSubBuiltins() throws Exception { rcljava.msg.Builtins.class, "test_topic_builtins"); RCLFuture future = - new RCLFuture(new WeakReference(node)); + new RCLFuture(); Subscription subscription = node.createSubscription(rcljava.msg.Builtins.class, @@ -398,7 +417,7 @@ public final void testPubSubDynamicArrayNested() throws Exception { rcljava.msg.DynamicArrayNested.class, "test_topic_dynamic_array_nested"); RCLFuture future = - new RCLFuture(new WeakReference(node)); + new RCLFuture(); Subscription subscription = node.createSubscription( @@ -416,8 +435,8 @@ public final void testPubSubDynamicArrayNested() throws Exception { rcljava.msg.DynamicArrayNested value = future.get(); assertNotEquals(null, value.getPrimitiveValues()); - rcljava.msg.Primitives primitivesValue1 = value.getPrimitiveValues().get(0); - rcljava.msg.Primitives primitivesValue2 = value.getPrimitiveValues().get(1); + rcljava.msg.Primitives primitivesValue1 = value.getPrimitiveValues()[0]; + rcljava.msg.Primitives primitivesValue2 = value.getPrimitiveValues()[1]; assertTrue(checkPrimitives(primitivesValue1, boolValue1, byteValue1, charValue1, float32Value1, float64Value1, int8Value1, uint8Value1, int16Value1, uint16Value1, int32Value1, @@ -440,7 +459,7 @@ public final void testPubSubDynamicArrayPrimitives() throws Exception { rcljava.msg.DynamicArrayPrimitives.class, "test_topic_dynamic_array_primitives"); RCLFuture future = - new RCLFuture(new WeakReference(node)); + new RCLFuture(); Subscription subscription = node.createSubscription( @@ -486,20 +505,20 @@ public final void testPubSubDynamicArrayPrimitives() throws Exception { rcljava.msg.DynamicArrayPrimitives value = future.get(); - assertEquals(boolValues, value.getBoolValues()); - assertEquals(byteValues, value.getByteValues()); - assertEquals(charValues, value.getCharValues()); - assertEquals(float32Values, value.getFloat32Values()); - assertEquals(float64Values, value.getFloat64Values()); - assertEquals(int8Values, value.getInt8Values()); - assertEquals(uint8Values, value.getUint8Values()); - assertEquals(int16Values, value.getInt16Values()); - assertEquals(uint16Values, value.getUint16Values()); - assertEquals(int32Values, value.getInt32Values()); - assertEquals(uint32Values, value.getUint32Values()); - assertEquals(int64Values, value.getInt64Values()); - assertEquals(uint64Values, value.getUint64Values()); - assertEquals(stringValues, value.getStringValues()); + assertEquals(boolValues, value.getBoolValuesAsList()); + assertEquals(byteValues, value.getByteValuesAsList()); + assertEquals(charValues, value.getCharValuesAsList()); + assertEquals(float32Values, value.getFloat32ValuesAsList()); + assertEquals(float64Values, value.getFloat64ValuesAsList()); + assertEquals(int8Values, value.getInt8ValuesAsList()); + assertEquals(uint8Values, value.getUint8ValuesAsList()); + assertEquals(int16Values, value.getInt16ValuesAsList()); + assertEquals(uint16Values, value.getUint16ValuesAsList()); + assertEquals(int32Values, value.getInt32ValuesAsList()); + assertEquals(uint32Values, value.getUint32ValuesAsList()); + assertEquals(int64Values, value.getInt64ValuesAsList()); + assertEquals(uint64Values, value.getUint64ValuesAsList()); + assertEquals(stringValues, value.getStringValuesAsList()); publisher.dispose(); assertEquals(0, publisher.getHandle()); @@ -513,7 +532,7 @@ public final void testPubSubEmpty() throws Exception { node.createPublisher(rcljava.msg.Empty.class, "test_topic_empty"); RCLFuture future = - new RCLFuture(new WeakReference(node)); + new RCLFuture(); Subscription subscription = node.createSubscription( rcljava.msg.Empty.class, "test_topic_empty", new TestConsumer(future)); @@ -541,7 +560,7 @@ public final void testPubSubFieldsWithSameType() throws Exception { rcljava.msg.FieldsWithSameType.class, "test_topic_fields_with_same_type"); RCLFuture future = - new RCLFuture(new WeakReference(node)); + new RCLFuture(); Subscription subscription = node.createSubscription( @@ -585,7 +604,7 @@ public final void testPubSubNested() throws Exception { node.createPublisher(rcljava.msg.Nested.class, "test_topic_nested"); RCLFuture future = - new RCLFuture(new WeakReference(node)); + new RCLFuture(); Subscription subscription = node.createSubscription(rcljava.msg.Nested.class, "test_topic_nested", @@ -620,7 +639,7 @@ public final void testPubSubPrimitives() throws Exception { rcljava.msg.Primitives.class, "test_topic_primitives"); RCLFuture future = - new RCLFuture(new WeakReference(node)); + new RCLFuture(); Subscription subscription = node.createSubscription(rcljava.msg.Primitives.class, @@ -651,7 +670,7 @@ public final void testPubSubStaticArrayNested() throws Exception { rcljava.msg.StaticArrayNested.class, "test_topic_static_array_nested"); RCLFuture future = - new RCLFuture(new WeakReference(node)); + new RCLFuture(); Subscription subscription = node.createSubscription(rcljava.msg.StaticArrayNested.class, @@ -670,12 +689,12 @@ public final void testPubSubStaticArrayNested() throws Exception { rcljava.msg.StaticArrayNested value = future.get(); assertNotEquals(null, value.getPrimitiveValues()); - assertEquals(4, value.getPrimitiveValues().size()); + assertEquals(4, value.getPrimitiveValues().length); - rcljava.msg.Primitives primitivesValue1 = value.getPrimitiveValues().get(0); - rcljava.msg.Primitives primitivesValue2 = value.getPrimitiveValues().get(1); - rcljava.msg.Primitives primitivesValue3 = value.getPrimitiveValues().get(2); - rcljava.msg.Primitives primitivesValue4 = value.getPrimitiveValues().get(3); + rcljava.msg.Primitives primitivesValue1 = value.getPrimitiveValues()[0]; + rcljava.msg.Primitives primitivesValue2 = value.getPrimitiveValues()[1]; + rcljava.msg.Primitives primitivesValue3 = value.getPrimitiveValues()[2]; + rcljava.msg.Primitives primitivesValue4 = value.getPrimitiveValues()[3]; assertTrue(checkPrimitives(primitivesValue1, boolValue1, byteValue1, charValue1, float32Value1, float64Value1, int8Value1, uint8Value1, int16Value1, uint16Value1, int32Value1, @@ -706,7 +725,7 @@ public final void testPubSubStaticArrayPrimitives() throws Exception { rcljava.msg.StaticArrayPrimitives.class, "test_topic_static_array_primitives"); RCLFuture future = - new RCLFuture(new WeakReference(node)); + new RCLFuture(); Subscription subscription = node.createSubscription( @@ -753,20 +772,20 @@ public final void testPubSubStaticArrayPrimitives() throws Exception { rcljava.msg.StaticArrayPrimitives value = future.get(); - assertEquals(boolValues, value.getBoolValues()); - assertEquals(byteValues, value.getByteValues()); - assertEquals(charValues, value.getCharValues()); - assertEquals(float32Values, value.getFloat32Values()); - assertEquals(float64Values, value.getFloat64Values()); - assertEquals(int8Values, value.getInt8Values()); - assertEquals(uint8Values, value.getUint8Values()); - assertEquals(int16Values, value.getInt16Values()); - assertEquals(uint16Values, value.getUint16Values()); - assertEquals(int32Values, value.getInt32Values()); - assertEquals(uint32Values, value.getUint32Values()); - assertEquals(int64Values, value.getInt64Values()); - assertEquals(uint64Values, value.getUint64Values()); - assertEquals(stringValues, value.getStringValues()); + assertEquals(boolValues, value.getBoolValuesAsList()); + assertEquals(byteValues, value.getByteValuesAsList()); + assertEquals(charValues, value.getCharValuesAsList()); + assertEquals(float32Values, value.getFloat32ValuesAsList()); + assertEquals(float64Values, value.getFloat64ValuesAsList()); + assertEquals(int8Values, value.getInt8ValuesAsList()); + assertEquals(uint8Values, value.getUint8ValuesAsList()); + assertEquals(int16Values, value.getInt16ValuesAsList()); + assertEquals(uint16Values, value.getUint16ValuesAsList()); + assertEquals(int32Values, value.getInt32ValuesAsList()); + assertEquals(uint32Values, value.getUint32ValuesAsList()); + assertEquals(int64Values, value.getInt64ValuesAsList()); + assertEquals(uint64Values, value.getUint64ValuesAsList()); + assertEquals(stringValues, value.getStringValuesAsList()); publisher.dispose(); assertEquals(0, publisher.getHandle()); @@ -780,7 +799,7 @@ public final void testPubUInt32() throws Exception { node.createPublisher(rcljava.msg.UInt32.class, "test_topic_uint32"); RCLFuture future = - new RCLFuture(new WeakReference(node)); + new RCLFuture(); Subscription subscription = node.createSubscription(rcljava.msg.UInt32.class, "test_topic_uint32", @@ -814,13 +833,13 @@ public final void testPubUInt32MultipleNodes() throws Exception { Publisher publisher = publisherNode.createPublisher( rcljava.msg.UInt32.class, "test_topic_multiple"); - RCLFuture futureOne = new RCLFuture(executor); + RCLFuture futureOne = new RCLFuture(); Subscription subscriptionOne = subscriptionNodeOne.createSubscription(rcljava.msg.UInt32.class, "test_topic_multiple", new TestConsumer(futureOne)); - RCLFuture futureTwo = new RCLFuture(executor); + RCLFuture futureTwo = new RCLFuture(); Subscription subscriptionTwo = subscriptionNodeTwo.createSubscription(rcljava.msg.UInt32.class, @@ -868,5 +887,698 @@ public Node getNode() { assertEquals(0, subscriptionOne.getHandle()); subscriptionTwo.dispose(); assertEquals(0, subscriptionTwo.getHandle()); + publisherNode.dispose(); + assertEquals(0, publisherNode.getHandle()); + subscriptionNodeOne.dispose(); + assertEquals(0, subscriptionNodeOne.getHandle()); + subscriptionNodeTwo.dispose(); + assertEquals(0, subscriptionNodeTwo.getHandle()); + } + + @Test + public final void testGetNodeNames() throws Exception { + final Node node1 = RCLJava.createNode("test_get_node_names_1"); + final Node node2 = RCLJava.createNode("test_get_node_names_2"); + final Node node3 = RCLJava.createNode("test_get_node_names_3"); + + Consumer> validateNodeNameInfo = + new Consumer>() { + public void accept(final Collection nodeNamesInfo) { + assertEquals(4, nodeNamesInfo.size()); + assertTrue( + "node 'test_node' was not discovered", + nodeNamesInfo.contains(new NodeNameInfo("test_node", "/", "/"))); + assertTrue( + "node 'test_get_node_names_1' was not discovered", + nodeNamesInfo.contains(new NodeNameInfo("test_get_node_names_1", "/", "/"))); + assertTrue( + "node 'test_get_node_names_2' was not discovered", + nodeNamesInfo.contains(new NodeNameInfo("test_get_node_names_1", "/", "/"))); + assertTrue( + "node 'test_get_node_names_3' was not discovered", + nodeNamesInfo.contains(new NodeNameInfo("test_get_node_names_1", "/", "/"))); + } + }; + + long start = System.currentTimeMillis(); + boolean ok = false; + Collection nodeNamesInfo = null; + do { + nodeNamesInfo = this.node.getNodeNames(); + try { + validateNodeNameInfo.accept(nodeNamesInfo); + ok = true; + } catch (AssertionError err) { + // ignore here, it's going to be validated again at the end. + } + // TODO(ivanpauno): We could wait for the graph guard condition to be triggered if that + // would be available. + try { + TimeUnit.MILLISECONDS.sleep(100); + } catch (InterruptedException err) { + // ignore + } + } while (!ok && System.currentTimeMillis() < start + 1000); + assertNotNull(nodeNamesInfo); + validateNodeNameInfo.accept(nodeNamesInfo); + + node1.dispose(); + node2.dispose(); + node3.dispose(); + } + + @Test + public final void testGetTopicNamesAndTypes() throws Exception { + Publisher publisher = node.createPublisher( + rcljava.msg.UInt32.class, "test_get_topic_names_and_types_one"); + Publisher publisher2 = node.createPublisher( + rcljava.msg.UInt32.class, "test_get_topic_names_and_types_two"); + Subscription subscription = node.createSubscription( + rcljava.msg.UInt32.class, "test_get_topic_names_and_types_one", + new Consumer() { + public void accept(final rcljava.msg.UInt32 msg) {} + }); + Subscription subscription2 = node.createSubscription( + rcljava.msg.Empty.class, "test_get_topic_names_and_types_three", + new Consumer() { + public void accept(final rcljava.msg.Empty msg) {} + }); + + Consumer> validateNameAndTypes = + new Consumer>() { + public void accept(final Collection namesAndTypes) { + // TODO(ivanpauno): Using assertj may help a lot here https://assertj.github.io/doc/. + assertEquals(namesAndTypes.size(), 3); + assertTrue( + "topic 'test_get_topic_names_and_types_one' was not discovered", + namesAndTypes.contains( + new NameAndTypes( + "/test_get_topic_names_and_types_one", + Arrays.asList("rcljava/msg/UInt32")))); + assertTrue( + "topic 'test_get_topic_names_and_types_two' was not discovered", + namesAndTypes.contains( + new NameAndTypes( + "/test_get_topic_names_and_types_two", + Arrays.asList("rcljava/msg/UInt32")))); + assertTrue( + "topic 'test_get_topic_names_and_types_three' was not discovered", + namesAndTypes.contains( + new NameAndTypes( + "/test_get_topic_names_and_types_three", + Arrays.asList("rcljava/msg/Empty")))); + } + }; + + long start = System.currentTimeMillis(); + boolean ok = false; + Collection namesAndTypes = null; + do { + namesAndTypes = this.node.getTopicNamesAndTypes(); + try { + validateNameAndTypes.accept(namesAndTypes); + ok = true; + } catch (AssertionError err) { + // ignore here, it's going to be validated again at the end. + } + // TODO(ivanpauno): We could wait for the graph guard condition to be triggered if that + // would be available. + try { + TimeUnit.MILLISECONDS.sleep(100); + } catch (InterruptedException err) { + // ignore + } + } while (!ok && System.currentTimeMillis() < start + 1000); + assertNotNull(namesAndTypes); + validateNameAndTypes.accept(namesAndTypes); + + publisher.dispose(); + publisher2.dispose(); + subscription.dispose(); + subscription2.dispose(); + } + + @Test + public final void testGetServiceNamesAndTypes() throws Exception { + Service service = node.createService( + rcljava.srv.AddTwoInts.class, "test_service_names_and_types_one", + new TriConsumer< + RMWRequestId, rcljava.srv.AddTwoInts_Request, rcljava.srv.AddTwoInts_Response>() + { + public final void accept( + final RMWRequestId header, + final rcljava.srv.AddTwoInts_Request request, + final rcljava.srv.AddTwoInts_Response response) + {} + }); + Client client = node.createClient( + rcljava.srv.AddTwoInts.class, "test_service_names_and_types_two"); + + Consumer> validateNameAndTypes = + new Consumer>() { + public void accept(final Collection namesAndTypes) { + assertEquals(namesAndTypes.size(), 2); + assertTrue( + "service 'test_service_names_and_types_one' was not discovered", + namesAndTypes.contains( + new NameAndTypes( + "/test_service_names_and_types_one", + Arrays.asList("rcljava/srv/AddTwoInts")))); + assertTrue( + "service 'test_service_names_and_types_two' was not discovered", + namesAndTypes.contains( + new NameAndTypes( + "/test_service_names_and_types_two", + Arrays.asList("rcljava/srv/AddTwoInts")))); + } + }; + + long start = System.currentTimeMillis(); + boolean ok = false; + Collection namesAndTypes = null; + do { + namesAndTypes = this.node.getServiceNamesAndTypes(); + try { + validateNameAndTypes.accept(namesAndTypes); + ok = true; + } catch (AssertionError err) { + // ignore here, it's going to be validated again at the end. + } + // TODO(ivanpauno): We could wait for the graph guard condition to be triggered if that + // would be available. + try { + TimeUnit.MILLISECONDS.sleep(100); + } catch (InterruptedException err) { + // ignore + } + } while (!ok && System.currentTimeMillis() < start + 1000); + assertNotNull(namesAndTypes); + validateNameAndTypes.accept(namesAndTypes); + + service.dispose(); + client.dispose(); + } + + @Test + public final void testGetPublishersInfo() { + Publisher publisher = + node.createPublisher(rcljava.msg.UInt32.class, "test_get_publishers_info"); + Publisher publisher2 = + node.createPublisher( + rcljava.msg.UInt32.class, "test_get_publishers_info", QoSProfile.sensorData()); + + Consumer> validateEndpointInfo = + new Consumer>() { + public void accept(final Collection info) { + assertEquals(info.size(), 2); + Iterator it = info.iterator(); + EndpointInfo item = it.next(); + assertEquals("test_node", item.nodeName); + assertEquals("/", item.nodeNamespace); + assertEquals("rcljava/msg/UInt32", item.topicType); + assertEquals(item.endpointType, EndpointInfo.EndpointType.PUBLISHER); + assertEquals(item.qos.getReliability(), Reliability.RELIABLE); + item = it.next(); + assertEquals("test_node", item.nodeName); + assertEquals("/", item.nodeNamespace); + assertEquals("rcljava/msg/UInt32", item.topicType); + assertEquals(item.endpointType, EndpointInfo.EndpointType.PUBLISHER); + assertEquals(item.qos.getReliability(), Reliability.BEST_EFFORT); + assertFalse(it.hasNext()); + } + }; + + long start = System.currentTimeMillis(); + boolean ok = false; + Collection publishersInfo = null; + do { + publishersInfo = node.getPublishersInfo("/test_get_publishers_info"); + try { + validateEndpointInfo.accept(publishersInfo); + ok = true; + } catch (AssertionError err) { + // ignore here, it's going to be validated again at the end. + } + // TODO(ivanpauno): We could wait for the graph guard condition to be triggered if that + // would be available. + try { + TimeUnit.MILLISECONDS.sleep(100); + } catch (InterruptedException err) { + // ignore + } + } while (!ok && System.currentTimeMillis() < start + 1000); + assertNotNull(publishersInfo); + validateEndpointInfo.accept(publishersInfo); + publisher.dispose(); + publisher2.dispose(); + } + + @Test + public final void testGetSubscriptionsInfo() { + Subscription subscription = node.createSubscription( + rcljava.msg.UInt32.class, "test_get_subscriptions_info", new Consumer() { + public void accept(final rcljava.msg.UInt32 msg) {} + }); + Subscription subscription2 = node.createSubscription( + rcljava.msg.UInt32.class, "test_get_subscriptions_info", new Consumer() { + public void accept(final rcljava.msg.UInt32 msg) {} + }, QoSProfile.sensorData()); + + Consumer> validateEndpointInfo = + new Consumer>() { + public void accept(final Collection info) { + assertEquals(info.size(), 2); + Iterator it = info.iterator(); + EndpointInfo item = it.next(); + assertEquals("test_node", item.nodeName); + assertEquals("/", item.nodeNamespace); + assertEquals("rcljava/msg/UInt32", item.topicType); + assertEquals(item.endpointType, EndpointInfo.EndpointType.SUBSCRIPTION); + assertEquals(item.qos.getReliability(), Reliability.RELIABLE); + item = it.next(); + assertEquals("test_node", item.nodeName); + assertEquals("/", item.nodeNamespace); + assertEquals("rcljava/msg/UInt32", item.topicType); + assertEquals(item.endpointType, EndpointInfo.EndpointType.SUBSCRIPTION); + assertEquals(item.qos.getReliability(), Reliability.BEST_EFFORT); + assertFalse(it.hasNext()); + } + }; + + long start = System.currentTimeMillis(); + boolean ok = false; + Collection subscriptionsInfo = null; + do { + subscriptionsInfo = node.getSubscriptionsInfo("/test_get_subscriptions_info"); + try { + validateEndpointInfo.accept(subscriptionsInfo); + ok = true; + } catch (AssertionError err) { + // ignore here, it's going to be validated again at the end. + } + // TODO(ivanpauno): We could wait for the graph guard condition to be triggered if that + // would be available. + try { + TimeUnit.MILLISECONDS.sleep(100); + } catch (InterruptedException err) { + // ignore + } + } while (!ok && System.currentTimeMillis() < start + 1000); + assertNotNull(subscriptionsInfo); + validateEndpointInfo.accept(subscriptionsInfo); + subscription.dispose(); + subscription2.dispose(); + } + + @Test + public final void testGetPublisherNamesAndTypesByNode() throws Exception { + final Node remoteNode = RCLJava.createNode("test_get_publisher_names_and_types_remote_node"); + Publisher publisher1 = node.createPublisher( + rcljava.msg.UInt32.class, "test_get_publisher_names_and_types_one"); + Publisher publisher2 = node.createPublisher( + rcljava.msg.UInt32.class, "test_get_publisher_names_and_types_two"); + Publisher publisher3 = remoteNode.createPublisher( + rcljava.msg.UInt32.class, "test_get_publisher_names_and_types_two"); + Publisher publisher4 = remoteNode.createPublisher( + rcljava.msg.UInt32.class, "test_get_publisher_names_and_types_three"); + Subscription subscription = node.createSubscription( + rcljava.msg.Empty.class, "test_get_topic_names_and_types_this_should_not_appear", + new Consumer() { + public void accept(final rcljava.msg.Empty msg) {} + }); + + BiConsumer, Collection> validateNameAndTypes = + new BiConsumer, Collection>() { + public void accept(final Collection local, Collection remote) { + // TODO(ivanpauno): Using assertj may help a lot here https://assertj.github.io/doc/. + assertEquals(local.size(), 2); + assertTrue( + "topic 'test_get_publisher_names_and_types_one' was not discovered for local node", + local.contains( + new NameAndTypes( + "/test_get_publisher_names_and_types_one", + Arrays.asList("rcljava/msg/UInt32")))); + assertTrue( + "topic 'test_get_publisher_names_and_types_two' was not discovered for local node", + local.contains( + new NameAndTypes( + "/test_get_publisher_names_and_types_two", + Arrays.asList("rcljava/msg/UInt32")))); + + assertEquals(remote.size(), 2); + assertTrue( + "topic 'test_get_publisher_names_and_types_two' was not discovered for remote node", + remote.contains( + new NameAndTypes( + "/test_get_publisher_names_and_types_two", + Arrays.asList("rcljava/msg/UInt32")))); + assertTrue( + "topic 'test_get_publisher_names_and_types_three' was not discovered for remote node", + remote.contains( + new NameAndTypes( + "/test_get_publisher_names_and_types_three", + Arrays.asList("rcljava/msg/UInt32")))); + } + }; + + long start = System.currentTimeMillis(); + boolean ok = false; + Collection local = null; + Collection remote = null; + do { + local = this.node.getPublisherNamesAndTypesByNode("test_node", "/"); + remote = this.node.getPublisherNamesAndTypesByNode( + "test_get_publisher_names_and_types_remote_node", "/"); + try { + validateNameAndTypes.accept(local, remote); + ok = true; + } catch (AssertionError err) { + // ignore here, it's going to be validated again at the end. + } + // TODO(ivanpauno): We could wait for the graph guard condition to be triggered if that + // would be available. + try { + TimeUnit.MILLISECONDS.sleep(100); + } catch (InterruptedException err) { + // ignore + } + } while (!ok && System.currentTimeMillis() < start + 1000); + assertNotNull(local); + assertNotNull(remote); + validateNameAndTypes.accept(local, remote); + + publisher1.dispose(); + publisher2.dispose(); + publisher3.dispose(); + publisher4.dispose(); + subscription.dispose(); + remoteNode.dispose(); + } + + @Test + public final void testGetSubscriptionNamesAndTypesByNode() throws Exception { + final Node remoteNode = RCLJava.createNode("test_get_subscription_names_and_types_remote_node"); + Subscription subscription1 = node.createSubscription( + rcljava.msg.Empty.class, "test_get_subscription_names_and_types_one", + new Consumer() { + public void accept(final rcljava.msg.Empty msg) {} + }); + Subscription subscription2 = node.createSubscription( + rcljava.msg.Empty.class, "test_get_subscription_names_and_types_two", + new Consumer() { + public void accept(final rcljava.msg.Empty msg) {} + }); + Subscription subscription3 = remoteNode.createSubscription( + rcljava.msg.Empty.class, "test_get_subscription_names_and_types_two", + new Consumer() { + public void accept(final rcljava.msg.Empty msg) {} + }); + Subscription subscription4 = remoteNode.createSubscription( + rcljava.msg.Empty.class, "test_get_subscription_names_and_types_three", + new Consumer() { + public void accept(final rcljava.msg.Empty msg) {} + }); + Publisher publisher = node.createPublisher( + rcljava.msg.UInt32.class, "test_get_topic_names_and_types_this_should_not_appear"); + + BiConsumer, Collection> validateNameAndTypes = + new BiConsumer, Collection>() { + public void accept(final Collection local, Collection remote) { + // TODO(ivanpauno): Using assertj may help a lot here https://assertj.github.io/doc/. + assertEquals(local.size(), 2); + assertTrue( + "topic 'test_get_subscription_names_and_types_one' was not discovered for local node", + local.contains( + new NameAndTypes( + "/test_get_subscription_names_and_types_one", + Arrays.asList("rcljava/msg/Empty")))); + assertTrue( + "topic 'test_get_subscription_names_and_types_two' was not discovered for local node", + local.contains( + new NameAndTypes( + "/test_get_subscription_names_and_types_two", + Arrays.asList("rcljava/msg/Empty")))); + + assertEquals(remote.size(), 2); + assertTrue( + "topic 'test_get_subscription_names_and_types_two' was not discovered for remote node", + remote.contains( + new NameAndTypes( + "/test_get_subscription_names_and_types_two", + Arrays.asList("rcljava/msg/Empty")))); + assertTrue( + "topic 'test_get_subscription_names_and_types_three' was not discovered for remote node", + remote.contains( + new NameAndTypes( + "/test_get_subscription_names_and_types_three", + Arrays.asList("rcljava/msg/Empty")))); + } + }; + + long start = System.currentTimeMillis(); + boolean ok = false; + Collection local = null; + Collection remote = null; + do { + local = this.node.getSubscriptionNamesAndTypesByNode("test_node", "/"); + remote = this.node.getSubscriptionNamesAndTypesByNode( + "test_get_subscription_names_and_types_remote_node", "/"); + try { + validateNameAndTypes.accept(local, remote); + ok = true; + } catch (AssertionError err) { + // ignore here, it's going to be validated again at the end. + } + // TODO(ivanpauno): We could wait for the graph guard condition to be triggered if that + // would be available. + try { + TimeUnit.MILLISECONDS.sleep(100); + } catch (InterruptedException err) { + // ignore + } + } while (!ok && System.currentTimeMillis() < start + 1000); + assertNotNull(local); + assertNotNull(remote); + validateNameAndTypes.accept(local, remote); + + subscription1.dispose(); + subscription2.dispose(); + subscription3.dispose(); + subscription4.dispose(); + publisher.dispose(); + remoteNode.dispose(); + } + + @Test + public final void testGetServiceNamesAndTypesByNode() throws Exception { + final Node remoteNode = RCLJava.createNode( + "test_get_service_names_and_types_remote_node", "/", + new NodeOptions().setStartParameterServices(false)); + Service service1 = node.createService( + rcljava.srv.AddTwoInts.class, "test_get_service_names_and_types_one", + new TriConsumer< + RMWRequestId, rcljava.srv.AddTwoInts_Request, rcljava.srv.AddTwoInts_Response>() + { + public final void accept( + final RMWRequestId header, + final rcljava.srv.AddTwoInts_Request request, + final rcljava.srv.AddTwoInts_Response response) + {} + }); + Service service2 = node.createService( + rcljava.srv.AddTwoInts.class, "test_get_service_names_and_types_two", + new TriConsumer< + RMWRequestId, rcljava.srv.AddTwoInts_Request, rcljava.srv.AddTwoInts_Response>() + { + public final void accept( + final RMWRequestId header, + final rcljava.srv.AddTwoInts_Request request, + final rcljava.srv.AddTwoInts_Response response) + {} + }); + Service service3 = remoteNode.createService( + rcljava.srv.AddTwoInts.class, "test_get_service_names_and_types_two", + new TriConsumer< + RMWRequestId, rcljava.srv.AddTwoInts_Request, rcljava.srv.AddTwoInts_Response>() + { + public final void accept( + final RMWRequestId header, + final rcljava.srv.AddTwoInts_Request request, + final rcljava.srv.AddTwoInts_Response response) + {} + }); + Service service4 = remoteNode.createService( + rcljava.srv.AddTwoInts.class, "test_get_service_names_and_types_three", + new TriConsumer< + RMWRequestId, rcljava.srv.AddTwoInts_Request, rcljava.srv.AddTwoInts_Response>() + { + public final void accept( + final RMWRequestId header, + final rcljava.srv.AddTwoInts_Request request, + final rcljava.srv.AddTwoInts_Response response) + {} + }); + Client client = node.createClient( + rcljava.srv.AddTwoInts.class, "test_get_service_names_and_types_this_should_not_appear"); + + BiConsumer, Collection> validateNameAndTypes = + new BiConsumer, Collection>() { + public void accept(final Collection local, Collection remote) { + // TODO(ivanpauno): Using assertj may help a lot here https://assertj.github.io/doc/. + assertEquals(local.size(), 2); + assertTrue( + "service 'test_get_service_names_and_types_one' was not discovered for local node", + local.contains( + new NameAndTypes( + "/test_get_service_names_and_types_one", + Arrays.asList("rcljava/srv/AddTwoInts")))); + assertTrue( + "service 'test_get_service_names_and_types_two' was not discovered for local node", + local.contains( + new NameAndTypes( + "/test_get_service_names_and_types_two", + Arrays.asList("rcljava/srv/AddTwoInts")))); + + assertEquals(remote.size(), 2); + assertTrue( + "service 'test_get_service_names_and_types_two' was not discovered for remote node", + remote.contains( + new NameAndTypes( + "/test_get_service_names_and_types_two", + Arrays.asList("rcljava/srv/AddTwoInts")))); + assertTrue( + "service 'test_get_service_names_and_types_three' was not discovered for remote node", + remote.contains( + new NameAndTypes( + "/test_get_service_names_and_types_three", + Arrays.asList("rcljava/srv/AddTwoInts")))); + } + }; + + long start = System.currentTimeMillis(); + boolean ok = false; + Collection local = null; + Collection remote = null; + do { + local = this.node.getServiceNamesAndTypesByNode("test_node", "/"); + remote = this.node.getServiceNamesAndTypesByNode( + "test_get_service_names_and_types_remote_node", "/"); + try { + validateNameAndTypes.accept(local, remote); + ok = true; + } catch (AssertionError err) { + // ignore here, it's going to be validated again at the end. + } + // TODO(ivanpauno): We could wait for the graph guard condition to be triggered if that + // would be available. + try { + TimeUnit.MILLISECONDS.sleep(100); + } catch (InterruptedException err) { + // ignore + } + } while (!ok && System.currentTimeMillis() < start + 1000); + assertNotNull(local); + assertNotNull(remote); + validateNameAndTypes.accept(local, remote); + + service1.dispose(); + service2.dispose(); + service3.dispose(); + service4.dispose(); + client.dispose(); + remoteNode.dispose(); + } + + @Test + public final void testGetClientNamesAndTypesByNode() throws Exception { + final Node remoteNode = RCLJava.createNode( + "test_get_client_names_and_types_remote_node", "/", + new NodeOptions().setStartParameterServices(false)); + Client client1 = node.createClient( + rcljava.srv.AddTwoInts.class, "test_get_client_names_and_types_one"); + Client client2 = node.createClient( + rcljava.srv.AddTwoInts.class, "test_get_client_names_and_types_two"); + Client client3 = remoteNode.createClient( + rcljava.srv.AddTwoInts.class, "test_get_client_names_and_types_two"); + Client client4 = remoteNode.createClient( + rcljava.srv.AddTwoInts.class, "test_get_client_names_and_types_three"); + Service service = node.createService( + rcljava.srv.AddTwoInts.class, "test_get_client_names_and_types_this_should_not_appear", + new TriConsumer< + RMWRequestId, rcljava.srv.AddTwoInts_Request, rcljava.srv.AddTwoInts_Response>() + { + public final void accept( + final RMWRequestId header, + final rcljava.srv.AddTwoInts_Request request, + final rcljava.srv.AddTwoInts_Response response) + {} + }); + + BiConsumer, Collection> validateNameAndTypes = + new BiConsumer, Collection>() { + public void accept(final Collection local, Collection remote) { + // TODO(ivanpauno): Using assertj may help a lot here https://assertj.github.io/doc/. + assertEquals(local.size(), 2); + assertTrue( + "client 'test_get_client_names_and_types_one' was not discovered for local node", + local.contains( + new NameAndTypes( + "/test_get_client_names_and_types_one", + Arrays.asList("rcljava/srv/AddTwoInts")))); + assertTrue( + "client 'test_get_client_names_and_types_two' was not discovered for local node", + local.contains( + new NameAndTypes( + "/test_get_client_names_and_types_two", + Arrays.asList("rcljava/srv/AddTwoInts")))); + + assertEquals(remote.size(), 2); + assertTrue( + "client 'test_get_client_names_and_types_two' was not discovered for remote node", + remote.contains( + new NameAndTypes( + "/test_get_client_names_and_types_two", + Arrays.asList("rcljava/srv/AddTwoInts")))); + assertTrue( + "client 'test_get_client_names_and_types_three' was not discovered for remote node", + remote.contains( + new NameAndTypes( + "/test_get_client_names_and_types_three", + Arrays.asList("rcljava/srv/AddTwoInts")))); + } + }; + + long start = System.currentTimeMillis(); + boolean ok = false; + Collection local = null; + Collection remote = null; + do { + local = this.node.getClientNamesAndTypesByNode("test_node", "/"); + remote = this.node.getClientNamesAndTypesByNode( + "test_get_client_names_and_types_remote_node", "/"); + try { + validateNameAndTypes.accept(local, remote); + ok = true; + } catch (AssertionError err) { + // ignore here, it's going to be validated again at the end. + } + // TODO(ivanpauno): We could wait for the graph guard condition to be triggered if that + // would be available. + try { + TimeUnit.MILLISECONDS.sleep(100); + } catch (InterruptedException err) { + // ignore + } + } while (!ok && System.currentTimeMillis() < start + 1000); + assertNotNull(local); + assertNotNull(remote); + validateNameAndTypes.accept(local, remote); + + client1.dispose(); + client2.dispose(); + client3.dispose(); + client4.dispose(); + service.dispose(); + remoteNode.dispose(); } } diff --git a/rcljava/src/test/java/org/ros2/rcljava/node/NodeUndeclaredParametersTest.java b/rcljava/src/test/java/org/ros2/rcljava/node/NodeUndeclaredParametersTest.java index 454e62bd..92ee605b 100644 --- a/rcljava/src/test/java/org/ros2/rcljava/node/NodeUndeclaredParametersTest.java +++ b/rcljava/src/test/java/org/ros2/rcljava/node/NodeUndeclaredParametersTest.java @@ -67,7 +67,7 @@ public static void tearDownOnce() { public void setUp() { NodeOptions options = new NodeOptions(); options.setAllowUndeclaredParameters(true); - node = RCLJava.createNode("test_node", "", RCLJava.getDefaultContext(), options); + node = RCLJava.createNode("test_node", "", options); } @After diff --git a/rcljava/src/test/java/org/ros2/rcljava/parameters/AsyncParametersClientTest.java b/rcljava/src/test/java/org/ros2/rcljava/parameters/AsyncParametersClientTest.java index 1c94bce5..8ce0b5cb 100644 --- a/rcljava/src/test/java/org/ros2/rcljava/parameters/AsyncParametersClientTest.java +++ b/rcljava/src/test/java/org/ros2/rcljava/parameters/AsyncParametersClientTest.java @@ -15,6 +15,7 @@ package org.ros2.rcljava.parameters; +import static org.junit.Assert.assertArrayEquals; import static org.junit.Assert.assertEquals; import static org.junit.Assert.assertNotEquals; import static org.junit.Assert.assertTrue; @@ -33,11 +34,13 @@ import java.util.List; import java.util.concurrent.Future; +import java.util.concurrent.TimeUnit; import org.ros2.rcljava.RCLJava; import org.ros2.rcljava.concurrent.RCLFuture; import org.ros2.rcljava.consumers.Consumer; import org.ros2.rcljava.node.Node; +import org.ros2.rcljava.node.NodeOptions; import org.ros2.rcljava.parameters.ParameterVariant; import org.ros2.rcljava.parameters.client.AsyncParametersClient; import org.ros2.rcljava.parameters.client.AsyncParametersClientImpl; @@ -74,7 +77,7 @@ public static void setupOnce() throws Exception { { // Configure log4j. Doing this dynamically so that Android does not complain about missing // the log4j JARs, SLF4J uses Android's native logging mechanism instead. - Class c = Class.forName("org.apache.log4j.BasicConfigurator"); + Class c = Class.forName("org.apache.log4j.BasicConfigurator"); Method m = c.getDeclaredMethod("configure", (Class[]) null); Object o = m.invoke(null, (Object[]) null); } @@ -86,7 +89,9 @@ public static void setupOnce() throws Exception { @Before public void setUp() throws Exception { - node = RCLJava.createNode("test_node"); + NodeOptions opts = new NodeOptions(); + opts.setAllowUndeclaredParameters(true); + node = RCLJava.createNode("test_node", "", opts); parameterService = new ParameterServiceImpl(node); parametersClient = new AsyncParametersClientImpl(node); } @@ -109,15 +114,22 @@ public final void testSetParameters() throws Exception { new ParameterVariant("foo.second", 42), new ParameterVariant("foobar", true)}); RCLFuture> future = - new RCLFuture>( - new WeakReference(this.node)); - parametersClient.setParameters(parameters, new TestConsumer(future)); + new RCLFuture>(); + parametersClient.setParameters( + parameters, + new TestConsumer>(future)); + + RCLJava.spinUntilComplete(node, future); + List setParametersResults = future.get(); + assertEquals(6, setParametersResults.size()); + for (rcl_interfaces.msg.SetParametersResult result : setParametersResults) { + assertEquals(true, result.getSuccessful()); + } List parameterNames = - Arrays.asList(new String[] {"foo", "bar", "baz", "foo.first", "foo.second", "foobar"}); - + Arrays.asList(new String[] {"foo", "bar", "baz", "foo.first", "foo.second", "foobar"}); List results = node.getParameters(parameterNames); - assertEquals(parameters, future.get()); + assertEquals(parameters, results); } @Test @@ -132,10 +144,12 @@ public final void testGetParameters() throws Exception { List parameterNames = Arrays.asList(new String[] {"foo", "bar", "baz", "foo.first", "foo.second", "foobar"}); - RCLFuture> future = - new RCLFuture>(new WeakReference(this.node)); - parametersClient.getParameters(parameterNames, new TestConsumer(future)); + RCLFuture> future = new RCLFuture>(); + parametersClient.getParameters( + parameterNames, + new TestConsumer>(future)); + RCLJava.spinUntilComplete(node, future); assertEquals(parameters, future.get()); } @@ -149,12 +163,16 @@ public final void testListParameters() throws Exception { node.setParameters(parameters); RCLFuture future = - new RCLFuture(new WeakReference(this.node)); + new RCLFuture(); parametersClient.listParameters( - Arrays.asList(new String[] {"foo", "bar"}), 10, new TestConsumer(future)); - - assertEquals(Arrays.asList(new String[] {"foo.first", "foo.second"}), future.get().getNames()); - assertEquals(Arrays.asList(new String[] {"foo"}), future.get().getPrefixes()); + Arrays.asList( + new String[] {"foo", "bar"}), + 10, + new TestConsumer(future)); + + RCLJava.spinUntilComplete(node, future); + assertArrayEquals(new String[] {"foo.first", "foo.second"}, future.get().getNames()); + assertArrayEquals(new String[] {"foo"}, future.get().getPrefixes()); } @Test @@ -167,10 +185,10 @@ public final void testDescribeParameters() throws Exception { node.setParameters(parameters); RCLFuture> future = - new RCLFuture>( - new WeakReference(this.node)); + new RCLFuture>(); parametersClient.describeParameters( - Arrays.asList(new String[] {"foo", "bar"}), new TestConsumer(future)); + Arrays.asList(new String[] {"foo", "bar"}), + new TestConsumer>(future)); List expected = Arrays.asList(new rcl_interfaces.msg.ParameterDescriptor[] { @@ -178,6 +196,7 @@ public final void testDescribeParameters() throws Exception { rcl_interfaces.msg.ParameterType.PARAMETER_INTEGER), new rcl_interfaces.msg.ParameterDescriptor().setName("bar").setType( rcl_interfaces.msg.ParameterType.PARAMETER_STRING)}); + RCLJava.spinUntilComplete(node, future); assertEquals(expected, future.get()); } } diff --git a/rcljava/src/test/java/org/ros2/rcljava/parameters/SyncParametersClientTest.java b/rcljava/src/test/java/org/ros2/rcljava/parameters/SyncParametersClientTest.java index 197e5cef..89b3c346 100644 --- a/rcljava/src/test/java/org/ros2/rcljava/parameters/SyncParametersClientTest.java +++ b/rcljava/src/test/java/org/ros2/rcljava/parameters/SyncParametersClientTest.java @@ -15,6 +15,7 @@ package org.ros2.rcljava.parameters; +import static org.junit.Assert.assertArrayEquals; import static org.junit.Assert.assertEquals; import static org.junit.Assert.assertNotEquals; import static org.junit.Assert.assertTrue; @@ -38,6 +39,7 @@ import org.ros2.rcljava.concurrent.RCLFuture; import org.ros2.rcljava.consumers.Consumer; import org.ros2.rcljava.node.Node; +import org.ros2.rcljava.node.NodeOptions; import org.ros2.rcljava.parameters.ParameterVariant; import org.ros2.rcljava.parameters.client.SyncParametersClient; import org.ros2.rcljava.parameters.client.SyncParametersClientImpl; @@ -57,7 +59,7 @@ public static void setupOnce() throws Exception { { // Configure log4j. Doing this dynamically so that Android does not complain about missing // the log4j JARs, SLF4J uses Android's native logging mechanism instead. - Class c = Class.forName("org.apache.log4j.BasicConfigurator"); + Class c = Class.forName("org.apache.log4j.BasicConfigurator"); Method m = c.getDeclaredMethod("configure", (Class[]) null); Object o = m.invoke(null, (Object[]) null); } @@ -69,7 +71,9 @@ public static void setupOnce() throws Exception { @Before public void setUp() throws Exception { - node = RCLJava.createNode("test_node"); + NodeOptions opts = new NodeOptions(); + opts.setAllowUndeclaredParameters(true); + node = RCLJava.createNode("test_node", "", opts); parameterService = new ParameterServiceImpl(node); parametersClient = new SyncParametersClientImpl(node); } @@ -130,9 +134,9 @@ public final void testListParameters() throws Exception { rcl_interfaces.msg.ListParametersResult parametersAndPrefixes = parametersClient.listParameters(Arrays.asList(new String[] {"foo", "bar"}), 10); - assertEquals( - parametersAndPrefixes.getNames(), Arrays.asList(new String[] {"foo.first", "foo.second"})); - assertEquals(parametersAndPrefixes.getPrefixes(), Arrays.asList(new String[] {"foo"})); + assertArrayEquals( + parametersAndPrefixes.getNames(), new String[] {"foo.first", "foo.second"}); + assertArrayEquals(parametersAndPrefixes.getPrefixes(), new String[] {"foo"}); } @Test diff --git a/rcljava/src/test/java/org/ros2/rcljava/subscription/SubscriptionTest.java b/rcljava/src/test/java/org/ros2/rcljava/subscription/SubscriptionTest.java index 21299024..59976add 100644 --- a/rcljava/src/test/java/org/ros2/rcljava/subscription/SubscriptionTest.java +++ b/rcljava/src/test/java/org/ros2/rcljava/subscription/SubscriptionTest.java @@ -28,6 +28,7 @@ import org.ros2.rcljava.events.EventHandler; import org.ros2.rcljava.node.Node; import org.ros2.rcljava.subscription.statuses.LivelinessChanged; +import org.ros2.rcljava.subscription.statuses.MessageLost; import org.ros2.rcljava.subscription.statuses.RequestedDeadlineMissed; import org.ros2.rcljava.subscription.statuses.RequestedQosIncompatible; @@ -149,4 +150,32 @@ public void accept(final RequestedQosIncompatible status) { RCLJava.shutdown(); assertEquals(0, eventHandler.getHandle()); } + + @Test + public final void testCreateMessageLost() { + String identifier = RCLJava.getRMWIdentifier(); + if (identifier.equals("rmw_fastrtps_cpp") || identifier.equals("rmw_fastrtps_dynamic_cpp")) { + // event not supported in these implementations + return; + } + RCLJava.rclJavaInit(); + Node node = RCLJava.createNode("test_node"); + Subscription subscription = node.createSubscription( + std_msgs.msg.String.class, "test_topic", new Consumer() { + public void accept(final std_msgs.msg.String msg) {} + }); + EventHandler eventHandler = subscription.createEventHandler( + MessageLost.factory, new Consumer() { + public void accept(final MessageLost status) { + assertEquals(status.totalCount, 0); + assertEquals(status.totalCountChange, 0); + } + } + ); + assertNotEquals(0, eventHandler.getHandle()); + // force executing the callback, so we check that taking an event works + eventHandler.executeCallback(); + RCLJava.shutdown(); + assertEquals(0, eventHandler.getHandle()); + } } diff --git a/rcljava/src/test/java/org/ros2/rcljava/time/TimeSourceTest.java b/rcljava/src/test/java/org/ros2/rcljava/time/TimeSourceTest.java new file mode 100644 index 00000000..86bbddff --- /dev/null +++ b/rcljava/src/test/java/org/ros2/rcljava/time/TimeSourceTest.java @@ -0,0 +1,187 @@ +/* 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.time; + +import java.lang.SuppressWarnings; + +import static org.junit.Assert.assertTrue; +import static org.junit.Assert.assertFalse; + +import org.junit.AfterClass; +import org.junit.BeforeClass; +import org.junit.Test; + +import org.junit.runner.RunWith; + +import static org.mockito.Mockito.*; +import org.mockito.Mock; +import org.mockito.junit.MockitoJUnitRunner; + +import org.ros2.rcljava.consumers.Consumer; +import org.ros2.rcljava.RCLJava; +import org.ros2.rcljava.node.Node; +import org.ros2.rcljava.parameters.ParameterVariant; +import org.ros2.rcljava.subscription.Subscription; +import org.ros2.rcljava.time.TimeSource; + +@RunWith(MockitoJUnitRunner.StrictStubs.class) +public class TimeSourceTest { + @Mock + private Node mockedNode; + + @Mock + private Clock mockedClock; + + @Mock + private Subscription mockSubscription; + + @BeforeClass + public static void setupOnce() throws Exception { + // Just to quiet down warnings + org.apache.log4j.BasicConfigurator.configure(); + + RCLJava.rclJavaInit(); + } + + @AfterClass + public static void tearDownOnce() { + RCLJava.shutdown(); + } + + @Test + public final void testEmptyConstructor() { + TimeSource timeSource = new TimeSource(); + assertFalse(timeSource.getRosTimeIsActive()); + } + + @Test + public final void testConstructorWithNode() { + when(mockedNode.getParameter("use_sim_time")).thenReturn(new ParameterVariant("use_sim_time", false)); + + TimeSource timeSource = new TimeSource(mockedNode); + assertFalse(timeSource.getRosTimeIsActive()); + } + + @Test + public final void testAttachNodeUseSimTimeFalse() { + when(mockedNode.getParameter("use_sim_time")).thenReturn(new ParameterVariant("use_sim_time", false)); + + TimeSource timeSource = new TimeSource(); + timeSource.attachNode(mockedNode); + assertFalse(timeSource.getRosTimeIsActive()); + } + + @Test + public final void testAttachNodeUseSimTimeTrue() { + when(mockedNode.getParameter("use_sim_time")).thenReturn(new ParameterVariant("use_sim_time", true)); + + TimeSource timeSource = new TimeSource(); + timeSource.attachNode(mockedNode); + assertTrue(timeSource.getRosTimeIsActive()); + } + + @Test + public final void testAttachNodeTwice() { + when(mockedNode.getParameter("use_sim_time")).thenReturn(new ParameterVariant("use_sim_time", true)); + + TimeSource timeSource = new TimeSource(); + timeSource.attachNode(mockedNode); + assertTrue(timeSource.getRosTimeIsActive()); + + // Attach the same node again + timeSource.attachNode(mockedNode); + assertTrue(timeSource.getRosTimeIsActive()); + } + + @Test + public final void testDetachNode() { + when(mockedNode.getParameter("use_sim_time")).thenReturn(new ParameterVariant("use_sim_time", true)); + + // Attaches node with ROS time active + TimeSource timeSource = new TimeSource(mockedNode); + assertTrue(timeSource.getRosTimeIsActive()); + + timeSource.detachNode(); + assertFalse(timeSource.getRosTimeIsActive()); + + // Calling detach again shouldn't change anything + timeSource.detachNode(); + assertFalse(timeSource.getRosTimeIsActive()); + } + + @Test + public final void testAttachClock() { + when(mockedClock.getClockType()).thenReturn(ClockType.ROS_TIME); + + TimeSource timeSource = new TimeSource(); + // Attaching a clock should notifiy the clock + timeSource.attachClock(mockedClock); + verify(mockedClock).setRosTimeIsActive(false); + + // Setting ROS time active should notify clock + timeSource.setRosTimeIsActive(true); + verify(mockedClock).setRosTimeIsActive(true); + } + + @Test(expected = IllegalArgumentException.class) + public final void testAttachClockInvalidType() { + TimeSource timeSource = new TimeSource(); + timeSource.attachClock(mockedClock); + } + + @Test + public final void testDetachClock() { + when(mockedClock.getClockType()).thenReturn(ClockType.ROS_TIME); + + TimeSource timeSource = new TimeSource(); + timeSource.attachClock(mockedClock); + timeSource.detachClock(mockedClock); + + // Setting ROS time active should not notify a detached clock + timeSource.setRosTimeIsActive(true); + verify(mockedClock, never()).setRosTimeIsActive(true); + } + + @Test + public final void testSetRosTimeIsActiveNoNode() { + TimeSource timeSource = new TimeSource(); + timeSource.setRosTimeIsActive(false); + assertFalse(timeSource.getRosTimeIsActive()); + timeSource.setRosTimeIsActive(true); + assertTrue(timeSource.getRosTimeIsActive()); + } + + // there's no generic parameter type information at runtime, so we cannot write any(Consumer.class) + @SuppressWarnings("unchecked") + @Test + public final void testSetRosTimeIsActiveWithNode() { + when(mockedNode.getParameter("use_sim_time")).thenReturn(new ParameterVariant("use_sim_time", false)); + when(mockedNode.createSubscription(eq(rosgraph_msgs.msg.Clock.class), anyString(), any(Consumer.class))) + .thenReturn(mockSubscription); + + TimeSource timeSource = new TimeSource(mockedNode); + timeSource.setRosTimeIsActive(false); + assertFalse(timeSource.getRosTimeIsActive()); + timeSource.setRosTimeIsActive(true); + assertTrue(timeSource.getRosTimeIsActive()); + // Expect subscription for the "/clock" topic when set active + verify(mockedNode).createSubscription(eq(rosgraph_msgs.msg.Clock.class), eq("/clock"), any(Consumer.class)); + timeSource.setRosTimeIsActive(false); + assertFalse(timeSource.getRosTimeIsActive()); + // Expect subscription removed when set not active + verify(mockedNode).removeSubscription(any(Subscription.class)); + } +} diff --git a/rcljava/src/test/java/org/ros2/rcljava/timer/TimerTest.java b/rcljava/src/test/java/org/ros2/rcljava/timer/TimerTest.java index 9eff6b17..461f5734 100644 --- a/rcljava/src/test/java/org/ros2/rcljava/timer/TimerTest.java +++ b/rcljava/src/test/java/org/ros2/rcljava/timer/TimerTest.java @@ -25,6 +25,7 @@ import java.util.concurrent.Future; import java.util.concurrent.TimeUnit; +import org.junit.AfterClass; import org.junit.BeforeClass; import org.junit.Test; @@ -72,32 +73,58 @@ public static void setupOnce() throws Exception { { e.printStackTrace(); } + + RCLJava.rclJavaInit(); + } + + @AfterClass + public static void tearDownOnce() { + RCLJava.shutdown(); } @Test - public final void testCreate() { + public final void testCreateWallTimer() throws Exception { int max_iterations = 4; - RCLJava.rclJavaInit(); - Node node = RCLJava.createNode("test_node"); - RCLFuture future = new RCLFuture(new WeakReference(node)); + Node node = RCLJava.createNode("test_timer_node"); + RCLFuture future = new RCLFuture(); TimerCallback timerCallback = new TimerCallback(future, max_iterations); - WallTimer timer = node.createWallTimer(250, TimeUnit.MILLISECONDS, timerCallback); + Timer timer = node.createWallTimer(250, TimeUnit.MILLISECONDS, timerCallback); assertNotEquals(0, timer.getHandle()); + assertFalse(timer.isCanceled()); + assertEquals( + TimeUnit.NANOSECONDS.convert(250, TimeUnit.MILLISECONDS), timer.getTimerPeriodNS()); - while (RCLJava.ok() && !future.isDone()) { - RCLJava.spinOnce(node); - } + RCLJava.spinUntilComplete(node, future); + boolean result = future.get(3, TimeUnit.SECONDS); + assertTrue(result); + assertEquals(4, timerCallback.getCounter()); - assertFalse(timer.isCanceled()); timer.cancel(); + assertFalse(timer.isReady()); + assertTrue(timer.isCanceled()); + } + + @Test + public final void testCreateTimer() throws Exception { + int max_iterations = 4; + Node node = RCLJava.createNode("test_timer_node"); + RCLFuture future = new RCLFuture(); + TimerCallback timerCallback = new TimerCallback(future, max_iterations); + Timer timer = node.createTimer(250, TimeUnit.MILLISECONDS, timerCallback); + assertNotEquals(0, timer.getHandle()); + assertFalse(timer.isCanceled()); assertEquals( TimeUnit.NANOSECONDS.convert(250, TimeUnit.MILLISECONDS), timer.getTimerPeriodNS()); - assertFalse(timer.isReady()); - assertTrue(timer.isCanceled()); + RCLJava.spinUntilComplete(node, future); + boolean result = future.get(3, TimeUnit.SECONDS); + assertTrue(result); assertEquals(4, timerCallback.getCounter()); - RCLJava.shutdown(); + + timer.cancel(); + assertFalse(timer.isReady()); + assertTrue(timer.isCanceled()); } } diff --git a/rcljava_common/CMakeLists.txt b/rcljava_common/CMakeLists.txt index 3bc04fde..228f24ea 100644 --- a/rcljava_common/CMakeLists.txt +++ b/rcljava_common/CMakeLists.txt @@ -20,6 +20,7 @@ if(NOT ANDROID) find_package(JNI REQUIRED) endif() include(UseJava) +include(JavaExtra) # Default to C++14 if(NOT CMAKE_CXX_STANDARD) @@ -38,7 +39,15 @@ 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/FeedbackDefinition.java" + "src/main/java/org/ros2/rcljava/interfaces/FeedbackMessageDefinition.java" + "src/main/java/org/ros2/rcljava/interfaces/GoalDefinition.java" + "src/main/java/org/ros2/rcljava/interfaces/GoalResponseDefinition.java" + "src/main/java/org/ros2/rcljava/interfaces/GoalRequestDefinition.java" "src/main/java/org/ros2/rcljava/interfaces/MessageDefinition.java" + "src/main/java/org/ros2/rcljava/interfaces/ResultDefinition.java" + "src/main/java/org/ros2/rcljava/interfaces/ResultRequestDefinition.java" + "src/main/java/org/ros2/rcljava/interfaces/ResultResponseDefinition.java" "src/main/java/org/ros2/rcljava/interfaces/ServiceDefinition.java" ) @@ -146,6 +155,14 @@ list(APPEND ${PROJECT_NAME}_exported_jars "share/${PROJECT_NAME}/java/${PROJECT_ ament_export_jars(${${PROJECT_NAME}_exported_jars}) +add_source_jar("${PROJECT_NAME}-source_jar" + ${${PROJECT_NAME}_java_sources} + OUTPUT_NAME + ${PROJECT_NAME}-source +) + +install_jar("${PROJECT_NAME}-source_jar" "share/${PROJECT_NAME}/java") + include_directories(include) add_library(${PROJECT_NAME} SHARED ${${PROJECT_NAME}_cpp_sources}) diff --git a/rcljava_common/cmake/Modules/JavaExtra.cmake b/rcljava_common/cmake/Modules/JavaExtra.cmake index 0ab52441..259345f7 100644 --- a/rcljava_common/cmake/Modules/JavaExtra.cmake +++ b/rcljava_common/cmake/Modules/JavaExtra.cmake @@ -165,3 +165,46 @@ function(ament_add_junit_tests TARGET_NAME) add_dependencies(${TARGET_NAME} "${TARGET_NAME}_jar") endfunction() + +# +# Creates a sources jar from list of source files +# +# :param OUTPUT_NAME: Name of Jar +# :type OUTPUT_NAME: string +# :param SOURCES: Sources files +# :type SOURCES: String list with file names +# +# @public +# +function(add_source_jar _TARGET_NAME) + + cmake_parse_arguments(_add_source_jar + "" + "OUTPUT_NAME" + "SOURCES" + ${ARGN} + ) + + if(NOT _add_source_jar_OUTPUT_NAME) + set(_add_source_jar_OUTPUT_NAME "${_TARGET_NAME}.jar") + endif() + + set(_SOURCE_FILES ${_add_source_jar_SOURCES} ${_add_source_jar_UNPARSED_ARGUMENTS}) + set(_JAVA_JAR_SOURCES_OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/${_add_source_jar_OUTPUT_NAME}.jar) + + add_custom_command( + OUTPUT ${_add_source_jar_OUTPUT_NAME} + COMMAND ${Java_JAR_EXECUTABLE} -cf ${_JAVA_JAR_SOURCES_OUTPUT} ${_SOURCE_FILES} + WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} + ) + add_custom_target(${_TARGET_NAME} ALL DEPENDS ${_add_source_jar_OUTPUT_NAME}) + + # Set install property (for 'install_jar()'). + set_property( + TARGET ${_TARGET_NAME} + PROPERTY + INSTALL_FILES + ${_JAVA_JAR_SOURCES_OUTPUT} + ) + +endfunction() diff --git a/rcljava_common/include/rcljava_common/exceptions.hpp b/rcljava_common/include/rcljava_common/exceptions.hpp index 8a148611..b7f2d3c5 100644 --- a/rcljava_common/include/rcljava_common/exceptions.hpp +++ b/rcljava_common/include/rcljava_common/exceptions.hpp @@ -19,6 +19,57 @@ #include "rcljava_common/visibility_control.hpp" +/// Execute \a error_statement if an exception has happened. +/** + * \param env a JNIEnv pointer, used to check for exceptions. + * \param error_statement statement executed if an exception has happened. + */ +#define RCLJAVA_COMMON_CHECK_FOR_EXCEPTION_WITH_ERROR_STATEMENT(env, error_statement) \ + do { \ + if (env->ExceptionCheck()) { \ + error_statement; \ + } \ + } while (0) + +/// Return from the current function if a java exception has happened. +/** + * \param env a JNIEnv pointer, used to check for exceptions. + */ +#define RCLJAVA_COMMON_CHECK_FOR_EXCEPTION(env) \ + RCLJAVA_COMMON_CHECK_FOR_EXCEPTION_WITH_ERROR_STATEMENT(env, return ) + +/// Call \ref rcljava_throw_rclexception if \a ret is not RCL_RET_OK, +/// and execute \a error_statement in that case. +/** + * The rcl error message will be added to \a base_message, and the rcl error state will be reset. + * + * \param env a JNIEnv pointer, used to throw a java exception from the rcl error. + * \param ret rcl_ret_t error that will be checked. + * \param base_message error message that will be passed to the thrown exception. + * The complete error will be "${base_message}: ${rcl_error_string}". + * \a base_message can be either a `const char *` or as `std::string`. + * \param error_statement statement executed if ret was not RCL_RET_OK. + */ +#define RCLJAVA_COMMON_THROW_FROM_RCL_WITH_ERROR_STATEMENT( \ + env, ret, base_message, error_statement) \ + do { \ + if (RCL_RET_OK != ret) { \ + rcljava_common::exceptions::rcljava_throw_rclexception( \ + env, ret, static_cast(base_message) + ": " + rcl_get_error_string().str); \ + rcl_reset_error(); \ + error_statement; \ + } \ + } while (0) + +/// Call \ref rcljava_throw_rclexception if \a ret is not RCL_RET_OK and return. +/** + * \param env a JNIEnv pointer, used to check for exceptions. + * \param ret rcl_ret_t error that will be checked. + * \param base_message error message that will be passed to the thrown exception. + */ +#define RCLJAVA_COMMON_THROW_FROM_RCL(env, ret, base_message) \ + RCLJAVA_COMMON_THROW_FROM_RCL_WITH_ERROR_STATEMENT(env, ret, base_message, return ) + namespace rcljava_common { namespace exceptions diff --git a/rcljava_common/src/main/java/org/ros2/rcljava/common/JNIUtils.java b/rcljava_common/src/main/java/org/ros2/rcljava/common/JNIUtils.java index 58661a7f..c335be1c 100644 --- a/rcljava_common/src/main/java/org/ros2/rcljava/common/JNIUtils.java +++ b/rcljava_common/src/main/java/org/ros2/rcljava/common/JNIUtils.java @@ -43,14 +43,14 @@ private static String normalizeClassName(Class cls) { public static void loadImplementation(Class cls) { String libraryName = normalizeClassName(cls); libraryName = libraryName + "__jni"; - logger.info("Loading implementation: " + libraryName); + logger.debug("Loading implementation: " + libraryName); System.loadLibrary(libraryName); } public static void loadTypesupport(Class cls) { String libraryName = normalizeClassName(cls); libraryName = libraryName + "__jni__" + JNIUtils.TYPESUPPORT; - logger.info("Loading typesupport: " + libraryName); + logger.debug("Loading typesupport: " + libraryName); System.loadLibrary(libraryName); } } 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..d225901e 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,12 @@ package org.ros2.rcljava.interfaces; -public interface ActionDefinition {} +public interface ActionDefinition { + Class getSendGoalRequestType(); + Class getSendGoalResponseType(); + Class getGetResultRequestType(); + Class getGetResultResponseType(); + Class getResultType(); + Class getFeedbackType(); + Class getFeedbackMessageType(); +} diff --git a/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/FeedbackDefinition.java b/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/FeedbackDefinition.java new file mode 100644 index 00000000..daff4162 --- /dev/null +++ b/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/FeedbackDefinition.java @@ -0,0 +1,25 @@ +/* Copyright 2021 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; + +/** +* Definition of an action feedback. +* Implementation of this interface for an Action is automatically generated. +* +* Don't extend this interface yourself!! +*/ +public interface FeedbackDefinition extends MessageDefinition { +} diff --git a/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/FeedbackMessageDefinition.java b/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/FeedbackMessageDefinition.java new file mode 100644 index 00000000..8ef39114 --- /dev/null +++ b/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/FeedbackMessageDefinition.java @@ -0,0 +1,29 @@ +/* Copyright 2021 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; + +/** +* Definition of an action feedback message. +* Implementation of this interface for an Action is automatically generated. +* +* Don't extend this interface yourself!! +*/ +public interface FeedbackMessageDefinition extends MessageDefinition { + void setFeedback(FeedbackDefinition feedback); + void setGoalUuid(List goalUuid); +} diff --git a/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/GoalDefinition.java b/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/GoalDefinition.java new file mode 100644 index 00000000..3aeea1af --- /dev/null +++ b/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/GoalDefinition.java @@ -0,0 +1,25 @@ +/* Copyright 2021 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; + +/** +* Definition of an action goal. +* Implementation of this interface for an Action is automatically generated. +* +* Don't extend this interface yourself!! +*/ +public interface GoalDefinition extends MessageDefinition { +} 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..5191a4ab --- /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 { + GoalDefinition 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/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/ResultDefinition.java b/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/ResultDefinition.java new file mode 100644 index 00000000..daec4304 --- /dev/null +++ b/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/ResultDefinition.java @@ -0,0 +1,25 @@ +/* Copyright 2021 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; + +/** +* Definition of an action result. +* Implementation of this interface for an Action is automatically generated. +* +* Don't extend this interface yourself!! +*/ +public interface ResultDefinition extends MessageDefinition { +} diff --git a/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/ResultRequestDefinition.java b/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/ResultRequestDefinition.java new file mode 100644 index 00000000..f259e1e7 --- /dev/null +++ b/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/ResultRequestDefinition.java @@ -0,0 +1,22 @@ +/* Copyright 2021 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 ResultRequestDefinition extends MessageDefinition { + List getGoalUuid(); +} diff --git a/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/ResultResponseDefinition.java b/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/ResultResponseDefinition.java new file mode 100644 index 00000000..86a927e7 --- /dev/null +++ b/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/ResultResponseDefinition.java @@ -0,0 +1,23 @@ +/* Copyright 2021 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 ResultResponseDefinition extends MessageDefinition { + void setResult(ResultDefinition result); + void setGoalStatus(byte status); +} diff --git a/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/ServiceDefinition.java b/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/ServiceDefinition.java index 8e3bf79e..cade9a22 100644 --- a/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/ServiceDefinition.java +++ b/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/ServiceDefinition.java @@ -15,4 +15,7 @@ package org.ros2.rcljava.interfaces; -public interface ServiceDefinition {} +public interface ServiceDefinition { + MessageDefinition newRequestInstance(); + MessageDefinition newResponseInstance(); +} diff --git a/ros2_java_desktop.repos b/ros2_java_desktop.repos index 1b10ef90..9ed0e919 100644 --- a/ros2_java_desktop.repos +++ b/ros2_java_desktop.repos @@ -23,6 +23,10 @@ repositories: type: git url: https://github.com/ros2-java/ament_java.git version: main + ros2-java/mockito_vendor: + type: git + url: https://github.com/ros2-java/mockito_vendor.git + version: main ros2-java/ros2_java: type: git url: https://github.com/ros2-java/ros2_java.git diff --git a/rosidl_generator_java/CMakeLists.txt b/rosidl_generator_java/CMakeLists.txt index d0c5d304..777cac9a 100644 --- a/rosidl_generator_java/CMakeLists.txt +++ b/rosidl_generator_java/CMakeLists.txt @@ -40,7 +40,7 @@ if(BUILD_TESTING) # TODO(jacobperron): This fixes generating messages with wstrings. # Figure out why this is needed as of Foxy, and how we can remove this workaround - set(CMAKE_JAVA_COMPILE_FLAGS "-encoding" "ISO-8859-1") + list(APPEND CMAKE_JAVA_COMPILE_FLAGS "-encoding" "ISO-8859-1") include(cmake/register_java.cmake) include(cmake/rosidl_generator_java_get_typesupports.cmake) diff --git a/rosidl_generator_java/cmake/custom_command.cmake b/rosidl_generator_java/cmake/custom_command.cmake index db7d1cc1..9cdaaa79 100644 --- a/rosidl_generator_java/cmake/custom_command.cmake +++ b/rosidl_generator_java/cmake/custom_command.cmake @@ -43,4 +43,11 @@ add_jar("${PROJECT_NAME}_messages_jar" ${_jar_deps} ) +add_source_jar("${PROJECT_NAME}_messages_source_jar" + ${_generated_java_files} + OUTPUT_NAME + ${PROJECT_NAME}_messages-source +) + add_dependencies("${PROJECT_NAME}_messages_jar" "${rosidl_generate_interfaces_TARGET}${_target_suffix}") +add_dependencies("${PROJECT_NAME}_messages_source_jar" "${rosidl_generate_interfaces_TARGET}${_target_suffix}") diff --git a/rosidl_generator_java/cmake/rosidl_generator_java_generate_interfaces.cmake b/rosidl_generator_java/cmake/rosidl_generator_java_generate_interfaces.cmake index 0093329c..5ebdba1a 100644 --- a/rosidl_generator_java/cmake/rosidl_generator_java_generate_interfaces.cmake +++ b/rosidl_generator_java/cmake/rosidl_generator_java_generate_interfaces.cmake @@ -31,6 +31,7 @@ if(NOT ANDROID) find_package(JNI REQUIRED) endif() include(UseJava) +include(JavaExtra) if(NOT WIN32) if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU") @@ -86,6 +87,7 @@ foreach(_abs_idl_file ${rosidl_generate_interfaces_ABS_IDL_FILES}) "${_output_path}/${_parent_folder}/${_idl_name}_Goal.java" "${_output_path}/${_parent_folder}/${_idl_name}_Result.java" "${_output_path}/${_parent_folder}/${_idl_name}_Feedback.java" + "${_output_path}/${_parent_folder}/${_idl_name}_FeedbackMessage.java" "${_output_path}/${_parent_folder}/${_idl_name}_SendGoal.java" "${_output_path}/${_parent_folder}/${_idl_name}_SendGoal_Request.java" "${_output_path}/${_parent_folder}/${_idl_name}_SendGoal_Response.java" @@ -98,6 +100,7 @@ foreach(_abs_idl_file ${rosidl_generate_interfaces_ABS_IDL_FILES}) "${_output_path}/${_parent_folder}/${_idl_name}_Goal.ep.${_typesupport_impl}.cpp" "${_output_path}/${_parent_folder}/${_idl_name}_Result.ep.${_typesupport_impl}.cpp" "${_output_path}/${_parent_folder}/${_idl_name}_Feedback.ep.${_typesupport_impl}.cpp" + "${_output_path}/${_parent_folder}/${_idl_name}_FeedbackMessage.ep.${_typesupport_impl}.cpp" "${_output_path}/${_parent_folder}/${_idl_name}_SendGoal.ep.${_typesupport_impl}.cpp" "${_output_path}/${_parent_folder}/${_idl_name}_SendGoal_Request.ep.${_typesupport_impl}.cpp" "${_output_path}/${_parent_folder}/${_idl_name}_SendGoal_Response.ep.${_typesupport_impl}.cpp" @@ -306,6 +309,8 @@ if(NOT rosidl_generate_interfaces_SKIP_INSTALL) if(NOT _generated_java_files STREQUAL "") install_jar("${PROJECT_NAME}_messages_jar" "share/${PROJECT_NAME}/java") ament_export_jars("share/${PROJECT_NAME}/java/${PROJECT_NAME}_messages.jar") + install_jar("${PROJECT_NAME}_messages_source_jar" "share/${PROJECT_NAME}/java") + ament_export_jars("share/${PROJECT_NAME}/java/${PROJECT_NAME}_messages-source.jar") endif() endif() diff --git a/rosidl_generator_java/resource/action.cpp.em b/rosidl_generator_java/resource/action.cpp.em index 0d3492e4..1cae4e8c 100644 --- a/rosidl_generator_java/resource/action.cpp.em +++ b/rosidl_generator_java/resource/action.cpp.em @@ -1,4 +1,7 @@ @# Included from rosidl_generator_java/resource/idl.cpp.em +// generated from rosidl_generator_java/resource/action.cpp.em +// with input from @(package_name):@(interface_path) +// generated code does not contain a copyright notice @{ import os @@ -16,6 +19,7 @@ get_result_type_name = action.get_result_service.namespaced_type.name data = { 'package_name': package_name, + 'interface_path': interface_path, 'output_dir': output_dir, 'template_basepath': template_basepath, } @@ -60,6 +64,7 @@ expand_template( data = { 'package_name': package_name, + 'interface_path': interface_path, 'output_dir': output_dir, 'template_basepath': template_basepath, 'typesupport_impl': typesupport_impl, diff --git a/rosidl_generator_java/resource/action.java.em b/rosidl_generator_java/resource/action.java.em index 5e7ebbe9..f5e33b2b 100644 --- a/rosidl_generator_java/resource/action.java.em +++ b/rosidl_generator_java/resource/action.java.em @@ -13,6 +13,7 @@ type_name = action.namespaced_type.name goal_type_name = action.goal.structure.namespaced_type.name result_type_name = action.result.structure.namespaced_type.name feedback_type_name = action.feedback.structure.namespaced_type.name +feedback_message_type_name = action.feedback_message.structure.namespaced_type.name send_goal_type_name = action.send_goal_service.namespaced_type.name get_result_type_name = action.get_result_service.namespaced_type.name @@ -22,7 +23,10 @@ data = { 'output_dir': output_dir, 'template_basepath': template_basepath, } -data.update({'message': action.goal}) +data.update({ + 'message': action.goal, + 'marker_interfaces': [f'org.ros2.rcljava.interfaces.GoalDefinition<{type_name}>'], +}) output_file = os.path.join(output_dir, *namespaces[1:], goal_type_name + '.java') expand_template( 'msg.java.em', @@ -30,7 +34,10 @@ expand_template( output_file, template_basepath=template_basepath) -data.update({'message': action.result}) +data.update({ + 'message': action.result, + 'marker_interfaces': [f'org.ros2.rcljava.interfaces.ResultDefinition<{type_name}>'], +}) output_file = os.path.join(output_dir, *namespaces[1:], result_type_name + '.java') expand_template( 'msg.java.em', @@ -38,7 +45,10 @@ expand_template( output_file, template_basepath=template_basepath) -data.update({'message': action.feedback}) +data.update({ + 'message': action.feedback, + 'marker_interfaces': [f'org.ros2.rcljava.interfaces.FeedbackDefinition<{type_name}>'], +}) output_file = os.path.join(output_dir, *namespaces[1:], feedback_type_name + '.java') expand_template( 'msg.java.em', @@ -46,6 +56,19 @@ expand_template( output_file, template_basepath=template_basepath) +data.update({ + 'message': action.feedback_message, + 'marker_interfaces': [], +}) +output_file = os.path.join(output_dir, *namespaces[1:], feedback_message_type_name + '.java') +expand_template( + 'msg.java.em', + data, + output_file, + template_basepath=template_basepath) + +del data['marker_interfaces'] +del data['message'] data.update({'service': action.send_goal_service}) output_file = os.path.join(output_dir, *namespaces[1:], send_goal_type_name + '.java') expand_template( @@ -63,8 +86,18 @@ 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.FeedbackDefinition', + 'org.ros2.rcljava.interfaces.FeedbackMessageDefinition', + 'org.ros2.rcljava.interfaces.GoalDefinition', + 'org.ros2.rcljava.interfaces.GoalRequestDefinition', + 'org.ros2.rcljava.interfaces.GoalResponseDefinition', + 'org.ros2.rcljava.interfaces.MessageDefinition', + 'org.ros2.rcljava.interfaces.ResultDefinition', + 'org.ros2.rcljava.interfaces.ResultRequestDefinition', + 'org.ros2.rcljava.interfaces.ResultResponseDefinition', 'org.slf4j.Logger', 'org.slf4j.LoggerFactory', ] @@ -76,6 +109,77 @@ 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 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 static class GetResultRequest extends @(type_name)_GetResult_Request implements ResultRequestDefinition<@(type_name)> { + public List getGoalUuid() { + return super.getGoalId().getUuidAsList(); + } + } + + public static class GetResultResponse extends @(type_name)_GetResult_Response implements ResultResponseDefinition<@(type_name)> { + public void setResult(ResultDefinition<@(type_name)> result) { + super.setResult((@(type_name)_Result)result); + } + public void setGoalStatus(byte status) { + super.setStatus(status); + } + } + + public static class FeedbackMessage extends @(type_name)_FeedbackMessage implements FeedbackMessageDefinition<@(type_name)> { + public void setFeedback(FeedbackDefinition<@(type_name)> feedback) { + super.setFeedback((@(type_name)_Feedback) feedback); + } + public void setGoalUuid(List goalUuid) { + super.getGoalId().setUuid(goalUuid); + } + } + + public Class> getSendGoalRequestType() { + return SendGoalRequest.class; + } + + public Class> getSendGoalResponseType() { + return SendGoalResponse.class; + } + + public Class> getGetResultRequestType() { + return GetResultRequest.class; + } + + public Class> getGetResultResponseType() { + return GetResultResponse.class; + } + + public Class> getResultType() { + return @(type_name)_Result.class; + } + + public Class> getFeedbackType() { + return @(type_name)_Feedback.class; + } + + public Class> getFeedbackMessageType() { + return FeedbackMessage.class; + } + private static final Logger logger = LoggerFactory.getLogger(@(type_name).class); static { diff --git a/rosidl_generator_java/resource/idl.cpp.em b/rosidl_generator_java/resource/idl.cpp.em index c909006a..3f355186 100644 --- a/rosidl_generator_java/resource/idl.cpp.em +++ b/rosidl_generator_java/resource/idl.cpp.em @@ -29,6 +29,7 @@ from rosidl_parser.definition import Service @{ data = { 'package_name': package_name, + 'interface_path': interface_path, 'output_dir': output_dir, 'template_basepath': template_basepath, } diff --git a/rosidl_generator_java/resource/idl.java.em b/rosidl_generator_java/resource/idl.java.em index f34d75c7..4714151d 100644 --- a/rosidl_generator_java/resource/idl.java.em +++ b/rosidl_generator_java/resource/idl.java.em @@ -13,6 +13,9 @@ @####################################################################### @# Handle messages @####################################################################### +// generated from rosidl_generator_java/resource/idl.java.em +// with input from @(package_name):@(interface_path) +// generated code does not contain a copyright notice @{ import os @@ -26,6 +29,7 @@ data = { 'interface_path': interface_path, 'output_dir': output_dir, 'template_basepath': template_basepath, + 'marker_interfaces': [], } for message in content.get_elements_of_type(Message): diff --git a/rosidl_generator_java/resource/msg.cpp.em b/rosidl_generator_java/resource/msg.cpp.em index a7008a53..fade6a71 100644 --- a/rosidl_generator_java/resource/msg.cpp.em +++ b/rosidl_generator_java/resource/msg.cpp.em @@ -1,4 +1,7 @@ @# Included from rosidl_generator_java/resource/idl.cpp.em +// generated from rosidl_generator_java/resource/msg.cpp.em +// with input from @(package_name):@(interface_path) +// generated code does not contain a copyright notice @{ from collections import defaultdict @@ -22,15 +25,6 @@ from rosidl_parser.definition import NamespacedType msg_normalized_type = '__'.join(message.structure.namespaced_type.namespaced_name()) msg_jni_type = '/'.join(message.structure.namespaced_type.namespaced_name()) -list_java_type = "java.util.List" -array_list_java_type = "java.util.ArrayList" - -list_normalized_type = "java__util__List" -list_jni_type = "java/util/List" - -array_list_normalized_type = "java__util__ArrayList" -array_list_jni_type = "java/util/ArrayList" - # Collect JNI types and includes cache = defaultdict(lambda: False) cache[msg_normalized_type] = msg_jni_type @@ -39,8 +33,6 @@ member_includes = set() for member in message.structure.members: type_ = member.type if isinstance(type_, AbstractNestedType): - cache[list_normalized_type] = list_jni_type - cache[array_list_normalized_type] = array_list_jni_type type_ = type_.value_type if isinstance(type_, BasicType): member_includes.add('rosidl_runtime_c/primitives_sequence.h') @@ -77,6 +69,8 @@ for member in message.structure.members: include_prefix = include_prefix[:-8] elif include_prefix.endswith('__feedback'): include_prefix = include_prefix[:-10] + elif include_prefix.endswith('__feedback_message'): + include_prefix = include_prefix[:-18] elif include_prefix.endswith('__send_goal'): include_prefix = include_prefix[:-11] elif include_prefix.endswith('__get_result'): @@ -98,14 +92,18 @@ elif message_c_include_prefix.endswith('__result'): message_c_include_prefix = message_c_include_prefix[:-8] elif message_c_include_prefix.endswith('__feedback'): message_c_include_prefix = message_c_include_prefix[:-10] +elif message_c_include_prefix.endswith('__feedback_message'): + message_c_include_prefix = message_c_include_prefix[:-18] elif message_c_include_prefix.endswith('__send_goal'): message_c_include_prefix = message_c_include_prefix[:-11] elif message_c_include_prefix.endswith('__get_result'): message_c_include_prefix = message_c_include_prefix[:-12] +member_includes.add(f'{message_c_include_prefix}.h') }@ - +@ #include +#include #include #include #include @@ -119,8 +117,6 @@ elif message_c_include_prefix.endswith('__get_result'): #include "@(include)" @[end for]@ -#include "@(message_c_include_prefix).h" - // Ensure that a jlong is big enough to store raw pointers static_assert(sizeof(jlong) >= sizeof(std::intptr_t), "jlong must be able to store pointers"); @@ -220,32 +216,46 @@ JNIEXPORT jlong JNICALL Java_@(underscore_separated_jni_type_name)_getDestructor } @[for member in message.structure.members]@ @{ -normalized_type = get_normalized_type(member.type) +base_type = member.type.value_type if isinstance(member.type, AbstractNestedType) else member.type +normalized_type = get_normalized_type(base_type) +get_java_name = get_java_type(base_type, use_primitives=True) +get_method_name = get_java_name.capitalize() +jni_signature = get_jni_signature(base_type) }@ @[ if isinstance(member.type, AbstractNestedType)] - auto _jfield_@(member.name)_fid = env->GetFieldID(_j@(msg_normalized_type)_class_global, "@(member.name)", "L@(list_jni_type);"); - jobject _jlist_@(member.name)_object = env->GetObjectField(_jmessage_obj, _jfield_@(member.name)_fid); +@[ if isinstance(member.type.value_type, BasicType)]@ + auto _jfield_@(member.name)_fid = env->GetFieldID(_j@(msg_normalized_type)_class_global, "@(member.name)", "[@(jni_signature)"); + j@(get_java_name)Array _jarray_@(member.name)_obj = (j@(get_java_name)Array)env->GetObjectField(_jmessage_obj, _jfield_@(member.name)_fid); +@[ elif isinstance(member.type.value_type, AbstractGenericString)]@ + auto _jfield_@(member.name)_fid = env->GetFieldID(_j@(msg_normalized_type)_class_global, "@(member.name)", "[Ljava/lang/String;"); + jobjectArray _jarray_@(member.name)_obj = (jobjectArray)env->GetObjectField(_jmessage_obj, _jfield_@(member.name)_fid); +@[ else]@ + auto _jfield_@(member.name)_fid = env->GetFieldID(_j@(msg_normalized_type)_class_global, "@(member.name)", "[L@('/'.join(member.type.value_type.namespaced_name()));"); + jobjectArray _jarray_@(member.name)_obj = (jobjectArray)env->GetObjectField(_jmessage_obj, _jfield_@(member.name)_fid); +@[ end if]@ - if (_jlist_@(member.name)_object != nullptr) { - jmethodID _jlist_@(member.name)_get_mid = env->GetMethodID(_j@(list_normalized_type)_class_global, "get", "(I)Ljava/lang/Object;"); + if (_jarray_@(member.name)_obj != nullptr) { @[ if isinstance(member.type, AbstractSequence)]@ - jmethodID _jlist_@(member.name)_size_mid = env->GetMethodID(_j@(list_normalized_type)_class_global, "size", "()I"); - jint _jlist_@(member.name)_size = env->CallIntMethod(_jlist_@(member.name)_object, _jlist_@(member.name)_size_mid); +@[ if isinstance(member.type, Array)]@ + jint _jarray_@(member.name)_size = @(member.type.size); +@[ else]@ + jint _jarray_@(member.name)_size = env->GetArrayLength(_jarray_@(member.name)_obj); +@[ end if]@ @[ if isinstance(member.type.value_type, AbstractString)]@ - if (!rosidl_runtime_c__String__Sequence__init(&(ros_message->@(member.name)), _jlist_@(member.name)_size)) { + if (!rosidl_runtime_c__String__Sequence__init(&(ros_message->@(member.name)), _jarray_@(member.name)_size)) { rcljava_throw_exception(env, "java/lang/IllegalStateException", "unable to create String__Array ros_message"); } @[ elif isinstance(member.type.value_type, AbstractWString)]@ - if (!rosidl_runtime_c__U16String__Sequence__init(&(ros_message->@(member.name)), _jlist_@(member.name)_size)) { + if (!rosidl_runtime_c__U16String__Sequence__init(&(ros_message->@(member.name)), _jarray_@(member.name)_size)) { rcljava_throw_exception(env, "java/lang/IllegalStateException", "unable to create U16String__Array ros_message"); } @[ else]@ @[ if isinstance(member.type.value_type, BasicType)]@ - if (!rosidl_runtime_c__@(member.type.value_type.typename)__Sequence__init(&(ros_message->@(member.name)), _jlist_@(member.name)_size)) { + if (!rosidl_runtime_c__@(member.type.value_type.typename)__Sequence__init(&(ros_message->@(member.name)), _jarray_@(member.name)_size)) { rcljava_throw_exception(env, "java/lang/IllegalStateException", "unable to create @(member.type.value_type)__Array ros_message"); } @[ else]@ - if (!@('__'.join(member.type.value_type.namespaced_name()))__Sequence__init(&(ros_message->@(member.name)), _jlist_@(member.name)_size)) { + if (!@('__'.join(member.type.value_type.namespaced_name()))__Sequence__init(&(ros_message->@(member.name)), _jarray_@(member.name)_size)) { rcljava_throw_exception(env, "java/lang/IllegalStateException", "unable to create @(member.type.value_type)__Array ros_message"); } @@ -253,39 +263,43 @@ normalized_type = get_normalized_type(member.type) @[ end if]@ auto _dest_@(member.name) = ros_message->@(member.name).data; @[ else]@ - jint _jlist_@(member.name)_size = @(member.type.size); + jint _jarray_@(member.name)_size = @(member.type.size); auto _dest_@(member.name) = ros_message->@(member.name); @[ end if]@ - - for (jint i = 0; i < _jlist_@(member.name)_size; ++i) { - auto element = env->CallObjectMethod(_jlist_@(member.name)_object, _jlist_@(member.name)_get_mid, i); -@[ if isinstance(member.type.value_type, AbstractString)]@ +@[ if isinstance(member.type.value_type, BasicType)]@ + j@(get_java_name) * _jarray_@(member.name)_ptr = env->Get@(get_method_name)ArrayElements(_jarray_@(member.name)_obj, nullptr); + std::copy(_jarray_@(member.name)_ptr, _jarray_@(member.name)_ptr + _jarray_@(member.name)_size, _dest_@(member.name)); + env->Release@(get_method_name)ArrayElements(_jarray_@(member.name)_obj, _jarray_@(member.name)_ptr, 0); +@[ else]@ + for (jint i = 0; i < _jarray_@(member.name)_size; ++i) { + auto element = env->GetObjectArrayElement(_jarray_@(member.name)_obj, i); + if (element == nullptr) { + continue; + } +@[ if isinstance(member.type.value_type, AbstractString)]@ jstring _jfield_@(member.name)_value = static_cast(element); if (_jfield_@(member.name)_value != nullptr) { - const char * _str@(member.name) = env->GetStringUTFChars(_jfield_@(member.name)_value, 0); + const char * _str_@(member.name) = env->GetStringUTFChars(_jfield_@(member.name)_value, 0); rosidl_runtime_c__String__assign( - &_dest_@(member.name)[i], _str@(member.name)); - env->ReleaseStringUTFChars(_jfield_@(member.name)_value, _str@(member.name)); + &_dest_@(member.name)[i], _str_@(member.name)); + env->ReleaseStringUTFChars(_jfield_@(member.name)_value, _str_@(member.name)); } -@[ elif isinstance(member.type.value_type, AbstractWString)]@ +@[ elif isinstance(member.type.value_type, AbstractWString)]@ jstring _jfield_@(member.name)_value = static_cast(element); if (_jfield_@(member.name)_value != nullptr) { - const jchar * _str@(member.name) = env->GetStringChars(_jfield_@(member.name)_value, 0); + const jchar * _str_@(member.name) = env->GetStringChars(_jfield_@(member.name)_value, 0); rosidl_runtime_c__U16String__assign( - &_dest_@(member.name)[i], _str@(member.name)); - env->ReleaseStringChars(_jfield_@(member.name)_value, _str@(member.name)); + &_dest_@(member.name)[i], _str_@(member.name)); + env->ReleaseStringChars(_jfield_@(member.name)_value, _str_@(member.name)); } -@[ elif isinstance(member.type.value_type, BasicType)]@ -@{ -call_method_name = 'Call%sMethod' % get_java_type(member.type.value_type, use_primitives=True).capitalize() -}@ - _dest_@(member.name)[i] = env->@(call_method_name)(element, _j@(normalized_type)_value_global); -@[ else]@ +@[ else]@ _dest_@(member.name)[i] = *_j@(normalized_type)_from_java_function(element, nullptr); -@[ end if]@ +@[ end if]@ env->DeleteLocalRef(element); } +@[ end if]@ + env->DeleteLocalRef(_jarray_@(member.name)_obj); } @[ else]@ @[ if isinstance(member.type, AbstractGenericString)]@ @@ -349,61 +363,71 @@ jobject @(underscore_separated_type_name)__convert_to_java(@(msg_normalized_type } @[for member in message.structure.members]@ @{ -normalized_type = get_normalized_type(member.type) +base_type = member.type.value_type if isinstance(member.type, AbstractNestedType) else member.type +normalized_type = get_normalized_type(base_type) +get_java_name = get_java_type(base_type, use_primitives=True) +get_method_name = get_java_name.capitalize() +jni_signature = get_jni_signature(base_type) }@ @[ if isinstance(member.type, AbstractNestedType)]@ -@[ if isinstance(member.type.value_type, (BasicType, AbstractGenericString))]@ - auto _jfield_@(member.name)_fid = env->GetFieldID(_j@(msg_normalized_type)_class_global, "@(member.name)", "L@(list_jni_type);"); - jobject _jarray_list_@(member.name)_obj = env->NewObject(_j@(array_list_normalized_type)_class_global, _j@(array_list_normalized_type)_constructor_global); - // TODO(esteve): replace ArrayList with a jobjectArray to initialize the array beforehand - jmethodID _jlist_@(member.name)_add_mid = env->GetMethodID(_j@(array_list_normalized_type)_class_global, "add", "(Ljava/lang/Object;)Z"); +@[ if isinstance(member.type.value_type, BasicType)]@ + auto _jfield_@(member.name)_fid = env->GetFieldID(_j@(msg_normalized_type)_class_global, "@(member.name)", "[@(jni_signature)"); +@[ elif isinstance(member.type.value_type, AbstractGenericString)]@ + auto _jfield_@(member.name)_fid = env->GetFieldID(_j@(msg_normalized_type)_class_global, "@(member.name)", "[Ljava/lang/String;"); +@[ else]@ + auto _jfield_@(member.name)_fid = env->GetFieldID(_j@(msg_normalized_type)_class_global, "@(member.name)", "[L@('/'.join(member.type.value_type.namespaced_name()));"); +@[ end if]@ + +@[ if isinstance(member.type.value_type, BasicType)]@ +@[ if isinstance(member.type, Array)]@ + j@(get_java_name)Array _jarray_@(member.name)_obj = env->New@(get_method_name)Array(@(member.type.size)); + auto * _j@(get_java_name)_@(member.name)_buf = static_cast(malloc(sizeof(j@(get_java_name)) * @(member.type.size))); + std::copy(_ros_message->@(member.name), _ros_message->@(member.name) + @(member.type.size), _j@(get_java_name)_@(member.name)_buf); + env->Set@(get_method_name)ArrayRegion(_jarray_@(member.name)_obj, 0, @(member.type.size), (const j@(get_java_name) *)_j@(get_java_name)_@(member.name)_buf); +@[ else]@ + j@(get_java_name)Array _jarray_@(member.name)_obj = env->New@(get_method_name)Array(_ros_message->@(member.name).size); + auto * _j@(get_java_name)_@(member.name)_buf = static_cast(malloc(sizeof(j@(get_java_name)) * _ros_message->@(member.name).size)); + std::copy(_ros_message->@(member.name).data, _ros_message->@(member.name).data + _ros_message->@(member.name).size, _j@(get_java_name)_@(member.name)_buf); + env->Set@(get_method_name)ArrayRegion(_jarray_@(member.name)_obj, 0, _ros_message->@(member.name).size, (const j@(get_java_name) *)_j@(get_java_name)_@(member.name)_buf); +@[ end if]@ + free(_j@(get_java_name)_@(member.name)_buf); +@[ elif isinstance(member.type.value_type, AbstractGenericString)]@ @[ if isinstance(member.type, Array)]@ - for (size_t i = 0; i < @(member.type.size); ++i) { + jobjectArray _jarray_@(member.name)_obj = (jobjectArray)env->NewObjectArray(@(member.type.size), env->FindClass("java/lang/String"), NULL); + for (size_t i = 0; i < @(member.type.size); i++) { auto _ros_@(member.name)_element = _ros_message->@(member.name)[i]; @[ else]@ - for (size_t i = 0; i < _ros_message->@(member.name).size; ++i) { + jobjectArray _jarray_@(member.name)_obj = (jobjectArray)env->NewObjectArray(_ros_message->@(member.name).size, env->FindClass("java/lang/String"), NULL); + for (size_t i = 0; i < _ros_message->@(member.name).size; i++) { auto _ros_@(member.name)_element = _ros_message->@(member.name).data[i]; @[ end if]@ -@[ if isinstance(member.type.value_type, AbstractGenericString)]@ - jobject _jlist_@(member.name)_element = nullptr; + jstring _jarray_@(member.name)_element = nullptr; if (_ros_@(member.name)_element.data != nullptr) { -@[ if isinstance(member.type.value_type, AbstractString)]@ - _jlist_@(member.name)_element = env->NewStringUTF(_ros_@(member.name)_element.data); -@[ else]@ - _jlist_@(member.name)_element = env->NewString(_ros_@(member.name)_element.data, _ros_@(member.name)_element.size); -@[ end if]@ - } +@[ if isinstance(member.type.value_type, AbstractString)]@ + _jarray_@(member.name)_element = env->NewStringUTF(_ros_@(member.name)_element.data); @[ else]@ - jobject _jlist_@(member.name)_element = env->NewObject( - _j@(normalized_type)_class_global, _j@(normalized_type)_constructor_global, _ros_@(member.name)_element); -@[ end if]@ - if (_jlist_@(member.name)_element != nullptr) { - jboolean _jlist_@(member.name)_add_result = env->CallBooleanMethod(_jarray_list_@(member.name)_obj, _jlist_@(member.name)_add_mid, _jlist_@(member.name)_element); - assert(_jlist_@(member.name)_add_result); + _jarray_@(member.name)_element = env->NewString(_ros_@(member.name)_element.data, _ros_@(member.name)_element.size); +@[ end if]@ } + env->SetObjectArrayElement(_jarray_@(member.name)_obj, i, _jarray_@(member.name)_element); + env->DeleteLocalRef(_jarray_@(member.name)_element); } @[ else]@ - - auto _jfield_@(member.name)_fid = env->GetFieldID(_j@(msg_normalized_type)_class_global, "@(member.name)", "L@(list_jni_type);"); - jobject _jarray_list_@(member.name)_obj = env->NewObject(_j@(array_list_normalized_type)_class_global, _j@(array_list_normalized_type)_constructor_global); - // TODO(esteve): replace ArrayList with a jobjectArray to initialize the array beforehand - jmethodID _jlist_@(member.name)_add_mid = env->GetMethodID(_j@(array_list_normalized_type)_class_global, "add", "(Ljava/lang/Object;)Z"); - @[ if isinstance(member.type, Array)]@ - for (size_t i = 0; i < @(member.type.size); ++i) { - jobject _jlist_@(member.name)_element = _j@(normalized_type)_to_java_function(&(_ros_message->@(member.name)[i]), nullptr); + jobjectArray _jarray_@(member.name)_obj = (jobjectArray)env->NewObjectArray(@(member.type.size), _j@(normalized_type)_class_global, NULL); + for (size_t i = 0; i < @(member.type.size); i++) { + jobject _jarray_@(member.name)_element = _j@(normalized_type)_to_java_function(&(_ros_message->@(member.name)[i]), nullptr); @[ else]@ - for (size_t i = 0; i < _ros_message->@(member.name).size; ++i) { - jobject _jlist_@(member.name)_element = _j@(normalized_type)_to_java_function(&(_ros_message->@(member.name).data[i]), nullptr); + jobjectArray _jarray_@(member.name)_obj = (jobjectArray)env->NewObjectArray(_ros_message->@(member.name).size, _j@(normalized_type)_class_global, NULL); + for (size_t i = 0; i < _ros_message->@(member.name).size; i++) { + jobject _jarray_@(member.name)_element = _j@(normalized_type)_to_java_function(&(_ros_message->@(member.name).data[i]), nullptr); @[ end if]@ - if (_jlist_@(member.name)_element != nullptr) { - jboolean _jlist_@(member.name)_add_result = env->CallBooleanMethod(_jarray_list_@(member.name)_obj, _jlist_@(member.name)_add_mid, _jlist_@(member.name)_element); - assert(_jlist_@(member.name)_add_result); - } + env->SetObjectArrayElement(_jarray_@(member.name)_obj, i, _jarray_@(member.name)_element); + env->DeleteLocalRef(_jarray_@(member.name)_element); } @[ end if]@ - env->SetObjectField(_jmessage_obj, _jfield_@(member.name)_fid, _jarray_list_@(member.name)_obj); - env->DeleteLocalRef(_jarray_list_@(member.name)_obj); + env->SetObjectField(_jmessage_obj, _jfield_@(member.name)_fid, _jarray_@(member.name)_obj); + env->DeleteLocalRef(_jarray_@(member.name)_obj); @[ else]@ @[ if isinstance(member.type, AbstractGenericString)]@ auto _jfield_@(member.name)_fid = env->GetFieldID(_j@(msg_normalized_type)_class_global, "@(member.name)", "Ljava/lang/String;"); @@ -498,6 +522,7 @@ if value_method: JNIEXPORT void JNICALL JNI_OnUnload(JavaVM * vm, void *) { + (void)vm; assert(g_vm != nullptr); assert(g_vm == vm); @@ -541,7 +566,7 @@ JNIEXPORT jlong JNICALL Java_@(underscore_separated_jni_type_name)_getToJavaConv JNIEXPORT jlong JNICALL Java_@(underscore_separated_jni_type_name)_getTypeSupport(JNIEnv *, jclass) { - jlong ptr = reinterpret_cast(ROSIDL_GET_MSG_TYPE_SUPPORT(@(','.join(message.structure.namespaced_type.namespaced_name())))); + jlong ptr = reinterpret_cast(ROSIDL_GET_MSG_TYPE_SUPPORT(@(', '.join(message.structure.namespaced_type.namespaced_name())))); return ptr; } diff --git a/rosidl_generator_java/resource/msg.java.em b/rosidl_generator_java/resource/msg.java.em index cb6e1b3a..f5b62f2e 100644 --- a/rosidl_generator_java/resource/msg.java.em +++ b/rosidl_generator_java/resource/msg.java.em @@ -17,6 +17,9 @@ from rosidl_parser.definition import BoundedSequence from rosidl_parser.definition import NamespacedType type_name = message.structure.namespaced_type.name +interfaces_implemented = ['MessageDefinition'] +interfaces_implemented.extend(f"{t.rsplit('.', 1)[1]}" for t in marker_interfaces) +interfaces_implemented = ', '.join(interfaces_implemented) message_imports = [ 'org.apache.commons.lang3.builder.EqualsBuilder', @@ -26,6 +29,7 @@ message_imports = [ 'org.slf4j.Logger', 'org.slf4j.LoggerFactory', ] +message_imports.extend(f"{t.split('<', 1)[0]}" for t in marker_interfaces) }@ @[for message_import in message_imports]@ import @(message_import); @@ -38,7 +42,7 @@ import @('.'.join(member.type.namespaced_name())); @[ end if]@ @[end for]@ -public final class @(type_name) implements MessageDefinition { +public class @(type_name) implements @(interfaces_implemented) { private static final Logger logger = LoggerFactory.getLogger(@(type_name).class); @@ -80,15 +84,29 @@ public final class @(type_name) implements MessageDefinition { @[ if isinstance(member.type, AbstractNestedType)]@ @[ if member.has_annotation('default')]@ - private java.util.List<@(get_java_type(member.type, use_primitives=False))> @(member.name) = java.util.Arrays.asList(new @(get_java_type(member.type, use_primitives=False))[] @(value_to_java(member.type, member.get_annotation_value('default')['value']))); + private @(get_java_type(member.type))[] @(member.name) = new @(get_java_type(member.type))[] @(value_to_java(member.type, member.get_annotation_value('default')['value'])); @[ else]@ @[ if isinstance(member.type, Array)]@ - private java.util.List<@(get_java_type(member.type, use_primitives=False))> @(member.name); + private @(get_java_type(member.type))[] @(member.name) = new @(get_java_type(member.type))[@(member.type.size)]; @[ else]@ - private java.util.List<@(get_java_type(member.type, use_primitives=False))> @(member.name) = new java.util.ArrayList<@(get_java_type(member.type, use_primitives=False))>(); + private @(get_java_type(member.type))[] @(member.name) = new @(get_java_type(member.type))[]{}; @[ end if]@ @[ end if]@ + public final @(type_name) set@(convert_lower_case_underscore_to_camel_case(member.name))(final @(get_java_type(member.type))[] @(member.name)) { +@[ if isinstance(member.type, BoundedSequence)]@ + if(@(member.name).length > @(member.type.maximum_size)) { + throw new IllegalArgumentException("Array too big, maximum size allowed: @(member.type.maximum_size)"); + } +@[ elif isinstance(member.type, Array)]@ + if(@(member.name).length != @(member.type.size)) { + throw new IllegalArgumentException("Invalid size for fixed array, must be exactly: @(member.type.size)"); + } +@[ end if]@ + this.@(member.name) = @(member.name); + return this; + } + public final @(type_name) set@(convert_lower_case_underscore_to_camel_case(member.name))(final java.util.List<@(get_java_type(member.type, use_primitives=False))> @(member.name)) { @[ if isinstance(member.type, BoundedSequence)]@ if(@(member.name).size() > @(member.type.maximum_size)) { @@ -99,22 +117,33 @@ public final class @(type_name) implements MessageDefinition { throw new IllegalArgumentException("Invalid size for fixed array, must be exactly: @(member.type.size)"); } @[ end if]@ - this.@(member.name) = @(member.name); +@[ if isinstance(member.type.value_type, BasicType)]@ + @(get_java_type(member.type, use_primitives=False))[] boxed_arr = @(member.name).toArray(new @(get_java_type(member.type, use_primitives=False))[]{}); + @(get_java_type(member.type))[] unboxed_arr = new @(get_java_type(member.type))[@(member.name).size()]; + for (int i = 0; i < @(member.name).size(); i++) { + unboxed_arr[i] = boxed_arr[i].@(get_java_type(member.type))Value(); + } + this.@(member.name) = unboxed_arr; +@[ else]@ + this.@(member.name) = @(member.name).toArray(new @(get_java_type(member.type))[0]); +@[ end if]@ return this; } -@[ if isinstance(member.type.value_type, (BasicType, AbstractGenericString))]@ - public final @(type_name) set@(convert_lower_case_underscore_to_camel_case(member.name))(final @(get_java_type(member.type, use_primitives=True))[] @(member.name)) { - java.util.List<@(get_java_type(member.type, use_primitives=False))> @(member.name)_tmp = new java.util.ArrayList<@(get_java_type(member.type, use_primitives=False))>(); - for(@(get_java_type(member.type, use_primitives=True)) @(member.name)_value : @(member.name)) { - @(member.name)_tmp.add(@(member.name)_value); - } - return set@(convert_lower_case_underscore_to_camel_case(member.name))(@(member.name)_tmp); + public final @(get_java_type(member.type))[] get@(convert_lower_case_underscore_to_camel_case(member.name))() { + return this.@(member.name); } -@[ end if]@ - public final java.util.List<@(get_java_type(member.type, use_primitives=False))> get@(convert_lower_case_underscore_to_camel_case(member.name))() { - return this.@(member.name); + /** + * For better performance, use @@{link @(type_name)#get@(convert_lower_case_underscore_to_camel_case(member.name))} instead. + */ + public final java.util.List<@(get_java_type(member.type, use_primitives=False))> get@(convert_lower_case_underscore_to_camel_case(member.name))AsList() { + // TODO(jacobperron): We could cache the List value for subsequent calls + java.util.List<@(get_java_type(member.type, use_primitives=False))> list = new java.util.ArrayList<@(get_java_type(member.type, use_primitives=False))>(this.@(member.name).length); + for (@(get_java_type(member.type)) element : this.@(member.name)) { + list.add(element); + } + return list; } @[ else]@ @[ if member.has_annotation('default')]@ diff --git a/rosidl_generator_java/resource/srv.cpp.em b/rosidl_generator_java/resource/srv.cpp.em index 4c13f6b5..d5357025 100644 --- a/rosidl_generator_java/resource/srv.cpp.em +++ b/rosidl_generator_java/resource/srv.cpp.em @@ -1,4 +1,7 @@ @# Included from rosidl_generator_java/resource/idl.cpp.em +// generated from rosidl_generator_java/resource/srv.cpp.em +// with input from @(package_name):@(interface_path) +// generated code does not contain a copyright notice @{ import os from rosidl_cmake import expand_template @@ -11,6 +14,7 @@ response_type_name = service.response_message.structure.namespaced_type.name data = { 'package_name': package_name, + 'interface_path': interface_path, 'output_dir': output_dir, 'template_basepath': template_basepath, } @@ -83,6 +87,6 @@ JNIEXPORT jlong JNICALL Java_@(underscore_separated_jni_type_name)_getServiceTyp JNIEXPORT jlong JNICALL Java_@(underscore_separated_jni_type_name)_getServiceTypeSupport(JNIEnv *, jclass) { const rosidl_service_type_support_t * ts = ROSIDL_GET_SRV_TYPE_SUPPORT( - @(','.join(service_fqn))); + @(', '.join(service_fqn))); return reinterpret_cast(ts); } diff --git a/rosidl_generator_java/resource/srv.java.em b/rosidl_generator_java/resource/srv.java.em index 9be7a6a7..a238ecfd 100644 --- a/rosidl_generator_java/resource/srv.java.em +++ b/rosidl_generator_java/resource/srv.java.em @@ -18,6 +18,7 @@ data = { 'interface_path': interface_path, 'output_dir': output_dir, 'template_basepath': template_basepath, + 'marker_interfaces': [], } data.update({'message': service.request_message}) output_file = os.path.join(output_dir, *namespaces[1:], request_type_name + '.java') @@ -60,6 +61,14 @@ public class @(type_name) implements ServiceDefinition { } } + public @(type_name)_Request newRequestInstance() { + return new @(type_name)_Request(); + } + + public @(type_name)_Response newResponseInstance() { + return new @(type_name)_Response(); + } + public static native long getServiceTypeSupport(); public static final Class<@(type_name)_Request> RequestType = @(type_name)_Request.class; diff --git a/rosidl_generator_java/src/test/java/org/ros2/generator/InterfacesTest.java b/rosidl_generator_java/src/test/java/org/ros2/generator/InterfacesTest.java index d64af3eb..8630148a 100644 --- a/rosidl_generator_java/src/test/java/org/ros2/generator/InterfacesTest.java +++ b/rosidl_generator_java/src/test/java/org/ros2/generator/InterfacesTest.java @@ -15,12 +15,13 @@ package org.ros2.generator; +import static org.junit.Assert.assertArrayEquals; import static org.junit.Assert.assertEquals; import static org.junit.Assert.assertNotEquals; import static org.junit.Assert.assertTrue; import java.lang.reflect.Method; -import java.util.concurrent.Callable; +import java.lang.Runnable; import java.util.Arrays; import java.util.List; import org.junit.BeforeClass; @@ -29,14 +30,103 @@ import org.junit.rules.ExpectedException; public class InterfacesTest { + public class ListFixtureData { + public boolean[] boolArr = new boolean[]{true, false, true}; + public List boolList = Arrays.asList(true, false, true); + public boolean[] boolArrShort = new boolean[]{false}; + public List boolListShort = Arrays.asList(false); + public byte[] byteArr = new byte[]{(byte) 0, (byte) 1, (byte) 255}; + public List byteList = Arrays.asList((byte) 0, (byte) 1, (byte) 255); + public byte[] byteArrShort = new byte[]{1}; + public List byteListShort = Arrays.asList((byte) 1); + public byte[] charArr = new byte[]{(byte) ' ', (byte) 'a', (byte) 'Z'}; + public List charList = Arrays.asList(Byte.valueOf((byte) ' '), Byte.valueOf((byte) 'a'), Byte.valueOf((byte) 'Z')); + public byte[] charArrShort = new byte[]{(byte) 'z', (byte) 'A'}; + public List charListShort = Arrays.asList(Byte.valueOf((byte) 'z'), Byte.valueOf((byte) 'A')); + public float[] float32Arr = new float[]{0.0f, -1.125f, 1.125f}; + public List float32List = Arrays.asList(0.0f, -1.125f, 1.125f); + public float[] float32ArrShort = new float[]{1.125f, -1.125f}; + public List float32ListShort = Arrays.asList(1.125f, -1.125f); + public double[] float64Arr = new double[]{0.0, -3.1415, 3.1415}; + public List float64List = Arrays.asList(0.0, -3.1415, 3.1415); + public double[] float64ArrShort = new double[]{3.1415, -3.1415}; + public List float64ListShort = Arrays.asList(3.1415, -3.1415); + public byte[] int8Arr = new byte[]{0, -128, 127}; + public List int8List = Arrays.asList((byte) 0, (byte) -128, (byte) 127); + public byte[] int8ArrShort = new byte[]{127, -128}; + public List int8ListShort = Arrays.asList((byte) 127, (byte) -128); + public byte[] uint8Arr = new byte[]{(byte) 0, (byte) 1, (byte) 255}; + public List uint8List = Arrays.asList((byte) 0, (byte) 1, (byte) 255); + public byte[] uint8ArrShort = new byte[]{(byte) 255, (byte) 1}; + public List uint8ListShort = Arrays.asList((byte) 255, (byte) 1); + public short[] int16Arr = new short[]{0, -32768, 32767}; + public List int16List = Arrays.asList((short) 0, (short) -32768, (short) 32767); + public short[] int16ArrShort = new short[]{32767, -32768}; + public List int16ListShort = Arrays.asList((short) 32767, (short) -32768); + public short[] uint16Arr = new short[]{(short) 0, (short) 1, (short) 65535}; + public List uint16List = Arrays.asList((short) 0, (short) 1, (short) 65535); + public short[] uint16ArrShort = new short[]{(short) 0, (short) 1, (short) 65535}; + public List uint16ListShort = Arrays.asList((short) 0, (short) 1, (short) 65535); + public int[] int32Arr = new int[]{0, -2147483648, 2147483647}; + public List int32List = Arrays.asList(0, -2147483648, 2147483647); + public int[] int32ArrShort = new int[]{2147483647, -2147483648}; + public List int32ListShort = Arrays.asList(2147483647, -2147483648); + public int[] uint32Arr = new int[]{0, 1, (int) 4294967295L}; + public List uint32List = Arrays.asList(0, 1, (int) 4294967295L); + public int[] uint32ArrShort = new int[]{(int) 4294967295L, 1}; + public List uint32ListShort = Arrays.asList((int) 4294967295L, 1); + public long[] int64Arr = new long[]{0L, -9223372036854775808L, 9223372036854775807L}; + public List int64List = Arrays.asList(0L, -9223372036854775808L, 9223372036854775807L); + public long[] int64ArrShort = new long[]{0L, -9223372036854775808L, 9223372036854775807L}; + public List int64ListShort = Arrays.asList(0L, -9223372036854775808L, 9223372036854775807L); + public long[] uint64Arr = new long[]{0L, 1L, -1L}; + public List uint64List = Arrays.asList(0L, 1L, -1L); + public long[] uint64ArrShort = new long[]{0L, 1L, -1L}; + public List uint64ListShort = Arrays.asList(0L, 1L, -1L); + public String[] stringArr = new String[]{"", "min value", "max_value"}; + public List stringList = Arrays.asList(stringArr); + public String[] stringArrShort = new String[]{"max_value", ""}; + public List stringListShort = Arrays.asList(stringArrShort); + private rosidl_generator_java.msg.BasicTypes basicTypes = new rosidl_generator_java.msg.BasicTypes(); + public rosidl_generator_java.msg.BasicTypes[] basicTypesArr = + new rosidl_generator_java.msg.BasicTypes[] {basicTypes, basicTypes, basicTypes}; + public List basicTypesList = Arrays.asList(basicTypesArr); + public rosidl_generator_java.msg.BasicTypes[] basicTypesArrShort = + new rosidl_generator_java.msg.BasicTypes[] {basicTypes}; + public List basicTypesListShort = Arrays.asList(basicTypesArrShort); + public rosidl_generator_java.msg.BasicTypes[] basicTypesArrLong = + new rosidl_generator_java.msg.BasicTypes[] {basicTypes, basicTypes, basicTypes, basicTypes}; + public List basicTypesListLong = Arrays.asList(basicTypesArrLong); + private rosidl_generator_java.msg.Constants constants = new rosidl_generator_java.msg.Constants(); + public rosidl_generator_java.msg.Constants[] constantsArr = + new rosidl_generator_java.msg.Constants[] {constants, constants, constants}; + public List constantsList = Arrays.asList(constantsArr); + public rosidl_generator_java.msg.Constants[] constantsArrShort = + new rosidl_generator_java.msg.Constants[] {constants}; + public List constantsListShort = Arrays.asList(constantsArrShort); + public rosidl_generator_java.msg.Constants[] constantsArrLong = + new rosidl_generator_java.msg.Constants[] {constants, constants, constants, constants}; + public List constantsListLong = Arrays.asList(constantsArrLong); + private rosidl_generator_java.msg.Defaults defaults = new rosidl_generator_java.msg.Defaults(); + public rosidl_generator_java.msg.Defaults[] defaultsArr = + new rosidl_generator_java.msg.Defaults[] {defaults, defaults, defaults}; + public List defaultsList = Arrays.asList(defaultsArr); + public rosidl_generator_java.msg.Defaults[] defaultsArrShort = + new rosidl_generator_java.msg.Defaults[] {defaults}; + public List defaultsListShort = Arrays.asList(defaultsArrShort); + public rosidl_generator_java.msg.Defaults[] defaultsArrLong = + new rosidl_generator_java.msg.Defaults[] {defaults, defaults, defaults, defaults}; + public List defaultsListLong = Arrays.asList(defaultsArrLong); + } + @BeforeClass public static void setupOnce() { try { // Configure log4j. Doing this dynamically so that Android does not complain about missing // the log4j JARs, SLF4J uses Android's native logging mechanism instead. - Class c = Class.forName("org.apache.log4j.BasicConfigurator"); - Method m = c.getDeclaredMethod("configure", (Class[]) null); + Class c = Class.forName("org.apache.log4j.BasicConfigurator"); + Method m = c.getDeclaredMethod("configure", (Class[]) null); Object o = m.invoke(null, (Object[]) null); } catch (Exception e) @@ -49,16 +139,16 @@ public static void setupOnce() { // TODO(jacobperron): Replace with JUnit's assertThrows method when we switch to JUnit 5 // See: https://junit.org/junit5/docs/5.0.1/api/org/junit/jupiter/api/Assertions.html - private static void assertThrows(Class expectedException, Callable func) { + private static void assertThrows(Class expectedException, Runnable func) { try { - func.call(); + func.run(); } catch(Exception exception) { if (expectedException.isInstance(exception)) { return; } } - assertTrue("Callable did not throw the expected exception", false); + assertTrue("Runnable did not throw the expected exception", false); } @Test @@ -203,271 +293,656 @@ public final void testCheckStringConstraints() { @Test public final void testArrays() { - rosidl_generator_java.msg.Arrays arrays = new rosidl_generator_java.msg.Arrays(); + final rosidl_generator_java.msg.Arrays arrays = new rosidl_generator_java.msg.Arrays(); + final ListFixtureData fixture = new ListFixtureData(); // This value should not change and is asserted at end of test arrays.setAlignmentCheck(42); // Test setting/getting fixed length arrays of primitive types - List boolList = Arrays.asList(true, false, true); - arrays.setBoolValues(boolList); - assertEquals(boolList, arrays.getBoolValues()); - assertThrows(IllegalArgumentException.class, - () -> arrays.setBoolValues(Arrays.asList(true, false, true, false))); - List byteList = Arrays.asList((byte) 0, (byte) 1, (byte) 255); - arrays.setByteValues(byteList); - assertEquals(byteList, arrays.getByteValues()); - assertThrows(IllegalArgumentException.class, - () -> arrays.setByteValues(Arrays.asList((byte) 1, (byte) 2))); - List charList = Arrays.asList(' ', 'a', 'Z'); - arrays.setCharValues(charList); - assertEquals(charList, arrays.getCharValues()); - assertThrows(IllegalArgumentException.class, - () -> arrays.setCharValues(Arrays.asList((byte) 'a', (byte) 'b', (byte) 'c', (byte) 'd'))); - List float32List = Arrays.asList(0.0f, -1.125f, 1.125f); - arrays.setFloat32Values(float32List); - assertEquals(float32List, arrays.getFloat32Values()); - assertThrows(IllegalArgumentException.class, - () -> arrays.setFloat32Values(Arrays.asList(1.0f, 2.0f))); - List float64List = Arrays.asList(0.0f, -3.1415, 3.1415); - arrays.setFloat64Values(float64List); - assertEquals(float64List, arrays.getFloat64Values()); - assertThrows(IllegalArgumentException.class, - () -> arrays.setFloat64Values(Arrays.asList(1.0, 2.0, 3.0, 4.0))); - List int8List = Arrays.asList(0, -128, 127); - arrays.setInt8Values(int8List); - assertEquals(int8List, arrays.getInt8Values()); - assertThrows(IllegalArgumentException.class, - () ->arrays.setInt8Values(Arrays.asList((byte) 1, (byte) 2))); - List uint8List = Arrays.asList(0, 1, 255); - arrays.setUint8Values(uint8List); - assertEquals(uint8List, arrays.getUint8Values()); - assertThrows(IllegalArgumentException.class, - () -> arrays.setUint8Values(Arrays.asList((byte) 1, (byte) 2, (byte) 3, (byte) 4))); - List int16List = Arrays.asList(0, -32768, 32767); - arrays.setInt16Values(int16List); - assertEquals(int16List, arrays.getInt16Values()); - assertThrows(IllegalArgumentException.class, - () -> arrays.setInt16Values(Arrays.asList((short) 1, (short) 2))); - List uint16List = Arrays.asList(0, 1, 65535); - arrays.setUint16Values(uint16List); - assertEquals(uint16List, arrays.getUint16Values()); - assertThrows(IllegalArgumentException.class, - () -> arrays.setUint16Values(Arrays.asList((short) 1, (short) 2, (short) 3, (short) 4))); - List int32List = Arrays.asList(0, -2147483648, 2147483647); - arrays.setInt32Values(int32List); - assertEquals(int32List, arrays.getInt32Values()); - assertThrows(IllegalArgumentException.class, - () -> arrays.setInt32Values(Arrays.asList(1, 2))); - List uint32List = Arrays.asList(0, 1, 4294967295L); - arrays.setUint32Values(uint32List); - assertEquals(uint32List, arrays.getUint32Values()); - assertThrows(IllegalArgumentException.class, - () -> arrays.setUint32Values(Arrays.asList(1, 2, 3, 4))); - List int64List = Arrays.asList(0, -9223372036854775808L, 9223372036854775807L); - arrays.setInt64Values(int64List); - assertEquals(int64List, arrays.getInt64Values()); - assertThrows(IllegalArgumentException.class, - () -> arrays.setInt64Values(Arrays.asList(1L, 2L))); - List uint64List = Arrays.asList(0, 1, -1); - arrays.setUint64Values(uint64List); - assertEquals(uint64List, arrays.getUint64Values()); - assertThrows(IllegalArgumentException.class, - () ->arrays.setUint64Values(Arrays.asList(1L, 2L, 3L, 4L))); + arrays.setBoolValues(fixture.boolList); + assertArrayEquals(fixture.boolArr, arrays.getBoolValues()); + assertEquals(fixture.boolList, arrays.getBoolValuesAsList()); + assertThrows(IllegalArgumentException.class, + new Runnable() { + @Override + public void run() { + arrays.setBoolValues(new boolean[]{true, false, true, false}); + } + }); + assertThrows(IllegalArgumentException.class, + new Runnable() { + @Override + public void run() { + arrays.setBoolValues(Arrays.asList(true, false, true, false)); + } + }); + arrays.setByteValues(fixture.byteList); + assertArrayEquals(fixture.byteArr, arrays.getByteValues()); + assertEquals(fixture.byteList, arrays.getByteValuesAsList()); + assertThrows(IllegalArgumentException.class, + new Runnable() { + @Override + public void run() { + arrays.setByteValues(new byte[]{(byte) 1, (byte) 2}); + } + }); + assertThrows(IllegalArgumentException.class, + new Runnable() { + @Override + public void run() { + arrays.setByteValues(Arrays.asList((byte) 1, (byte) 2)); + } + }); + arrays.setCharValues(fixture.charList); + assertArrayEquals(fixture.charArr, arrays.getCharValues()); + assertEquals(fixture.charList, arrays.getCharValuesAsList()); + assertThrows(IllegalArgumentException.class, + new Runnable() { + @Override + public void run() { + arrays.setCharValues(new byte[]{(byte) 'a', (byte) 'b', (byte) 'c', (byte) 'd'}); + } + }); + assertThrows(IllegalArgumentException.class, + new Runnable() { + @Override + public void run() { + arrays.setCharValues(Arrays.asList((byte) 'a', (byte) 'b', (byte) 'c', (byte) 'd')); + } + }); + arrays.setFloat32Values(fixture.float32List); + assertTrue(Arrays.equals(fixture.float32Arr, arrays.getFloat32Values())); + assertEquals(fixture.float32List, arrays.getFloat32ValuesAsList()); + assertThrows(IllegalArgumentException.class, + new Runnable() { + @Override + public void run() { + arrays.setFloat32Values(new float[]{1.0f, 2.0f}); + } + }); + assertThrows(IllegalArgumentException.class, + new Runnable() { + @Override + public void run() { + arrays.setFloat32Values(Arrays.asList(1.0f, 2.0f)); + } + }); + arrays.setFloat64Values(fixture.float64List); + assertTrue(Arrays.equals(fixture.float64Arr, arrays.getFloat64Values())); + assertEquals(fixture.float64List, arrays.getFloat64ValuesAsList()); + assertThrows(IllegalArgumentException.class, + new Runnable() { + @Override + public void run() { + arrays.setFloat64Values(new double[]{1.0, 2.0, 3.0, 4.0}); + } + }); + assertThrows(IllegalArgumentException.class, + new Runnable() { + @Override + public void run() { + arrays.setFloat64Values(Arrays.asList(1.0, 2.0, 3.0, 4.0)); + } + }); + arrays.setInt8Values(fixture.int8List); + assertArrayEquals(fixture.int8Arr, arrays.getInt8Values()); + assertEquals(fixture.int8List, arrays.getInt8ValuesAsList()); + assertThrows(IllegalArgumentException.class, + new Runnable() { + @Override + public void run() { + arrays.setInt8Values(new byte[]{(byte) 1, (byte) 2}); + } + }); + assertThrows(IllegalArgumentException.class, + new Runnable() { + @Override + public void run() { + arrays.setInt8Values(Arrays.asList((byte) 1, (byte) 2)); + } + }); + arrays.setUint8Values(fixture.uint8List); + assertArrayEquals(fixture.uint8Arr, arrays.getUint8Values()); + assertEquals(fixture.uint8List, arrays.getUint8ValuesAsList()); + assertThrows(IllegalArgumentException.class, + new Runnable() { + @Override + public void run() { + arrays.setUint8Values(new byte[]{(byte) 1, (byte) 2, (byte) 3, (byte) 4}); + } + }); + assertThrows(IllegalArgumentException.class, + new Runnable() { + @Override + public void run() { + arrays.setUint8Values(Arrays.asList((byte) 1, (byte) 2, (byte) 3, (byte) 4)); + } + }); + arrays.setInt16Values(fixture.int16List); + assertTrue(Arrays.equals(fixture.int16Arr, arrays.getInt16Values())); + assertEquals(fixture.int16List, arrays.getInt16ValuesAsList()); + assertThrows(IllegalArgumentException.class, + new Runnable() { + @Override + public void run() { + arrays.setInt16Values(new short[]{(short) 1, (short) 2}); + } + }); + assertThrows(IllegalArgumentException.class, + new Runnable() { + @Override + public void run() { + arrays.setInt16Values(Arrays.asList((short) 1, (short) 2)); + } + }); + arrays.setUint16Values(fixture.uint16List); + assertTrue(Arrays.equals(fixture.uint16Arr, arrays.getUint16Values())); + assertEquals(fixture.uint16List, arrays.getUint16ValuesAsList()); + assertThrows(IllegalArgumentException.class, + new Runnable() { + @Override + public void run() { + arrays.setUint16Values(new short[]{(short) 1, (short) 2, (short) 3, (short) 4}); + } + }); + assertThrows(IllegalArgumentException.class, + new Runnable() { + @Override + public void run() { + arrays.setUint16Values(Arrays.asList((short) 1, (short) 2, (short) 3, (short) 4)); + } + }); + arrays.setInt32Values(fixture.int32List); + assertArrayEquals(fixture.int32Arr, arrays.getInt32Values()); + assertEquals(fixture.int32List, arrays.getInt32ValuesAsList()); + assertThrows(IllegalArgumentException.class, + new Runnable() { + @Override + public void run() { + arrays.setInt32Values(new int[]{1, 2}); + } + }); + assertThrows(IllegalArgumentException.class, + new Runnable() { + @Override + public void run() { + arrays.setInt32Values(Arrays.asList(1, 2)); + } + }); + arrays.setUint32Values(fixture.uint32List); + assertArrayEquals(fixture.uint32Arr, arrays.getUint32Values()); + assertEquals(fixture.uint32List, arrays.getUint32ValuesAsList()); + assertThrows(IllegalArgumentException.class, + new Runnable() { + @Override + public void run() { + arrays.setUint32Values(new int[]{1, 2, 3, 4}); + } + }); + assertThrows(IllegalArgumentException.class, + new Runnable() { + @Override + public void run() { + arrays.setUint32Values(Arrays.asList(1, 2, 3, 4)); + } + }); + arrays.setInt64Values(fixture.int64List); + assertArrayEquals(fixture.int64Arr, arrays.getInt64Values()); + assertEquals(fixture.int64List, arrays.getInt64ValuesAsList()); + assertThrows(IllegalArgumentException.class, + new Runnable() { + @Override + public void run() { + arrays.setInt64Values(new long[]{1L, 2L}); + } + }); + assertThrows(IllegalArgumentException.class, + new Runnable() { + @Override + public void run() { + arrays.setInt64Values(Arrays.asList(1L, 2L)); + } + }); + arrays.setUint64Values(fixture.uint64List); + assertArrayEquals(fixture.uint64Arr, arrays.getUint64Values()); + assertEquals(fixture.uint64List, arrays.getUint64ValuesAsList()); + assertThrows(IllegalArgumentException.class, + new Runnable() { + @Override + public void run() { + arrays.setUint64Values(new long[]{1L, 2L, 3L, 4L}); + } + }); + assertThrows(IllegalArgumentException.class, + new Runnable() { + @Override + public void run() { + arrays.setUint64Values(Arrays.asList(1L, 2L, 3L, 4L)); + } + }); // Test setting/getting fixed length arrays of strings - List stringList = Arrays.asList("", "min value", "max_value"); - arrays.setStringValues(stringList); - assertEquals(stringList, arrays.getStringValues()); + arrays.setStringValues(fixture.stringList); + assertArrayEquals(fixture.stringArr, arrays.getStringValues()); + assertEquals(fixture.stringList, arrays.getStringValuesAsList()); + assertThrows(IllegalArgumentException.class, + new Runnable() { + @Override + public void run() { + arrays.setStringValues(new String[]{"too", "few"}); + } + }); assertThrows(IllegalArgumentException.class, - () -> arrays.setStringValues(Arrays.asList("too", "few"))); + new Runnable() { + @Override + public void run() { + arrays.setStringValues(Arrays.asList("too", "few")); + } + }); // Test setting/getting fixed length arrays of nested types - rosidl_generator_java.msg.BasicTypes basicTypes = new rosidl_generator_java.msg.BasicTypes(); - List basicTypesList = Arrays.asList( - new rosidl_generator_java.msg.BasicTypes[] {basicTypes, basicTypes, basicTypes}); - arrays.setBasicTypesValues(basicTypesList); - assertEquals(basicTypesList, arrays.getBasicTypesValues()); - assertThrows(IllegalArgumentException.class, - () -> arrays.setBasicTypesValues(Arrays.asList(new rosidl_generator_java.msg.BasicTypes[] {basicTypes}))); - rosidl_generator_java.msg.Constants constants = new rosidl_generator_java.msg.Constants(); - List constantsList = Arrays.asList( - new rosidl_generator_java.msg.Constants[] {constants, constants, constants}); - arrays.setConstantsValues(constantsList); - assertEquals(constantsList, arrays.getConstantsValues()); - assertThrows(IllegalArgumentException.class, - () -> arrays.setConstantsValues(Arrays.asList(new rosidl_generator_java.msg.Constants[] {constants}))); - rosidl_generator_java.msg.Defaults defaults = new rosidl_generator_java.msg.Defaults(); - List defaultsList = Arrays.asList( - new rosidl_generator_java.msg.Defaults[] {defaults, defaults, defaults}); - arrays.setDefaultsValues(defaultsList); - assertEquals(defaultsList, arrays.getDefaultsValues()); + arrays.setBasicTypesValues(fixture.basicTypesList); + assertArrayEquals(fixture.basicTypesArr, arrays.getBasicTypesValues()); + assertEquals(fixture.basicTypesList, arrays.getBasicTypesValuesAsList()); + assertThrows(IllegalArgumentException.class, + new Runnable() { + @Override + public void run() { + arrays.setBasicTypesValues(fixture.basicTypesArrShort); + } + }); + assertThrows(IllegalArgumentException.class, + new Runnable() { + @Override + public void run() { + arrays.setBasicTypesValues(fixture.basicTypesListShort); + } + }); + arrays.setConstantsValues(fixture.constantsList); + assertArrayEquals(fixture.constantsArr, arrays.getConstantsValues()); + assertEquals(fixture.constantsList, arrays.getConstantsValuesAsList()); + assertThrows(IllegalArgumentException.class, + new Runnable() { + @Override + public void run() { + arrays.setConstantsValues(fixture.constantsArrShort); + } + }); + assertThrows(IllegalArgumentException.class, + new Runnable() { + @Override + public void run() { + arrays.setConstantsValues(fixture.constantsListShort); + } + }); + arrays.setDefaultsValues(fixture.defaultsList); + assertArrayEquals(fixture.defaultsArr, arrays.getDefaultsValues()); + assertEquals(fixture.defaultsList, arrays.getDefaultsValuesAsList()); + assertThrows(IllegalArgumentException.class, + new Runnable() { + @Override + public void run() { + arrays.setDefaultsValues(fixture.defaultsArrShort); + } + }); assertThrows(IllegalArgumentException.class, - () -> arrays.setDefaultsValues(Arrays.asList(new rosidl_generator_java.msg.Defaults[] {defaults}))); + new Runnable() { + @Override + public void run() { + arrays.setDefaultsValues(fixture.defaultsListShort); + } + }); assertEquals(42, arrays.getAlignmentCheck()); } @Test public final void testBoundedSequences() { - rosidl_generator_java.msg.BoundedSequences bounded_seq = new rosidl_generator_java.msg.BoundedSequences(); + final rosidl_generator_java.msg.BoundedSequences bounded_seq = new rosidl_generator_java.msg.BoundedSequences(); + final ListFixtureData fixture = new ListFixtureData(); // This value should not change and is asserted at end of test bounded_seq.setAlignmentCheck(42); // Test setting/getting fixed length bounded_seq of primitive types - List boolList = Arrays.asList(true, false, true); - bounded_seq.setBoolValues(boolList); - assertEquals(boolList, bounded_seq.getBoolValues()); - List boolListShort = Arrays.asList(false); - bounded_seq.setBoolValues(boolListShort); - assertEquals(boolListShort, bounded_seq.getBoolValues()); - assertThrows(IllegalArgumentException.class, - () -> bounded_seq.setBoolValues(Arrays.asList(true, false, true, false))); - List byteList = Arrays.asList((byte) 0, (byte) 1, (byte) 255); - bounded_seq.setByteValues(byteList); - assertEquals(byteList, bounded_seq.getByteValues()); - List byteListShort = Arrays.asList((byte) 1); - bounded_seq.setByteValues(byteListShort); - assertEquals(byteListShort, bounded_seq.getByteValues()); - assertThrows(IllegalArgumentException.class, - () -> bounded_seq.setByteValues(Arrays.asList((byte) 1, (byte) 2, (byte) 3, (byte) 4))); - List charList = Arrays.asList(' ', 'a', 'Z'); - bounded_seq.setCharValues(charList); - assertEquals(charList, bounded_seq.getCharValues()); - List charListShort = Arrays.asList('z', 'A'); - bounded_seq.setCharValues(charListShort); - assertEquals(charListShort, bounded_seq.getCharValues()); - assertThrows(IllegalArgumentException.class, - () -> bounded_seq.setCharValues(Arrays.asList((byte) 'a', (byte) 'b', (byte) 'c', (byte) 'd'))); - List float32List = Arrays.asList(0.0f, -1.125f, 1.125f); - bounded_seq.setFloat32Values(float32List); - assertEquals(float32List, bounded_seq.getFloat32Values()); - List float32ListShort = Arrays.asList(1.125f, -1.125f); - bounded_seq.setFloat32Values(float32ListShort); - assertEquals(float32ListShort, bounded_seq.getFloat32Values()); - assertThrows(IllegalArgumentException.class, - () -> bounded_seq.setFloat32Values(Arrays.asList(1.0f, 2.0f, 3.0f, 4.0f))); - List float64List = Arrays.asList(0.0f, -3.1415, 3.1415); - bounded_seq.setFloat64Values(float64List); - assertEquals(float64List, bounded_seq.getFloat64Values()); - List float64ListShort = Arrays.asList(3.1415, -3.1415); - bounded_seq.setFloat64Values(float64ListShort); - assertEquals(float64ListShort, bounded_seq.getFloat64Values()); - assertThrows(IllegalArgumentException.class, - () -> bounded_seq.setFloat64Values(Arrays.asList(1.0, 2.0, 3.0, 4.0))); - List int8List = Arrays.asList(0, -128, 127); - bounded_seq.setInt8Values(int8List); - assertEquals(int8List, bounded_seq.getInt8Values()); - List int8ListShort = Arrays.asList(127, -128); - bounded_seq.setInt8Values(int8ListShort); - assertEquals(int8ListShort, bounded_seq.getInt8Values()); - assertThrows(IllegalArgumentException.class, - () -> bounded_seq.setInt8Values(Arrays.asList((byte) 1, (byte) 2, (byte) 3, (byte) 4))); - List uint8List = Arrays.asList(0, 1, 255); - bounded_seq.setUint8Values(uint8List); - assertEquals(uint8List, bounded_seq.getUint8Values()); - List uint8ListShort = Arrays.asList(255, 1); - bounded_seq.setUint8Values(uint8ListShort); - assertEquals(uint8ListShort, bounded_seq.getUint8Values()); - assertThrows(IllegalArgumentException.class, - () -> bounded_seq.setUint8Values(Arrays.asList((byte) 1, (byte) 2, (byte) 3, (byte) 4))); - List int16List = Arrays.asList(0, -32768, 32767); - bounded_seq.setInt16Values(int16List); - assertEquals(int16List, bounded_seq.getInt16Values()); - List int16ListShort = Arrays.asList(32767, -32768); - bounded_seq.setInt16Values(int16ListShort); - assertEquals(int16ListShort, bounded_seq.getInt16Values()); - assertThrows(IllegalArgumentException.class, - () -> bounded_seq.setInt16Values(Arrays.asList((short) 1, (short) 2, (short) 3, (short) 4))); - List uint16List = Arrays.asList(0, 1, 65535); - bounded_seq.setUint16Values(uint16List); - assertEquals(uint16List, bounded_seq.getUint16Values()); - List uint16ListShort = Arrays.asList(0, 1, 65535); - bounded_seq.setUint16Values(uint16ListShort); - assertEquals(uint16ListShort, bounded_seq.getUint16Values()); - assertThrows(IllegalArgumentException.class, - () -> bounded_seq.setUint16Values(Arrays.asList((short) 1, (short) 2, (short) 3, (short) 4))); - List int32List = Arrays.asList(0, -2147483648, 2147483647); - bounded_seq.setInt32Values(int32List); - assertEquals(int32List, bounded_seq.getInt32Values()); - List int32ListShort = Arrays.asList(2147483647, -2147483648); - bounded_seq.setInt32Values(int32ListShort); - assertEquals(int32ListShort, bounded_seq.getInt32Values()); - assertThrows(IllegalArgumentException.class, - () -> bounded_seq.setInt32Values(Arrays.asList(1, 2, 3, 4))); - List uint32List = Arrays.asList(0, 1, 4294967295L); - bounded_seq.setUint32Values(uint32List); - assertEquals(uint32List, bounded_seq.getUint32Values()); - List uint32ListShort = Arrays.asList(4294967295L, 1); - bounded_seq.setUint32Values(uint32ListShort); - assertEquals(uint32ListShort, bounded_seq.getUint32Values()); - assertThrows(IllegalArgumentException.class, - () -> bounded_seq.setUint32Values(Arrays.asList(1, 2, 3, 4))); - List int64List = Arrays.asList(0, -9223372036854775808L, 9223372036854775807L); - bounded_seq.setInt64Values(int64List); - assertEquals(int64List, bounded_seq.getInt64Values()); - List int64ListShort = Arrays.asList(0, -9223372036854775808L, 9223372036854775807L); - bounded_seq.setInt64Values(int64ListShort); - assertEquals(int64ListShort, bounded_seq.getInt64Values()); - assertThrows(IllegalArgumentException.class, - () -> bounded_seq.setInt64Values(Arrays.asList(1L, 2L, 3L, 4L))); - List uint64List = Arrays.asList(0, 1, -1); - bounded_seq.setUint64Values(uint64List); - assertEquals(uint64List, bounded_seq.getUint64Values()); - List uint64ListShort = Arrays.asList(0, 1, -1); - bounded_seq.setUint64Values(uint64ListShort); - assertEquals(uint64ListShort, bounded_seq.getUint64Values()); - assertThrows(IllegalArgumentException.class, - () -> bounded_seq.setUint64Values(Arrays.asList(1L, 2L, 3L, 4L))); + bounded_seq.setBoolValues(fixture.boolList); + assertArrayEquals(fixture.boolArr, bounded_seq.getBoolValues()); + assertEquals(fixture.boolList, bounded_seq.getBoolValuesAsList()); + bounded_seq.setBoolValues(fixture.boolListShort); + assertArrayEquals(fixture.boolArrShort, bounded_seq.getBoolValues()); + assertEquals(fixture.boolListShort, bounded_seq.getBoolValuesAsList()); + assertThrows(IllegalArgumentException.class, + new Runnable() { + @Override + public void run() { + bounded_seq.setBoolValues(new boolean[]{true, false, true, false}); + } + }); + assertThrows(IllegalArgumentException.class, + new Runnable() { + @Override + public void run() { + bounded_seq.setBoolValues(Arrays.asList(true, false, true, false)); + } + }); + bounded_seq.setByteValues(fixture.byteList); + assertArrayEquals(fixture.byteArr, bounded_seq.getByteValues()); + assertEquals(fixture.byteList, bounded_seq.getByteValuesAsList()); + bounded_seq.setByteValues(fixture.byteListShort); + assertArrayEquals(fixture.byteArrShort, bounded_seq.getByteValues()); + assertEquals(fixture.byteListShort, bounded_seq.getByteValuesAsList()); + assertThrows(IllegalArgumentException.class, + new Runnable() { + @Override + public void run() { + bounded_seq.setByteValues(new byte[]{(byte) 1, (byte) 2, (byte) 3, (byte) 4}); + } + }); + assertThrows(IllegalArgumentException.class, + new Runnable() { + @Override + public void run() { + bounded_seq.setByteValues(Arrays.asList((byte) 1, (byte) 2, (byte) 3, (byte) 4)); + } + }); + bounded_seq.setCharValues(fixture.charList); + assertArrayEquals(fixture.charArr, bounded_seq.getCharValues()); + assertEquals(fixture.charList, bounded_seq.getCharValuesAsList()); + bounded_seq.setCharValues(fixture.charListShort); + assertArrayEquals(fixture.charArrShort, bounded_seq.getCharValues()); + assertEquals(fixture.charListShort, bounded_seq.getCharValuesAsList()); + assertThrows(IllegalArgumentException.class, + new Runnable() { + @Override + public void run() { + bounded_seq.setCharValues(new byte[]{(byte) 'a', (byte) 'b', (byte) 'c', (byte) 'd'}); + } + }); + assertThrows(IllegalArgumentException.class, + new Runnable() { + @Override + public void run() { + bounded_seq.setCharValues(Arrays.asList((byte) 'a', (byte) 'b', (byte) 'c', (byte) 'd')); + } + }); + bounded_seq.setFloat32Values(fixture.float32List); + assertTrue(Arrays.equals(fixture.float32Arr, bounded_seq.getFloat32Values())); + assertEquals(fixture.float32List, bounded_seq.getFloat32ValuesAsList()); + bounded_seq.setFloat32Values(fixture.float32ListShort); + assertTrue(Arrays.equals(fixture.float32ArrShort, bounded_seq.getFloat32Values())); + assertEquals(fixture.float32ListShort, bounded_seq.getFloat32ValuesAsList()); + assertThrows(IllegalArgumentException.class, + new Runnable() { + @Override + public void run() { + bounded_seq.setFloat32Values(new float[]{1.0f, 2.0f, 3.0f, 4.0f}); + } + }); + assertThrows(IllegalArgumentException.class, + new Runnable() { + @Override + public void run() { + bounded_seq.setFloat32Values(Arrays.asList(1.0f, 2.0f, 3.0f, 4.0f)); + } + }); + bounded_seq.setFloat64Values(fixture.float64List); + assertTrue(Arrays.equals(fixture.float64Arr, bounded_seq.getFloat64Values())); + assertEquals(fixture.float64List, bounded_seq.getFloat64ValuesAsList()); + bounded_seq.setFloat64Values(fixture.float64ListShort); + assertTrue(Arrays.equals(fixture.float64ArrShort, bounded_seq.getFloat64Values())); + assertEquals(fixture.float64ListShort, bounded_seq.getFloat64ValuesAsList()); + assertThrows(IllegalArgumentException.class, + new Runnable() { + @Override + public void run() { + bounded_seq.setFloat64Values(Arrays.asList(1.0, 2.0, 3.0, 4.0)); + } + }); + bounded_seq.setInt8Values(fixture.int8List); + assertArrayEquals(fixture.int8Arr, bounded_seq.getInt8Values()); + assertEquals(fixture.int8List, bounded_seq.getInt8ValuesAsList()); + bounded_seq.setInt8Values(fixture.int8ListShort); + assertArrayEquals(fixture.int8ArrShort, bounded_seq.getInt8Values()); + assertEquals(fixture.int8ListShort, bounded_seq.getInt8ValuesAsList()); + assertThrows(IllegalArgumentException.class, + new Runnable() { + @Override + public void run() { + bounded_seq.setInt8Values(new byte[]{(byte) 1, (byte) 2, (byte) 3, (byte) 4}); + } + }); + assertThrows(IllegalArgumentException.class, + new Runnable() { + @Override + public void run() { + bounded_seq.setInt8Values(Arrays.asList((byte) 1, (byte) 2, (byte) 3, (byte) 4)); + } + }); + bounded_seq.setUint8Values(fixture.uint8List); + assertArrayEquals(fixture.uint8Arr, bounded_seq.getUint8Values()); + assertEquals(fixture.uint8List, bounded_seq.getUint8ValuesAsList()); + bounded_seq.setUint8Values(fixture.uint8ListShort); + assertArrayEquals(fixture.uint8ArrShort, bounded_seq.getUint8Values()); + assertEquals(fixture.uint8ListShort, bounded_seq.getUint8ValuesAsList()); + assertThrows(IllegalArgumentException.class, + new Runnable() { + @Override + public void run() { + bounded_seq.setUint8Values(new byte[]{(byte) 1, (byte) 2, (byte) 3, (byte) 4}); + } + }); + assertThrows(IllegalArgumentException.class, + new Runnable() { + @Override + public void run() { + bounded_seq.setUint8Values(Arrays.asList((byte) 1, (byte) 2, (byte) 3, (byte) 4)); + } + }); + bounded_seq.setInt16Values(fixture.int16List); + assertTrue(Arrays.equals(fixture.int16Arr, bounded_seq.getInt16Values())); + assertEquals(fixture.int16List, bounded_seq.getInt16ValuesAsList()); + bounded_seq.setInt16Values(fixture.int16ListShort); + assertTrue(Arrays.equals(fixture.int16ArrShort, bounded_seq.getInt16Values())); + assertEquals(fixture.int16ListShort, bounded_seq.getInt16ValuesAsList()); + assertThrows(IllegalArgumentException.class, + new Runnable() { + @Override + public void run() { + bounded_seq.setInt16Values(new short[]{(short) 1, (short) 2, (short) 3, (short) 4}); + } + }); + assertThrows(IllegalArgumentException.class, + new Runnable() { + @Override + public void run() { + bounded_seq.setInt16Values(Arrays.asList((short) 1, (short) 2, (short) 3, (short) 4)); + } + }); + bounded_seq.setUint16Values(fixture.uint16List); + assertTrue(Arrays.equals(fixture.uint16Arr, bounded_seq.getUint16Values())); + assertEquals(fixture.uint16List, bounded_seq.getUint16ValuesAsList()); + bounded_seq.setUint16Values(fixture.uint16ListShort); + assertTrue(Arrays.equals(fixture.uint16ArrShort, bounded_seq.getUint16Values())); + assertEquals(fixture.uint16ListShort, bounded_seq.getUint16ValuesAsList()); + assertThrows(IllegalArgumentException.class, + new Runnable() { + @Override + public void run() { + bounded_seq.setUint16Values(new short[]{(short) 1, (short) 2, (short) 3, (short) 4}); + } + }); + assertThrows(IllegalArgumentException.class, + new Runnable() { + @Override + public void run() { + bounded_seq.setUint16Values(Arrays.asList((short) 1, (short) 2, (short) 3, (short) 4)); + } + }); + bounded_seq.setInt32Values(fixture.int32List); + assertArrayEquals(fixture.int32Arr, bounded_seq.getInt32Values()); + assertEquals(fixture.int32List, bounded_seq.getInt32ValuesAsList()); + bounded_seq.setInt32Values(fixture.int32ListShort); + assertArrayEquals(fixture.int32ArrShort, bounded_seq.getInt32Values()); + assertEquals(fixture.int32ListShort, bounded_seq.getInt32ValuesAsList()); + assertThrows(IllegalArgumentException.class, + new Runnable() { + @Override + public void run() { + bounded_seq.setInt32Values(new int[]{1, 2, 3, 4}); + } + }); + assertThrows(IllegalArgumentException.class, + new Runnable() { + @Override + public void run() { + bounded_seq.setInt32Values(Arrays.asList(1, 2, 3, 4)); + } + }); + bounded_seq.setUint32Values(fixture.uint32List); + assertArrayEquals(fixture.uint32Arr, bounded_seq.getUint32Values()); + assertEquals(fixture.uint32List, bounded_seq.getUint32ValuesAsList()); + bounded_seq.setUint32Values(fixture.uint32ListShort); + assertArrayEquals(fixture.uint32ArrShort, bounded_seq.getUint32Values()); + assertEquals(fixture.uint32ListShort, bounded_seq.getUint32ValuesAsList()); + assertThrows(IllegalArgumentException.class, + new Runnable() { + @Override + public void run() { + bounded_seq.setUint32Values(new int[]{1, 2, 3, 4}); + } + }); + assertThrows(IllegalArgumentException.class, + new Runnable() { + @Override + public void run() { + bounded_seq.setUint32Values(Arrays.asList(1, 2, 3, 4)); + } + }); + bounded_seq.setInt64Values(fixture.int64List); + assertArrayEquals(fixture.int64Arr, bounded_seq.getInt64Values()); + assertEquals(fixture.int64List, bounded_seq.getInt64ValuesAsList()); + bounded_seq.setInt64Values(fixture.int64ListShort); + assertArrayEquals(fixture.int64ArrShort, bounded_seq.getInt64Values()); + assertEquals(fixture.int64ListShort, bounded_seq.getInt64ValuesAsList()); + assertThrows(IllegalArgumentException.class, + new Runnable() { + @Override + public void run() { + bounded_seq.setInt64Values(new long[]{1L, 2L, 3L, 4L}); + } + }); + assertThrows(IllegalArgumentException.class, + new Runnable() { + @Override + public void run() { + bounded_seq.setInt64Values(Arrays.asList(1L, 2L, 3L, 4L)); + } + }); + bounded_seq.setUint64Values(fixture.uint64List); + assertArrayEquals(fixture.uint64Arr, bounded_seq.getUint64Values()); + assertEquals(fixture.uint64List, bounded_seq.getUint64ValuesAsList()); + bounded_seq.setUint64Values(fixture.uint64ListShort); + assertArrayEquals(fixture.uint64ArrShort, bounded_seq.getUint64Values()); + assertEquals(fixture.uint64ListShort, bounded_seq.getUint64ValuesAsList()); + assertThrows(IllegalArgumentException.class, + new Runnable() { + @Override + public void run() { + bounded_seq.setUint64Values(new long[]{1L, 2L, 3L, 4L}); + } + }); + assertThrows(IllegalArgumentException.class, + new Runnable() { + @Override + public void run() { + bounded_seq.setUint64Values(Arrays.asList(1L, 2L, 3L, 4L)); + } + }); // Test setting/getting fixed length bounded_seq of strings - List stringList = Arrays.asList("", "min value", "max_value"); - bounded_seq.setStringValues(stringList); - assertEquals(stringList, bounded_seq.getStringValues()); - List stringListShort = Arrays.asList("max_value", ""); - bounded_seq.setStringValues(stringListShort); - assertEquals(stringListShort, bounded_seq.getStringValues()); + bounded_seq.setStringValues(fixture.stringList); + assertArrayEquals(fixture.stringArr, bounded_seq.getStringValues()); + assertEquals(fixture.stringList, bounded_seq.getStringValuesAsList()); + bounded_seq.setStringValues(fixture.stringListShort); + assertArrayEquals(fixture.stringArrShort, bounded_seq.getStringValues()); + assertEquals(fixture.stringListShort, bounded_seq.getStringValuesAsList()); + assertThrows(IllegalArgumentException.class, + new Runnable() { + @Override + public void run() { + bounded_seq.setStringValues(new String[]{"too", "many", "values", "!"}); + } + }); assertThrows(IllegalArgumentException.class, - () -> bounded_seq.setStringValues(Arrays.asList("too", "many", "values", "!"))); + new Runnable() { + @Override + public void run() { + bounded_seq.setStringValues(Arrays.asList("too", "many", "values", "!")); + } + }); // Test setting/getting fixed length bounded_seq of nested types - rosidl_generator_java.msg.BasicTypes basicTypes = new rosidl_generator_java.msg.BasicTypes(); - List basicTypesList = Arrays.asList( - new rosidl_generator_java.msg.BasicTypes[] {basicTypes, basicTypes, basicTypes}); - bounded_seq.setBasicTypesValues(basicTypesList); - assertEquals(basicTypesList, bounded_seq.getBasicTypesValues()); - List basicTypesListShort = Arrays.asList( - new rosidl_generator_java.msg.BasicTypes[] {basicTypes}); - bounded_seq.setBasicTypesValues(basicTypesListShort); - assertEquals(basicTypesListShort, bounded_seq.getBasicTypesValues()); - assertThrows(IllegalArgumentException.class, - () -> bounded_seq.setBasicTypesValues( - Arrays.asList(new rosidl_generator_java.msg.BasicTypes[] {basicTypes, basicTypes, basicTypes, basicTypes}))); - rosidl_generator_java.msg.Constants constants = new rosidl_generator_java.msg.Constants(); - List constantsList = Arrays.asList( - new rosidl_generator_java.msg.Constants[] {constants, constants, constants}); - bounded_seq.setConstantsValues(constantsList); - assertEquals(constantsList, bounded_seq.getConstantsValues()); - List constantsListShort = Arrays.asList( - new rosidl_generator_java.msg.Constants[] {constants}); - bounded_seq.setConstantsValues(constantsListShort); - assertEquals(constantsListShort, bounded_seq.getConstantsValues()); - assertThrows(IllegalArgumentException.class, - () -> bounded_seq.setConstantsValues( - Arrays.asList(new rosidl_generator_java.msg.Constants[] {constants, constants, constants, constants}))); - rosidl_generator_java.msg.Defaults defaults = new rosidl_generator_java.msg.Defaults(); - List defaultsList = Arrays.asList( - new rosidl_generator_java.msg.Defaults[] {defaults, defaults, defaults}); - bounded_seq.setDefaultsValues(defaultsList); - assertEquals(defaultsList, bounded_seq.getDefaultsValues()); - List defaultsListShort = Arrays.asList( - new rosidl_generator_java.msg.Defaults[] {defaults, defaults, defaults}); - bounded_seq.setDefaultsValues(defaultsListShort); - assertEquals(defaultsListShort, bounded_seq.getDefaultsValues()); - assertThrows(IllegalArgumentException.class, - () -> bounded_seq.setDefaultsValues( - Arrays.asList(new rosidl_generator_java.msg.Defaults[] {defaults, defaults, defaults, defaults}))); + bounded_seq.setBasicTypesValues(fixture.basicTypesList); + assertArrayEquals(fixture.basicTypesArr, bounded_seq.getBasicTypesValues()); + assertEquals(fixture.basicTypesList, bounded_seq.getBasicTypesValuesAsList()); + bounded_seq.setBasicTypesValues(fixture.basicTypesListShort); + assertArrayEquals(fixture.basicTypesArrShort, bounded_seq.getBasicTypesValues()); + assertEquals(fixture.basicTypesListShort, bounded_seq.getBasicTypesValuesAsList()); + assertThrows(IllegalArgumentException.class, + new Runnable() { + @Override + public void run() { + bounded_seq.setBasicTypesValues(fixture.basicTypesArrLong); + } + }); + assertThrows(IllegalArgumentException.class, + new Runnable() { + @Override + public void run() { + bounded_seq.setBasicTypesValues(fixture.basicTypesListLong); + } + }); + bounded_seq.setConstantsValues(fixture.constantsList); + assertArrayEquals(fixture.constantsArr, bounded_seq.getConstantsValues()); + assertEquals(fixture.constantsList, bounded_seq.getConstantsValuesAsList()); + bounded_seq.setConstantsValues(fixture.constantsListShort); + assertArrayEquals(fixture.constantsArrShort, bounded_seq.getConstantsValues()); + assertEquals(fixture.constantsListShort, bounded_seq.getConstantsValuesAsList()); + assertThrows(IllegalArgumentException.class, + new Runnable() { + @Override + public void run() { + bounded_seq.setConstantsValues(fixture.constantsArrLong); + } + }); + assertThrows(IllegalArgumentException.class, + new Runnable() { + @Override + public void run() { + bounded_seq.setConstantsValues(fixture.constantsListLong); + } + }); + bounded_seq.setDefaultsValues(fixture.defaultsList); + assertArrayEquals(fixture.defaultsArr, bounded_seq.getDefaultsValues()); + assertEquals(fixture.defaultsList, bounded_seq.getDefaultsValuesAsList()); + bounded_seq.setDefaultsValues(fixture.defaultsListShort); + assertArrayEquals(fixture.defaultsArrShort, bounded_seq.getDefaultsValues()); + assertEquals(fixture.defaultsListShort, bounded_seq.getDefaultsValuesAsList()); + assertThrows(IllegalArgumentException.class, + new Runnable() { + @Override + public void run() { + bounded_seq.setDefaultsValues(fixture.defaultsArrLong); + } + }); + assertThrows(IllegalArgumentException.class, + new Runnable() { + @Override + public void run() { + bounded_seq.setDefaultsValues(fixture.defaultsListLong); + } + }); assertEquals(42, bounded_seq.getAlignmentCheck()); } @@ -475,72 +950,67 @@ public final void testBoundedSequences() { @Test public final void testUnboundedSequences() { rosidl_generator_java.msg.UnboundedSequences unbounded_seq = new rosidl_generator_java.msg.UnboundedSequences(); + ListFixtureData fixture = new ListFixtureData(); // This value should not change and is asserted at end of test unbounded_seq.setAlignmentCheck(42); // Test setting/getting fixed length unbounded_seq of primitive types - List boolList = Arrays.asList(true, false, true); - unbounded_seq.setBoolValues(boolList); - assertEquals(boolList, unbounded_seq.getBoolValues()); - List byteList = Arrays.asList((byte) 0, (byte) 1, (byte) 255); - unbounded_seq.setByteValues(byteList); - assertEquals(byteList, unbounded_seq.getByteValues()); - List charList = Arrays.asList(' ', 'a', 'Z'); - unbounded_seq.setCharValues(charList); - assertEquals(charList, unbounded_seq.getCharValues()); - List float32List = Arrays.asList(0.0f, -1.125f, 1.125f); - unbounded_seq.setFloat32Values(float32List); - assertEquals(float32List, unbounded_seq.getFloat32Values()); - List float64List = Arrays.asList(0.0f, -3.1415, 3.1415); - unbounded_seq.setFloat64Values(float64List); - assertEquals(float64List, unbounded_seq.getFloat64Values()); - List int8List = Arrays.asList(0, -128, 127); - unbounded_seq.setInt8Values(int8List); - assertEquals(int8List, unbounded_seq.getInt8Values()); - List uint8List = Arrays.asList(0, 1, 255); - unbounded_seq.setUint8Values(uint8List); - assertEquals(uint8List, unbounded_seq.getUint8Values()); - List int16List = Arrays.asList(0, -32768, 32767); - unbounded_seq.setInt16Values(int16List); - assertEquals(int16List, unbounded_seq.getInt16Values()); - List uint16List = Arrays.asList(0, 1, 65535); - unbounded_seq.setUint16Values(uint16List); - assertEquals(uint16List, unbounded_seq.getUint16Values()); - List int32List = Arrays.asList(0, -2147483648, 2147483647); - unbounded_seq.setInt32Values(int32List); - assertEquals(int32List, unbounded_seq.getInt32Values()); - List uint32List = Arrays.asList(0, 1, 4294967295L); - unbounded_seq.setUint32Values(uint32List); - assertEquals(uint32List, unbounded_seq.getUint32Values()); - List int64List = Arrays.asList(0, -9223372036854775808L, 9223372036854775807L); - unbounded_seq.setInt64Values(int64List); - assertEquals(int64List, unbounded_seq.getInt64Values()); - List uint64List = Arrays.asList(0, 1, -1); - unbounded_seq.setUint64Values(uint64List); - assertEquals(uint64List, unbounded_seq.getUint64Values()); + unbounded_seq.setBoolValues(fixture.boolList); + assertArrayEquals(fixture.boolArr, unbounded_seq.getBoolValues()); + assertEquals(fixture.boolList, unbounded_seq.getBoolValuesAsList()); + unbounded_seq.setByteValues(fixture.byteList); + assertArrayEquals(fixture.byteArr, unbounded_seq.getByteValues()); + assertEquals(fixture.byteList, unbounded_seq.getByteValuesAsList()); + unbounded_seq.setCharValues(fixture.charList); + assertArrayEquals(fixture.charArr, unbounded_seq.getCharValues()); + assertEquals(fixture.charList, unbounded_seq.getCharValuesAsList()); + unbounded_seq.setFloat32Values(fixture.float32List); + assertTrue(Arrays.equals(fixture.float32Arr, unbounded_seq.getFloat32Values())); + assertEquals(fixture.float32List, unbounded_seq.getFloat32ValuesAsList()); + unbounded_seq.setFloat64Values(fixture.float64List); + assertTrue(Arrays.equals(fixture.float64Arr, unbounded_seq.getFloat64Values())); + assertEquals(fixture.float64List, unbounded_seq.getFloat64ValuesAsList()); + unbounded_seq.setInt8Values(fixture.int8List); + assertArrayEquals(fixture.int8Arr, unbounded_seq.getInt8Values()); + assertEquals(fixture.int8List, unbounded_seq.getInt8ValuesAsList()); + unbounded_seq.setUint8Values(fixture.uint8List); + assertArrayEquals(fixture.uint8Arr, unbounded_seq.getUint8Values()); + assertEquals(fixture.uint8List, unbounded_seq.getUint8ValuesAsList()); + unbounded_seq.setInt16Values(fixture.int16List); + assertTrue(Arrays.equals(fixture.int16Arr, unbounded_seq.getInt16Values())); + assertEquals(fixture.int16List, unbounded_seq.getInt16ValuesAsList()); + unbounded_seq.setUint16Values(fixture.uint16List); + assertTrue(Arrays.equals(fixture.uint16Arr, unbounded_seq.getUint16Values())); + assertEquals(fixture.uint16List, unbounded_seq.getUint16ValuesAsList()); + unbounded_seq.setInt32Values(fixture.int32List); + assertArrayEquals(fixture.int32Arr, unbounded_seq.getInt32Values()); + assertEquals(fixture.int32List, unbounded_seq.getInt32ValuesAsList()); + unbounded_seq.setUint32Values(fixture.uint32List); + assertArrayEquals(fixture.uint32Arr, unbounded_seq.getUint32Values()); + assertEquals(fixture.uint32List, unbounded_seq.getUint32ValuesAsList()); + unbounded_seq.setInt64Values(fixture.int64List); + assertArrayEquals(fixture.int64Arr, unbounded_seq.getInt64Values()); + assertEquals(fixture.int64List, unbounded_seq.getInt64ValuesAsList()); + unbounded_seq.setUint64Values(fixture.uint64List); + assertArrayEquals(fixture.uint64Arr, unbounded_seq.getUint64Values()); + assertEquals(fixture.uint64List, unbounded_seq.getUint64ValuesAsList()); // Test setting/getting fixed length unbounded_seq of strings - List stringList = Arrays.asList("", "min value", "max_value"); - unbounded_seq.setStringValues(stringList); - assertEquals(stringList, unbounded_seq.getStringValues()); + unbounded_seq.setStringValues(fixture.stringList); + assertArrayEquals(fixture.stringArr, unbounded_seq.getStringValues()); + assertEquals(fixture.stringList, unbounded_seq.getStringValuesAsList()); // Test setting/getting fixed length unbounded_seq of nested types - rosidl_generator_java.msg.BasicTypes basicTypes = new rosidl_generator_java.msg.BasicTypes(); - List basicTypesList = Arrays.asList( - new rosidl_generator_java.msg.BasicTypes[] {basicTypes, basicTypes, basicTypes}); - unbounded_seq.setBasicTypesValues(basicTypesList); - assertEquals(basicTypesList, unbounded_seq.getBasicTypesValues()); - rosidl_generator_java.msg.Constants constants = new rosidl_generator_java.msg.Constants(); - List constantsList = Arrays.asList( - new rosidl_generator_java.msg.Constants[] {constants, constants, constants}); - unbounded_seq.setConstantsValues(constantsList); - assertEquals(constantsList, unbounded_seq.getConstantsValues()); - rosidl_generator_java.msg.Defaults defaults = new rosidl_generator_java.msg.Defaults(); - List defaultsList = Arrays.asList( - new rosidl_generator_java.msg.Defaults[] {defaults, defaults, defaults}); - unbounded_seq.setDefaultsValues(defaultsList); - assertEquals(defaultsList, unbounded_seq.getDefaultsValues()); + unbounded_seq.setBasicTypesValues(fixture.basicTypesList); + assertArrayEquals(fixture.basicTypesArr, unbounded_seq.getBasicTypesValues()); + assertEquals(fixture.basicTypesList, unbounded_seq.getBasicTypesValuesAsList()); + unbounded_seq.setConstantsValues(fixture.constantsList); + assertArrayEquals(fixture.constantsArr, unbounded_seq.getConstantsValues()); + assertEquals(fixture.constantsList, unbounded_seq.getConstantsValuesAsList()); + unbounded_seq.setDefaultsValues(fixture.defaultsList); + assertArrayEquals(fixture.defaultsArr, unbounded_seq.getDefaultsValues()); + assertEquals(fixture.defaultsList, unbounded_seq.getDefaultsValuesAsList()); assertEquals(42, unbounded_seq.getAlignmentCheck()); }